From 080d7615fc742fd3fd303cdca1440467196103e3 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 23 Jan 2023 15:39:59 +1100 Subject: [PATCH] AP_BattMonitor: support EFI "battery" backend this maps Ah to Litres and Amps to Litres/hour --- libraries/AP_BattMonitor/AP_BattMonitor.cpp | 6 ++ libraries/AP_BattMonitor/AP_BattMonitor.h | 3 + .../AP_BattMonitor/AP_BattMonitor_EFI.cpp | 55 +++++++++++++++++++ libraries/AP_BattMonitor/AP_BattMonitor_EFI.h | 29 ++++++++++ .../AP_BattMonitor/AP_BattMonitor_Params.cpp | 2 +- 5 files changed, 94 insertions(+), 1 deletion(-) create mode 100644 libraries/AP_BattMonitor/AP_BattMonitor_EFI.cpp create mode 100644 libraries/AP_BattMonitor/AP_BattMonitor_EFI.h diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.cpp b/libraries/AP_BattMonitor/AP_BattMonitor.cpp index 43558022b4..ad2d07a076 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor.cpp @@ -13,6 +13,7 @@ #include "AP_BattMonitor_FuelFlow.h" #include "AP_BattMonitor_FuelLevel_PWM.h" #include "AP_BattMonitor_Generator.h" +#include "AP_BattMonitor_EFI.h" #include "AP_BattMonitor_INA2xx.h" #include "AP_BattMonitor_INA239.h" #include "AP_BattMonitor_LTC2946.h" @@ -356,6 +357,11 @@ AP_BattMonitor::init() drivers[instance] = new AP_BattMonitor_INA239(*this, state[instance], _params[instance]); break; #endif +#if HAL_EFI_ENABLED + case Type::EFI: + drivers[instance] = new AP_BattMonitor_EFI(*this, state[instance], _params[instance]); + break; +#endif // HAL_EFI_ENABLED case Type::NONE: default: break; diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.h b/libraries/AP_BattMonitor/AP_BattMonitor.h index 2257cff7cc..5cbe509812 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor.h @@ -44,6 +44,7 @@ class AP_BattMonitor_INA239; class AP_BattMonitor_LTC2946; class AP_BattMonitor_Torqeedo; class AP_BattMonitor_FuelLevel_Analog; +class AP_BattMonitor_EFI; class AP_BattMonitor @@ -60,6 +61,7 @@ class AP_BattMonitor friend class AP_BattMonitor_FuelFlow; friend class AP_BattMonitor_FuelLevel_PWM; friend class AP_BattMonitor_Generator; + friend class AP_BattMonitor_EFI; friend class AP_BattMonitor_INA2XX; friend class AP_BattMonitor_INA239; friend class AP_BattMonitor_LTC2946; @@ -104,6 +106,7 @@ public: FuelLevel_Analog = 24, Analog_Volt_Synthetic_Current = 25, INA239_SPI = 26, + EFI = 27, }; FUNCTOR_TYPEDEF(battery_failsafe_handler_fn_t, void, const char *, const int8_t); diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_EFI.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_EFI.cpp new file mode 100644 index 0000000000..23d37e8832 --- /dev/null +++ b/libraries/AP_BattMonitor/AP_BattMonitor_EFI.cpp @@ -0,0 +1,55 @@ +/* + 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 "AP_BattMonitor_EFI.h" + +#if HAL_EFI_ENABLED + +#include +#include +#include + +extern const AP_HAL::HAL& hal; + +// update state +void AP_BattMonitor_EFI::read() +{ + AP_EFI *efi = AP::EFI(); + if (efi == nullptr) { + return; + } + + if (!efi->is_healthy()) { + _state.healthy = false; + return; + } + + struct EFI_State efi_state; + /* + a simple mapping of 1 Amp == 1 litre/hour and 1Ah = 1Litre + */ + efi->get_state(efi_state); + + _state.current_amps = efi_state.fuel_consumption_rate_cm3pm*0.001*60; // litres/hour + // use arbitrary 1.0 Volts + _state.voltage = 1.0; + _state.consumed_mah = efi_state.estimated_consumed_fuel_volume_cm3; // millilitres + _state.consumed_wh = 0.001 * _state.consumed_mah; + _state.healthy = true; + _state.last_time_micros = AP_HAL::micros(); +} + +#endif // HAL_EFI_ENABLED + diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_EFI.h b/libraries/AP_BattMonitor/AP_BattMonitor_EFI.h new file mode 100644 index 0000000000..1ff0637bbe --- /dev/null +++ b/libraries/AP_BattMonitor/AP_BattMonitor_EFI.h @@ -0,0 +1,29 @@ +#pragma once + +#include + +#if HAL_EFI_ENABLED + +#include "AP_BattMonitor.h" +#include "AP_BattMonitor_Backend.h" + +class AP_BattMonitor_EFI : public AP_BattMonitor_Backend +{ +public: + + // Inherit constructor + using AP_BattMonitor_Backend::AP_BattMonitor_Backend; + + // update state + void read(void) override; + + bool has_current(void) const override { + return true; + } + + bool has_consumed_energy(void) const override { + return true; + } +}; +#endif // HAL_EFI_ENABLED + diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp index 4edf2ee4fc..60c9a8ac67 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:DroneCAN-BatteryInfo,9:ESC,10:Sum Of Selected Monitors,11:FuelFlow,12:FuelLevelPWM,13:SMBUS-SUI3,14:SMBUS-SUI6,15:NeoDesign,16:SMBus-Maxell,17:Generator-Elec,18:Generator-Fuel,19:Rotoye,20:MPPT,21:INA2XX,22:LTC2946,23:Torqeedo,24:FuelLevelAnalog,25:Synthetic Current and Analog Voltage + // @Values: 0:Disabled,3:Analog Voltage Only,4:Analog Voltage and Current,5:Solo,6:Bebop,7:SMBus-Generic,8:DroneCAN-BatteryInfo,9:ESC,10:Sum Of Selected Monitors,11:FuelFlow,12:FuelLevelPWM,13:SMBUS-SUI3,14:SMBUS-SUI6,15:NeoDesign,16:SMBus-Maxell,17:Generator-Elec,18:Generator-Fuel,19:Rotoye,20:MPPT,21:INA2XX,22:LTC2946,23:Torqeedo,24:FuelLevelAnalog,25:Synthetic Current and Analog Voltage,26:INA239_SPI,27:EFI // @User: Standard // @RebootRequired: True AP_GROUPINFO_FLAGS("MONITOR", 1, AP_BattMonitor_Params, _type, int8_t(AP_BattMonitor::Type::NONE), AP_PARAM_FLAG_ENABLE),