Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: rename SafeRTL to SmartRTL

This commit is contained in:
squilter 2017-09-09 11:45:31 +09:00 committed by Randy Mackay
parent 7abb5b10fc
commit 4b57a4a231
15 changed files with 71 additions and 71 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduCopter/ArduCopter.cpp
View File

@ -101,7 +101,7 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = {
SCHED_TASK(update_altitude, 10, 100),
SCHED_TASK(run_nav_updates, 50, 100),
SCHED_TASK(update_throttle_hover,100, 90),
SCHED_TASK(safe_rtl_save_position, 3, 100),
SCHED_TASK(smart_rtl_save_position, 3, 100),
SCHED_TASK(three_hz_loop, 3, 75),
SCHED_TASK(compass_accumulate, 100, 100),
SCHED_TASK(barometer_accumulate, 50, 90),

2
ArduCopter/Copter.cpp
View File

@ -37,7 +37,7 @@ Copter::Copter(void) :
guided_mode(Guided_TakeOff),
rtl_state(RTL_InitialClimb),
rtl_state_complete(false),
safe_rtl_state(SafeRTL_PathFollow),
smart_rtl_state(SafeRTL_PathFollow),
circle_pilot_yaw_override(false),
simple_cos_yaw(1.0f),
simple_sin_yaw(0.0f),

20
ArduCopter/Copter.h
View File

@ -92,7 +92,7 @@
#include <AP_Button/AP_Button.h>
#include <AP_Arming/AP_Arming.h>
#include <AP_VisualOdom/AP_VisualOdom.h>
#include <AP_SafeRTL/AP_SafeRTL.h>
#include <AP_SmartRTL/AP_SmartRTL.h>
// Configuration
#include "defines.h"
@ -415,7 +415,7 @@ private:
} rtl_path;
// SafeRTL
SafeRTLState safe_rtl_state; // records state of SafeRTL
SafeRTLState smart_rtl_state; // records state of SafeRTL
// Circle
bool circle_pilot_yaw_override; // true if pilot is overriding yaw
@ -950,14 +950,14 @@ private:
void rtl_land_run();
void rtl_build_path(bool terrain_following_allowed);
void rtl_compute_return_target(bool terrain_following_allowed);
bool safe_rtl_init(bool ignore_checks);
void safe_rtl_exit();
void safe_rtl_run();
void safe_rtl_wait_cleanup_run();
void safe_rtl_path_follow_run();
void safe_rtl_pre_land_position_run();
void safe_rtl_land();
void safe_rtl_save_position();
bool smart_rtl_init(bool ignore_checks);
void smart_rtl_exit();
void smart_rtl_run();
void smart_rtl_wait_cleanup_run();
void smart_rtl_path_follow_run();
void smart_rtl_pre_land_position_run();
void smart_rtl_land();
void smart_rtl_save_position();
bool sport_init(bool ignore_checks);
void sport_run();
bool stabilize_init(bool ignore_checks);

2
ArduCopter/GCS_Mavlink.cpp
View File

@ -52,7 +52,7 @@ NOINLINE void Copter::send_heartbeat(mavlink_channel_t chan)
case CIRCLE:
case POSHOLD:
case BRAKE:
case SAFE_RTL:
case SMART_RTL:
base_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
// note that MAV_MODE_FLAG_AUTO_ENABLED does not match what
// APM does in any mode, as that is defined as "system finds its own goal

20
ArduCopter/Parameters.cpp
View File

