AP_BattMonitor: Support critical and low battery failsafes

Also removes the example script, as it was broken, and causing more
headaches then it was worth

libraries/AP_BattMonitor/AP_BattMonitor.cpp

@@ -7,6 +7,7 @@
 #endif
 #include <AP_Vehicle/AP_Vehicle_Type.h>
 #include <DataFlash/DataFlash.h>
+#include <GCS_MAVLink/GCS.h>
 
 extern const AP_HAL::HAL& hal;
 
@@ -30,8 +31,10 @@ const AP_Param::GroupInfo AP_BattMonitor::var_info[] = {
 // Note that the Vector/Matrix constructors already implicitly zero
 // their values.
 //
-AP_BattMonitor::AP_BattMonitor(uint32_t log_battery_bit) :
+AP_BattMonitor::AP_BattMonitor(uint32_t log_battery_bit, battery_failsafe_handler_fn_t battery_failsafe_handler_fn, const int8_t *failsafe_priorities) :
     _log_battery_bit(log_battery_bit),
+    _battery_failsafe_handler_fn(battery_failsafe_handler_fn),
+    _failsafe_priorities(failsafe_priorities),
     _num_instances(0)
 {
     AP_Param::setup_object_defaults(this, var_info);
@@ -51,6 +54,8 @@ AP_BattMonitor::init()
         return;
     }
 
+    _highest_failsafe_priority = INT8_MAX;
+
     convert_params();
 
 #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP || CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO
@@ -197,6 +202,8 @@ AP_BattMonitor::read()
         df->Log_Write_Current();
         df->Log_Write_Power();
     }
+
+    check_failsafes();
 }
 
 // healthy - returns true if monitor is functioning
@@ -295,14 +302,68 @@ int32_t AP_BattMonitor::pack_capacity_mah(uint8_t instance) const
     }
 }
 
