AP_BattMonitor: add PWM Fuel Level Sensor

this sensor produces PWM between 1100 and 1900, giving fuel level as a
percentage

libraries/AP_BattMonitor/AP_BattMonitor.cpp

@@ -5,6 +5,7 @@
 #include "AP_BattMonitor_BLHeliESC.h"
 #include "AP_BattMonitor_Sum.h"
 #include "AP_BattMonitor_FuelFlow.h"
+#include "AP_BattMonitor_FuelLevel_PWM.h"
 
 #include <AP_HAL/AP_HAL.h>
 
@@ -142,6 +143,9 @@ AP_BattMonitor::init()
             case AP_BattMonitor_Params::BattMonitor_TYPE_FuelFlow:
                 drivers[instance] = new AP_BattMonitor_FuelFlow(*this, state[instance], _params[instance]);
                 break;
+            case AP_BattMonitor_Params::BattMonitor_TYPE_FuelLevel_PWM:
+                drivers[instance] = new AP_BattMonitor_FuelLevel_PWM(*this, state[instance], _params[instance]);
+                break;
             case AP_BattMonitor_Params::BattMonitor_TYPE_NONE:
             default:
                 break;

libraries/AP_BattMonitor/AP_BattMonitor.h

@@ -37,6 +37,7 @@ class AP_BattMonitor
     friend class AP_BattMonitor_UAVCAN;
     friend class AP_BattMonitor_Sum;
     friend class AP_BattMonitor_FuelFlow;
+    friend class AP_BattMonitor_FuelLevel_PWM;
 
 public:
 

libraries/AP_BattMonitor/AP_BattMonitor_FuelLevel_PWM.cpp

@@ -0,0 +1,94 @@
+#include <AP_HAL/AP_HAL.h>
+#include "AP_BattMonitor_FuelLevel_PWM.h"
+#include <GCS_MAVLink/GCS.h>
+
+/*
+  "battery" monitor for liquid fuel level systems that give a PWM value indicating quantity of remaining fuel.
+
+  Output is:
+
+    - mAh remaining is fuel level in millilitres
+    - consumed mAh is in consumed millilitres
+    - fixed 1.0v voltage
+ */
+extern const AP_HAL::HAL& hal;
+
+/// Constructor
+AP_BattMonitor_FuelLevel_PWM::AP_BattMonitor_FuelLevel_PWM(AP_BattMonitor &mon, AP_BattMonitor::BattMonitor_State &mon_state, AP_BattMonitor_Params &params) :
+    AP_BattMonitor_Backend(mon, mon_state, params)
+{
+    _state.voltage = 1.0; // show a fixed voltage of 1v
+
+    // need to add check
+    _state.healthy = false;
+}
+
+/*
+  handle interrupt on an instance
+ */
+void AP_BattMonitor_FuelLevel_PWM::irq_handler(uint8_t pin, bool pin_state, uint32_t timestamp)
+{
+    if (pin_state == 1) {
+        irq_state.last_pulse_us = timestamp;
+    } else if (irq_state.last_pulse_us != 0) {
+        irq_state.pulse_width_us = timestamp - irq_state.last_pulse_us;
+        irq_state.pulse_count1 ++;
+    }
+}
+
+/*
+  read - read the "voltage" and "current"
+*/
+void AP_BattMonitor_FuelLevel_PWM::read()
+{
+    int8_t pin = _params._curr_pin;
+    if (last_pin != pin) {
+        // detach from last pin
+        if (last_pin != -1) {
+            hal.gpio->detach_interrupt(last_pin);
+        }
+        // attach to new pin
+        last_pin = pin;
+        if (last_pin > 0) {
+            hal.gpio->pinMode(last_pin, HAL_GPIO_INPUT);
+            if (!hal.gpio->attach_interrupt(
+                    last_pin,
+                    FUNCTOR_BIND_MEMBER(&AP_BattMonitor_FuelLevel_PWM::irq_handler, void, uint8_t, bool, uint32_t),
+                    AP_HAL::GPIO::INTERRUPT_BOTH)) {
+                gcs().send_text(MAV_SEVERITY_WARNING, "FuelLevelPWM: Failed to attach to pin %u", unsigned(last_pin));
+            }
+        }
+    }
+    uint32_t now_us = AP_HAL::micros();
+    if (pulse_count2 == irq_state.pulse_count1) {
+        _state.healthy = (now_us - _state.last_time_micros) < 250000U;
+        return;
+    }
+    uint32_t pulse_width = irq_state.pulse_width_us;
+    pulse_count2 = irq_state.pulse_count1;
+
+    /*
+      this driver assumes that CAPACITY is set to tank volume in millilitres.
+     */
+    const uint16_t pwm_empty = 1100;
+    const uint16_t pwm_full = 1900;
+    const uint16_t pwm_buffer = 20;
+
+
+    // check for invalid pulse
+    if (pulse_width <= (pwm_empty - pwm_buffer)|| pulse_width >= (pwm_full + pwm_buffer)) {
+        return;
+    }
+    pulse_width = constrain_int16(pulse_width, pwm_empty, pwm_full);
+    float proportion_full = (pulse_width - pwm_empty) / float(pwm_full - pwm_empty);
+    float proportion_used = 1.0 - proportion_full;
+
+    _state.last_time_micros = now_us;
+    _state.healthy = true;
+
+    // map consumed_mah to consumed millilitres
+    _state.consumed_mah = proportion_used * _params._pack_capacity;
+
+    // map consumed_wh using fixed voltage of 1
+    _state.consumed_wh = _state.consumed_mah;
+}

libraries/AP_BattMonitor/AP_BattMonitor_FuelLevel_PWM.h

@@ -0,0 +1,35 @@
+#pragma once
+
+#include "AP_BattMonitor.h"
+#include "AP_BattMonitor_Backend.h"
+
+class AP_BattMonitor_FuelLevel_PWM : public AP_BattMonitor_Backend
+{
+public:
+
+    /// Constructor
+    AP_BattMonitor_FuelLevel_PWM(AP_BattMonitor &mon, AP_BattMonitor::BattMonitor_State &mon_state, AP_BattMonitor_Params &params);
+
+    /// 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:
+    void irq_handler(uint8_t pin, bool pin_state, uint32_t timestamp);
+
+    struct IrqState {
+        uint32_t last_pulse_us;
+        uint32_t pulse_width_us;
+        uint32_t pulse_count1;
+    } irq_state;
+
+    int8_t last_pin = -1;
+    uint32_t pulse_count2;
+};

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-Maxell,8:UAVCAN-BatteryInfo,9:BLHeli ESC,10:SumOfFollowing,11:FuelFlow
+    // @Values: 0:Disabled,3:Analog Voltage Only,4:Analog Voltage and Current,5:Solo,6:Bebop,7:SMBus-Maxell,8:UAVCAN-BatteryInfo,9:BLHeli ESC,10:SumOfFollowing,11:FuelFlow,12:FuelLevelPWM
     // @User: Standard
     // @RebootRequired: True
     AP_GROUPINFO_FLAGS("MONITOR", 1, AP_BattMonitor_Params, _type, BattMonitor_TYPE_NONE, AP_PARAM_FLAG_ENABLE),

libraries/AP_BattMonitor/AP_BattMonitor_Params.h

@@ -24,6 +24,7 @@ public:
         BattMonitor_TYPE_BLHeliESC                  = 9,
         BattMonitor_TYPE_Sum                        = 10,
         BattMonitor_TYPE_FuelFlow                   = 11,
+        BattMonitor_TYPE_FuelLevel_PWM              = 12,
     };
 
     // low voltage sources (used for BATT_LOW_TYPE parameter)