diff --git a/ArduCopter/ArduCopter.cpp b/ArduCopter/ArduCopter.cpp index d7bc6cec37..709ffdb5f5 100644 --- a/ArduCopter/ArduCopter.cpp +++ b/ArduCopter/ArduCopter.cpp @@ -64,6 +64,7 @@ * Roberto Navoni :Library testing, Porting to VRBrain * Sandro Benigno :Camera support, MinimOSD * Sandro Tognana :PosHold flight mode + * Sebastian Quilter :SafeRTL * ..and many more. * * Code commit statistics can be found here: https://github.com/ArduPilot/ardupilot/graphs/contributors @@ -100,6 +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(three_hz_loop, 3, 75), SCHED_TASK(compass_accumulate, 100, 100), SCHED_TASK(barometer_accumulate, 50, 90), diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index 019c9f9bef..951717709f 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -37,6 +37,7 @@ Copter::Copter(void) : guided_mode(Guided_TakeOff), rtl_state(RTL_InitialClimb), rtl_state_complete(false), + safe_rtl_state(SafeRTL_PathFollow), circle_pilot_yaw_override(false), simple_cos_yaw(1.0f), simple_sin_yaw(0.0f), diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 9acc462d8d..a654fa804d 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -92,6 +92,7 @@ #include #include #include +#include // Configuration #include "defines.h" @@ -413,6 +414,9 @@ private: bool terrain_used; } rtl_path; + // SafeRTL + SafeRTLState safe_rtl_state; // records state of SafeRTL + // Circle bool circle_pilot_yaw_override; // true if pilot is overriding yaw @@ -946,6 +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 sport_init(bool ignore_checks); void sport_run(); bool stabilize_init(bool ignore_checks); diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index 743541b1f7..baeded1908 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -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 + // @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 // @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 + // @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 // @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 + // @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 // @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 + // @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 // @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 + // @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 // @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 + // @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 // @User: Standard GSCALAR(flight_mode6, "FLTMODE6", FLIGHT_MODE_6), @@ -389,42 +389,42 @@ const AP_Param::Info Copter::var_info[] = { // @Param: CH7_OPT // @DisplayName: Channel 7 option // @Description: Select which function is performed when CH7 is above 1800 pwm - // @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 13:Super Simple Mode, 14:Acro Trainer, 15:Sprayer, 16:Auto, 17:AutoTune, 18:Land, 19:Gripper, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw, 38:ADSB-Avoidance, 39:PrecLoiter, 40:Object Avoidance, 41:ArmDisarm + // @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 13:Super Simple Mode, 14:Acro Trainer, 15:Sprayer, 16:Auto, 17:AutoTune, 18:Land, 19:Gripper, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw, 38:ADSB-Avoidance, 39:PrecLoiter, 40:Object Avoidance, 41:ArmDisarm, 52:SafeRTL // @User: Standard GSCALAR(ch7_option, "CH7_OPT", AUXSW_DO_NOTHING), // @Param: CH8_OPT // @DisplayName: Channel 8 option // @Description: Select which function is performed when CH8 is above 1800 pwm - // @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 13:Super Simple Mode, 14:Acro Trainer, 15:Sprayer, 16:Auto, 17:AutoTune, 18:Land, 19:Gripper, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw, 38:ADSB-Avoidance, 39:PrecLoiter, 40:Object Avoidance, 41:ArmDisarm + // @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 13:Super Simple Mode, 14:Acro Trainer, 15:Sprayer, 16:Auto, 17:AutoTune, 18:Land, 19:Gripper, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw, 38:ADSB-Avoidance, 39:PrecLoiter, 40:Object Avoidance, 41:ArmDisarm, 52:SafeRTL // @User: Standard GSCALAR(ch8_option, "CH8_OPT", AUXSW_DO_NOTHING), // @Param: CH9_OPT // @DisplayName: Channel 9 option // @Description: Select which function is performed when CH9 is above 1800 pwm - // @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 13:Super Simple Mode, 14:Acro Trainer, 15:Sprayer, 16:Auto, 17:AutoTune, 18:Land, 19:Gripper, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw, 38:ADSB-Avoidance, 39:PrecLoiter, 40:Object Avoidance, 41:ArmDisarm + // @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 13:Super Simple Mode, 14:Acro Trainer, 15:Sprayer, 16:Auto, 17:AutoTune, 18:Land, 19:Gripper, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw, 38:ADSB-Avoidance, 39:PrecLoiter, 40:Object Avoidance, 41:ArmDisarm, 52:SafeRTL // @User: Standard GSCALAR(ch9_option, "CH9_OPT", AUXSW_DO_NOTHING), // @Param: CH10_OPT // @DisplayName: Channel 10 option // @Description: Select which function is performed when CH10 is above 1800 pwm - // @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 13:Super Simple Mode, 14:Acro Trainer, 15:Sprayer, 16:Auto, 17:AutoTune, 18:Land, 19:Gripper, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw, 38:ADSB-Avoidance, 39:PrecLoiter, 40:Object Avoidance, 41:ArmDisarm + // @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 13:Super Simple Mode, 14:Acro Trainer, 15:Sprayer, 16:Auto, 17:AutoTune, 18:Land, 19:Gripper, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw, 38:ADSB-Avoidance, 39:PrecLoiter, 40:Object Avoidance, 41:ArmDisarm, 52:SafeRTL // @User: Standard GSCALAR(ch10_option, "CH10_OPT", AUXSW_DO_NOTHING), // @Param: CH11_OPT // @DisplayName: Channel 11 option // @Description: Select which function is performed when CH11 is above 1800 pwm - // @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 13:Super Simple Mode, 14:Acro Trainer, 15:Sprayer, 16:Auto, 17:AutoTune, 18:Land, 19:Gripper, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw, 38:ADSB-Avoidance, 39:PrecLoiter, 40:Object Avoidance, 41:ArmDisarm + // @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 13:Super Simple Mode, 14:Acro Trainer, 15:Sprayer, 16:Auto, 17:AutoTune, 18:Land, 19:Gripper, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw, 38:ADSB-Avoidance, 39:PrecLoiter, 40:Object Avoidance, 41:ArmDisarm, 52:SafeRTL // @User: Standard GSCALAR(ch11_option, "CH11_OPT", AUXSW_DO_NOTHING), // @Param: CH12_OPT // @DisplayName: Channel 12 option // @Description: Select which function is performed when CH12 is above 1800 pwm - // @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 13:Super Simple Mode, 14:Acro Trainer, 15:Sprayer, 16:Auto, 17:AutoTune, 18:Land, 19:Gripper, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw, 38:ADSB-Avoidance, 39:PrecLoiter, 40:Object Avoidance, 41:ArmDisarm + // @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 13:Super Simple Mode, 14:Acro Trainer, 15:Sprayer, 16:Auto, 17:AutoTune, 18:Land, 19:Gripper, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw, 38:ADSB-Avoidance, 39:PrecLoiter, 40:Object Avoidance, 41:ArmDisarm, 52:SafeRTL // @User: Standard GSCALAR(ch12_option, "CH12_OPT", AUXSW_DO_NOTHING), @@ -1005,7 +1005,11 @@ 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), + AP_GROUPEND }; @@ -1021,6 +1025,7 @@ ParametersG2::ParametersG2(void) #if ADVANCED_FAILSAFE == ENABLED ,afs(copter.mission, copter.barometer, copter.gps, copter.rcmap) #endif + ,safe_rtl(copter.ahrs) { AP_Param::setup_object_defaults(this, var_info); } diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 039d173b92..4bdd90a397 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -567,6 +567,9 @@ public: // control over servo output ranges SRV_Channels servo_channels; + + // Safe RTL library + AP_SafeRTL safe_rtl; }; extern const AP_Param::Info var_info[]; diff --git a/ArduCopter/control_safe_rtl.cpp b/ArduCopter/control_safe_rtl.cpp new file mode 100644 index 0000000000..46d4e12384 --- /dev/null +++ b/ArduCopter/control_safe_rtl.cpp @@ -0,0 +1,136 @@ +#include "Copter.h" + +/* + * Init and run calls for Safe_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) +{ + if ((position_ok() || ignore_checks) && g2.safe_rtl.is_active()) { + // initialise waypoint and spline controller + wp_nav->wp_and_spline_init(); + + // set current target to a reasonable stopping point + Vector3f stopping_point; + pos_control->get_stopping_point_xy(stopping_point); + pos_control->get_stopping_point_z(stopping_point); + wp_nav->set_wp_destination(stopping_point); + + // initialise yaw to obey user parameter + set_auto_yaw_mode(get_default_auto_yaw_mode(true)); + + // wait for cleanup of return path + safe_rtl_state = SafeRTL_WaitForPathCleanup; + return true; + } else { + return false; + } +} + +// perform cleanup required when leaving safe_rtl +void Copter::safe_rtl_exit() +{ + g2.safe_rtl.cancel_request_for_thorough_cleanup(); +} + +void Copter::safe_rtl_run() +{ + switch(safe_rtl_state){ + case SafeRTL_WaitForPathCleanup: + safe_rtl_wait_cleanup_run(); + break; + case SafeRTL_PathFollow: + safe_rtl_path_follow_run(); + break; + case SafeRTL_PreLandPosition: + safe_rtl_pre_land_position_run(); + break; + case SafeRTL_Descend: + rtl_descent_run(); // Re-using the descend method from normal rtl mode. + break; + case SafeRTL_Land: + rtl_land_run(); // Re-using the land method from normal rtl mode. + break; + } +} + +void Copter::safe_rtl_wait_cleanup_run() +{ + // hover at current target position + motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); + wp_nav->update_wpnav(); + pos_control->update_z_controller(); + 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; + } +} + +void Copter::safe_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)) { + // this is the very last point, add 2m to the target alt and move to pre-land state + if (g2.safe_rtl.get_num_points() == 0) { + next_point.z -= 2.0f; + safe_rtl_state = SafeRTL_PreLandPosition; + } + // send target to waypoint controller + wp_nav->set_wp_destination_NED(next_point); + } 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; + } + } + + // update controllers + motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); + wp_nav->update_wpnav(); + pos_control->update_z_controller(); + + // call attitude controller + if (auto_yaw_mode == AUTO_YAW_HOLD) { + // roll & pitch from waypoint controller, yaw rate from pilot + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), 0, get_smoothing_gain()); + } else { + // roll, pitch from waypoint controller, yaw heading from auto_heading() + attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), get_auto_heading(),true, get_smoothing_gain()); + } +} + +void Copter::safe_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; + } else { + rtl_path.descent_target.alt = g.rtl_alt_final; + rtl_descent_start(); + safe_rtl_state = SafeRTL_Descend; + } + } + + // update controllers + motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); + wp_nav->update_wpnav(); + pos_control->update_z_controller(); + 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() +{ + bool save_position = motors->armed() && (control_mode != SAFE_RTL); + + g2.safe_rtl.update(position_ok(), save_position); +} diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index ba2f85e544..da94f388ee 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -70,6 +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_SWITCH_MAX, }; @@ -102,6 +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 }; enum mode_reason_t { @@ -218,6 +220,15 @@ enum RTLState { RTL_Land }; +// Safe RTL states +enum SafeRTLState { + SafeRTL_WaitForPathCleanup, + SafeRTL_PathFollow, + SafeRTL_PreLandPosition, + SafeRTL_Descend, + SafeRTL_Land +}; + // Alt_Hold states enum AltHoldModeState { AltHold_MotorStopped, diff --git a/ArduCopter/flight_mode.cpp b/ArduCopter/flight_mode.cpp index 0433fec921..396daa8150 100644 --- a/ArduCopter/flight_mode.cpp +++ b/ArduCopter/flight_mode.cpp @@ -109,6 +109,10 @@ 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); + break; + default: success = false; break; @@ -246,6 +250,11 @@ void Copter::update_flight_mode() guided_nogps_run(); break; + + case SAFE_RTL: + safe_rtl_run(); + break; + default: break; } @@ -279,6 +288,11 @@ 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(); + } + #if FRAME_CONFIG == HELI_FRAME // firmly reset the flybar passthrough to false when exiting acro mode. if (old_control_mode == ACRO) { @@ -307,6 +321,7 @@ bool Copter::mode_requires_GPS(control_mode_t mode) case GUIDED: case LOITER: case RTL: + case SAFE_RTL: case CIRCLE: case DRIFT: case POSHOLD: diff --git a/ArduCopter/motors.cpp b/ArduCopter/motors.cpp index 54071289e2..f9aa17da8a 100644 --- a/ArduCopter/motors.cpp +++ b/ArduCopter/motors.cpp @@ -179,6 +179,9 @@ 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()); + // enable gps velocity based centrefugal force compensation ahrs.set_correct_centrifugal(true); hal.util->set_soft_armed(true); diff --git a/ArduCopter/switches.cpp b/ArduCopter/switches.cpp index c423d008e5..cdc5539e12 100644 --- a/ArduCopter/switches.cpp +++ b/ArduCopter/switches.cpp @@ -594,6 +594,18 @@ void Copter::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag) break; } break; + + case AUXSW_SAFE_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); + } else { + // return to flight mode switch's flight mode if we are currently in RTL + if (control_mode == SAFE_RTL) { + reset_control_switch(); + } + } + break; } } diff --git a/ArduCopter/system.cpp b/ArduCopter/system.cpp index 806b286c5d..8e0d91d399 100644 --- a/ArduCopter/system.cpp +++ b/ArduCopter/system.cpp @@ -228,6 +228,9 @@ void Copter::init_ardupilot() // initialise mission library mission.init(); + // initialize SafeRTL cleanup methods + g2.safe_rtl.init(); + // initialise DataFlash library DataFlash.set_mission(&mission); DataFlash.setVehicle_Startup_Log_Writer(FUNCTOR_BIND(&copter, &Copter::Log_Write_Vehicle_Startup_Messages, void));