- /// exhausted - returns true if the voltage remains below the low_voltage for 10 seconds or remaining capacity falls below min_capacity_mah
+void AP_BattMonitor::check_failsafes(void)
-bool AP_BattMonitor::exhausted(uint8_t instance, float low_voltage, float min_capacity_mah)
+{
+    if (hal.util->get_soft_armed()) {
+        for (uint8_t i = 0; i < _num_instances; i++) {
+            const BatteryFailsafe type = check_failsafe(i);
+            if (type <= state[i].failsafe) {
+                continue;
+            }
+
+            int8_t action = 0;
+            const char *type_str = nullptr;
+            switch (type) {
+                case AP_BattMonitor::BatteryFailsafe_None:
+                    continue; // should not have been called in this case
+                case AP_BattMonitor::BatteryFailsafe_Low:
+                    action = _params[i]._failsafe_low_action;
+                    type_str = "low";
+                    break;
+                case AP_BattMonitor::BatteryFailsafe_Critical:
+                    action = _params[i]._failsafe_critical_action;
+                    type_str = "critical";
+                    break;
+            }
+
+            gcs().send_text(MAV_SEVERITY_WARNING, "Battery %d is %s %.2fV used %.0f mAh", i + 1, type_str,
+                            (double)voltage(i), (double)consumed_mah(i));
+            _has_triggered_failsafe = true;
+            AP_Notify::flags.failsafe_battery = true;
+            state[i].failsafe = type;
+
+            // map the desired failsafe action to a prioritiy level
+            int8_t priority = 0;
+            if (_failsafe_priorities != nullptr) {
+                while (_failsafe_priorities[priority] != -1) {
+                    if (_failsafe_priorities[priority] == action) {
+                        break;
+                    }
+                    priority++;
+                }
+
+            }
+
+            // trigger failsafe if the action was equal or higher priority
+            // It's valid to retrigger the same action if a different battery provoked the event
+            if (priority <= _highest_failsafe_priority) {
+                _battery_failsafe_handler_fn(type_str, action);
+                _highest_failsafe_priority = priority;
+            }
+        }
+    }
+}
+
+// returns the failsafe state of the battery
+AP_BattMonitor::BatteryFailsafe AP_BattMonitor::check_failsafe(const uint8_t instance)
 {
     // exit immediately if no monitors setup
     if (_num_instances == 0 || instance >= _num_instances) {
-        return false;
+        return BatteryFailsafe_None;
     }
 
+    const uint32_t now = AP_HAL::millis();
+
     // use voltage or sag compensated voltage
     float voltage_used;
     switch (_params[instance].failsafe_voltage_source()) {
@@ -315,14 +376,33 @@ bool AP_BattMonitor::exhausted(uint8_t instance, float low_voltage, float min_ca
             break;
     }
 
-    // check voltage
+    // check critical battery levels
-    if ((voltage_used > 0) && (low_voltage > 0) && (voltage_used < low_voltage)) {
+    if ((voltage_used > 0) && (_params[instance]._critical_voltage > 0) && (voltage_used < _params[instance]._critical_voltage)) {
+        // this is the first time our voltage has dropped below minimum so start timer
+        if (state[instance].critical_voltage_start_ms == 0) {
+            state[instance].critical_voltage_start_ms = now;
+        } else if (_params[instance]._low_voltage_timeout > 0 &&
+                   now - state[instance].critical_voltage_start_ms > uint32_t(_params[instance]._low_voltage_timeout)*1000U) {
+            return BatteryFailsafe_Critical;
+        }
+    } else {
+        // acceptable voltage so reset timer
+        state[instance].critical_voltage_start_ms = 0;
+    }
+
+    // check capacity if current monitoring is enabled
+    if (has_current(instance) && (_params[instance]._critical_capacity > 0) &&
+        ((_params[instance]._pack_capacity - state[instance].consumed_mah) < _params[instance]._critical_capacity)) {
+        return BatteryFailsafe_Critical;
+    }
+
+    if ((voltage_used > 0) && (_params[instance]._low_voltage > 0) && (voltage_used < _params[instance]._low_voltage)) {
         // this is the first time our voltage has dropped below minimum so start timer
         if (state[instance].low_voltage_start_ms == 0) {
-            state[instance].low_voltage_start_ms = AP_HAL::millis();
+            state[instance].low_voltage_start_ms = now;
         } else if (_params[instance]._low_voltage_timeout > 0 &&
-                   AP_HAL::millis() - state[instance].low_voltage_start_ms > uint32_t(_params[instance]._low_voltage_timeout)*1000U) {
+                   now - state[instance].low_voltage_start_ms > uint32_t(_params[instance]._low_voltage_timeout)*1000U) {
-            return true;
+            return BatteryFailsafe_Low;
         }
     } else {
         // acceptable voltage so reset timer
@@ -330,13 +410,13 @@ bool AP_BattMonitor::exhausted(uint8_t instance, float low_voltage, float min_ca
     }
 
     // check capacity if current monitoring is enabled
-    if (has_current(instance) && (min_capacity_mah > 0) &&
+    if (has_current(instance) && (_params[instance]._low_capacity > 0) &&
-        (_params[instance]._pack_capacity - state[instance].consumed_mah < min_capacity_mah)) {
+        ((_params[instance]._pack_capacity - state[instance].consumed_mah) < _params[instance]._low_capacity)) {
-        return true;
+        return BatteryFailsafe_Low;
     }
 
     // if we've gotten this far then battery is ok
-    return false;
+    return BatteryFailsafe_None;
 }
 
 // return true if any battery is pushing too much power

libraries/AP_BattMonitor/AP_BattMonitor.h

@@ -37,7 +37,17 @@ class AP_BattMonitor
     friend class AP_BattMonitor_UAVCAN;
 
 public:
-    AP_BattMonitor(uint32_t log_battery_bit);
+
+    // battery failsafes must be defined in levels of severity so that vehicles wont fall backwards
+    enum BatteryFailsafe {
+        BatteryFailsafe_None = 0,
+        BatteryFailsafe_Low,
+        BatteryFailsafe_Critical
+    };
+
+    FUNCTOR_TYPEDEF(battery_failsafe_handler_fn_t, void, const char *, const int8_t);
+
+    AP_BattMonitor(uint32_t log_battery_bit, battery_failsafe_handler_fn_t battery_failsafe_handler_fn, const int8_t *failsafe_priorities);
 
     /* Do not allow copies */
     AP_BattMonitor(const AP_BattMonitor &other) = delete;
@@ -53,18 +63,20 @@ public:
 
     // The BattMonitor_State structure is filled in by the backend driver
     struct BattMonitor_State {
-        cells       cell_voltages;      // battery cell voltages in millivolts, 10 cells matches the MAVLink spec
+        cells       cell_voltages;             // battery cell voltages in millivolts, 10 cells matches the MAVLink spec
-        float       voltage;            // voltage in volts
+        float       voltage;                   // voltage in volts
-        float       current_amps;       // current in amperes
+        float       current_amps;              // current in amperes
-        float       consumed_mah;       // total current draw in milliampere.hours since start-up
+        float       consumed_mah;              // total current draw in milliamp hours since start-up
-        float       consumed_wh;        // total energy consumed in Watt.hours since start-up
+        float       consumed_wh;               // total energy consumed in Wh since start-up
-        uint32_t    last_time_micros;   // time when voltage and current was last read
+        uint32_t    last_time_micros;          // time when voltage and current was last read
-        uint32_t    low_voltage_start_ms;  // time when voltage dropped below the minimum
+        uint32_t    low_voltage_start_ms;      // time when voltage dropped below the minimum
-        float       temperature;        // battery temperature in celsius
+        uint32_t    critical_voltage_start_ms; // critical voltage failsafe start timer in milliseconds
-        uint32_t    temperature_time;   // timestamp of the last received temperature message
+        float       temperature;               // battery temperature in celsius
-        float       voltage_resting_estimate; // voltage with sag removed based on current and resistance estimate
+        uint32_t    temperature_time;          // timestamp of the last received temperature message
-        float       resistance;         // resistance calculated by comparing resting voltage vs in flight voltage
+        float       voltage_resting_estimate;  // voltage with sag removed based on current and resistance estimate
-        bool        healthy;            // battery monitor is communicating correctly
+        float       resistance;                // resistance, in Ohms, calculated by comparing resting voltage vs in flight voltage
+        BatteryFailsafe failsafe;              // stage failsafe the battery is in
+        bool        healthy;                   // battery monitor is communicating correctly
     };
 
     // Return the number of battery monitor instances
@@ -117,9 +129,15 @@ public:
     int32_t pack_capacity_mah(uint8_t instance) const;
     int32_t pack_capacity_mah() const { return pack_capacity_mah(AP_BATT_PRIMARY_INSTANCE); }
  
-    /// exhausted - returns true if the battery's voltage remains below the low_voltage for 10 seconds or remaining capacity falls below min_capacity
+    /// returns the failsafe state of the battery
-    bool exhausted(uint8_t instance, float low_voltage, float min_capacity_mah);
+    BatteryFailsafe check_failsafe(const uint8_t instance);
-    bool exhausted(float low_voltage, float min_capacity_mah) { return exhausted(AP_BATT_PRIMARY_INSTANCE, low_voltage, min_capacity_mah); }
+    void check_failsafes(void); // checks all batteries failsafes
+
+    /// returns true if a battery failsafe has ever been triggered
+    bool has_failsafed(void) const { return _has_triggered_failsafe; };
+
+    /// returns the highest failsafe action that has been triggered
+    int8_t get_highest_failsafe_priority(void) const { return _highest_failsafe_priority; };
 
     /// get_type - returns battery monitor type
     enum AP_BattMonitor_Params::BattMonitor_Type get_type() { return get_type(AP_BATT_PRIMARY_INSTANCE); }
@@ -163,6 +181,12 @@ private:
 
     void convert_params(void);
 
+    battery_failsafe_handler_fn_t _battery_failsafe_handler_fn;
+    const int8_t *_failsafe_priorities; // array of failsafe priorities, sorted highest to lowest priority, -1 indicates no more entries
+
+    int8_t      _highest_failsafe_priority; // highest selected failsafe action level (used to restrict what actions we move into)
+    bool        _has_triggered_failsafe;  // true after a battery failsafe has been triggered for the first time
+
 };
 
 namespace AP {

libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp

@@ -3,6 +3,12 @@
 #include "AP_BattMonitor_Params.h"
 #include "AP_BattMonitor_Analog.h"
 
+#if APM_BUILD_TYPE(APM_BUILD_ArduCopter)
+  #define DEFAULT_LOW_BATTERY_VOLTAGE 10.5f
+#else
+  #define DEFAULT_LOW_BATTERY_VOLTAGE 0.0f
+#endif // APM_BUILD_TYPE(APM_BUILD_ArduCopter)
+
 const AP_Param::GroupInfo AP_BattMonitor_Params::var_info[] = {
     // @Param: MONITOR
     // @DisplayName: Battery monitoring
@@ -27,7 +33,7 @@ const AP_Param::GroupInfo AP_BattMonitor_Params::var_info[] = {
 
     // @Param: VOLT_MULT
     // @DisplayName: Voltage Multiplier
-    // @Description: Used to convert the voltage of the voltage sensing pin (BATT_VOLT_PIN) to the actual battery's voltage (pin_voltage * VOLT_MULT). For the 3DR Power brick on APM2 or Pixhawk, this should be set to 10.1. For the Pixhawk with the 3DR 4in1 ESC this should be 12.02. For the PX using the PX4IO power supply this should be set to 1.
+    // @Description: Used to convert the voltage of the voltage sensing pin (@PREFIX@VOLT_PIN) to the actual battery's voltage (pin_voltage * VOLT_MULT). For the 3DR Power brick on APM2 or Pixhawk, this should be set to 10.1. For the Pixhawk with the 3DR 4in1 ESC this should be 12.02. For the PX using the PX4IO power supply this should be set to 1.
     // @User: Advanced
     AP_GROUPINFO("VOLT_MULT", 4, AP_BattMonitor_Params, _volt_multiplier, AP_BATT_VOLTDIVIDER_DEFAULT),
 
@@ -83,6 +89,60 @@ const AP_Param::GroupInfo AP_BattMonitor_Params::var_info[] = {
     // @User: Advanced
     AP_GROUPINFO("FS_VOLTSRC", 11, AP_BattMonitor_Params, _failsafe_voltage_source, BattMonitor_LowVoltageSource_Raw),
 
+    // @Param: LOW_VOLT
+    // @DisplayName: Low battery voltage
+    // @Description: Battery voltage that triggers a low battery failsafe. Set to 0 to disable. If the battery voltage drops below this voltage continuously for more then the period specified by the @PREFIX@LOW_TIMER parameter then the vehicle will perform the failsafe specified by the @PREFIX@FS_LOW_ACT parameter.
+    // @Units: V
+    // @Increment: 0.1
+    // @User: Standard
+    AP_GROUPINFO("LOW_VOLT", 12, AP_BattMonitor_Params, _low_voltage, DEFAULT_LOW_BATTERY_VOLTAGE),
+
+    // @Param: LOW_MAH
+    // @DisplayName: Low battery capacity
+    // @Description: Battery capacity at which the low battery failsafe is triggered. Set to 0 to disable battery remaining failsafe. If the battery capacity drops below this level the vehicle will perform the failsafe specified by the @PREFIX@FS_LOW_ACT parameter.
+    // @Units: mA.h
+    // @Increment: 50
+    // @User: Standard
+    AP_GROUPINFO("LOW_MAH", 13, AP_BattMonitor_Params, _low_capacity, 0),
+
+    // @Param: CRT_VOLT
+    // @DisplayName: Critical battery voltage
+    // @Description: Battery voltage that triggers a critical battery failsafe. Set to 0 to disable. If the battery voltage drops below this voltage continuously for more then the period specified by the @PREFIX@LOW_TIMER parameter then the vehicle will perform the failsafe specified by the @PREFIX@FS_CRT_ACT parameter.
+    // @Units: V
+    // @Increment: 0.1
+    // @User: Standard
+    AP_GROUPINFO("CRT_VOLT", 14, AP_BattMonitor_Params, _critical_voltage, 0),
+
+    // @Param: CRT_MAH
+    // @DisplayName: Battery critical capacity
+    // @Description: Battery capacity at which the critical battery failsafe is triggered. Set to 0 to disable battery remaining failsafe. If the battery capacity drops below this level the vehicle will perform the failsafe specified by the @PREFIX@_FS_CRT_ACT parameter.
+    // @Units: mA.h
+    // @Increment: 50
+    // @User: Standard
+    AP_GROUPINFO("CRT_MAH", 15, AP_BattMonitor_Params, _critical_capacity, 0),
+
+    // @Param: FS_LOW_ACT
+    // @DisplayName: Low battery failsafe action
+    // @Description: What action the vehicle should perform if it hits a low battery failsafe
+    // @Values{Plane}: 0:Nothing,1:RTL,2:Land,3:Terminate
+    // @Values{Copter}: 0:Nothing,1:Land,2:RTL,3:SmartRTL,4:SmartRTL or Land,5:Terminate
+    // @Values{Sub}: 0:Nothing,2:Disarm,3:Enter surface mode
+    // @Values{Rover}: 0:Nothing,1:RTL,2:Hold,3:SmartRTL,4:SmartRTL or Hold,5:Terminate
+    // @Values{Tracker}: 0:Nothing
+    // @User: Standard
+    AP_GROUPINFO("FS_LOW_ACT", 16, AP_BattMonitor_Params, _failsafe_low_action, 0),
+
+    // @Param: FS_CRT_ACT
+    // @DisplayName: Critical battery failsafe action
+    // @Description: What action the vehicle should perform if it hits a critical battery failsafe
+    // @Values{Plane}: 0:Nothing,1:RTL,2:Land,3:Terminate
+    // @Values{Copter}: 0:Nothing,1:Land,2:RTL,3:SmartRTL,4:SmartRTL or Land,5:Terminate
+    // @Values{Sub}: 0:Nothing,2:Disarm,3:Enter surface mode
+    // @Values{Rover}: 0:Nothing,1:RTL,2:Hold,3:SmartRTL,4:SmartRTL or Hold,5:Terminate
+    // @Values{Tracker}: 0:Nothing
+    // @User: Standard
+    AP_GROUPINFO("FS_CRT_ACT", 17, AP_BattMonitor_Params, _failsafe_critical_action, 0),
+
     AP_GROUPEND
 
 };

libraries/AP_BattMonitor/AP_BattMonitor_Params.h

@@ -32,16 +32,22 @@ public:
     BattMonitor_Type type(void) const { return (enum BattMonitor_Type)_type.get(); }
     BattMonitor_LowVoltage_Source failsafe_voltage_source(void) { return (enum BattMonitor_LowVoltage_Source)_failsafe_voltage_source.get(); }
 
-    AP_Int8     _type;                    /// 0=disabled, 3=voltage only, 4=voltage and current
+    AP_Int8  _type;                     /// 0=disabled, 3=voltage only, 4=voltage and current
-    AP_Int8     _volt_pin;                /// board pin used to measure battery voltage
+    AP_Int8  _volt_pin;                 /// board pin used to measure battery voltage
-    AP_Int8     _curr_pin;                /// board pin used to measure battery current
+    AP_Int8  _curr_pin;                 /// board pin used to measure battery current
-    AP_Float    _volt_multiplier;         /// voltage on volt pin multiplied by this to calculate battery voltage
+    AP_Float _volt_multiplier;          /// voltage on volt pin multiplied by this to calculate battery voltage
-    AP_Float    _curr_amp_per_volt;       /// voltage on current pin multiplied by this to calculate current in amps
+    AP_Float _curr_amp_per_volt;        /// voltage on current pin multiplied by this to calculate current in amps
-    AP_Float    _curr_amp_offset;         /// offset voltage that is subtracted from current pin before conversion to amps
+    AP_Float _curr_amp_offset;          /// offset voltage that is subtracted from current pin before conversion to amps
-    AP_Int32    _pack_capacity;           /// battery pack capacity less reserve in mAh
+    AP_Int32 _pack_capacity;            /// battery pack capacity less reserve in mAh
-    AP_Int16    _watt_max;                /// max battery power allowed. Reduce max throttle to reduce current to satisfy t    his limit
+    AP_Int16 _watt_max;                 /// max battery power allowed. Reduce max throttle to reduce current to satisfy t    his limit
-    AP_Int32    _serial_number;           /// battery serial number, automatically filled in on SMBus batteries
+    AP_Int32 _serial_number;            /// battery serial number, automatically filled in on SMBus batteries
-    AP_Int8     _low_voltage_timeout;     /// timeout in seconds before a low voltage event will be triggered
+    AP_Int8  _low_voltage_timeout;      /// timeout in seconds before a low voltage event will be triggered
-    AP_Int8     _failsafe_voltage_source; /// voltage type used for detection of low voltage event
+    AP_Int8  _failsafe_voltage_source;  /// voltage type used for detection of low voltage event
+    AP_Float _low_voltage;              /// voltage level used to trigger a low battery failsafe
+    AP_Float _low_capacity;             /// capacity level used to trigger a low battery failsafe
+    AP_Float _critical_voltage;         /// voltage level used to trigger a critical battery failsafe
+    AP_Float _critical_capacity;        /// capacity level used to trigger a critical battery failsafe
+    AP_Int8  _failsafe_low_action;      /// action to preform on a low battery failsafe
+    AP_Int8  _failsafe_critical_action; /// action to preform on a critical battery failsafe
 
 };

libraries/AP_BattMonitor/examples/AP_BattMonitor_test/AP_BattMonitor_test.cpp

@@ -1,60 +0,0 @@
-/*
- *       Example of AP_BattMonitor library
- *       Code by DIYDrones.com
- */
-
-#include <AP_BattMonitor/AP_BattMonitor.h>
-#include <AP_HAL/AP_HAL.h>
-
-void setup();
-void loop();
-
-const AP_HAL::HAL& hal = AP_HAL::get_HAL();
-
-static AP_BattMonitor battery_mon{1<<9};
-uint32_t timer;
-
-void setup() {
-    hal.console->printf("Battery monitor library test\n");
-
-    // set battery monitor to smbus
-    battery_mon.set_monitoring(0, AP_BattMonitor_Params::BattMonitor_TYPE_SOLO);
-
-    // initialise the battery monitor
-    battery_mon.init();
-
-    hal.scheduler->delay(1000);
-    timer = AP_HAL::millis();
-}
-
-void loop()
-{
-    static uint8_t counter;  // counter to slow output to the user
-    uint32_t now = AP_HAL::millis();
-
-    // call battery monitor at 10hz
-    if ((now - timer) > 100) {
-        // update voltage and current readings
-        battery_mon.read();
-
-        // reset timer
-        timer = now;
-
-        // increment counter
-        counter++;
-    }
-
-    // display output at 1hz
-    if (counter >= 10) {
-        counter = 0;
-        hal.console->printf("\nVoltage: %.2f \tCurrent: %.2f \tTotCurr:%.2f",
-                            (double)battery_mon.voltage(),
-                            (double)battery_mon.current_amps(),
-                            (double)battery_mon.consumed_mah());
-    }
-
-    // delay 1ms
-    hal.scheduler->delay(1);
-}
-
-AP_HAL_MAIN();

libraries/AP_BattMonitor/examples/AP_BattMonitor_test/wscript

@@ -1,7 +0,0 @@
-#!/usr/bin/env python
-# encoding: utf-8
-
-def build(bld):
-    bld.ap_example(
-        use='ap',
-    )