AP_BattMonitor: support logging state-of-health percentage

Only DroneCAN backend implements this feature for now

libraries/AP_BattMonitor/AP_BattMonitor.cpp

@@ -1056,6 +1056,15 @@ uint32_t AP_BattMonitor::get_mavlink_fault_bitmask(const uint8_t instance) const
     return drivers[instance]->get_mavlink_fault_bitmask();
 }
 
+// return true if state of health (as a percentage) can be provided and fills in soh_pct argument
+bool AP_BattMonitor::get_state_of_health_pct(uint8_t instance, uint8_t &soh_pct) const
+{
+    if (instance >= _num_instances || drivers[instance] == nullptr) {
+        return false;
+    }
+    return drivers[instance]->get_state_of_health_pct(soh_pct);
+}
+
 // Enable/Disable (Turn on/off) MPPT power to all backends who are MPPTs
 void AP_BattMonitor::MPPT_set_powered_state_to_all(const bool power_on)
 {

libraries/AP_BattMonitor/AP_BattMonitor.h

@@ -154,6 +154,8 @@ public:
         bool        powerOffNotified;          // only send powering off notification once
         uint32_t    time_remaining;            // remaining battery time
         bool        has_time_remaining;        // time_remaining is only valid if this is true
+        uint8_t     state_of_health_pct;       // state of health (SOH) in percent
+        bool        has_state_of_health_pct;   // state_of_health_pct is only valid if this is true
         uint8_t     instance;                  // instance number of this backend
         const struct AP_Param::GroupInfo *var_info;
     };
@@ -274,6 +276,9 @@ public:
     // Returns mavlink fault state
     uint32_t get_mavlink_fault_bitmask(const uint8_t instance) const;
 
+    // return true if state of health (as a percentage) can be provided and fills in soh_pct argument
+    bool get_state_of_health_pct(uint8_t instance, uint8_t &soh_pct) const;
+
     static const struct AP_Param::GroupInfo var_info[];
 
 #if AP_BATTERY_SCRIPTING_ENABLED

libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp

@@ -107,6 +107,16 @@ void AP_BattMonitor_Backend::update_resistance_estimate()
     _state.voltage_resting_estimate = _state.voltage + _state.current_amps * _state.resistance;
 }
 
