diff --git a/libraries/AP_RPM/RPM_Pin.cpp b/libraries/AP_RPM/RPM_Pin.cpp index 638bee03d3..7336070990 100644 --- a/libraries/AP_RPM/RPM_Pin.cpp +++ b/libraries/AP_RPM/RPM_Pin.cpp @@ -50,22 +50,24 @@ void AP_RPM_Pin::update(void) { if (last_pin != get_pin()) { // detach from last pin - if (last_pin != (uint8_t)-1 && - !hal.gpio->detach_interrupt(last_pin)) { - gcs().send_text(MAV_SEVERITY_WARNING, "RPM: Failed to detach from pin %u", last_pin); - // ignore this failure or the user may be stuck + if (interrupt_attached) { + // ignore this failure of the user may be stuck + IGNORE_RETURN(hal.gpio->detach_interrupt(last_pin)); + interrupt_attached = false; } irq_state[state.instance].dt_count = 0; irq_state[state.instance].dt_sum = 0; // attach to new pin last_pin = get_pin(); - if (last_pin) { + if (last_pin > 0) { hal.gpio->pinMode(last_pin, HAL_GPIO_INPUT); - if (!hal.gpio->attach_interrupt( + if (hal.gpio->attach_interrupt( last_pin, FUNCTOR_BIND_MEMBER(&AP_RPM_Pin::irq_handler, void, uint8_t, bool, uint32_t), AP_HAL::GPIO::INTERRUPT_RISING)) { - gcs().send_text(MAV_SEVERITY_WARNING, "RPM: Failed to attach to pin %u", last_pin); + interrupt_attached = true; + } else { + gcs().send_text(MAV_SEVERITY_WARNING, "RPM: Failed to attach to pin %d", last_pin); } } } diff --git a/libraries/AP_RPM/RPM_Pin.h b/libraries/AP_RPM/RPM_Pin.h index 160bb8a419..0bc069c681 100644 --- a/libraries/AP_RPM/RPM_Pin.h +++ b/libraries/AP_RPM/RPM_Pin.h @@ -32,7 +32,8 @@ public: private: ModeFilterFloat_Size5 signal_quality_filter {3}; - uint8_t last_pin = -1; + int8_t last_pin = -1; // last pin number checked vs PIN parameter + bool interrupt_attached; // true if an interrupt has been attached to last_pin struct IrqState { uint32_t last_pulse_us; uint32_t dt_sum;