Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-08 17:08:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: add SafeRTL flight mode

Browse Source
This commit is contained in:
squilter 2017-07-26 13:14:40 -04:00 committed by Randy Mackay
parent ec8c4b1744
commit ba0e08552f
11 changed files with 216 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduCopter/ArduCopter.cpp
View File

@ -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),

1
ArduCopter/Copter.cpp
View File

@ -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),

12
ArduCopter/Copter.h
View File

@ -92,6 +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>
// 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);

31
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
// @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);
}

3
ArduCopter/Parameters.h
View File

@ -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[];

136
ArduCopter/control_safe_rtl.cpp Normal file
View File

@ -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);
}

11
ArduCopter/defines.h
View File

@ -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,

15
ArduCopter/flight_mode.cpp
View File

@ -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:

3
ArduCopter/motors.cpp
View File

@ -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);

12
ArduCopter/switches.cpp
View File

@ -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;
}
}

3
ArduCopter/system.cpp
View File

@ -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));