diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.cpp b/libraries/AP_BattMonitor/AP_BattMonitor.cpp index 80d471c1a5..6a5f93193a 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor.cpp @@ -3,6 +3,7 @@ #include "AP_BattMonitor_SMBus.h" #include "AP_BattMonitor_Bebop.h" #include "AP_BattMonitor_BLHeliESC.h" +#include "AP_BattMonitor_Sum.h" #include @@ -133,6 +134,9 @@ AP_BattMonitor::init() drivers[instance] = new AP_BattMonitor_BLHeliESC(*this, state[instance], _params[instance]); #endif break; + case AP_BattMonitor_Params::BattMonitor_TYPE_Sum: + drivers[instance] = new AP_BattMonitor_Sum(*this, state[instance], _params[instance], instance); + break; case AP_BattMonitor_Params::BattMonitor_TYPE_NONE: default: break; diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.h b/libraries/AP_BattMonitor/AP_BattMonitor.h index d962e18ee6..78716b08eb 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor.h @@ -35,6 +35,7 @@ class AP_BattMonitor friend class AP_BattMonitor_SMBus_Solo; friend class AP_BattMonitor_SMBus_Maxell; friend class AP_BattMonitor_UAVCAN; + friend class AP_BattMonitor_Sum; public: diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp index 997cfe1cfd..7babdd7b0f 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp @@ -13,7 +13,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,8:UAVCAN-BatteryInfo,9:BLHeli ESC + // @Values: 0:Disabled,3:Analog Voltage Only,4:Analog Voltage and Current,5:Solo,6:Bebop,7:SMBus-Maxell,8:UAVCAN-BatteryInfo,9:BLHeli ESC,10:SumOfFollowing // @User: Standard // @RebootRequired: True AP_GROUPINFO_FLAGS("MONITOR", 1, AP_BattMonitor_Params, _type, BattMonitor_TYPE_NONE, AP_PARAM_FLAG_ENABLE), diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Params.h b/libraries/AP_BattMonitor/AP_BattMonitor_Params.h index f4e1c3b18a..6ba4e02483 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Params.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Params.h @@ -21,7 +21,8 @@ public: BattMonitor_TYPE_BEBOP = 6, BattMonitor_TYPE_MAXELL = 7, BattMonitor_TYPE_UAVCAN_BatteryInfo = 8, - BattMonitor_TYPE_BLHeliESC = 9 + BattMonitor_TYPE_BLHeliESC = 9, + BattMonitor_TYPE_Sum = 10, }; // low voltage sources (used for BATT_LOW_TYPE parameter) diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Sum.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Sum.cpp new file mode 100644 index 0000000000..e22478da20 --- /dev/null +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Sum.cpp @@ -0,0 +1,54 @@ +#include +#include +#include +#include "AP_BattMonitor.h" +#include "AP_BattMonitor_Sum.h" + +/* + battery monitor that is the sum of other battery monitors after this one + + This can be used to combined other current/voltage sensors into a + single backend + */ +extern const AP_HAL::HAL& hal; + +/// Constructor +AP_BattMonitor_Sum::AP_BattMonitor_Sum(AP_BattMonitor &mon, + AP_BattMonitor::BattMonitor_State &mon_state, + AP_BattMonitor_Params ¶ms, + uint8_t instance) : + AP_BattMonitor_Backend(mon, mon_state, params), + _instance(instance) +{ +} + +// read - read the voltage and current +void +AP_BattMonitor_Sum::read() +{ + float voltage_sum = 0; + uint8_t voltage_count = 0; + float current_sum = 0; + uint8_t current_count = 0; + + for (uint8_t i=_instance+1; i<_mon.num_instances(); i++) { + if (!_mon.healthy(i)) { + continue; + } + voltage_sum += _mon.voltage(i); + voltage_count++; + if (_mon.has_current(i)) { + current_sum += _mon.current_amps(i); + current_count++; + } + } + if (voltage_count > 0) { + _state.voltage = voltage_sum / voltage_count; + _state.last_time_micros = AP_HAL::micros(); + } + if (current_count > 0) { + _state.current_amps = current_sum; + } + _has_current = (current_count > 0); + _state.healthy = (voltage_count > 0); +} diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Sum.h b/libraries/AP_BattMonitor/AP_BattMonitor_Sum.h new file mode 100644 index 0000000000..56ef33c9dc --- /dev/null +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Sum.h @@ -0,0 +1,27 @@ +#pragma once + +#include "AP_BattMonitor.h" +#include "AP_BattMonitor_Backend.h" + +class AP_BattMonitor_Sum : public AP_BattMonitor_Backend +{ +public: + + /// Constructor + AP_BattMonitor_Sum(AP_BattMonitor &mon, AP_BattMonitor::BattMonitor_State &mon_state, AP_BattMonitor_Params ¶ms, uint8_t instance); + + /// Read the battery voltage and current. Should be called at 10hz + void read() override; + + /// returns true if battery monitor provides consumed energy info + bool has_consumed_energy() const override { return has_current(); } + + /// returns true if battery monitor provides current info + bool has_current() const override { return _has_current; } + + void init(void) override {} + +private: + uint8_t _instance; + bool _has_current; +};