AP_BattMonitor: Add support for BatteryInfoAux message

libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.cpp

@@ -9,14 +9,17 @@
 #include <AP_Common/AP_Common.h>
 #include <AP_Math/AP_Math.h>
 #include <AP_UAVCAN/AP_UAVCAN.h>
+#include <AP_BoardConfig/AP_BoardConfig.h>
 
 #include <uavcan/equipment/power/BatteryInfo.hpp>
+#include <ardupilot/equipment/power/BatteryInfoAux.hpp>
 
 #define LOG_TAG "BattMon"
 
 extern const AP_HAL::HAL& hal;
 
 UC_REGISTRY_BINDER(BattInfoCb, uavcan::equipment::power::BatteryInfo);
+UC_REGISTRY_BINDER(BattInfoAuxCb, ardupilot::equipment::power::BatteryInfoAux);
 
 /// Constructor
 AP_BattMonitor_UAVCAN::AP_BattMonitor_UAVCAN(AP_BattMonitor &mon, AP_BattMonitor::BattMonitor_State &mon_state, BattMonitor_UAVCAN_Type type, AP_BattMonitor_Params &params) :
@@ -40,7 +43,16 @@ void AP_BattMonitor_UAVCAN::subscribe_msgs(AP_UAVCAN* ap_uavcan)
     // Backend Msg Handler
     const int battinfo_listener_res = battinfo_listener->start(BattInfoCb(ap_uavcan, &handle_battery_info_trampoline));
     if (battinfo_listener_res < 0) {
-        AP_HAL::panic("UAVCAN BatteryInfo subscriber start problem\n\r");
+        AP_BoardConfig::allocation_error("UAVCAN BatteryInfo subscriber start problem\n\r");
+        return;
+    }
+
+    uavcan::Subscriber<ardupilot::equipment::power::BatteryInfoAux, BattInfoAuxCb> *battinfo_aux_listener;
+    battinfo_aux_listener = new uavcan::Subscriber<ardupilot::equipment::power::BatteryInfoAux, BattInfoAuxCb>(*node);
+    // Backend Msg Handler
+    const int battinfo_aux_listener_res = battinfo_aux_listener->start(BattInfoAuxCb(ap_uavcan, &handle_battery_info_aux_trampoline));
+    if (battinfo_aux_listener_res < 0) {
+        AP_BoardConfig::allocation_error("UAVCAN BatteryInfoAux subscriber start problem");
         return;
     }
 }
@@ -90,6 +102,8 @@ void AP_BattMonitor_UAVCAN::handle_battery_info(const BattInfoCb &cb)
     _interim_state.voltage = cb.msg->voltage;
     _interim_state.current_amps = cb.msg->current;
     _soc = cb.msg->state_of_charge_pct;
+    _remaining_capacity_wh = cb.msg->remaining_capacity_wh;
+    _full_charge_capacity_wh = cb.msg->full_charge_capacity_wh;
 
     if (!isnanf(cb.msg->temperature) && cb.msg->temperature > 0) {
         // Temperature reported from battery in kelvin and stored internally in Celsius.
@@ -98,6 +112,8 @@ void AP_BattMonitor_UAVCAN::handle_battery_info(const BattInfoCb &cb)
     }
 
     uint32_t tnow = AP_HAL::micros();
+
+    if (!_has_battery_info_aux) {
         uint32_t dt = tnow - _interim_state.last_time_micros;
 
         // update total current drawn since startup
@@ -107,13 +123,36 @@ void AP_BattMonitor_UAVCAN::handle_battery_info(const BattInfoCb &cb)
             _interim_state.consumed_mah += mah;
             _interim_state.consumed_wh  += 0.001f * mah * _interim_state.voltage;
         }
+    }
 
     // record time
     _interim_state.last_time_micros = tnow;
-
     _interim_state.healthy = true;
 }
 
