Line data    Source code

       1             : #include "AP_BattMonitor_config.h"
       2             : 
       3             : #if AP_BATTERY_FUELLEVEL_PWM_ENABLED
       4             : 
       5             : #include <AP_HAL/AP_HAL.h>
       6             : 
       7             : #include "AP_BattMonitor_FuelLevel_PWM.h"
       8             : 
       9             : /*
      10             :   "battery" monitor for liquid fuel level systems that give a PWM value indicating quantity of remaining fuel.
      11             : 
      12             :   Output is:
      13             : 
      14             :     - mAh remaining is fuel level in millilitres
      15             :     - consumed mAh is in consumed millilitres
      16             :     - fixed 1.0v voltage
      17             :  */
      18             : extern const AP_HAL::HAL& hal;
      19             : 
      20             : /// Constructor
      21           0 : AP_BattMonitor_FuelLevel_PWM::AP_BattMonitor_FuelLevel_PWM(AP_BattMonitor &mon, AP_BattMonitor::BattMonitor_State &mon_state, AP_BattMonitor_Params &params) :
      22           0 :     AP_BattMonitor_Analog(mon, mon_state, params)
      23             : {
      24           0 :     _state.voltage = 1.0; // show a fixed voltage of 1v
      25             : 
      26             :     // need to add check
      27           0 :     _state.healthy = false;
      28           0 : }
      29             : 
      30             : /*
      31             :   read - read the "voltage" and "current"
      32             : */
      33           0 : void AP_BattMonitor_FuelLevel_PWM::read()
      34             : {
      35           0 :     if (!pwm_source.set_pin(_curr_pin, "FuelLevelPWM")) {
      36           0 :         _state.healthy = false;
      37           0 :         return;
      38             :     }
      39             : 
      40           0 :     uint16_t pulse_width = pwm_source.get_pwm_us();
      41             : 
      42             :     /*
      43             :       this driver assumes that CAPACITY is set to tank volume in millilitres.
      44             :      */
      45           0 :     const uint16_t pwm_empty = 1100;
      46           0 :     const uint16_t pwm_full = 1900;
      47           0 :     const uint16_t pwm_buffer = 20;
      48             : 
      49           0 :     uint32_t now_us = AP_HAL::micros();
      50             : 
      51             :     // check for invalid pulse
      52           0 :     if (pulse_width <= (pwm_empty - pwm_buffer)|| pulse_width >= (pwm_full + pwm_buffer)) {
      53           0 :         _state.healthy = (now_us - _state.last_time_micros) < 250000U;
      54           0 :         return;
      55             :     }
      56           0 :     pulse_width = constrain_int16(pulse_width, pwm_empty, pwm_full);
      57           0 :     float proportion_full = (pulse_width - pwm_empty) / float(pwm_full - pwm_empty);
      58           0 :     float proportion_used = 1.0 - proportion_full;
      59             : 
      60           0 :     _state.last_time_micros = now_us;
      61           0 :     _state.healthy = true;
      62             : 
      63             :     // map consumed_mah to consumed millilitres
      64           0 :     _state.consumed_mah = proportion_used * _params._pack_capacity;
      65             : 
      66             :     // map consumed_wh using fixed voltage of 1
      67           0 :     _state.consumed_wh = _state.consumed_mah;
      68             : }
      69             : 
      70             : #endif  // AP_BATTERY_FUELLEVEL_PWM_ENABLED