aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_BattMonitor: create and use new AP_HAL::PWMSource object

Browse Source
This commit is contained in:
Peter Barker 2020-09-01 16:57:55 +10:00 committed by Peter Barker
parent abfd995fff
commit 4d59939db6
2 changed files with 7 additions and 45 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

42
libraries/AP_BattMonitor/AP_BattMonitor_FuelLevel_PWM.cpp
View File

@ -23,49 +23,17 @@ AP_BattMonitor_FuelLevel_PWM::AP_BattMonitor_FuelLevel_PWM(AP_BattMonitor &mon,
_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;
if (!pwm_source.set_pin(_params._curr_pin, "FuelLevelPWM")) {
_state.healthy = false;
return;
}
uint32_t pulse_width = irq_state.pulse_width_us;
pulse_count2 = irq_state.pulse_count1;
uint16_t pulse_width = pwm_source.get_pwm_us();
/*
this driver assumes that CAPACITY is set to tank volume in millilitres.
@ -74,9 +42,11 @@ void AP_BattMonitor_FuelLevel_PWM::read()
const uint16_t pwm_full = 1900;
const uint16_t pwm_buffer = 20;
uint32_t now_us = AP_HAL::micros();
// check for invalid pulse
if (pulse_width <= (pwm_empty - pwm_buffer)|| pulse_width >= (pwm_full + pwm_buffer)) {
_state.healthy = (now_us - _state.last_time_micros) < 250000U;
return;
}
pulse_width = constrain_int16(pulse_width, pwm_empty, pwm_full);

10
libraries/AP_BattMonitor/AP_BattMonitor_FuelLevel_PWM.h
View File

@ -22,14 +22,6 @@ public:
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;
AP_HAL::PWMSource pwm_source;
};