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: Thr and Batt failsafe disarm when landed

Browse Source 
When throttle/radio failsafe or battery failsafe are triggered, the
vehicle will disarm if it is landed even if the throttle is not at zero.
Auto mode will disarm if landed and mission has not started.  This
ensures that the vehicle does not disarm during a mission land command
that appears mid way through a mission.
This commit is contained in:
Randy Mackay 2014-07-31 14:58:34 +09:00
parent 77d5b2171e
commit 85b4ba6142
1 changed files with 46 additions and 47 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

93
ArduCopter/events.pde
View File

@ -15,25 +15,37 @@ static void failsafe_radio_on_event()
switch(control_mode) {
case STABILIZE:
case ACRO:
case SPORT:
// if throttle is zero disarm motors
if (g.rc_3.control_in == 0) {
// if throttle is zero OR vehicle is landed disarm motors
if (g.rc_3.control_in == 0 || ap.land_complete) {
init_disarm_motors();
// if failsafe_throttle is FS_THR_ENABLED_ALWAYS_LAND then land immediately
}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_with_pause();
// if far from home then RTL
}else if(home_distance > wp_nav.get_wp_radius()) {
if (!set_mode(RTL)) {
set_mode_land_with_pause();
}
// We have no GPS or are very close to home so we will land
}else{
// We have no GPS or are very close to home so we will land
set_mode_land_with_pause();
}
break;
case AUTO:
// failsafe_throttle is 1 do RTL, 2 means continue with the mission
if (g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_RTL) {
// if mission has not started AND vehicle is landed, disarm motors
if (!ap.auto_armed && ap.land_complete) {
init_disarm_motors();
// if failsafe_throttle is FS_THR_ENABLED_ALWAYS_LAND then land immediately
} else if(g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_LAND) {
set_mode_land_with_pause();
// if failsafe_throttle is FS_THR_ENABLED_ALWAYS_RTL do RTL
} else if (g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_RTL) {
if(home_distance > wp_nav.get_wp_radius()) {
if (!set_mode(RTL)) {
set_mode_land_with_pause();
@ -42,30 +54,10 @@ static void failsafe_radio_on_event()
// We are very close to home so we will 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_with_pause();
}
// if failsafe_throttle is 2 (i.e. FS_THR_ENABLED_CONTINUE_MISSION) no need to do anything
break;
case LOITER:
case ALT_HOLD:
case POSHOLD:
// if landed with throttle at zero disarm, otherwise do the regular thing
if (g.rc_3.control_in == 0 && ap.land_complete) {
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_with_pause();
}else if(home_distance > wp_nav.get_wp_radius()) {
if (!set_mode(RTL)) {
set_mode_land_with_pause();
}
}else{
// We have no GPS or are very close to home so we will land
set_mode_land_with_pause();
}
// failsafe_throttle must be FS_THR_ENABLED_CONTINUE_MISSION so no need to do anything
break;
case LAND:
// continue to land if battery failsafe is also active otherwise fall through to default handling
if (g.failsafe_battery_enabled == FS_BATT_LAND && failsafe.battery) {
@ -73,11 +65,19 @@ static void failsafe_radio_on_event()
}
// no break
default:
if(g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_LAND) {
// if failsafe_throttle is 3 (i.e. FS_THR_ENABLED_ALWAYS_LAND) land immediately
// used for AltHold, Guided, Loiter, RTL, Circle, OF_Loiter, Drift, Sport, Flip, Autotune, PosHold
// if landed disarm
if (ap.land_complete) {
init_disarm_motors();
// if failsafe_throttle is FS_THR_ENABLED_ALWAYS_LAND then land immediately
} else if(g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_LAND) {
set_mode_land_with_pause();
// if far from home then RTL
}else if(home_distance > wp_nav.get_wp_radius()) {
if (!set_mode(RTL)){
// if RTL fails because of no GPS, then LAND
set_mode_land_with_pause();
}
}else{
@ -114,9 +114,8 @@ static void failsafe_battery_event(void)
switch(control_mode) {
case STABILIZE:
case ACRO:
case SPORT:
// if throttle is zero disarm motors
if (g.rc_3.control_in == 0) {
// if throttle is zero OR vehicle is landed disarm motors
if (g.rc_3.control_in == 0 || ap.land_complete) {
init_disarm_motors();
}else{
// set mode to RTL or LAND
@ -130,31 +129,31 @@ static void failsafe_battery_event(void)
}
break;
case AUTO:
// if mission has not started AND vehicle is landed, disarm motors
if (!ap.auto_armed && ap.land_complete) {
init_disarm_motors();
// set mode to RTL or LAND
if (home_distance > wp_nav.get_wp_radius()) {
} else if (home_distance > wp_nav.get_wp_radius()) {
if (!set_mode(RTL)) {
set_mode_land_with_pause();
}
}else{
} else {
set_mode_land_with_pause();
}
break;
case LOITER:
case ALT_HOLD:
case POSHOLD:
// if landed with throttle at zero disarm, otherwise fall through to default handling
if (g.rc_3.control_in == 0 && ap.land_complete) {
init_disarm_motors();
break;
}
// no break
default:
// used for AltHold, Guided, Loiter, RTL, Circle, OF_Loiter, Drift, Sport, Flip, Autotune, PosHold
// if landed disarm
if (ap.land_complete) {
init_disarm_motors();
// set mode to RTL or LAND
if (g.failsafe_battery_enabled == FS_BATT_RTL && home_distance > wp_nav.get_wp_radius()) {
} else if (g.failsafe_battery_enabled == FS_BATT_RTL && home_distance > wp_nav.get_wp_radius()) {
if (!set_mode(RTL)) {
set_mode_land_with_pause();
}
}else{
} else {
set_mode_land_with_pause();
}
break;