diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.cpp b/libraries/AP_BattMonitor/AP_BattMonitor.cpp index 7f3204ab1e..c24391f51e 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor.cpp @@ -188,9 +188,14 @@ AP_BattMonitor::init() hal.i2c_mgr->get_device(_params[instance]._i2c_bus, AP_BATTMONITOR_SMBUS_I2C_ADDR, 100000, true, 20)); break; - case Type::Generator: - drivers[instance] = new AP_BattMonitor_Generator(*this, state[instance], _params[instance]); +#if GENERATOR_ENABLED + case Type::GENERATOR_ELEC: + drivers[instance] = new AP_BattMonitor_Generator_Elec(*this, state[instance], _params[instance]); break; + case Type::GENERATOR_FUEL: + drivers[instance] = new AP_BattMonitor_Generator_FuelLevel(*this, state[instance], _params[instance]); + break; +#endif // GENERATOR_ENABLED case Type::NONE: default: break; diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.h b/libraries/AP_BattMonitor/AP_BattMonitor.h index 1e5661065f..3a067d1061 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor.h @@ -81,7 +81,8 @@ public: SUI6 = 14, NeoDesign = 15, MAXELL = 16, - Generator = 17, + GENERATOR_ELEC = 17, + GENERATOR_FUEL = 18, Rotoye = 19, }; diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.h b/libraries/AP_BattMonitor/AP_BattMonitor_Backend.h index 818fbd1ecf..facff1ccbf 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Backend.h @@ -57,7 +57,7 @@ public: void update_resistance_estimate(); // updates failsafe timers, and returns what failsafes are active - AP_BattMonitor::BatteryFailsafe update_failsafes(void); + virtual AP_BattMonitor::BatteryFailsafe update_failsafes(void); // returns false if we fail arming checks, in which case the buffer will be populated with a failure message bool arming_checks(char * buffer, size_t buflen) const; diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Generator.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Generator.cpp index 2acb231759..63997ec247 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Generator.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Generator.cpp @@ -1,38 +1,159 @@ -#include +/* + 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_Generator.h" + +#if GENERATOR_ENABLED + #include #include -#include "AP_BattMonitor.h" -#include "AP_BattMonitor_Generator.h" extern const AP_HAL::HAL& hal; -// read - read the voltage and current -void AP_BattMonitor_Generator::read() +/* + Fuel class +*/ +// This is where we tell the battery monitor 'we have current' if we want to report a fuel level remaining +bool AP_BattMonitor_Generator_FuelLevel::has_current(void) const +{ + // If the generator has fuel remaining we must also state that we have current + return has_consumed_energy(); +} + +// This is where we tell the battery monitor 'we have consummed energy' if we want to report a fuel level remaining +bool AP_BattMonitor_Generator_FuelLevel::has_consumed_energy(void) const +{ + // Get pointer to generator singleton + AP_Generator *generator = AP::generator(); + + if (generator == nullptr) { + return false; + } + + // Use consumed_mAh in BattMonitor to display fuel remaining + return generator->has_fuel_remaining(); +} + +void AP_BattMonitor_Generator_FuelLevel::init() +{ + // Set params for users: + // Fuel level is only reported as a percentage + _params._pack_capacity.set(100); + // Fuel only reports a fixed 1v, don't want batt monitor failsafes on this instance + _params._low_voltage.set(0); + _params._critical_voltage.set(0); +} + +// Read the fuel level. Should be called at 10hz +void AP_BattMonitor_Generator_FuelLevel::read() { _state.healthy = false; -#if GENERATOR_ENABLED - AP_Generator_RichenPower *generator = AP::generator(); + // Get pointer to generator singleton + AP_Generator *generator = AP::generator(); - // healthy if we can find a generator + // Not healthy if we can't find a generator if (generator == nullptr) { return; } - // get voltage - if (!generator->voltage(_state.voltage)) { - return; - } - - // get current - if (!generator->current(_state.current_amps)) { - return; - } - if (!generator->healthy()) { return; } + // As this is a battery monitor instance report voltage + // Report fixed voltage of 1V + _state.voltage = 1.0f; + + // This is a bodge to display tank level as a percentage on GCS. Users should set _params.pack_capacity == 100 to get a clear percentage in GCS + _state.consumed_mah = (1 - generator->get_fuel_remain()) * _params._pack_capacity.get(); + + // If we got this far then must be healthy _state.healthy = true; -#endif + _state.last_time_micros = AP_HAL::micros(); } + +/* + Electrical class +*/ +bool AP_BattMonitor_Generator_Elec::has_current(void) const +{ + // Get pointer to generator singleton + AP_Generator *generator = AP::generator(); + + if (generator == nullptr) { + return false; + } + + return generator->has_current(); +} + +bool AP_BattMonitor_Generator_Elec::has_consumed_energy(void) const +{ + // Get pointer to generator singleton + AP_Generator *generator = AP::generator(); + + if (generator == nullptr) { + return false; + } + + return generator->has_consumed_energy(); +} + +// Read the electrical measurements from the generator +void AP_BattMonitor_Generator_Elec::read() +{ + _state.healthy = false; + + // Get pointer to generator singleton + AP_Generator *generator = AP::generator(); + + // Not healthy if we can't find a generator + if (generator == nullptr) { + return; + } + + if (!generator->healthy()) { + return; + } + + // Update readings + _state.voltage = generator->get_voltage(); + + _state.current_amps = generator->get_current(); + + // Always reset consumed value, integration is done in AP_Generator library + _state.consumed_mah = generator->get_batt_consumed(); + _state.consumed_wh = 0.001f * _state.consumed_mah * _state.voltage; + + // If we got this far then must be healthy + _state.healthy = true; + _state.last_time_micros = AP_HAL::micros(); +} + +AP_BattMonitor::BatteryFailsafe AP_BattMonitor_Generator_Elec::update_failsafes() +{ + AP_BattMonitor::BatteryFailsafe failsafe = AP_BattMonitor::BatteryFailsafe::BatteryFailsafe_None; + + AP_Generator *generator = AP::generator(); + + // Only check for failsafes on the electrical moniter + // no point in having the same failsafe on two battery monitors + if (generator != nullptr) { + failsafe = generator->update_failsafes(); + } + return MAX(AP_BattMonitor_Backend::update_failsafes(), failsafe); +} +#endif diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Generator.h b/libraries/AP_BattMonitor/AP_BattMonitor_Generator.h index 989dc727c7..8d8dfafccb 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Generator.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Generator.h @@ -1,22 +1,50 @@ #pragma once +#include + +#if GENERATOR_ENABLED + #include "AP_BattMonitor.h" #include "AP_BattMonitor_Backend.h" -#include - -class AP_BattMonitor_Generator : public AP_BattMonitor_Backend +// Sub class for generator electrical +class AP_BattMonitor_Generator_Elec : public AP_BattMonitor_Backend { public: + // Inherit constructor using AP_BattMonitor_Backend::AP_BattMonitor_Backend; - /// Read the battery voltage and current. Should be called at 10hz - void read() override; + void init(void) override {}; - /// returns true if battery monitor provides current info - bool has_current() const override { return true; } + // Read the battery voltage and current + void read(void) override; - void init(void) override {} + bool has_current(void) const override; + bool has_consumed_energy(void) const override; + + // Override backend update_failsafes. No point in failsafing twice so generator failsafes are only updated from the electrical instance of the generator drivers + AP_BattMonitor::BatteryFailsafe update_failsafes() override; }; + +// Sub class for generator fuel +class AP_BattMonitor_Generator_FuelLevel : public AP_BattMonitor_Backend +{ +public: + + // Inherit constructor + using AP_BattMonitor_Backend::AP_BattMonitor_Backend; + + void init(void) override; + + // Read the fuel level + void read(void) override; + + // This is where we tell the battery monitor 'we have current' if we want to report a fuel level remaining + bool has_current(void) const override; + + // This is where we tell the battery monitor 'we have consummed energy' if we want to report a fuel level remaining + bool has_consumed_energy(void) const override; +}; +#endif diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp index 930f11119b..9713840059 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,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: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 // @User: Standard // @RebootRequired: True AP_GROUPINFO_FLAGS("MONITOR", 1, AP_BattMonitor_Params, _type, int8_t(AP_BattMonitor::Type::NONE), AP_PARAM_FLAG_ENABLE),