Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_BattMonitor: refector to use AP_ESC_Telem 

rename AP_BattMonitor_BLHeliESC -> AP_BattMonitor_ESC
record volts, amps and consumption as floats
Correct ESC-telemetry-based voltage and temperature (<amilcar.lucas@iav.de>)
Correct ESC-telemetry-based voltage and temperature when less than 12 ESCs are used (<amilcar.lucas@iav.de>)
fix jumps in consumed current (<amilcar.lucas@iav.de>)
Implement temperature readings (<amilcar.lucas@iav.de>)
Fix temperature scaling (<amilcar.lucas@iav.de>)
This commit is contained in:
Andy Piper 2021-03-01 20:38:54 +00:00 committed by Andrew Tridgell
parent f1078d00a3
commit 71e7f7e476
5 changed files with 112 additions and 98 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_BattMonitor/AP_BattMonitor.cpp
View File

@ -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:

90
libraries/AP_BattMonitor/AP_BattMonitor_BLHeliESC.cpp
View File

@ -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 <http://www.gnu.org/licenses/>.
*/
#include <AP_HAL/AP_HAL.h>
#include <AP_BLHeli/AP_BLHeli.h>
#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; i<AP_BLHELI_MAX_ESCS; i++) {
AP_BLHeli::telem_data td;
if (!blheli->get_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

95
libraries/AP_BattMonitor/AP_BattMonitor_ESC.cpp Normal file
View File

@ -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 <http://www.gnu.org/licenses/>.
*/
#include <AP_HAL/AP_HAL.h>
#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<ESC_TELEM_MAX_ESCS; i++) {
int16_t temperature_cdeg;
float voltage;
float current;
float consumption_mah;
if (telem.get_consumption_mah(i, consumption_mah)) {
// accumulate consumed_sum regardless of age, to cope with ESC
// dropping out
_state.consumed_mah += consumption_mah;
}
if (telem.get_voltage(i, voltage)) {
voltage_sum += voltage;
voltage_escs++;
}
if (telem.get_current(i, current)) {
current_sum += current;
}
if (telem.get_temperature(i, temperature_cdeg)) {
temperature_sum += float(temperature_cdeg) * 0.01f;
temperature_escs++;
}
if (telem.get_last_telem_data_ms(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

17
libraries/AP_BattMonitor/AP_BattMonitor_BLHeliESC.h → libraries/AP_BattMonitor/AP_BattMonitor_ESC.h
View File

@ -16,17 +16,20 @@
#include <AP_Common/AP_Common.h>
#include <AP_HAL/AP_HAL.h>
#include <AP_ESC_Telem/AP_ESC_Telem.h>
#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 &params):
AP_BattMonitor_ESC(AP_BattMonitor &mon, AP_BattMonitor::BattMonitor_State &mon_state, AP_BattMonitor_Params &params):
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

2
libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp
View File

@ -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),