ArduCopter: low baterry failsafe

ArduCopter/Parameters.h

@@ -174,6 +174,7 @@ public:
         k_param_radio_tuning_high,
         k_param_radio_tuning_low,
         k_param_rc_speed = 192,
+        k_param_battery_fs_enabled, // 193
 
         //
         // 200: flight modes
@@ -246,8 +247,9 @@ public:
     AP_Float        volt_div_ratio;
     AP_Float        curr_amp_per_volt;
     AP_Float        input_voltage;
-    AP_Int16        pack_capacity;              // Battery pack capacity less
-                                                // reserve
+    AP_Int16        pack_capacity;              // Battery pack capacity less reserve
+    AP_Int8         battery_fs_enabled;         // battery failsafe enabled
+
     AP_Int8         compass_enabled;
     AP_Int8         optflow_enabled;
     AP_Float        low_voltage;

ArduCopter/Parameters.pde

@@ -53,8 +53,16 @@ const AP_Param::Info var_info[] PROGMEM = {
     GSCALAR(sonar_enabled,  "SONAR_ENABLE",     DISABLED),
 
     GSCALAR(sonar_type,     "SONAR_TYPE",           AP_RANGEFINDER_MAXSONARXL),
+
     GSCALAR(battery_monitoring, "BATT_MONITOR", DISABLED),
 
+    // @Param: BATT_FAILSAFE
+    // @DisplayName: Battery Failsafe Enable
+    // @Description: Controls whether failsafe will be invoked when battery voltage or current runs low
+    // @Values: 0:Disabled,1:Enabled
+    // @User: Standard
+    GSCALAR(battery_fs_enabled, "BATT_FAILSAFE", BATTERY_FAILSAFE),
+
     // @Param: VOLT_DIVIDER
     // @DisplayName: Voltage Divider
     // @Description: TODO

ArduCopter/config.h

@@ -303,9 +303,6 @@
 //////////////////////////////////////////////////////////////////////////////
 // Battery monitoring
 //
-#ifndef BATTERY_EVENT
- # define BATTERY_EVENT                  DISABLED
-#endif
 #ifndef LOW_VOLTAGE
  # define LOW_VOLTAGE                    9.6
 #endif
@@ -323,7 +320,10 @@
  # define HIGH_DISCHARGE                 1760
 #endif
 
-
+// Battery failsafe
+#ifndef BATTERY_FAILSAFE
+ # define BATTERY_FAILSAFE              DISABLED
+#endif
 
 
 

ArduCopter/events.pde

@@ -6,6 +6,11 @@
  */
 static void failsafe_on_event()
 {
+    // if motors are not armed there is nothing to do
+    if( !motors.armed() ) {
+        return;
+    }
+
     // This is how to handle a failsafe.
     switch(control_mode) {
         case STABILIZE:
@@ -27,6 +32,7 @@ static void failsafe_on_event()
         case AUTO:
             // throttle_fs_action is 1 do RTL, everything else means continue with the mission
             if (g.throttle_fs_action == THROTTLE_FAILSAFE_ACTION_ALWAYS_RTL) {
+                // To-Do: check distance from home before initiating RTL
                 set_mode(RTL);
             }
             // if throttle_fs_action is 2 (i.e. THROTTLE_FAILSAFE_ACTION_CONTINUE_MISSION) no need to do anything
@@ -35,6 +41,7 @@ static void failsafe_on_event()
             if(ap.home_is_set == true) {
                 // same as above ^
                 // do_rtl sets the altitude to the current altitude by default
+                // To-Do: check distance from home before initiating RTL
                 set_mode(RTL);
             }else{
                 // We have no GPS so we will crash land in alt hold mode
@@ -58,19 +65,39 @@ static void failsafe_off_event()
 
 static void low_battery_event(void)
 {
-    static uint32_t last_low_battery_message;
-    uint32_t tnow = millis();
-    if (((uint32_t)(tnow - last_low_battery_message)) > 5000) {
-        // only send this message at 5s intervals at most or we may
-        // flood the link
-        gcs_send_text_P(SEVERITY_LOW,PSTR("Low Battery!"));
-        last_low_battery_message = tnow;
+    // warn the ground station
+    gcs_send_text_P(SEVERITY_LOW,PSTR("Low Battery!"));
+
+    // failsafe check
+    if (g.battery_fs_enabled && !ap.low_battery && motors.armed()) {
+        switch(control_mode) {
+            case STABILIZE:
+            case ACRO:
+                // if throttle is zero disarm motors
+                if (g.rc_3.control_in == 0) {
+                    init_disarm_motors();
+                }else{
+                    set_mode(LAND);
+                }
+                break;
+            case AUTO:
+                // To-Do: check distance to home before initiating RTL?
+                set_mode(RTL);
+                break;
+            default:
+                set_mode(LAND);
+                break;
+        }
     }
 
+    // set the low battery flag
     set_low_battery(true);
-    // if we are in Auto mode, come home
-    if(control_mode >= AUTO)
-        set_mode(RTL);
+
+#if COPTER_LEDS == ENABLED
+    if ( bitRead(g.copter_leds_mode, 3) ) {         // Only Activate if a battery is connected to avoid alarm on USB only
+        piezo_on();
+    }
+#endif // COPTER_LEDS
 }
 
 

ArduCopter/sensors.pde

@@ -68,8 +68,12 @@ static void init_optflow()
 #endif
 }
 
+// read_battery - check battery voltage and current and invoke failsafe if necessary
+// called at 10hz
+#define BATTERY_FS_COUNTER  100     // 100 iterations at 10hz is 10 seconds
 static void read_battery(void)
 {
+    static uint8_t low_battery_counter = 0;
 
     if(g.battery_monitoring == 0) {
         battery_voltage1 = 0;
@@ -88,28 +92,15 @@ static void read_battery(void)
         current_total1   += current_amps1 * 0.02778;            // called at 100ms on average, .0002778 is 1/3600 (conversion to hours)
     }
 
-#if BATTERY_EVENT == ENABLED
-    //if(battery_voltage < g.low_voltage)
-    //	low_battery_event();
-
-    if((battery_voltage1 < g.low_voltage) || (g.battery_monitoring == 4 && current_total1 > g.pack_capacity)) {
-        low_battery_event();
-
- #if COPTER_LEDS == ENABLED
-        if ( bitRead(g.copter_leds_mode, 3) ) {         // Only Activate if a battery is connected to avoid alarm on USB only
-            if (battery_voltage1 > 1) {
-                piezo_on();
-            }else{
-                piezo_off();
-            }
+    // check for low voltage or current if the low voltage check hasn't already been triggered
+    if (!ap.low_battery && ( battery_voltage1 < g.low_voltage || (g.battery_monitoring == 4 && current_total1 > g.pack_capacity))) {
+        low_battery_counter++;
+        if( low_battery_counter >= BATTERY_FS_COUNTER ) {
+            low_battery_counter = BATTERY_FS_COUNTER;   // ensure counter does not overflow
+            low_battery_event();
         }
-
-
-    }else if ( bitRead(g.copter_leds_mode, 3) ) {
-        piezo_off();
- #endif                // COPTER_LEDS
+    }else{
+        // reset low_battery_counter in case it was a temporary voltage dip
+        low_battery_counter = 0;
     }
-#endif         //BATTERY_EVENT
 }
-
-//v: 10.9453, a: 17.4023, mah: 8.2