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

Copter: Wait 4 seconds before beginning to land during failsafes

This commit is contained in:
Jonathan Challinger 2014-07-06 02:16:27 -07:00 committed by Randy Mackay
parent cd08ec5a5f
commit 09a01a4668
3 changed files with 62 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
ArduCopter/config.h
View File

@ -464,6 +464,9 @@
#ifndef LAND_REPOSITION_DEFAULT
# define LAND_REPOSITION_DEFAULT 0 // by default the pilot cannot override roll/pitch during landing
#endif
#ifndef LAND_WITH_DELAY_MS
# define LAND_WITH_DELAY_MS 4000 // default delay (in milliseconds) when a land-with-delay is triggered during a failsafe event
#endif
//////////////////////////////////////////////////////////////////////////////
// CAMERA TRIGGER AND CONTROL

36
ArduCopter/control_land.pde
View File

@ -4,6 +4,9 @@
static uint16_t land_detector;
static bool land_with_gps;
static uint32_t land_start_time;
static bool land_pause;
// land_init - initialise land controller
static bool land_init(bool ignore_checks)
{
@ -23,6 +26,10 @@ static bool land_init(bool ignore_checks)
// initialise altitude target to stopping point
pos_control.set_target_to_stopping_point_z();
land_start_time = millis();
land_pause = false;
return true;
}
@ -90,8 +97,17 @@ static void land_gps_run()
// call attitude controller
attitude_control.angle_ef_roll_pitch_rate_ef_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
//pause 4 seconds before beginning land descent
float cmb_rate;
if(land_pause && millis()-land_start_time < 4000) {
cmb_rate = 0;
} else {
land_pause = false;
cmb_rate = get_throttle_land();
}
// update altitude target and call position controller
pos_control.set_alt_target_from_climb_rate(get_throttle_land(), G_Dt);
pos_control.set_alt_target_from_climb_rate(cmb_rate, G_Dt);
pos_control.update_z_controller();
}
@ -139,8 +155,17 @@ static void land_nogps_run()
// call attitude controller
attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
//pause 4 seconds before beginning land descent
float cmb_rate;
if(land_pause && millis()-land_start_time < LAND_WITH_DELAY_MS) {
cmb_rate = 0;
} else {
land_pause = false;
cmb_rate = get_throttle_land();
}
// call position controller
pos_control.set_alt_target_from_climb_rate(get_throttle_land(), G_Dt);
pos_control.set_alt_target_from_climb_rate(cmb_rate, G_Dt);
pos_control.update_z_controller();
}
@ -191,3 +216,10 @@ static void land_do_not_use_GPS()
{
land_with_gps = false;
}
// set_mode_land_with_pause - sets mode to LAND and triggers 4 second delay before descent starts
static void set_mode_land_with_pause()
{
set_mode(LAND);
land_pause = true;
}

50
ArduCopter/events.pde
View File

