From 6e85003b56e124c01c0bac258178093bdef0bd3d Mon Sep 17 00:00:00 2001 From: Siddharth Purohit Date: Fri, 20 Jul 2018 18:31:13 +0530 Subject: [PATCH] AP_BattMonitor: move BattInfo message subscription and handling to AP_BattMonitor --- libraries/AP_BattMonitor/AP_BattMonitor.cpp | 4 + .../AP_BattMonitor/AP_BattMonitor_Backend.h | 4 - .../AP_BattMonitor/AP_BattMonitor_UAVCAN.cpp | 137 +++++++++++++----- .../AP_BattMonitor/AP_BattMonitor_UAVCAN.h | 24 ++- 4 files changed, 119 insertions(+), 50 deletions(-) diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.cpp b/libraries/AP_BattMonitor/AP_BattMonitor.cpp index 99bdc97e6f..e03821aad2 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor.cpp @@ -3,9 +3,13 @@ #include "AP_BattMonitor_SMBus.h" #include "AP_BattMonitor_Bebop.h" #include "AP_BattMonitor_BLHeliESC.h" + +#include + #if HAL_WITH_UAVCAN #include "AP_BattMonitor_UAVCAN.h" #endif + #include #include #include diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.h b/libraries/AP_BattMonitor/AP_BattMonitor_Backend.h index 5f69712aad..e6549c394f 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Backend.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) diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.cpp index 8acd82c89a..b4ba7d2a6d 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.cpp @@ -2,17 +2,22 @@ #if HAL_WITH_UAVCAN -#include -#include #include "AP_BattMonitor.h" #include "AP_BattMonitor_UAVCAN.h" #include +#include +#include +#include +#include + +#include 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 *battinfo_listener; + battinfo_listener = new uavcan::Subscriber(*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 diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.h b/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.h index 7b63ee7a1c..0f70a4acda 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.h @@ -1,15 +1,17 @@ #pragma once -#include #include "AP_BattMonitor.h" #include "AP_BattMonitor_Backend.h" +#include + #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; };