AP_BattMonitor: Add Fuel Level Analog driver

libraries/AP_BattMonitor/AP_BattMonitor.cpp

@@ -16,6 +16,7 @@
 #include "AP_BattMonitor_INA2xx.h"
 #include "AP_BattMonitor_LTC2946.h"
 #include "AP_BattMonitor_Torqeedo.h"
+#include "AP_BattMonitor_FuelLevel_Analog.h"
 
 #include <AP_HAL/AP_HAL.h>
 
@@ -49,6 +50,8 @@ const AP_Param::GroupInfo AP_BattMonitor::var_info[] = {
     // @Path: AP_BattMonitor_Sum.cpp
     // @Group: _
     // @Path: AP_BattMonitor_UAVCAN.cpp
+    // @Group: _
+    // @Path: AP_BattMonitor_FuelLevel_Analog.cpp
     AP_SUBGROUPVARPTR(drivers[0], "_", 41, AP_BattMonitor, backend_var_info[0]),
 
 #if AP_BATT_MONITOR_MAX_INSTANCES > 1
@@ -64,6 +67,8 @@ const AP_Param::GroupInfo AP_BattMonitor::var_info[] = {
     // @Path: AP_BattMonitor_Sum.cpp
     // @Group: 2_
     // @Path: AP_BattMonitor_UAVCAN.cpp
+    // @Group: 2_
+    // @Path: AP_BattMonitor_FuelLevel_Analog.cpp
     AP_SUBGROUPVARPTR(drivers[1], "2_", 42, AP_BattMonitor, backend_var_info[1]),
 #endif
 
@@ -80,6 +85,8 @@ const AP_Param::GroupInfo AP_BattMonitor::var_info[] = {
     // @Path: AP_BattMonitor_Sum.cpp
     // @Group: 3_
     // @Path: AP_BattMonitor_UAVCAN.cpp
+    // @Group: 3_
+    // @Path: AP_BattMonitor_FuelLevel_Analog.cpp
     AP_SUBGROUPVARPTR(drivers[2], "3_", 43, AP_BattMonitor, backend_var_info[2]),
 #endif
 
@@ -96,6 +103,8 @@ const AP_Param::GroupInfo AP_BattMonitor::var_info[] = {
     // @Path: AP_BattMonitor_Sum.cpp
     // @Group: 4_
     // @Path: AP_BattMonitor_UAVCAN.cpp
+    // @Group: 4_
+    // @Path: AP_BattMonitor_FuelLevel_Analog.cpp
     AP_SUBGROUPVARPTR(drivers[3], "4_", 44, AP_BattMonitor, backend_var_info[3]),
 #endif
 
@@ -112,6 +121,8 @@ const AP_Param::GroupInfo AP_BattMonitor::var_info[] = {
     // @Path: AP_BattMonitor_Sum.cpp
     // @Group: 5_
     // @Path: AP_BattMonitor_UAVCAN.cpp
+    // @Group: 5_
+    // @Path: AP_BattMonitor_FuelLevel_Analog.cpp
     AP_SUBGROUPVARPTR(drivers[4], "5_", 45, AP_BattMonitor, backend_var_info[4]),
 #endif
 
@@ -128,6 +139,8 @@ const AP_Param::GroupInfo AP_BattMonitor::var_info[] = {
     // @Path: AP_BattMonitor_Sum.cpp
     // @Group: 6_
     // @Path: AP_BattMonitor_UAVCAN.cpp
+    // @Group: 6_
+    // @Path: AP_BattMonitor_FuelLevel_Analog.cpp
     AP_SUBGROUPVARPTR(drivers[5], "6_", 46, AP_BattMonitor, backend_var_info[5]),
 #endif
 
@@ -144,6 +157,8 @@ const AP_Param::GroupInfo AP_BattMonitor::var_info[] = {
     // @Path: AP_BattMonitor_Sum.cpp
     // @Group: 7_
     // @Path: AP_BattMonitor_UAVCAN.cpp
+    // @Group: 7_
+    // @Path: AP_BattMonitor_FuelLevel_Analog.cpp
     AP_SUBGROUPVARPTR(drivers[6], "7_", 47, AP_BattMonitor, backend_var_info[6]),
 #endif
 
@@ -160,6 +175,8 @@ const AP_Param::GroupInfo AP_BattMonitor::var_info[] = {
     // @Path: AP_BattMonitor_Sum.cpp
     // @Group: 8_
     // @Path: AP_BattMonitor_UAVCAN.cpp
+    // @Group: 8_
+    // @Path: AP_BattMonitor_FuelLevel_Analog.cpp
     AP_SUBGROUPVARPTR(drivers[7], "8_", 48, AP_BattMonitor, backend_var_info[7]),
 #endif
 
@@ -176,6 +193,8 @@ const AP_Param::GroupInfo AP_BattMonitor::var_info[] = {
     // @Path: AP_BattMonitor_Sum.cpp
     // @Group: 9_
     // @Path: AP_BattMonitor_UAVCAN.cpp
+    // @Group: 9_
+    // @Path: AP_BattMonitor_FuelLevel_Analog.cpp
     AP_SUBGROUPVARPTR(drivers[8], "9_", 49, AP_BattMonitor, backend_var_info[8]),
 #endif
 
@@ -274,6 +293,9 @@ AP_BattMonitor::init()
             case Type::FuelLevel_PWM:
                 drivers[instance] = new AP_BattMonitor_FuelLevel_PWM(*this, state[instance], _params[instance]);
                 break;
+            case Type::FuelLevel_Analog:
+                drivers[instance] = new AP_BattMonitor_FuelLevel_Analog(*this, state[instance], _params[instance]);
+                break;
 #endif // AP_BATTMON_FUEL_ENABLE
 #if HAL_GENERATOR_ENABLED
             case Type::GENERATOR_ELEC:

libraries/AP_BattMonitor/AP_BattMonitor.h

@@ -48,6 +48,7 @@ class AP_BattMonitor_Generator;
 class AP_BattMonitor_INA2XX;
 class AP_BattMonitor_LTC2946;
 class AP_BattMonitor_Torqeedo;
+class AP_BattMonitor_FuelLevel_Analog;
 
 class AP_BattMonitor
 {
@@ -67,6 +68,7 @@ class AP_BattMonitor
     friend class AP_BattMonitor_LTC2946;
 
     friend class AP_BattMonitor_Torqeedo;
+    friend class AP_BattMonitor_FuelLevel_Analog;
 
 public:
 
@@ -101,6 +103,7 @@ public:
         INA2XX                     = 21,
         LTC2946                    = 22,
         Torqeedo                   = 23,
+        FuelLevel_Analog           = 24,
     };
 
     FUNCTOR_TYPEDEF(battery_failsafe_handler_fn_t, void, const char *, const int8_t);

libraries/AP_BattMonitor/AP_BattMonitor_FuelLevel_Analog.cpp

@@ -0,0 +1,118 @@
+/*
+ * This file 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 file 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/>.
+ *
+ * Code by Charlie Johnson
+ */
+
+#include <AP_HAL/AP_HAL.h>
+#include <AP_Math/AP_Math.h>
+#include "AP_BattMonitor_FuelLevel_Analog.h"
+
+extern const AP_HAL::HAL& hal;
+
+const AP_Param::GroupInfo AP_BattMonitor_FuelLevel_Analog::var_info[] = {
+
+    // @Param: FL_VLT_MIN
+    // @DisplayName: Empty fuel level voltage
+    // @Description: The voltage seen on the analog pin when the fuel tank is empty. Note: For this type of battery monitor, the voltage seen by the analog pin is displayed as battery voltage on a GCS.
+    // @Range: 0.01 10
+    // @Units: V
+    // @User: Advanced
+    AP_GROUPINFO("FL_VLT_MIN", 40, AP_BattMonitor_FuelLevel_Analog, _fuel_level_empty_voltage, 0.5),
+
+    // @Param: FL_V_MULT
+    // @DisplayName: Fuel level voltage multiplier
+    // @Description: Voltage multiplier to determine what the full tank voltage reading is. This is calculated as 1 / (Voltage_Full - Voltage_Empty) Note: For this type of battery monitor, the voltage seen by the analog pin is displayed as battery voltage on a GCS.
+    // @Range: 0.01 10
+    // @User: Advanced
+    AP_GROUPINFO("FL_V_MULT", 41, AP_BattMonitor_FuelLevel_Analog, _fuel_level_voltage_mult, 0.5),
+
+    // @Param: FL_FLTR
+    // @DisplayName: Fuel level filter frequency
+    // @Description: Filter frequency in Hertz where a low pass filter is used. This is used to filter out tank slosh from the fuel level reading. A value of -1 disables the filter and unfiltered voltage is used to determine the fuel level. The suggested values at in the range of 0.2 Hz to 0.5 Hz.
+    // @Range: -1 1
+    // @User: Advanced
+    // @Units: Hz
+    // @RebootRequired: True
+    AP_GROUPINFO("FL_FLTR", 42, AP_BattMonitor_FuelLevel_Analog, _fuel_level_filter_frequency, 0.3),
+
+    // @Param: FL_PIN
+    // @DisplayName: Fuel level analog pin number
+    // @Description: Analog input pin that fuel level sensor is connected to. Airspeed ports can be used for Analog input. When using analog pin 103, the maximum value of the input in 3.3V.
+    // @Values: -1:Not Used,11:Pixracer,13:Pixhawk ADC4,14:Pixhawk ADC3,15:Pixhawk ADC6/Pixhawk2 ADC,103:Pixhawk SBUS
+    AP_GROUPINFO("FL_PIN", 43, AP_BattMonitor_FuelLevel_Analog, _pin, -1),
+
+    // Param indexes must be between 40 and 49 to avoid conflict with other battery monitor param tables loaded by pointer
+
+    AP_GROUPEND
+};
+
+// Constructor
+AP_BattMonitor_FuelLevel_Analog::AP_BattMonitor_FuelLevel_Analog(AP_BattMonitor &mon, AP_BattMonitor::BattMonitor_State &mon_state, AP_BattMonitor_Params &params) :
+    AP_BattMonitor_Backend(mon, mon_state, params)
+{
+    AP_Param::setup_object_defaults(this, var_info);
+    _state.var_info = var_info;
+
+    _analog_source = hal.analogin->channel(_pin);
+
+    // create a low pass filter
+    // use a pole at 0.3 Hz if the filter is not being used
+    _voltage_filter.set_cutoff_frequency((_fuel_level_filter_frequency >= 0) ? _fuel_level_filter_frequency : 0.3f);
+}
+
+/*
+  read - read the "voltage" and "current"
+*/
+void AP_BattMonitor_FuelLevel_Analog::read()
+{
+    if (_analog_source == nullptr) {
+        return;
+    }
+
+    if (!_analog_source->set_pin(_pin)) {
+        _state.healthy = false;
+        return;
+    }
+    _state.healthy = true;
+
+    // get a dt and a timestamp
+    const uint32_t tnow = AP_HAL::micros();
+    const uint32_t dt_us = tnow - _state.last_time_micros;
+
+    // get voltage from an ADC pin and filter it
+    const float voltage = _analog_source->voltage_average();
+    const float filtered_voltage = _voltage_filter.apply(voltage, float(dt_us) * 1.0e-6f);
+    const float &voltage_used = (_fuel_level_filter_frequency >= 0) ? filtered_voltage : voltage;
+
+    // output the ADC voltage to the voltage field for easier calibration of sensors
+    // also output filtered voltage as a measure of tank slosh filtering
+    // this could be useful for tuning the LPF
+    _state.voltage = voltage;
+    _state.current_amps = filtered_voltage;
+
+    // this driver assumes that CAPACITY is set to tank volume in millilitres.
+    // _fuel_level_voltage_mult is calculated by the user as 1 / (full_voltage - empty_voltage)
+    const float fuel_level_ratio = (voltage_used - _fuel_level_empty_voltage) * _fuel_level_voltage_mult;
+    const float fuel_level_used_ratio = 1.0 - fuel_level_ratio;
+
+    // map consumed_mah to consumed millilitres
+    _state.consumed_mah = fuel_level_used_ratio * _params._pack_capacity;
+
+    // map consumed_wh using fixed voltage of 1
+    _state.consumed_wh = _state.consumed_mah;
+
+    // record time
+    _state.last_time_micros = tnow;
+}

libraries/AP_BattMonitor/AP_BattMonitor_FuelLevel_Analog.h

@@ -0,0 +1,54 @@
+/*
+ * This file 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 file 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/>.
+ *
+ * Code by Charlie Johnson
+ */
+
+#pragma once
+
+#include <Filter/LowPassFilter.h>
+#include "AP_BattMonitor.h"
+#include "AP_BattMonitor_Backend.h"
+
+class AP_BattMonitor_FuelLevel_Analog : public AP_BattMonitor_Backend
+{
+public:
+
+    // Constructor
+    AP_BattMonitor_FuelLevel_Analog(AP_BattMonitor &mon, AP_BattMonitor::BattMonitor_State &mon_state, AP_BattMonitor_Params &params);
+
+    static const struct AP_Param::GroupInfo var_info[];
+
+    // Read the battery voltage and current.  Should be called at 10hz
+    void read() override;
+
+    // returns true if battery monitor provides consumed energy info
+    bool has_consumed_energy() const override { return true; }
+
+    // returns true if battery monitor provides current info
+    bool has_current() const override { return true; }
+
+    void init(void) override {}
+
+private:
+
+    AP_Float _fuel_level_empty_voltage;
+    AP_Float _fuel_level_voltage_mult;
+    AP_Float _fuel_level_filter_frequency;
+    AP_Int8  _pin;
+
+    AP_HAL::AnalogSource *_analog_source;
+
+    LowPassFilterFloat _voltage_filter;
+};

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
+    // @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
     // @User: Standard
     // @RebootRequired: True
     AP_GROUPINFO_FLAGS("MONITOR", 1, AP_BattMonitor_Params, _type, int8_t(AP_BattMonitor::Type::NONE), AP_PARAM_FLAG_ENABLE),