AP_BattMonitor: resistance learning

libraries/AP_BattMonitor/AP_BattMonitor.cpp

@@ -227,6 +227,7 @@ AP_BattMonitor::read()
     for (uint8_t i=0; i<_num_instances; i++) {
         if (drivers[i] != nullptr && _monitoring[i] != BattMonitor_TYPE_NONE) {
             drivers[i]->read();
+            drivers[i]->update_resistance_estimate();
         }
     }
 }
@@ -261,6 +262,17 @@ float AP_BattMonitor::voltage(uint8_t instance) const
     }
 }
 
+/// get voltage with sag removed (based on battery current draw and resistance)
+float AP_BattMonitor::voltage_resting_estimate(uint8_t instance) const
+{
+    if (instance < _num_instances) {
+        // resting voltage should always be greater than or equal to the raw voltage
+        return MAX(_BattMonitor_STATE(instance).voltage, _BattMonitor_STATE(instance).voltage_resting_estimate);
+    } else {
+        return 0.0f;
+    }
+}
+
 /// current_amps - returns the instantaneous current draw in amperes
 float AP_BattMonitor::current_amps(uint8_t instance) const {
     if (instance < _num_instances) {

libraries/AP_BattMonitor/AP_BattMonitor.h

@@ -18,6 +18,9 @@
 
 #define AP_BATT_MONITOR_TIMEOUT             5000
 
+#define AP_BATT_MONITOR_RES_EST_TC_1        0.5f
+#define AP_BATT_MONITOR_RES_EST_TC_2        0.1f
+
 // declare backend class
 class AP_BattMonitor_Backend;
 class AP_BattMonitor_Analog;
@@ -65,6 +68,8 @@ public:
         cells       cell_voltages;      // battery cell voltages in millivolts, 10 cells matches the MAVLink spec
         float       temperature;        // battery temperature in celsius
         uint32_t    temperature_time;   // timestamp of the last recieved temperature message
+        float       voltage_resting_estimate; // voltage with sag removed based on current and resistance estimate
+        float       resistance;         // resistance calculated by comparing resting voltage vs in flight voltage
     };
 
     // Return the number of battery monitor instances
@@ -93,6 +98,10 @@ public:
     float voltage(uint8_t instance) const;
     float voltage() const { return voltage(AP_BATT_PRIMARY_INSTANCE); }
 
+    /// get voltage with sag removed (based on battery current draw and resistance)
+    float voltage_resting_estimate(uint8_t instance) const;
+    float voltage_resting_estimate() const { return voltage_resting_estimate(AP_BATT_PRIMARY_INSTANCE); }
+
     /// current_amps - returns the instantaneous current draw in amperes
     float current_amps(uint8_t instance) const;
     float current_amps() const { return current_amps(AP_BATT_PRIMARY_INSTANCE); }
@@ -135,6 +144,10 @@ public:
     bool get_temperature(float &temperature) const { return get_temperature(temperature, AP_BATT_PRIMARY_INSTANCE); };
     bool get_temperature(float &temperature, const uint8_t instance) const;
 
+    // get battery resistance estimate in ohms
+    float get_resistance() const { return get_resistance(AP_BATT_PRIMARY_INSTANCE); }
+    float get_resistance(uint8_t instance) const { return state[instance].resistance; }
+
     static const struct AP_Param::GroupInfo var_info[];
 
 protected:

libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp

@@ -44,3 +44,54 @@ int32_t AP_BattMonitor_Backend::get_capacity() const
 {
     return _mon.pack_capacity_mah(_state.instance);
 }
+
+// update battery resistance estimate
+// faster rates of change of the current and voltage readings cause faster updates to the resistance estimate
+// the battery resistance is calculated by comparing the latest current and voltage readings to a low-pass filtered current and voltage
+// high current steps are integrated into the resistance estimate by varying the time constant of the resistance filter
+void AP_BattMonitor_Backend::update_resistance_estimate()
+{
+    // return immediately if no current
+    if (!has_current() || !is_positive(_state.current_amps)) {
+        return;
+    }
+
+    // update maximum current seen since startup and protect against divide by zero
+    _current_max_amps = MAX(_current_max_amps, _state.current_amps);
+    float current_delta = _state.current_amps - _current_filt_amps;
+    if (is_zero(current_delta)) {
+        return;
+    }
+
+    // update reference voltage and current
+    if (_state.voltage > _resistance_voltage_ref) {
+        _resistance_voltage_ref = _state.voltage;
+        _resistance_current_ref = _state.current_amps;
+    }
+
+    // calculate time since last update
+    uint32_t now = AP_HAL::millis();
+    float loop_interval = (now - _resistance_timer_ms) / 1000.0f;
+    _resistance_timer_ms = now;
+
+    // estimate short-term resistance
+    float filt_alpha = constrain_float(loop_interval/(loop_interval + AP_BATT_MONITOR_RES_EST_TC_1), 0.0f, 0.5f);
+    float resistance_alpha = MIN(1, AP_BATT_MONITOR_RES_EST_TC_2*fabsf((_state.current_amps-_current_filt_amps)/_current_max_amps));
+    float resistance_estimate = -(_state.voltage-_voltage_filt)/current_delta;
+    if (is_positive(resistance_estimate)) {
+        _state.resistance = _state.resistance*(1-resistance_alpha) + resistance_estimate*resistance_alpha;
+    }
+
+    // calculate maximum resistance
+    if ((_resistance_voltage_ref > _state.voltage) && (_state.current_amps > _resistance_current_ref)) {
+        float resistance_max = (_resistance_voltage_ref - _state.voltage) / (_state.current_amps - _resistance_current_ref);
+        _state.resistance = MIN(_state.resistance, resistance_max);
+    }
+
+    // update the filtered voltage and currents
+    _voltage_filt = _voltage_filt*(1-filt_alpha) + _state.voltage*filt_alpha;
+    _current_filt_amps = _current_filt_amps*(1-filt_alpha) + _state.current_amps*filt_alpha;
+
+    // update estimated voltage without sag
+    _state.voltage_resting_estimate = _state.voltage + _state.current_amps * _state.resistance;
+}

libraries/AP_BattMonitor/AP_BattMonitor_Backend.h

@@ -43,7 +43,19 @@ public:
     /// get capacity for this instance
     int32_t get_capacity() const;
 
+    // update battery resistance estimate and voltage_resting_estimate
+    void update_resistance_estimate();
+
 protected:
     AP_BattMonitor                      &_mon;      // reference to front-end
     AP_BattMonitor::BattMonitor_State   &_state;    // reference to this instances state (held in the front-end)
+
+private:
+    // resistance estimate
+    uint32_t    _resistance_timer_ms;    // system time of last resistance estimate update
+    float       _voltage_filt;           // filtered voltage
+    float       _current_max_amps;       // maximum current since start-up
+    float       _current_filt_amps;      // filtered current
+    float       _resistance_voltage_ref; // voltage used for maximum resistance calculation
+    float       _resistance_current_ref; // current used for maximum resistance calculation
 };