@ -297,42 +297,42 @@ const AP_Param::Info Copter::var_info[] = {
// @Param: FLTMODE1
// @DisplayName: Flight Mode 1
// @Description: Flight mode when Channel 5 pwm is <= 1230
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Safe_RTL
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL
// @User: Standard
GSCALAR(flight_mode1, "FLTMODE1", FLIGHT_MODE_1),
// @Param: FLTMODE2
// @DisplayName: Flight Mode 2
// @Description: Flight mode when Channel 5 pwm is >1230, <= 1360
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Safe_RTL
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL
// @User: Standard
GSCALAR(flight_mode2, "FLTMODE2", FLIGHT_MODE_2),
// @Param: FLTMODE3
// @DisplayName: Flight Mode 3
// @Description: Flight mode when Channel 5 pwm is >1360, <= 1490
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Safe_RTL
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL
// @User: Standard
GSCALAR(flight_mode3, "FLTMODE3", FLIGHT_MODE_3),
// @Param: FLTMODE4
// @DisplayName: Flight Mode 4
// @Description: Flight mode when Channel 5 pwm is >1490, <= 1620
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Safe_RTL
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL
// @User: Standard
GSCALAR(flight_mode4, "FLTMODE4", FLIGHT_MODE_4),
// @Param: FLTMODE5
// @DisplayName: Flight Mode 5
// @Description: Flight mode when Channel 5 pwm is >1620, <= 1749
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Safe_RTL
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL
// @User: Standard
GSCALAR(flight_mode5, "FLTMODE5", FLIGHT_MODE_5),
// @Param: FLTMODE6
// @DisplayName: Flight Mode 6
// @Description: Flight mode when Channel 5 pwm is >=1750
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Safe_RTL
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL
// @User: Standard
GSCALAR(flight_mode6, "FLTMODE6", FLIGHT_MODE_6),
@ -1006,9 +1006,9 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
// ID 19 reserved for TCAL (PR pending)
// ID 20 reserved for TX_TYPE (PR pending)
// @Group: SAFERTL_
// @Path: ../libraries/AP_SafeRTL/AP_SafeRTL.cpp
AP_SUBGROUPINFO(safe_rtl, "SAFERTL_", 21, ParametersG2, AP_SafeRTL),
// @Group: SRTL_
// @Path: ../libraries/AP_SmartRTL/AP_SmartRTL.cpp
AP_SUBGROUPINFO(smart_rtl, "SRTL_", 21, ParametersG2, AP_SmartRTL),
AP_GROUPEND
};
@ -1025,7 +1025,7 @@ ParametersG2::ParametersG2(void)
#if ADVANCED_FAILSAFE == ENABLED
,afs(copter.mission, copter.barometer, copter.gps, copter.rcmap)
#endif
,safe_rtl(copter.ahrs)
,smart_rtl(copter.ahrs)
{
AP_Param::setup_object_defaults(this, var_info);
}

2
ArduCopter/Parameters.h
View File

@ -569,7 +569,7 @@ public:
SRV_Channels servo_channels;
// Safe RTL library
AP_SafeRTL safe_rtl;
AP_SmartRTL smart_rtl;
};
extern const AP_Param::Info var_info[];

54
ArduCopter/control_safe_rtl.cpp
View File

