diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.cpp b/libraries/AP_BattMonitor/AP_BattMonitor.cpp index b23d0b58c8..2ce27e2c5a 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.cpp +++ b/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 @@ -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; diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.h b/libraries/AP_BattMonitor/AP_BattMonitor.h index ba78d44f68..af285ac1d2 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.h +++ b/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: diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_FuelLevel_PWM.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_FuelLevel_PWM.cpp new file mode 100644 index 0000000000..6414507acd --- /dev/null +++ b/libraries/AP_BattMonitor/AP_BattMonitor_FuelLevel_PWM.cpp @@ -0,0 +1,94 @@ +#include +#include "AP_BattMonitor_FuelLevel_PWM.h" +#include + +/* + "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 ¶ms) : + 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; +} diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_FuelLevel_PWM.h b/libraries/AP_BattMonitor/AP_BattMonitor_FuelLevel_PWM.h new file mode 100644 index 0000000000..32d842c6c2 --- /dev/null +++ b/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 ¶ms); + + /// 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; +}; diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp index b5f5a07c97..7039487bb7 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp +++ b/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), diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Params.h b/libraries/AP_BattMonitor/AP_BattMonitor_Params.h index 8e5ae70aed..072ecfca7c 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Params.h +++ b/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)