mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-03-09 08:04:14 -03:00
AP_BattMonitor: move BattInfo message subscription and handling to AP_BattMonitor
This commit is contained in:
parent
14b701cff8
commit
6e85003b56
@ -3,9 +3,13 @@
|
||||
#include "AP_BattMonitor_SMBus.h"
|
||||
#include "AP_BattMonitor_Bebop.h"
|
||||
#include "AP_BattMonitor_BLHeliESC.h"
|
||||
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
|
||||
#if HAL_WITH_UAVCAN
|
||||
#include "AP_BattMonitor_UAVCAN.h"
|
||||
#endif
|
||||
|
||||
#include <AP_Vehicle/AP_Vehicle_Type.h>
|
||||
#include <DataFlash/DataFlash.h>
|
||||
#include <GCS_MAVLink/GCS.h>
|
||||
|
@ -49,10 +49,6 @@ public:
|
||||
// update battery resistance estimate and voltage_resting_estimate
|
||||
void update_resistance_estimate();
|
||||
|
||||
// callback for UAVCAN messages
|
||||
virtual void handle_bi_msg(float voltage, float current,
|
||||
float temperature) {}
|
||||
|
||||
protected:
|
||||
AP_BattMonitor &_mon; // reference to front-end
|
||||
AP_BattMonitor::BattMonitor_State &_state; // reference to this instances state (held in the front-end)
|
||||
|
@ -2,17 +2,22 @@
|
||||
|
||||
#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>
|
||||
#include <AP_Common/AP_Common.h>
|
||||
#include <AP_Common/Semaphore.h>
|
||||
#include <AP_Math/AP_Math.h>
|
||||
#include <AP_UAVCAN/AP_UAVCAN.h>
|
||||
|
||||
#include <uavcan/equipment/power/BatteryInfo.hpp>
|
||||
|
||||
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)
|
||||
|
||||
UC_REGISTRY_BINDER(BattInfoCb, uavcan::equipment::power::BatteryInfo);
|
||||
|
||||
/// 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),
|
||||
@ -22,24 +27,90 @@ AP_BattMonitor_UAVCAN::AP_BattMonitor_UAVCAN(AP_BattMonitor &mon, AP_BattMonitor
|
||||
_state.healthy = false;
|
||||
}
|
||||
|
||||
void AP_BattMonitor_UAVCAN::init()
|
||||
void AP_BattMonitor_UAVCAN::subscribe_msgs(AP_UAVCAN* ap_uavcan)
|
||||
{
|
||||
uint8_t can_num_drivers = AP::can().get_num_drivers();
|
||||
if (ap_uavcan == nullptr) {
|
||||
return;
|
||||
}
|
||||
|
||||
for (uint8_t i = 0; i < can_num_drivers; i++) {
|
||||
AP_UAVCAN *ap_uavcan = AP_UAVCAN::get_uavcan(i);
|
||||
if (ap_uavcan == nullptr) {
|
||||
auto* node = ap_uavcan->get_node();
|
||||
|
||||
uavcan::Subscriber<uavcan::equipment::power::BatteryInfo, BattInfoCb> *battinfo_listener;
|
||||
battinfo_listener = new uavcan::Subscriber<uavcan::equipment::power::BatteryInfo, BattInfoCb>(*node);
|
||||
// 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");
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
AP_BattMonitor_UAVCAN* AP_BattMonitor_UAVCAN::get_uavcan_backend(AP_UAVCAN* ap_uavcan, uint8_t node_id)
|
||||
{
|
||||
if (ap_uavcan == nullptr) {
|
||||
return nullptr;
|
||||
}
|
||||
for (uint8_t i = 0; i < AP::battery()._num_instances; i++) {
|
||||
if (AP::battery().drivers[i] == nullptr ||
|
||||
AP::battery().get_type(i) != AP_BattMonitor_Params::BattMonitor_TYPE_UAVCAN_BatteryInfo) {
|
||||
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;
|
||||
AP_BattMonitor_UAVCAN* driver = (AP_BattMonitor_UAVCAN*)AP::battery().drivers[i];
|
||||
if (driver->_ap_uavcan == ap_uavcan && driver->_node_id == node_id) {
|
||||
return driver;
|
||||
}
|
||||
}
|
||||
// find empty uavcan driver
|
||||
for (uint8_t i = 0; i < AP::battery()._num_instances; i++) {
|
||||
if (AP::battery().drivers[i] != nullptr &&
|
||||
AP::battery().get_type(i) == AP_BattMonitor_Params::BattMonitor_TYPE_UAVCAN_BatteryInfo) {
|
||||
|
||||
AP_BattMonitor_UAVCAN* batmon = (AP_BattMonitor_UAVCAN*)AP::battery().drivers[i];
|
||||
batmon->_ap_uavcan = ap_uavcan;
|
||||
batmon->_node_id = node_id;
|
||||
batmon->init();
|
||||
debug_bm_uavcan(2,
|
||||
ap_uavcan->get_driver_index(),
|
||||
"Registered BattMonitor Node %d on Bus %d\n",
|
||||
node_id,
|
||||
ap_uavcan->get_driver_index());
|
||||
return batmon;
|
||||
}
|
||||
}
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
void AP_BattMonitor_UAVCAN::handle_battery_info(const BattInfoCb &cb)
|
||||
{
|
||||
WITH_SEMAPHORE(_sem_battmon);
|
||||
_interim_state.temperature = cb.msg->temperature;
|
||||
_interim_state.voltage = cb.msg->voltage;
|
||||
_interim_state.current_amps = cb.msg->current;
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
// record time
|
||||
_interim_state.last_time_micros = tnow;
|
||||
|
||||
_interim_state.healthy = 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);
|
||||
if (driver == nullptr) {
|
||||
return;
|
||||
}
|
||||
driver->handle_battery_info(cb);
|
||||
}
|
||||
|
||||
// read - read the voltage and current
|
||||
@ -48,32 +119,18 @@ 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;
|
||||
if ((tnow - _interim_state.last_time_micros) > AP_BATTMONITOR_UAVCAN_TIMEOUT_MICROS) {
|
||||
_interim_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;
|
||||
// Copy over relevant states over to main state
|
||||
WITH_SEMAPHORE(_sem_battmon);
|
||||
_state.temperature = _interim_state.temperature;
|
||||
_state.voltage = _interim_state.voltage;
|
||||
_state.current_amps = _interim_state.current_amps;
|
||||
_state.consumed_mah = _interim_state.consumed_mah;
|
||||
_state.consumed_wh = _interim_state.consumed_wh;
|
||||
_state.last_time_micros = _interim_state.last_time_micros;
|
||||
_state.healthy = _interim_state.healthy;
|
||||
}
|
||||
|
||||
#endif
|
||||
|
@ -1,15 +1,17 @@
|
||||
#pragma once
|
||||
|
||||
#include <AP_UAVCAN/AP_UAVCAN.h>
|
||||
#include "AP_BattMonitor.h"
|
||||
#include "AP_BattMonitor_Backend.h"
|
||||
|
||||
#include <AP_UAVCAN/AP_UAVCAN.h>
|
||||
|
||||
#define AP_BATTMONITOR_UAVCAN_TIMEOUT_MICROS 5000000 // sensor becomes unhealthy if no successful readings for 5 seconds
|
||||
|
||||
class BattInfoCb;
|
||||
|
||||
class AP_BattMonitor_UAVCAN : public AP_BattMonitor_Backend
|
||||
{
|
||||
public:
|
||||
|
||||
enum BattMonitor_UAVCAN_Type {
|
||||
UAVCAN_BATTERY_INFO = 0
|
||||
};
|
||||
@ -17,17 +19,27 @@ public:
|
||||
/// Constructor
|
||||
AP_BattMonitor_UAVCAN(AP_BattMonitor &mon, AP_BattMonitor::BattMonitor_State &mon_state, BattMonitor_UAVCAN_Type type, AP_BattMonitor_Params ¶ms);
|
||||
|
||||
void init() override {}
|
||||
|
||||
/// Read the battery voltage and current. Should be called at 10hz
|
||||
void read() override;
|
||||
|
||||
void init() override;
|
||||
|
||||
bool has_current() const override {
|
||||
return true;
|
||||
}
|
||||
|
||||
void handle_bi_msg(float voltage, float current, float temperature) override;
|
||||
static void subscribe_msgs(AP_UAVCAN* ap_uavcan);
|
||||
static AP_BattMonitor_UAVCAN* get_uavcan_backend(AP_UAVCAN* ap_uavcan, uint8_t node_id);
|
||||
static void handle_battery_info_trampoline(AP_UAVCAN* ap_uavcan, uint8_t node_id, const BattInfoCb &cb);
|
||||
|
||||
protected:
|
||||
private:
|
||||
void handle_battery_info(const BattInfoCb &cb);
|
||||
|
||||
AP_BattMonitor::BattMonitor_State _interim_state;
|
||||
BattMonitor_UAVCAN_Type _type;
|
||||
|
||||
HAL_Semaphore _sem_battmon;
|
||||
|
||||
AP_UAVCAN* _ap_uavcan;
|
||||
uint8_t _node_id;
|
||||
};
|
||||
|
Loading…
Reference in New Issue
Block a user