ardupilot/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.cpp

81 lines
2.5 KiB
C++

#include <AP_HAL/AP_HAL.h>
#if HAL_WITH_UAVCAN
#include <AP_Common/AP_Common.h>
#include <AP_Math/AP_Math.h>
#include "AP_BattMonitor.h"
#include "AP_BattMonitor_UAVCAN.h"
#include <AP_BoardConfig/AP_BoardConfig.h>
#include <AP_BoardConfig/AP_BoardConfig_CAN.h>
extern const AP_HAL::HAL& hal;
#define debug_bm_uavcan(level, fmt, args...) do { if ((level) <= AP_BoardConfig_CAN::get_can_debug()) { printf(fmt, ##args); }} while (0)
/// Constructor
AP_BattMonitor_UAVCAN::AP_BattMonitor_UAVCAN(AP_BattMonitor &mon, AP_BattMonitor::BattMonitor_State &mon_state, BattMonitor_UAVCAN_Type type, AP_BattMonitor_Params &params) :
AP_BattMonitor_Backend(mon, mon_state, params),
_type(type)
{
// starts with not healthy
_state.healthy = false;
}
void AP_BattMonitor_UAVCAN::init()
{
if (AP_BoardConfig_CAN::get_can_num_ifaces() != 0) {
for (uint8_t i = 0; i < MAX_NUMBER_OF_CAN_DRIVERS; i++) {
if (hal.can_mgr[i] != nullptr) {
AP_UAVCAN *uavcan = hal.can_mgr[i]->get_UAVCAN();
if (uavcan != nullptr) {
switch (_type) {
case UAVCAN_BATTERY_INFO:
if (uavcan->register_BM_bi_listener_to_id(this, _params._serial_number)) {
debug_bm_uavcan(2, "UAVCAN BattMonitor BatteryInfo registered id: %d\n\r", _params._serial_number);
}
break;
}
}
}
}
}
}
// read - read the voltage and current
void AP_BattMonitor_UAVCAN::read()
{
uint32_t tnow = AP_HAL::micros();
// timeout after 5 seconds
if ((tnow - _state.last_time_micros) > AP_BATTMONITOR_UAVCAN_TIMEOUT_MICROS) {
_state.healthy = false;
}
}
void AP_BattMonitor_UAVCAN::handle_bi_msg(float voltage, float current, float temperature)
{
_state.temperature = temperature;
_state.voltage = voltage;
_state.current_amps = current;
uint32_t tnow = AP_HAL::micros();
uint32_t dt = tnow - _state.last_time_micros;
// update total current drawn since startup
if (_state.last_time_micros != 0 && dt < 2000000) {
// .0002778 is 1/3600 (conversion to hours)
float mah = (float) ((double) _state.current_amps * (double) dt * (double) 0.0000002778f);
_state.current_total_mah += mah;
_state.consumed_wh += 0.001f * mah * _state.voltage;
}
// record time
_state.last_time_micros = tnow;
_state.healthy = true;
}
#endif