Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-11 02:13:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: allow battery failsafe to trigger RTL

Browse Source
This commit is contained in:
Randy Mackay 2013-11-16 14:46:57 +09:00
parent a812ef4660
commit 87127f0ab8
5 changed files with 21 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ArduCopter/Parameters.pde
View File

@ -98,9 +98,9 @@ const AP_Param::Info var_info[] PROGMEM = {
// @Param: FS_BATT_ENABLE // @Param: FS_BATT_ENABLE
// @DisplayName: Battery Failsafe Enable // @DisplayName: Battery Failsafe Enable
// @Description: Controls whether failsafe will be invoked when battery voltage or current runs low // @Description: Controls whether failsafe will be invoked when battery voltage or current runs low
// @Values: 0:Disabled,1:Enabled // @Values: 0:Disabled,1:Land,2:RTL
// @User: Standard // @User: Standard
GSCALAR(failsafe_battery_enabled, "FS_BATT_ENABLE", FS_BATTERY), GSCALAR(failsafe_battery_enabled, "FS_BATT_ENABLE", FS_BATT_DISABLED),
// @Param: FS_BATT_VOLTAGE // @Param: FS_BATT_VOLTAGE
// @DisplayName: Failsafe battery voltage // @DisplayName: Failsafe battery voltage

5
ArduCopter/config.h
View File

@ -356,11 +356,6 @@
# define BOARD_VOLTAGE_MAX 5800 // max board voltage in milli volts for pre-arm checks # define BOARD_VOLTAGE_MAX 5800 // max board voltage in milli volts for pre-arm checks
#endif #endif
// Battery failsafe
#ifndef FS_BATTERY
# define FS_BATTERY DISABLED
#endif
// GPS failsafe // GPS failsafe
#ifndef FAILSAFE_GPS_TIMEOUT_MS #ifndef FAILSAFE_GPS_TIMEOUT_MS
# define FAILSAFE_GPS_TIMEOUT_MS 5000 // gps failsafe triggers after 5 seconds with no GPS # define FAILSAFE_GPS_TIMEOUT_MS 5000 // gps failsafe triggers after 5 seconds with no GPS

5
ArduCopter/defines.h
View File

@ -485,6 +485,11 @@ enum ap_message {
#define FS_THR_ENABLED_CONTINUE_MISSION 2 #define FS_THR_ENABLED_CONTINUE_MISSION 2
#define FS_THR_ENABLED_ALWAYS_LAND 3 #define FS_THR_ENABLED_ALWAYS_LAND 3
// Battery failsafe definitions (FS_BATT_ENABLE parameter)
#define FS_BATT_DISABLED 0 // battery failsafe disabled
#define FS_BATT_LAND 1 // switch to LAND mode on battery failsafe
#define FS_BATT_RTL 2 // switch to RTL mode on battery failsafe
// GPS Failsafe definitions (FS_GPS_ENABLE parameter) // GPS Failsafe definitions (FS_GPS_ENABLE parameter)
#define FS_GPS_DISABLED 0 // GPS failsafe disabled #define FS_GPS_DISABLED 0 // GPS failsafe disabled
#define FS_GPS_LAND 1 // switch to LAND mode on GPS Failsafe #define FS_GPS_LAND 1 // switch to LAND mode on GPS Failsafe

23
ArduCopter/events.pde
View File

@ -50,7 +50,7 @@ static void failsafe_radio_on_event()
break; break;
case LAND: case LAND:
// continue to land if battery failsafe is also active otherwise fall through to default handling // continue to land if battery failsafe is also active otherwise fall through to default handling
if (g.failsafe_battery_enabled && failsafe.battery) { if (g.failsafe_battery_enabled == FS_BATT_LAND && failsafe.battery) {
break; break;
} }
default: default:
@ -91,7 +91,7 @@ static void failsafe_battery_event(void)
} }
// failsafe check // failsafe check
if (g.failsafe_battery_enabled && motors.armed()) { if (g.failsafe_battery_enabled != FS_BATT_DISABLED && motors.armed()) {
switch(control_mode) { switch(control_mode) {
case STABILIZE: case STABILIZE:
case ACRO: case ACRO:
@ -100,23 +100,26 @@ static void failsafe_battery_event(void)
if (g.rc_3.control_in == 0) { if (g.rc_3.control_in == 0) {
init_disarm_motors(); init_disarm_motors();
}else{ }else{
set_mode(LAND); // set mode to RTL or LAND
if (g.failsafe_battery_enabled == FS_BATT_RTL && home_distance > wp_nav.get_waypoint_radius()) {
if (!set_mode(RTL)) {
set_mode(LAND);
}
}else{
set_mode(LAND);
}
} }
break; break;
case AUTO: default:
// set_mode to RTL or LAND // set mode to RTL or LAND
if (home_distance > wp_nav.get_waypoint_radius()) { if (g.failsafe_battery_enabled == FS_BATT_RTL && home_distance > wp_nav.get_waypoint_radius()) {
if (!set_mode(RTL)) { if (!set_mode(RTL)) {
set_mode(LAND); set_mode(LAND);
} }
}else{ }else{
// We have no GPS or are very close to home so we will land
set_mode(LAND); set_mode(LAND);
} }
break; break;
default:
set_mode(LAND);
break;
} }
} }

2
ArduCopter/setup.pde
View File

@ -164,7 +164,7 @@ setup_compassmot(uint8_t argc, const Menu::arg *argv)
// disable throttle and battery failsafe // disable throttle and battery failsafe
g.failsafe_throttle = FS_THR_DISABLED; g.failsafe_throttle = FS_THR_DISABLED;
g.failsafe_battery_enabled = false; g.failsafe_battery_enabled = FS_BATT_DISABLED;
// read radio // read radio
read_radio(); read_radio();