+void AP_BattMonitor_UAVCAN::handle_battery_info_aux(const BattInfoAuxCb &cb)
+{
+    WITH_SEMAPHORE(_sem_battmon);
+    uint8_t cell_count = MIN(ARRAY_SIZE(_interim_state.cell_voltages.cells), cb.msg->voltage_cell.size());
+    float remaining_capacity_ah = _remaining_capacity_wh / cb.msg->nominal_voltage;
+    float full_charge_capacity_ah = _full_charge_capacity_wh / cb.msg->nominal_voltage;
+
+    _cycle_count = cb.msg->cycle_count;
+    for (uint8_t i = 0; i < cell_count; i++) {
+        _interim_state.cell_voltages.cells[i] = cb.msg->voltage_cell[i] * 1000;
+    }
+    _interim_state.is_powering_off = cb.msg->is_powering_off;
+    _interim_state.consumed_mah = (full_charge_capacity_ah - remaining_capacity_ah) * 1000;
+    _interim_state.consumed_wh = _full_charge_capacity_wh - _remaining_capacity_wh;
+    _interim_state.time_remaining =  is_zero(_interim_state.current_amps) ? 0 : (remaining_capacity_ah / _interim_state.current_amps * 3600);
+    _interim_state.has_time_remaining = true;
+
+    _has_cell_voltages = true;
+    _has_time_remaining = true;
+    _has_consumed_energy = true;
+    _has_battery_info_aux = true;
+}
+
 void AP_BattMonitor_UAVCAN::handle_battery_info_trampoline(AP_UAVCAN* ap_uavcan, uint8_t node_id, const BattInfoCb &cb)
 {
     AP_BattMonitor_UAVCAN* driver = get_uavcan_backend(ap_uavcan, node_id, cb.msg->battery_id);
@@ -123,6 +162,15 @@ void AP_BattMonitor_UAVCAN::handle_battery_info_trampoline(AP_UAVCAN* ap_uavcan,
     driver->handle_battery_info(cb);
 }
 
+void AP_BattMonitor_UAVCAN::handle_battery_info_aux_trampoline(AP_UAVCAN* ap_uavcan, uint8_t node_id, const BattInfoAuxCb &cb)
+{
+    AP_BattMonitor_UAVCAN* driver = get_uavcan_backend(ap_uavcan, node_id, cb.msg->battery_id);
+    if (driver == nullptr) {
+        return;
+    }
+    driver->handle_battery_info_aux(cb);
+}
+
 // read - read the voltage and current
 void AP_BattMonitor_UAVCAN::read()
 {
@@ -142,6 +190,10 @@ void AP_BattMonitor_UAVCAN::read()
     _state.consumed_wh = _interim_state.consumed_wh;
     _state.last_time_micros = _interim_state.last_time_micros;
     _state.healthy = _interim_state.healthy;
+    _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;
+    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;
 }
@@ -165,4 +217,14 @@ bool AP_BattMonitor_UAVCAN::capacity_remaining_pct(uint8_t &percentage) const
     return true;
 }
 
+/// get_cycle_count - return true if cycle count can be provided and fills in cycles argument
+bool AP_BattMonitor_UAVCAN::get_cycle_count(uint16_t &cycles) const
+{
+    if (_has_battery_info_aux) {
+        cycles = _cycle_count;
+        return true;
+    }
+    return false;
+}
+
 #endif

libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.h

@@ -8,6 +8,7 @@
 #define AP_BATTMONITOR_UAVCAN_TIMEOUT_MICROS         5000000 // sensor becomes unhealthy if no successful readings for 5 seconds
 
 class BattInfoCb;
+class BattInfoAuxCb;
 
 class AP_BattMonitor_UAVCAN : public AP_BattMonitor_Backend
 {
@@ -29,16 +30,24 @@ public:
 
     bool has_temperature() const override { return _has_temperature; }
 
-    bool has_current() const override {
+    bool has_current() const override { return true; }
-        return true;
+
-    }
+    bool has_consumed_energy() const override { return _has_consumed_energy; }
+
+    bool has_time_remaining() const override { return _has_time_remaining; }
+
+    bool has_cell_voltages() const override { return _has_cell_voltages; }
+
+    bool get_cycle_count(uint16_t &cycles) const override;
 
     static void subscribe_msgs(AP_UAVCAN* ap_uavcan);
     static AP_BattMonitor_UAVCAN* get_uavcan_backend(AP_UAVCAN* ap_uavcan, uint8_t node_id, uint8_t battery_id);
     static void handle_battery_info_trampoline(AP_UAVCAN* ap_uavcan, uint8_t node_id, const BattInfoCb &cb);
+    static void handle_battery_info_aux_trampoline(AP_UAVCAN* ap_uavcan, uint8_t node_id, const BattInfoAuxCb &cb);
 
 private:
     void handle_battery_info(const BattInfoCb &cb);
+    void handle_battery_info_aux(const BattInfoAuxCb &cb);
 
     static bool match_battery_id(uint8_t instance, uint8_t battery_id) {
         // when serial number is negative, all batteries are accepted. Else, it must match
@@ -51,7 +60,14 @@ private:
     HAL_Semaphore _sem_battmon;
 
     AP_UAVCAN* _ap_uavcan;
-    uint8_t _node_id;
     uint8_t _soc;
+    uint8_t _node_id;
+    uint16_t _cycle_count;
+    float _remaining_capacity_wh;
+    float _full_charge_capacity_wh;
     bool _has_temperature;
+    bool _has_cell_voltages;
+    bool _has_time_remaining;
+    bool _has_consumed_energy;
+    bool _has_battery_info_aux;
 };