Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_BattMonitor: Add support for BatteryInfoAux message

This commit is contained in:
李孟晓 2021-10-11 07:26:42 +00:00 committed by Andrew Tridgell
parent 36ceb76cae
commit e5efc52324
2 changed files with 91 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

80
libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.cpp
View File

@ -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,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

24
libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.h
View File

@ -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;
};