mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-04 23:18:28 -04:00
80 lines
2.3 KiB
C++
80 lines
2.3 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_CAN.h>
|
|
|
|
extern const AP_HAL::HAL& hal;
|
|
|
|
#define debug_bm_uavcan(level_debug, can_driver, fmt, args...) do { if ((level_debug) <= AP::can().get_debug_level_driver(can_driver)) { 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 ¶ms) :
|
|
AP_BattMonitor_Backend(mon, mon_state, params),
|
|
_type(type)
|
|
{
|
|
// starts with not healthy
|
|
_state.healthy = false;
|
|
}
|
|
|
|
void AP_BattMonitor_UAVCAN::init()
|
|
{
|
|
uint8_t can_num_drivers = AP::can().get_num_drivers();
|
|
|
|
for (uint8_t i = 0; i < can_num_drivers; i++) {
|
|
AP_UAVCAN *ap_uavcan = AP_UAVCAN::get_uavcan(i);
|
|
if (ap_uavcan == nullptr) {
|
|
continue;
|
|
}
|
|
|
|
switch (_type) {
|
|
case UAVCAN_BATTERY_INFO:
|
|
if (ap_uavcan->register_BM_bi_listener_to_id(this, _params._serial_number)) {
|
|
debug_bm_uavcan(2, i, "UAVCAN BattMonitor BatteryInfo registered id: %d\n\r", _params._serial_number.get());
|
|
}
|
|
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.consumed_mah += mah;
|
|
_state.consumed_wh += 0.001f * mah * _state.voltage;
|
|
}
|
|
|
|
// record time
|
|
_state.last_time_micros = tnow;
|
|
|
|
_state.healthy = true;
|
|
}
|
|
|
|
#endif
|