diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.cpp b/libraries/AP_BattMonitor/AP_BattMonitor.cpp index ee2334426d..4cf1d808e3 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor.cpp @@ -6,7 +6,7 @@ #include "AP_BattMonitor_SMBus_Maxell.h" #include "AP_BattMonitor_SMBus_Rotoye.h" #include "AP_BattMonitor_Bebop.h" -#include "AP_BattMonitor_BLHeliESC.h" +#include "AP_BattMonitor_ESC.h" #include "AP_BattMonitor_SMBus_SUI.h" #include "AP_BattMonitor_SMBus_NeoDesign.h" #include "AP_BattMonitor_Sum.h" @@ -167,8 +167,8 @@ AP_BattMonitor::init() #endif break; case Type::BLHeliESC: -#ifdef HAVE_AP_BLHELI_SUPPORT - drivers[instance] = new AP_BattMonitor_BLHeliESC(*this, state[instance], _params[instance]); +#if HAL_WITH_ESC_TELEM && !defined(HAL_BUILD_AP_PERIPH) + drivers[instance] = new AP_BattMonitor_ESC(*this, state[instance], _params[instance]); #endif break; case Type::Sum: diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_BLHeliESC.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_BLHeliESC.cpp deleted file mode 100644 index 770ded6dc8..0000000000 --- a/libraries/AP_BattMonitor/AP_BattMonitor_BLHeliESC.cpp +++ /dev/null @@ -1,90 +0,0 @@ -/* - This program is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with this program. If not, see . - */ - - -#include -#include - -#ifdef HAVE_AP_BLHELI_SUPPORT - -#include "AP_BattMonitor_BLHeliESC.h" - -extern const AP_HAL::HAL &hal; - -void AP_BattMonitor_BLHeliESC::init(void) -{ -} - -void AP_BattMonitor_BLHeliESC::read(void) -{ - AP_BLHeli *blheli = AP_BLHeli::get_singleton(); - if (!blheli) { - return; - } - - uint8_t num_escs = 0; - float voltage_sum = 0; - float current_sum = 0; - float consumed_sum = 0; - float temperature_sum = 0; - uint32_t now = AP_HAL::millis(); - uint32_t highest_ms = 0; - - for (uint8_t i=0; iget_telem_data(i, td)) { - continue; - } - - // accumulate consumed_sum regardless of age, to cope with ESC - // dropping out - consumed_sum += td.consumption; - - if (now - td.timestamp_ms > 1000) { - // don't use old data - continue; - } - - num_escs++; - voltage_sum += td.voltage; - current_sum += td.current; - temperature_sum += td.temperature; - if (td.timestamp_ms > highest_ms) { - highest_ms = td.timestamp_ms; - } - } - - if (num_escs > 0) { - _state.voltage = (voltage_sum / num_escs) * 0.01; - _state.temperature = temperature_sum / num_escs; - _state.healthy = true; - } else { - _state.voltage = 0; - _state.temperature = 0; - _state.healthy = false; - } - _state.current_amps = current_sum * 0.01; - _state.consumed_mah = consumed_sum; - _state.last_time_micros = highest_ms * 1000; - _state.temperature_time = highest_ms; - - if (current_sum > 0) { - // if we have ever got a current value then we know we have a - // current sensor - have_current = true; - } -} - -#endif // HAVE_AP_BLHELI_SUPPORT diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_ESC.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_ESC.cpp new file mode 100644 index 0000000000..25c70ad3b3 --- /dev/null +++ b/libraries/AP_BattMonitor/AP_BattMonitor_ESC.cpp @@ -0,0 +1,95 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + + +#include +#include "AP_BattMonitor_ESC.h" + +#if HAL_WITH_ESC_TELEM + +extern const AP_HAL::HAL &hal; + +void AP_BattMonitor_ESC::init(void) +{ +} + +void AP_BattMonitor_ESC::read(void) +{ + AP_ESC_Telem& telem = AP::esc_telem(); + + uint8_t voltage_escs = 0; // number of ESCs with valid voltage + uint8_t temperature_escs = 0; // number of ESCs with valid temperature + float voltage_sum = 0; + float current_sum = 0; + float temperature_sum = 0; + uint32_t highest_ms = 0; + + for (uint8_t i=0; i highest_ms) { + highest_ms = telem.get_last_telem_data_ms(i); + } + } + + if (voltage_escs > 0) { + _state.voltage = voltage_sum / voltage_escs; + _state.healthy = true; + } else { + _state.voltage = 0; + _state.healthy = false; + } + if (temperature_escs > 0) { + _state.temperature = temperature_sum / temperature_escs; + have_temperature = true; + } else { + _state.temperature = 0; + } + + _state.current_amps = current_sum; + _state.last_time_micros = highest_ms * 1000; + _state.temperature_time = highest_ms; + + if (current_sum > 0) { + // if we have ever got a current value then we know we have a + // current sensor + have_current = true; + } +} + +#endif // HAL_WITH_ESC_TELEM diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_BLHeliESC.h b/libraries/AP_BattMonitor/AP_BattMonitor_ESC.h similarity index 70% rename from libraries/AP_BattMonitor/AP_BattMonitor_BLHeliESC.h rename to libraries/AP_BattMonitor/AP_BattMonitor_ESC.h index ebe55f7d24..5cb4800a9f 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_BLHeliESC.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_ESC.h @@ -16,17 +16,20 @@ #include #include +#include #include "AP_BattMonitor_Backend.h" -class AP_BattMonitor_BLHeliESC :public AP_BattMonitor_Backend +#if HAL_WITH_ESC_TELEM + +class AP_BattMonitor_ESC :public AP_BattMonitor_Backend { public: // constructor. This incorporates initialisation as well. - AP_BattMonitor_BLHeliESC(AP_BattMonitor &mon, AP_BattMonitor::BattMonitor_State &mon_state, AP_BattMonitor_Params ¶ms): + AP_BattMonitor_ESC(AP_BattMonitor &mon, AP_BattMonitor::BattMonitor_State &mon_state, AP_BattMonitor_Params ¶ms): AP_BattMonitor_Backend(mon, mon_state, params) {}; - virtual ~AP_BattMonitor_BLHeliESC(void) {}; + virtual ~AP_BattMonitor_ESC(void) {}; // initialise void init() override; @@ -34,9 +37,15 @@ public: // read the latest battery voltage void read() override; - // BLHeliESC provides current info + // ESC_Telem provides current info bool has_current() const override { return have_current; }; + // ESC_Telem provides temperature info + bool has_temperature() const override { return have_temperature; }; + private: bool have_current; + bool have_temperature; }; + +#endif diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp index 553a0ad57b..f1a09f1aae 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-Generic,8:UAVCAN-BatteryInfo,9:BLHeli ESC,10:SumOfFollowing,11:FuelFlow,12:FuelLevelPWM,13:SMBUS-SUI3,14:SMBUS-SUI6,15:NeoDesign,16:SMBus-Maxell,17:Generator-Elec,18:Generator-Fuel,19:Rotoye + // @Values: 0:Disabled,3:Analog Voltage Only,4:Analog Voltage and Current,5:Solo,6:Bebop,7:SMBus-Generic,8:UAVCAN-BatteryInfo,9:ESC,10:SumOfFollowing,11:FuelFlow,12:FuelLevelPWM,13:SMBUS-SUI3,14:SMBUS-SUI6,15:NeoDesign,16:SMBus-Maxell,17:Generator-Elec,18:Generator-Fuel,19:Rotoye // @User: Standard // @RebootRequired: True AP_GROUPINFO_FLAGS("MONITOR", 1, AP_BattMonitor_Params, _type, int8_t(AP_BattMonitor::Type::NONE), AP_PARAM_FLAG_ENABLE),