diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.cpp b/libraries/AP_BattMonitor/AP_BattMonitor.cpp index 35819b8d2efc0..bf04aeb514aac 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.cpp +++ b/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) { diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.h b/libraries/AP_BattMonitor/AP_BattMonitor.h index 2df3fd5210830..e9e10b7ef06ed 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor.h @@ -154,6 +154,8 @@ class AP_BattMonitor 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 @@ class AP_BattMonitor // 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 diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp index 1aef15a6efda3..842c6ac907852 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp +++ b/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 diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.h b/libraries/AP_BattMonitor/AP_BattMonitor_Backend.h index 8cc972e7e8a5e..907d8712c1174 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Backend.h @@ -58,6 +58,9 @@ class AP_BattMonitor_Backend // 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; diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.cpp index 123287c89a92c..3c7d35fd9e5b9 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.cpp +++ b/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; diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.h b/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.h index 819ade7316481..a25ef9667855c 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.h @@ -61,7 +61,7 @@ class AP_BattMonitor_DroneCAN : public AP_BattMonitor_Backend 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); diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Logging.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Logging.cpp index f5595d321aca3..4f3433d87adde 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Logging.cpp +++ b/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)); } diff --git a/libraries/AP_BattMonitor/LogStructure.h b/libraries/AP_BattMonitor/LogStructure.h index fdc97512ef006..5d0bb2cf13043 100644 --- a/libraries/AP_BattMonitor/LogStructure.h +++ b/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 },