AP_BattMonitor: Add support for AP_Generator frontend-backend

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;

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,
     };
 

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;

libraries/AP_BattMonitor/AP_BattMonitor_Generator.cpp

@@ -1,38 +1,159 @@
-#include <AP_HAL/AP_HAL.h>
+/*
+   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_BattMonitor_Generator.h"
+
+#if GENERATOR_ENABLED
+
 #include <AP_Common/AP_Common.h>
 #include <AP_Math/AP_Math.h>
-#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

libraries/AP_BattMonitor/AP_BattMonitor_Generator.h

@@ -1,22 +1,50 @@
 #pragma once
 
+#include <AP_Generator/AP_Generator.h>
+
+#if GENERATOR_ENABLED
+
 #include "AP_BattMonitor.h"
 #include "AP_BattMonitor_Backend.h"
 
-#include <AP_Generator/AP_Generator_RichenPower.h>
-
-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

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