From 7859f6426e104ab013c6325d1cc7ecdef8a96cba Mon Sep 17 00:00:00 2001 From: Jacob Walser Date: Fri, 24 Mar 2017 14:25:30 -0400 Subject: [PATCH] Sub: Implement battery failsafe --- ArduSub/AP_State.cpp | 7 ----- ArduSub/Parameters.cpp | 2 +- ArduSub/Sub.h | 2 +- ArduSub/defines.h | 5 ++-- ArduSub/failsafe.cpp | 64 ++++++++++++++++++++++++++---------------- ArduSub/motors.cpp | 3 -- 6 files changed, 45 insertions(+), 38 deletions(-) diff --git a/ArduSub/AP_State.cpp b/ArduSub/AP_State.cpp index f4b46022bd..40590877ec 100644 --- a/ArduSub/AP_State.cpp +++ b/ArduSub/AP_State.cpp @@ -37,13 +37,6 @@ void Sub::set_auto_armed(bool b) } } -// --------------------------------------------- -void Sub::set_failsafe_battery(bool b) -{ - failsafe.battery = b; - AP_Notify::flags.failsafe_battery = b; -} - // --------------------------------------------- void Sub::set_pre_arm_check(bool b) diff --git a/ArduSub/Parameters.cpp b/ArduSub/Parameters.cpp index 62a2eb5336..e1df41c236 100644 --- a/ArduSub/Parameters.cpp +++ b/ArduSub/Parameters.cpp @@ -105,7 +105,7 @@ const AP_Param::Info Sub::var_info[] = { // @Param: FS_BATT_ENABLE // @DisplayName: Battery Failsafe Enable // @Description: Controls whether failsafe will be invoked when battery voltage or current runs low - // @Values: 0:Disabled + // @Values: 0:Disabled,1:Warn only,2:Disarm,3:Enter surface mode // @User: Standard GSCALAR(failsafe_battery_enabled, "FS_BATT_ENABLE", FS_BATT_DISABLED), diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index 3fd23d4774..a16ac45273 100644 --- a/ArduSub/Sub.h +++ b/ArduSub/Sub.h @@ -286,6 +286,7 @@ private: uint32_t last_manual_control_ms; // last time MANUAL_CONTROL message arrived from GCS uint32_t terrain_first_failure_ms; // the first time terrain data access failed - used to calculate the duration of the failure uint32_t terrain_last_failure_ms; // the most recent time terrain data access failed + uint32_t last_battery_warn_ms; // last time a battery failsafe warning was sent to gcs } failsafe; // sensor health for logging @@ -481,7 +482,6 @@ private: void set_home_state(enum HomeState new_home_state); bool home_is_set(); void set_auto_armed(bool b); - void set_failsafe_battery(bool b); void set_pre_arm_check(bool b); void set_motor_emergency_stop(bool b); float get_smoothing_gain(); diff --git a/ArduSub/defines.h b/ArduSub/defines.h index 3d3256f7d5..19f4cf9ceb 100644 --- a/ArduSub/defines.h +++ b/ArduSub/defines.h @@ -276,8 +276,9 @@ enum RTLState { // Battery failsafe definitions (FS_BATT_ENABLE parameter) #define FS_BATT_DISABLED 0 // battery failsafe disabled -#define FS_BATT_SURFACE 1 // switch to SURFACE mode on battery failsafe -#define FS_BATT_RTL 2 // switch to RTL mode on battery failsafe +#define FS_BATT_WARN_ONLY 1 // only warn gcs on battery failsafe +#define FS_BATT_DISARM 2 // disarm on battery failsafe +#define FS_BATT_SURFACE 3 // switch to SURFACE mode on battery failsafe // GCS failsafe definitions (FS_GCS_ENABLE parameter) #define FS_GCS_DISABLED 0 // Disabled diff --git a/ArduSub/failsafe.cpp b/ArduSub/failsafe.cpp index f1d0e1ee37..1c14af3565 100644 --- a/ArduSub/failsafe.cpp +++ b/ArduSub/failsafe.cpp @@ -70,33 +70,49 @@ void Sub::failsafe_check() } } +// Battery failsafe check void Sub::failsafe_battery_check(void) { - // // return immediately if low battery event has already been triggered - // if (failsafe.battery) { - // return; - // } - // - // // failsafe check - // if (g.failsafe_battery_enabled != FS_BATT_DISABLED && motors.armed()) { - // if (should_disarm_on_failsafe()) { - // init_disarm_motors(); - // } else { - // if (g.failsafe_battery_enabled == FS_BATT_RTL || control_mode == AUTO) { - // set_mode_RTL_or_land_with_pause(MODE_REASON_BATTERY_FAILSAFE); - // } else { - // set_mode_land_with_pause(MODE_REASON_BATTERY_FAILSAFE); - // } - // } - // } - // - // // set the low battery flag - // set_failsafe_battery(true); - // - // // warn the ground station and log to dataflash - // gcs_send_text(MAV_SEVERITY_WARNING,"Low battery"); - // Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_BATT, ERROR_CODE_FAILSAFE_OCCURRED); + // Do nothing if the failsafe is disabled, or if we are disarmed + if (g.failsafe_battery_enabled == FS_BATT_DISABLED || !motors.armed()) { + failsafe.battery = false; + AP_Notify::flags.failsafe_battery = false; + return; // Failsafe disabled, nothing to do + } + if (!battery.exhausted(g.fs_batt_voltage, g.fs_batt_mah)) { + failsafe.battery = false; + AP_Notify::flags.failsafe_battery = false; + return; // Battery is doing well + } + + // Always warn when failsafe condition is met + if (AP_HAL::millis() > failsafe.last_battery_warn_ms + 20000) { + failsafe.last_battery_warn_ms = AP_HAL::millis(); + gcs_send_text(MAV_SEVERITY_WARNING, "Low battery"); + } + + // Don't do anything if failsafe has already been set + if (failsafe.battery) { + return; + } + + failsafe.battery = true; + AP_Notify::flags.failsafe_battery = true; + + // Log failsafe + Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_BATT, ERROR_CODE_FAILSAFE_OCCURRED); + + switch(g.failsafe_battery_enabled) { + case FS_BATT_SURFACE: + set_mode(SURFACE, MODE_REASON_BATTERY_FAILSAFE); + break; + case FS_BATT_DISARM: + init_disarm_motors(); + break; + default: + break; + } } void Sub::failsafe_manual_control_check() diff --git a/ArduSub/motors.cpp b/ArduSub/motors.cpp index 7f3df72ba7..53edbdfaec 100644 --- a/ArduSub/motors.cpp +++ b/ArduSub/motors.cpp @@ -56,9 +56,6 @@ bool Sub::init_arm_motors(bool arming_from_gcs) // disable cpu failsafe because initialising everything takes a while failsafe_disable(); - // reset battery failsafe - set_failsafe_battery(false); - // notify that arming will occur (we do this early to give plenty of warning) AP_Notify::flags.armed = true; // call update_notify a few times to ensure the message gets out