Sub: Implement battery failsafe

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)

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),
 

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();

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_WARN_ONLY                   1       // only warn gcs on battery failsafe
-#define FS_BATT_RTL                         2       // switch to RTL mode 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

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
+    // Do nothing if the failsafe is disabled, or if we are disarmed
-    //    if (failsafe.battery) {
+    if (g.failsafe_battery_enabled == FS_BATT_DISABLED || !motors.armed()) {
-    //        return;
+        failsafe.battery = false;
-    //    }
+        AP_Notify::flags.failsafe_battery = false;
-    //
+        return; // Failsafe disabled, nothing to do
-    //    // 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);
 
+    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()

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