+// return true if state of health can be provided and fills in soh_pct argument
+bool AP_BattMonitor_Backend::get_state_of_health_pct(uint8_t &soh_pct) const
+{
+    if (!_state.has_state_of_health_pct) {
+        return false;
+    }
+    soh_pct = _state.state_of_health_pct;
+    return true;
+}
+
 float AP_BattMonitor_Backend::voltage_resting_estimate() const
 {
     // resting voltage should always be greater than or equal to the raw voltage

libraries/AP_BattMonitor/AP_BattMonitor_Backend.h

@@ -58,6 +58,9 @@ public:
     // return true if cycle count can be provided and fills in cycles argument
     virtual bool get_cycle_count(uint16_t &cycles) const { return false; }
 
+    // return true if state of health (as a percentage) can be provided and fills in soh_pct argument
+    bool get_state_of_health_pct(uint8_t &soh_pct) const;
+
     /// get voltage with sag removed (based on battery current draw and resistance)
     /// this will always be greater than or equal to the raw voltage
     float voltage_resting_estimate() const;

libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.cpp

@@ -114,14 +114,20 @@ AP_BattMonitor_DroneCAN* AP_BattMonitor_DroneCAN::get_dronecan_backend(AP_DroneC
 
 void AP_BattMonitor_DroneCAN::handle_battery_info(const uavcan_equipment_power_BatteryInfo &msg)
 {
-    update_interim_state(msg.voltage, msg.current, msg.temperature, msg.state_of_charge_pct); 
+    update_interim_state(msg.voltage, msg.current, msg.temperature, msg.state_of_charge_pct, msg.state_of_health_pct); 
 
     WITH_SEMAPHORE(_sem_battmon);
     _remaining_capacity_wh = msg.remaining_capacity_wh;
     _full_charge_capacity_wh = msg.full_charge_capacity_wh;
+
+    // consume state of health
+    if (msg.state_of_health_pct != UAVCAN_EQUIPMENT_POWER_BATTERYINFO_STATE_OF_HEALTH_UNKNOWN) {
+        _interim_state.state_of_health_pct = msg.state_of_health_pct;
+        _interim_state.has_state_of_health_pct = true;
+    }
 }
 
-void AP_BattMonitor_DroneCAN::update_interim_state(const float voltage, const float current, const float temperature_K, const uint8_t soc)
+void AP_BattMonitor_DroneCAN::update_interim_state(const float voltage, const float current, const float temperature_K, const uint8_t soc, uint8_t soh_pct)
 {
     WITH_SEMAPHORE(_sem_battmon);
 
@@ -145,6 +151,12 @@ void AP_BattMonitor_DroneCAN::update_interim_state(const float voltage, const fl
         update_consumed(_interim_state, dt_us);
     }
 
+    // state of health
+    if (soh_pct != UAVCAN_EQUIPMENT_POWER_BATTERYINFO_STATE_OF_HEALTH_UNKNOWN) {
+        _interim_state.state_of_health_pct = soh_pct;
+        _interim_state.has_state_of_health_pct = true;
+    }
+
     // record time
     _interim_state.last_time_micros = tnow;
     _interim_state.healthy = true;
@@ -185,7 +197,7 @@ void AP_BattMonitor_DroneCAN::handle_mppt_stream(const mppt_Stream &msg)
     // convert C to Kelvin
     const float temperature_K = isnan(msg.temperature) ? 0 : C_TO_KELVIN(msg.temperature);
 
-    update_interim_state(voltage, current, temperature_K, soc); 
+    update_interim_state(voltage, current, temperature_K, soc, UAVCAN_EQUIPMENT_POWER_BATTERYINFO_STATE_OF_HEALTH_UNKNOWN); 
 
     if (!_mppt.is_detected) {
         // this is the first time the mppt message has been received
@@ -283,6 +295,8 @@ void AP_BattMonitor_DroneCAN::read()
     _state.time_remaining = _interim_state.time_remaining;
     _state.has_time_remaining = _interim_state.has_time_remaining;
     _state.is_powering_off = _interim_state.is_powering_off;
+    _state.state_of_health_pct = _interim_state.state_of_health_pct;
+    _state.has_state_of_health_pct = _interim_state.has_state_of_health_pct;
     memcpy(_state.cell_voltages.cells, _interim_state.cell_voltages.cells, sizeof(_state.cell_voltages));
 
     _has_temperature = (AP_HAL::millis() - _state.temperature_time) <= AP_BATT_MONITOR_TIMEOUT;

libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.h

@@ -61,7 +61,7 @@ public:
 private:
     void handle_battery_info(const uavcan_equipment_power_BatteryInfo &msg);
     void handle_battery_info_aux(const ardupilot_equipment_power_BatteryInfoAux &msg);
-    void update_interim_state(const float voltage, const float current, const float temperature_K, const uint8_t soc);
+    void update_interim_state(const float voltage, const float current, const float temperature_K, const uint8_t soc, uint8_t soh_pct);
 
     static bool match_battery_id(uint8_t instance, uint8_t battery_id);
 

libraries/AP_BattMonitor/AP_BattMonitor_Logging.cpp

@@ -16,6 +16,9 @@ void AP_BattMonitor_Backend::Log_Write_BAT(const uint8_t instance, const uint64_
         temperature_cd = temperature * 100.0;
     }
 
+    uint8_t soh_pct = 0;
+    IGNORE_RETURN(get_state_of_health_pct(soh_pct));
+
     const struct log_BAT pkt{
         LOG_PACKET_HEADER_INIT(LOG_BAT_MSG),
         time_us             : time_us,
@@ -28,7 +31,8 @@ void AP_BattMonitor_Backend::Log_Write_BAT(const uint8_t instance, const uint64_
         temperature         : temperature_cd,
         resistance          : _state.resistance,
         rem_percent         : percent,
-        health              : _state.healthy
+        health              : _state.healthy,
+        state_of_health_pct : soh_pct
     };
     AP::logger().WriteBlock(&pkt, sizeof(pkt));
 }

libraries/AP_BattMonitor/LogStructure.h

@@ -19,6 +19,7 @@
 // @Field: Res: estimated battery resistance
 // @Field: RemPct: remaining percentage
 // @Field: H: health
+// @Field: SH: state of health percentage.  0 if unknown
 struct PACKED log_BAT {
     LOG_PACKET_HEADER;
     uint64_t time_us;
@@ -32,6 +33,7 @@ struct PACKED log_BAT {
     float    resistance;
     uint8_t  rem_percent;
     uint8_t  health;
+    uint8_t  state_of_health_pct;
 };
 
 // @LoggerMessage: BCL
@@ -61,6 +63,6 @@ struct PACKED log_BCL {
 
 #define LOG_STRUCTURE_FROM_BATTMONITOR        \
     { LOG_BAT_MSG, sizeof(log_BAT), \
-        "BAT", "QBfffffcfBB", "TimeUS,Inst,Volt,VoltR,Curr,CurrTot,EnrgTot,Temp,Res,RemPct,H", "s#vvAaXOw%-", "F-000C0?000" , true },  \
+        "BAT", "QBfffffcfBBB", "TimeUS,Inst,Volt,VoltR,Curr,CurrTot,EnrgTot,Temp,Res,RemPct,H,SH", "s#vvAaXOw%-%", "F-000C0?0000" , true },  \
     { LOG_BCL_MSG, sizeof(log_BCL), \
         "BCL", "QBfHHHHHHHHHHHH", "TimeUS,Instance,Volt,V1,V2,V3,V4,V5,V6,V7,V8,V9,V10,V11,V12", "s#vvvvvvvvvvvvv", "F-0CCCCCCCCCCCC" , true },