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

Copter: simplify battery and radio failsafe logic

Browse Source
This commit is contained in:
Jonathan Challinger 2016-01-11 14:19:49 -08:00 committed by Randy Mackay
parent 91187899fd
commit 5cc969f01b
2 changed files with 39 additions and 110 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ArduCopter/Copter.h
View File

@ -828,6 +828,7 @@ private:
void esc_calibration_startup_check(); void esc_calibration_startup_check();
void esc_calibration_passthrough(); void esc_calibration_passthrough();
void esc_calibration_auto(); void esc_calibration_auto();
bool should_disarm_on_failsafe();
void failsafe_radio_on_event(); void failsafe_radio_on_event();
void failsafe_radio_off_event(); void failsafe_radio_off_event();
void failsafe_battery_event(void); void failsafe_battery_event(void);

130
ArduCopter/events.cpp
View File

@ -13,76 +13,20 @@ void Copter::failsafe_radio_on_event()
return; return;
} }
// This is how to handle a failsafe. if (should_disarm_on_failsafe()) {
switch(control_mode) {
case STABILIZE:
case ACRO:
// if throttle is zero OR vehicle is landed disarm motors
if (ap.throttle_zero || ap.land_complete) {
init_disarm_motors(); 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 > FS_CLOSE_TO_HOME_CM) {
// switch to RTL or if that fails, LAND
set_mode_RTL_or_land_with_pause();
// We have no GPS or are very close to home so we will land
} else { } else {
set_mode_land_with_pause(); if (control_mode == AUTO && g.failsafe_throttle == FS_THR_ENABLED_CONTINUE_MISSION) {
} // continue mission
break; } else if (control_mode == LAND && g.failsafe_battery_enabled == FS_BATT_LAND && failsafe.battery) {
// continue landing
case AUTO:
// 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 > FS_CLOSE_TO_HOME_CM) {
// switch to RTL or if that fails, LAND
set_mode_RTL_or_land_with_pause();
} else { } else {
// We are very close to home so we will land if (g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_LAND) {
set_mode_land_with_pause(); 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) {
break;
}
// no break
default:
// used for AltHold, Guided, Loiter, RTL, Circle, 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 > FS_CLOSE_TO_HOME_CM) {
// switch to RTL or if that fails, LAND
set_mode_RTL_or_land_with_pause();
} else { } else {
// We have no GPS or are very close to home so we will land set_mode_RTL_or_land_with_pause();
set_mode_land_with_pause(); }
} }
break;
} }
// log the error to the dataflash // log the error to the dataflash
@ -109,50 +53,15 @@ void Copter::failsafe_battery_event(void)
// failsafe check // failsafe check
if (g.failsafe_battery_enabled != FS_BATT_DISABLED && motors.armed()) { if (g.failsafe_battery_enabled != FS_BATT_DISABLED && motors.armed()) {
switch(control_mode) { if (should_disarm_on_failsafe()) {
case STABILIZE:
case ACRO:
// if throttle is zero OR vehicle is landed disarm motors
if (ap.throttle_zero || ap.land_complete) {
init_disarm_motors(); init_disarm_motors();
} else { } else {
// set mode to RTL or LAND if (g.failsafe_battery_enabled == FS_BATT_RTL || control_mode == AUTO) {
if (g.failsafe_battery_enabled == FS_BATT_RTL && home_distance > FS_CLOSE_TO_HOME_CM) {
// switch to RTL or if that fails, LAND
set_mode_RTL_or_land_with_pause(); set_mode_RTL_or_land_with_pause();
} else { } else {
set_mode_land_with_pause(); set_mode_land_with_pause();
} }
} }
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
} else if (home_distance > FS_CLOSE_TO_HOME_CM) {
// switch to RTL or if that fails, LAND
set_mode_RTL_or_land_with_pause();
} else {
set_mode_land_with_pause();
}
break;
default:
// used for AltHold, Guided, Loiter, RTL, Circle, Drift, Sport, Flip, Autotune, PosHold
// if landed disarm
if (ap.land_complete) {
init_disarm_motors();
// set mode to RTL or LAND
} else if (g.failsafe_battery_enabled == FS_BATT_RTL && home_distance > FS_CLOSE_TO_HOME_CM) {
// switch to RTL or if that fails, LAND
set_mode_RTL_or_land_with_pause();
} else {
set_mode_land_with_pause();
}
break;
}
} }
// set the low battery flag // set the low battery flag
@ -273,6 +182,25 @@ void Copter::set_mode_RTL_or_land_with_pause()
} }
} }
bool Copter::should_disarm_on_failsafe() {
switch(control_mode) {
case STABILIZE:
case ACRO:
// if throttle is zero OR vehicle is landed disarm motors
return ap.throttle_zero || ap.land_complete;
break;
case AUTO:
// if mission has not started AND vehicle is landed, disarm motors
return !ap.auto_armed && ap.land_complete;
break;
default:
// used for AltHold, Guided, Loiter, RTL, Circle, Drift, Sport, Flip, Autotune, PosHold
// if landed disarm
return ap.land_complete;
break;
}
}
void Copter::update_events() void Copter::update_events()
{ {
ServoRelayEvents.update_events(); ServoRelayEvents.update_events();