@ -1,15 +1,15 @@
#include "Copter.h"
/*
* Init and run calls for Safe_RTL flight mode
* Init and run calls for Smart_RTL flight mode
*
* This code uses the SafeRTL path that is already in memory, and feeds it into WPNav, one point at a time.
* Once the copter is close to home, it will run a standard land controller.
*/
bool Copter::safe_rtl_init(bool ignore_checks)
bool Copter::smart_rtl_init(bool ignore_checks)
{
if ((position_ok() || ignore_checks) && g2.safe_rtl.is_active()) {
if ((position_ok() || ignore_checks) && g2.smart_rtl.is_active()) {
// initialise waypoint and spline controller
wp_nav->wp_and_spline_init();
@ -23,30 +23,30 @@ bool Copter::safe_rtl_init(bool ignore_checks)
set_auto_yaw_mode(get_default_auto_yaw_mode(true));
// wait for cleanup of return path
safe_rtl_state = SafeRTL_WaitForPathCleanup;
smart_rtl_state = SafeRTL_WaitForPathCleanup;
return true;
}
return false;
}
// perform cleanup required when leaving safe_rtl
void Copter::safe_rtl_exit()
// perform cleanup required when leaving smart_rtl
void Copter::smart_rtl_exit()
{
g2.safe_rtl.cancel_request_for_thorough_cleanup();
g2.smart_rtl.cancel_request_for_thorough_cleanup();
}
void Copter::safe_rtl_run()
void Copter::smart_rtl_run()
{
switch (safe_rtl_state) {
switch (smart_rtl_state) {
case SafeRTL_WaitForPathCleanup:
safe_rtl_wait_cleanup_run();
smart_rtl_wait_cleanup_run();
break;
case SafeRTL_PathFollow:
safe_rtl_path_follow_run();
smart_rtl_path_follow_run();
break;
case SafeRTL_PreLandPosition:
safe_rtl_pre_land_position_run();
smart_rtl_pre_land_position_run();
break;
case SafeRTL_Descend:
rtl_descent_run(); // Re-using the descend method from normal rtl mode.
@ -57,7 +57,7 @@ void Copter::safe_rtl_run()
}
}
void Copter::safe_rtl_wait_cleanup_run()
void Copter::smart_rtl_wait_cleanup_run()
{
// hover at current target position
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
@ -66,22 +66,22 @@ void Copter::safe_rtl_wait_cleanup_run()
attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), get_auto_heading(),true, get_smoothing_gain());
// check if return path is computed and if yes, begin journey home
if (g2.safe_rtl.request_thorough_cleanup()) {
safe_rtl_state = SafeRTL_PathFollow;
if (g2.smart_rtl.request_thorough_cleanup()) {
smart_rtl_state = SafeRTL_PathFollow;
}
}
void Copter::safe_rtl_path_follow_run()
void Copter::smart_rtl_path_follow_run()
{
// if we are close to current target point, switch the next point to be our target.
if (wp_nav->reached_wp_destination()) {
Vector3f next_point;
if (g2.safe_rtl.pop_point(next_point)) {
if (g2.smart_rtl.pop_point(next_point)) {
bool fast_waypoint = true;
if (g2.safe_rtl.get_num_points() == 0) {
if (g2.smart_rtl.get_num_points() == 0) {
// this is the very last point, add 2m to the target alt and move to pre-land state
next_point.z -= 2.0f;
safe_rtl_state = SafeRTL_PreLandPosition;
smart_rtl_state = SafeRTL_PreLandPosition;
fast_waypoint = false;
}
// send target to waypoint controller
@ -89,7 +89,7 @@ void Copter::safe_rtl_path_follow_run()
wp_nav->set_fast_waypoint(fast_waypoint);
} else {
// this can only happen if we fail to get the semaphore which should never happen but just in case, land
safe_rtl_state = SafeRTL_PreLandPosition;
smart_rtl_state = SafeRTL_PreLandPosition;
}
}
@ -108,18 +108,18 @@ void Copter::safe_rtl_path_follow_run()
}
}
void Copter::safe_rtl_pre_land_position_run()
void Copter::smart_rtl_pre_land_position_run()
{
// if we are close to 2m above start point, we are ready to land.
if (wp_nav->reached_wp_destination()) {
// choose descend and hold, or land based on user parameter rtl_alt_final
if (g.rtl_alt_final <= 0 || failsafe.radio) {
rtl_land_start();
safe_rtl_state = SafeRTL_Land;
smart_rtl_state = SafeRTL_Land;
} else {
rtl_path.descent_target.alt = g.rtl_alt_final;
rtl_descent_start();
safe_rtl_state = SafeRTL_Descend;
smart_rtl_state = SafeRTL_Descend;
}
}
@ -130,10 +130,10 @@ void Copter::safe_rtl_pre_land_position_run()
attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), get_auto_heading(), true, get_smoothing_gain());
}
// save current position for use by the safe_rtl flight mode
void Copter::safe_rtl_save_position()
// save current position for use by the smart_rtl flight mode
void Copter::smart_rtl_save_position()
{
const bool save_position = motors->armed() && (control_mode != SAFE_RTL);
const bool save_position = motors->armed() && (control_mode != SMART_RTL);
g2.safe_rtl.update(position_ok(), save_position);
g2.smart_rtl.update(position_ok(), save_position);
}

4
ArduCopter/defines.h
View File