@ -21,14 +21,14 @@ static void failsafe_radio_on_event()
init_disarm_motors();
}else if(g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_LAND) {
// if failsafe_throttle is 3 (i.e. FS_THR_ENABLED_ALWAYS_LAND) land immediately
set_mode(LAND);
set_mode_land_with_pause();
}else if(home_distance > wp_nav.get_wp_radius()) {
if (!set_mode(RTL)) {
set_mode(LAND);
set_mode_land_with_pause();
}
}else{
// We have no GPS or are very close to home so we will land
set_mode(LAND);
set_mode_land_with_pause();
}
break;
case AUTO:
@ -36,15 +36,15 @@ static void failsafe_radio_on_event()
if (g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_RTL) {
if(home_distance > wp_nav.get_wp_radius()) {
if (!set_mode(RTL)) {
set_mode(LAND);
set_mode_land_with_pause();
}
}else{
// We are very close to home so we will land
set_mode(LAND);
set_mode_land_with_pause();
}
}else if(g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_LAND) {
// if failsafe_throttle is 3 (i.e. FS_THR_ENABLED_ALWAYS_LAND) land immediately
set_mode(LAND);
set_mode_land_with_pause();
}
// if failsafe_throttle is 2 (i.e. FS_THR_ENABLED_CONTINUE_MISSION) no need to do anything
break;
@ -56,14 +56,14 @@ static void failsafe_radio_on_event()
init_disarm_motors();
}else if(g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_LAND) {
// if failsafe_throttle is 3 (i.e. FS_THR_ENABLED_ALWAYS_LAND) land immediately
set_mode(LAND);
set_mode_land_with_pause();
}else if(home_distance > wp_nav.get_wp_radius()) {
if (!set_mode(RTL)) {
set_mode(LAND);
set_mode_land_with_pause();
}
}else{
// We have no GPS or are very close to home so we will land
set_mode(LAND);
set_mode_land_with_pause();
}
break;
case LAND:
@ -74,14 +74,14 @@ static void failsafe_radio_on_event()
default:
if(g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_LAND) {
// if failsafe_throttle is 3 (i.e. FS_THR_ENABLED_ALWAYS_LAND) land immediately
set_mode(LAND);
set_mode_land_with_pause();
}else if(home_distance > wp_nav.get_wp_radius()) {
if (!set_mode(RTL)){
set_mode(LAND);
set_mode_land_with_pause();
}
}else{
// We have no GPS or are very close to home so we will land
set_mode(LAND);
set_mode_land_with_pause();
}
break;
}
@ -121,10 +121,10 @@ static void failsafe_battery_event(void)
// set mode to RTL or LAND
if (g.failsafe_battery_enabled == FS_BATT_RTL && home_distance > wp_nav.get_wp_radius()) {
if (!set_mode(RTL)) {
set_mode(LAND);
set_mode_land_with_pause();
}
}else{
set_mode(LAND);
set_mode_land_with_pause();
}
}
break;
@ -132,10 +132,10 @@ static void failsafe_battery_event(void)
// set mode to RTL or LAND
if (home_distance > wp_nav.get_wp_radius()) {
if (!set_mode(RTL)) {
set_mode(LAND);
set_mode_land_with_pause();
}
}else{
set_mode(LAND);
set_mode_land_with_pause();
}
break;
case LOITER:
@ -150,10 +150,10 @@ static void failsafe_battery_event(void)
// set mode to RTL or LAND
if (g.failsafe_battery_enabled == FS_BATT_RTL && home_distance > wp_nav.get_wp_radius()) {
if (!set_mode(RTL)) {
set_mode(LAND);
set_mode_land_with_pause();
}
}else{
set_mode(LAND);
set_mode_land_with_pause();
}
break;
}
@ -212,7 +212,7 @@ static void failsafe_gps_check()
if (g.failsafe_gps_enabled == FS_GPS_ALTHOLD && !failsafe.radio) {
set_mode(ALT_HOLD);
}else{
set_mode(LAND);
set_mode_land_with_pause();
}
}
@ -277,11 +277,11 @@ static void failsafe_gcs_check()
init_disarm_motors();
}else if(home_distance > wp_nav.get_wp_radius()) {
if (!set_mode(RTL)) {
set_mode(LAND);
set_mode_land_with_pause();
}
}else{
// We have no GPS or are very close to home so we will land
set_mode(LAND);
set_mode_land_with_pause();
}
break;
case AUTO:
@ -289,11 +289,11 @@ static void failsafe_gcs_check()
if (g.failsafe_gcs == FS_GCS_ENABLED_ALWAYS_RTL) {
if (home_distance > wp_nav.get_wp_radius()) {
if (!set_mode(RTL)) {
set_mode(LAND);
set_mode_land_with_pause();
}
}else{
// We are very close to home so we will land
set_mode(LAND);
set_mode_land_with_pause();
}
}
// if failsafe_throttle is 2 (i.e. FS_THR_ENABLED_CONTINUE_MISSION) no need to do anything
@ -301,11 +301,11 @@ static void failsafe_gcs_check()
default:
if(home_distance > wp_nav.get_wp_radius()) {
if (!set_mode(RTL)) {
set_mode(LAND);
set_mode_land_with_pause();
}
}else{
// We have no GPS or are very close to home so we will land
set_mode(LAND);
set_mode_land_with_pause();
}
break;
}