diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.cpp b/libraries/AP_BattMonitor/AP_BattMonitor.cpp index 8ff98f20fd..d2d5c81070 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor.cpp @@ -2,6 +2,9 @@ #include "AP_BattMonitor_Analog.h" #include "AP_BattMonitor_SMBus.h" #include "AP_BattMonitor_Bebop.h" +#if HAL_WITH_UAVCAN +#include "AP_BattMonitor_UAVCAN.h" +#endif #include #include @@ -20,7 +23,6 @@ const AP_Param::GroupInfo AP_BattMonitor::var_info[] = { // @Path: AP_BattMonitor_Params.cpp AP_SUBGROUPINFO(_params[1], "2_", 24, AP_BattMonitor, AP_BattMonitor_Params), - AP_GROUPEND }; @@ -83,6 +85,12 @@ AP_BattMonitor::init() #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP || CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO drivers[instance] = new AP_BattMonitor_Bebop(*this, state[instance], _params[instance]); _num_instances++; +#endif + break; + case AP_BattMonitor_Params::BattMonitor_TYPE_UAVCAN_BatteryInfo: +#if HAL_WITH_UAVCAN + drivers[instance] = new AP_BattMonitor_UAVCAN(*this, state[instance], AP_BattMonitor_UAVCAN::UAVCAN_BATTERY_INFO, _params[instance]); + _num_instances++; #endif break; case AP_BattMonitor_Params::BattMonitor_TYPE_NONE: diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.h b/libraries/AP_BattMonitor/AP_BattMonitor.h index 3d0ac082af..3a7a5d159c 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor.h @@ -25,6 +25,7 @@ class AP_BattMonitor_Analog; class AP_BattMonitor_SMBus; class AP_BattMonitor_SMBus_Solo; class AP_BattMonitor_SMBus_Maxell; +class AP_BattMonitor_UAVCAN; class AP_BattMonitor { @@ -33,6 +34,7 @@ class AP_BattMonitor friend class AP_BattMonitor_SMBus; friend class AP_BattMonitor_SMBus_Solo; friend class AP_BattMonitor_SMBus_Maxell; + friend class AP_BattMonitor_UAVCAN; public: AP_BattMonitor(uint32_t log_battery_bit); diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.h b/libraries/AP_BattMonitor/AP_BattMonitor_Backend.h index e6549c394f..5f69712aad 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Backend.h @@ -49,6 +49,10 @@ 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_Params.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp index 37060b1088..26d73311cb 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp @@ -7,7 +7,7 @@ const AP_Param::GroupInfo AP_BattMonitor_Params::var_info[] = { // @Param: MONITOR // @DisplayName: Battery monitoring // @Description: Controls enabling monitoring of the battery's voltage and current - // @Values: 0:Disabled,3:Analog Voltage Only,4:Analog Voltage and Current,5:Solo,6:Bebop,7:SMBus-Maxell + // @Values: 0:Disabled,3:Analog Voltage Only,4:Analog Voltage and Current,5:Solo,6:Bebop,7:SMBus-Maxell,8:UAVCAN-BatteryInfo // @User: Standard AP_GROUPINFO_FLAGS("MONITOR", 1, AP_BattMonitor_Params, _type, BattMonitor_TYPE_NONE, AP_PARAM_FLAG_ENABLE), @@ -63,7 +63,7 @@ const AP_Param::GroupInfo AP_BattMonitor_Params::var_info[] = { // @Param: SERIAL_NUM // @DisplayName: Battery serial number - // @Description: Battery serial number, automatically filled in for SMBus batteries, otherwise will be -1 + // @Description: Battery serial number, automatically filled in for SMBus batteries, otherwise will be -1. With UAVCAN it is the battery_id. // @User: Advanced AP_GROUPINFO("SERIAL_NUM", 9, AP_BattMonitor_Params, _serial_number, AP_BATT_SERIAL_NUMBER_DEFAULT), diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Params.h b/libraries/AP_BattMonitor/AP_BattMonitor_Params.h index 260327cc07..db194af218 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Params.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Params.h @@ -19,7 +19,8 @@ public: BattMonitor_TYPE_ANALOG_VOLTAGE_AND_CURRENT = 4, BattMonitor_TYPE_SOLO = 5, BattMonitor_TYPE_BEBOP = 6, - BattMonitor_TYPE_MAXELL = 7 + BattMonitor_TYPE_MAXELL = 7, + BattMonitor_TYPE_UAVCAN_BatteryInfo = 8 }; // low voltage sources (used for BATT_LOW_TYPE parameter) diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.cpp new file mode 100644 index 0000000000..e1d7a140d0 --- /dev/null +++ b/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.cpp @@ -0,0 +1,80 @@ +#include + +#if HAL_WITH_UAVCAN + +#include +#include +#include "AP_BattMonitor.h" +#include "AP_BattMonitor_UAVCAN.h" + +#include +#include + +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 ¶ms) : + 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 diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.h b/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.h new file mode 100644 index 0000000000..7b63ee7a1c --- /dev/null +++ b/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.h @@ -0,0 +1,33 @@ +#pragma once + +#include +#include "AP_BattMonitor.h" +#include "AP_BattMonitor_Backend.h" + +#define AP_BATTMONITOR_UAVCAN_TIMEOUT_MICROS 5000000 // sensor becomes unhealthy if no successful readings for 5 seconds + +class AP_BattMonitor_UAVCAN : public AP_BattMonitor_Backend +{ +public: + + enum BattMonitor_UAVCAN_Type { + UAVCAN_BATTERY_INFO = 0 + }; + + /// Constructor + AP_BattMonitor_UAVCAN(AP_BattMonitor &mon, AP_BattMonitor::BattMonitor_State &mon_state, BattMonitor_UAVCAN_Type type, AP_BattMonitor_Params ¶ms); + + /// 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; + +protected: + BattMonitor_UAVCAN_Type _type; +};