@ -70,7 +70,7 @@ enum aux_sw_func {
AUXSW_PRECISION_LOITER = 39, // enable precision loiter
AUXSW_AVOID_PROXIMITY = 40, // enable object avoidance using proximity sensors (ie. horizontal lidar)
AUXSW_ARMDISARM = 41, // arm or disarm vehicle
AUXSW_SAFE_RTL = 42, // change to SafeRTL flight mode
AUXSW_SMART_RTL = 42, // change to SafeRTL flight mode
AUXSW_SWITCH_MAX,
};
@ -103,7 +103,7 @@ enum control_mode_t {
THROW = 18, // throw to launch mode using inertial/GPS system, no pilot input
AVOID_ADSB = 19, // automatic avoidance of obstacles in the macro scale - e.g. full-sized aircraft
GUIDED_NOGPS = 20, // guided mode but only accepts attitude and altitude
SAFE_RTL = 21, // SAFE_RTL returns to home by retracing its steps
SMART_RTL = 21, // SMART_RTL returns to home by retracing its steps
};
enum mode_reason_t {

18
ArduCopter/flight_mode.cpp
View File

@ -109,8 +109,8 @@ bool Copter::set_mode(control_mode_t mode, mode_reason_t reason)
success = guided_nogps_init(ignore_checks);
break;
case SAFE_RTL:
success = safe_rtl_init(ignore_checks);
case SMART_RTL:
success = smart_rtl_init(ignore_checks);
break;
default:
@ -251,8 +251,8 @@ void Copter::update_flight_mode()
break;
case SAFE_RTL:
safe_rtl_run();
case SMART_RTL:
smart_rtl_run();
break;
default:
@ -288,9 +288,9 @@ void Copter::exit_mode(control_mode_t old_control_mode, control_mode_t new_contr
// cancel any takeoffs in progress
takeoff_stop();
// call safe_rtl cleanup
if (old_control_mode == SAFE_RTL) {
safe_rtl_exit();
// call smart_rtl cleanup
if (old_control_mode == SMART_RTL) {
smart_rtl_exit();
}
#if FRAME_CONFIG == HELI_FRAME
@ -321,7 +321,7 @@ bool Copter::mode_requires_GPS(control_mode_t mode)
case GUIDED:
case LOITER:
case RTL:
case SAFE_RTL:
case SMART_RTL:
case CIRCLE:
case DRIFT:
case POSHOLD:
@ -369,7 +369,7 @@ void Copter::notify_flight_mode(control_mode_t mode)
case AVOID_ADSB:
case GUIDED_NOGPS:
case LAND:
case SAFE_RTL:
case SMART_RTL:
// autopilot modes
AP_Notify::flags.autopilot_mode = true;
break;

2
ArduCopter/heli.cpp
View File

@ -108,7 +108,7 @@ void Copter::heli_update_landing_swash()
break;
case RTL:
case SAFE_RTL:
case SMART_RTL:
if (rtl_state == RTL_Land) {
motors->set_collective_for_landing(true);
}else{

2
ArduCopter/motors.cpp
View File

@ -180,7 +180,7 @@ bool Copter::init_arm_motors(bool arming_from_gcs)
calc_distance_and_bearing();
// Reset SafeRTL return location. If activated, SafeRTL will ultimately try to land at this point
g2.safe_rtl.reset_path(position_ok());
g2.smart_rtl.reset_path(position_ok());
// enable gps velocity based centrefugal force compensation
ahrs.set_correct_centrifugal(true);

4
ArduCopter/navigation.cpp
View File

@ -32,7 +32,7 @@ void Copter::calc_wp_distance()
case AUTO:
case RTL:
case SAFE_RTL:
case SMART_RTL:
wp_distance = wp_nav->get_wp_distance_to_destination();
break;
@ -60,7 +60,7 @@ void Copter::calc_wp_bearing()
case AUTO:
case RTL:
case SAFE_RTL:
case SMART_RTL:
wp_bearing = wp_nav->get_wp_bearing_to_destination();
break;

2
ArduCopter/sensors.cpp
View File

@ -360,7 +360,7 @@ void Copter::update_sensor_status_flags(void)
case POSHOLD:
case BRAKE:
case THROW:
case SAFE_RTL:
case SMART_RTL:
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL;
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL;
break;

6
ArduCopter/switches.cpp
View File

@ -595,13 +595,13 @@ void Copter::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag)
}
break;
case AUXSW_SAFE_RTL:
case AUXSW_SMART_RTL:
if (ch_flag == AUX_SWITCH_HIGH) {
// engage SafeRTL (if not possible we remain in current flight mode)
set_mode(SAFE_RTL, MODE_REASON_TX_COMMAND);
set_mode(SMART_RTL, MODE_REASON_TX_COMMAND);
} else {
// return to flight mode switch's flight mode if we are currently in RTL
if (control_mode == SAFE_RTL) {
if (control_mode == SMART_RTL) {
reset_control_switch();
}
}

2
ArduCopter/system.cpp
View File

@ -229,7 +229,7 @@ void Copter::init_ardupilot()
mission.init();
// initialize SafeRTL
g2.safe_rtl.init();
g2.smart_rtl.init();
// initialise DataFlash library
DataFlash.set_mission(&mission);