diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.cpp index ee9223bb65..a597c326f9 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.cpp @@ -9,14 +9,17 @@ #include #include #include +#include #include +#include #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 ¶ms) : @@ -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 *battinfo_aux_listener; + battinfo_aux_listener = new uavcan::Subscriber(*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,22 +112,47 @@ void AP_BattMonitor_UAVCAN::handle_battery_info(const BattInfoCb &cb) } uint32_t tnow = AP_HAL::micros(); - uint32_t dt = tnow - _interim_state.last_time_micros; - // update total current drawn since startup - if (_interim_state.last_time_micros != 0 && dt < 2000000) { - // .0002778 is 1/3600 (conversion to hours) - float mah = (float) ((double) _interim_state.current_amps * (double) dt * (double) 0.0000002778f); - _interim_state.consumed_mah += mah; - _interim_state.consumed_wh += 0.001f * mah * _interim_state.voltage; + if (!_has_battery_info_aux) { + uint32_t dt = tnow - _interim_state.last_time_micros; + + // update total current drawn since startup + if (_interim_state.last_time_micros != 0 && dt < 2000000) { + // .0002778 is 1/3600 (conversion to hours) + float mah = (float) ((double) _interim_state.current_amps * (double) dt * (double) 0.0000002778f); + _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 diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.h b/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.h index a3ad6b9732..84f9e60fdd 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.h +++ b/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 { - return true; - } + bool has_current() const override { 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; };