Line data    Source code

       1             : #include "AP_BattMonitor_config.h"
       2             : 
       3             : #if AP_BATTERY_FUELFLOW_ENABLED
       4             : 
       5             : #include "AP_BattMonitor_FuelFlow.h"
       6             : 
       7             : #include <AP_HAL/AP_HAL.h>
       8             : #include <GCS_MAVLink/GCS.h>
       9             : 
      10             : /*
      11             :   "battery" monitor for liquid fuel flow systems that give a pulse on
      12             :   a pin for fixed volumes of fuel.
      13             : 
      14             :   this driver assumes that BATTx_AMP_PERVLT is set to give the
      15             :   number of millilitres per pulse.
      16             : 
      17             :   Output is:
      18             : 
      19             :     - current in Amps maps to in litres/hour
      20             :     - consumed mAh is in consumed millilitres
      21             :     - fixed 1.0v voltage
      22             :  */
      23             : extern const AP_HAL::HAL& hal;
      24             : 
      25             : #define FUELFLOW_MIN_PULSE_DELTA_US 10
      26             : 
      27             : /// Constructor
      28           0 : AP_BattMonitor_FuelFlow::AP_BattMonitor_FuelFlow(AP_BattMonitor &mon,
      29             :                                                  AP_BattMonitor::BattMonitor_State &mon_state,
      30           0 :                                                  AP_BattMonitor_Params &params) :
      31           0 :     AP_BattMonitor_Analog(mon, mon_state, params)
      32             : {
      33           0 :     _state.voltage = 1.0; // show a fixed voltage of 1v
      34             : 
      35             :     // we can't tell if it is healthy as we expect zero pulses when no
      36             :     // fuel is flowing
      37           0 :     _state.healthy = true;
      38           0 : }
      39             : 
      40             : /*
      41             :   handle interrupt on an instance
      42             :  */
      43           0 : void AP_BattMonitor_FuelFlow::irq_handler(uint8_t pin, bool pin_state, uint32_t timestamp)
      44             : {
      45           0 :     if (irq_state.last_pulse_us == 0) {
      46           0 :         irq_state.last_pulse_us = timestamp;
      47           0 :         return;
      48             :     }
      49           0 :     uint32_t delta = timestamp - irq_state.last_pulse_us;
      50           0 :     if (delta < FUELFLOW_MIN_PULSE_DELTA_US) {
      51             :         // simple de-bounce
      52           0 :         return;
      53             :     }
      54           0 :     irq_state.pulse_count++;
      55           0 :     irq_state.total_us += delta;
      56           0 :     irq_state.last_pulse_us = timestamp;
      57             : }
      58             : 
      59             : /*
      60             :   read - read the "voltage" and "current"
      61             : */
      62           0 : void AP_BattMonitor_FuelFlow::read()
      63             : {
      64           0 :     int8_t pin = _curr_pin;
      65           0 :     if (last_pin != pin) {
      66             :         // detach from last pin
      67           0 :         if (last_pin != -1) {
      68           0 :             hal.gpio->detach_interrupt(last_pin);
      69             :         }
      70             :         // attach to new pin
      71           0 :         last_pin = pin;
      72           0 :         if (last_pin > 0) {
      73           0 :             hal.gpio->pinMode(last_pin, HAL_GPIO_INPUT);
      74           0 :             if (!hal.gpio->attach_interrupt(
      75           0 :                     last_pin,
      76             :                     FUNCTOR_BIND_MEMBER(&AP_BattMonitor_FuelFlow::irq_handler, void, uint8_t, bool, uint32_t),
      77             :                     AP_HAL::GPIO::INTERRUPT_RISING)) {
      78           0 :                 GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "FuelFlow: Failed to attach to pin %u", last_pin);
      79             :             }
      80             :         }
      81             :     }
      82             : 
      83           0 :     uint32_t now_us = AP_HAL::micros();
      84           0 :     if (_state.last_time_micros == 0) {
      85             :         // need initial time, so we can work out expected pulse rate
      86           0 :         _state.last_time_micros = now_us;
      87           0 :         return;
      88             :     }
      89           0 :     float dt = (now_us - _state.last_time_micros) * 1.0e-6f;
      90             : 
      91           0 :     if (dt < 1 && irq_state.pulse_count == 0) {
      92             :         // we allow for up to 1 second with no pulses to cope with low
      93             :         // flow idling. After that we will start reading zero current
      94           0 :         return;
      95             :     }
      96             :     
      97             :     // get the IRQ state with interrupts disabled
      98             :     struct IrqState state;
      99           0 :     void *irqstate = hal.scheduler->disable_interrupts_save();
     100           0 :     state = irq_state;
     101           0 :     irq_state.pulse_count = 0;
     102           0 :     irq_state.total_us = 0;
     103           0 :     hal.scheduler->restore_interrupts(irqstate);
     104             : 
     105             :     /*
     106             :       this driver assumes that BATTx_AMP_PERVLT is set to give the
     107             :       number of millilitres per pulse.
     108             :      */
     109           0 :     float irq_dt = state.total_us * 1.0e-6f;
     110             :     float litres, litres_pec_sec;
     111           0 :     if (state.pulse_count == 0) {
     112           0 :         litres = 0;
     113           0 :         litres_pec_sec = 0;
     114             :     } else {
     115           0 :         litres = state.pulse_count * _curr_amp_per_volt * 0.001f;
     116           0 :         litres_pec_sec = litres / irq_dt;
     117             :     }
     118             : 
     119           0 :     _state.last_time_micros = now_us;
     120             : 
     121             :     // map amps to litres/hour
     122           0 :     _state.current_amps = litres_pec_sec * (60*60);
     123             : 
     124             :     // map consumed_mah to consumed millilitres
     125           0 :     _state.consumed_mah += litres * 1000;
     126             : 
     127             :     // map consumed_wh using fixed voltage of 1
     128           0 :     _state.consumed_wh = _state.consumed_mah;
     129             : }
     130             : 
     131             : #endif  // AP_BATTERY_FUELFLOW_ENABLED