diff --git a/libraries/AP_WheelEncoder/AP_WheelEncoder.cpp b/libraries/AP_WheelEncoder/AP_WheelEncoder.cpp index 40168d55ac..eced728481 100644 --- a/libraries/AP_WheelEncoder/AP_WheelEncoder.cpp +++ b/libraries/AP_WheelEncoder/AP_WheelEncoder.cpp @@ -152,20 +152,20 @@ void AP_WheelEncoder::init(void) return; } for (uint8_t i=0; i -#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN -#include -#include #include "WheelEncoder_Quadrature.h" -#include + +#include extern const AP_HAL::HAL& hal; -AP_WheelEncoder_Quadrature::IrqState AP_WheelEncoder_Quadrature::irq_state[WHEELENCODER_MAX_INSTANCES]; // constructor AP_WheelEncoder_Quadrature::AP_WheelEncoder_Quadrature(AP_WheelEncoder &frontend, uint8_t instance, AP_WheelEncoder::WheelEncoder_State &state) : @@ -30,107 +27,63 @@ AP_WheelEncoder_Quadrature::AP_WheelEncoder_Quadrature(AP_WheelEncoder &frontend { } -void AP_WheelEncoder_Quadrature::update(void) +// check if pin has changed and initialise gpio event callback +void AP_WheelEncoder_Quadrature::update_pin(uint8_t &pin, + uint8_t new_pin, + uint8_t &pin_value) { - uint8_t instance = _state.instance; - - // check if pin a has changed and initialise gpio event callback - if (last_pin_a != get_pin_a()) { - last_pin_a = get_pin_a(); - - // remove old gpio event callback if present - if (irq_state[instance].last_gpio_a != 0) { - stm32_gpiosetevent(irq_state[instance].last_gpio_a, false, false, false, nullptr); - irq_state[instance].last_gpio_a = 0; - } - - // install interrupt handler on rising or falling edge of gpio for pin a - irq_state[instance].last_gpio_a = get_gpio(last_pin_a); - if (irq_state[instance].last_gpio_a != 0) { - stm32_gpiosetevent(irq_state[instance].last_gpio_a, true, true, false, _state.instance==0 ? irq_handler0_pina : irq_handler1_pina); - } - + if (new_pin == pin) { + // no change + return; } - // check if pin b has changed and initialise gpio event callback - if (last_pin_b != get_pin_b()) { - last_pin_b = get_pin_b(); + // remove old gpio event callback if present + if (pin != (uint8_t)-1 && + !hal.gpio->detach_interrupt(pin)) { + gcs().send_text(MAV_SEVERITY_WARNING, "WEnc: Failed to detach from pin %u", pin); + // ignore this failure or the user may be stuck + } - // remove old gpio event callback if present - if (irq_state[instance].last_gpio_b != 0) { - stm32_gpiosetevent(irq_state[instance].last_gpio_b, false, false, false, nullptr); - irq_state[instance].last_gpio_b = 0; + pin = new_pin; + + // install interrupt handler on rising or falling edge of gpio for pin a + if (new_pin != (uint8_t)-1) { + if (!hal.gpio->attach_interrupt( + pin, + FUNCTOR_BIND_MEMBER(&AP_WheelEncoder_Quadrature::irq_handler, + void, + uint8_t, + bool, + uint32_t), + AP_HAL::GPIO::INTERRUPT_BOTH)) { + gcs().send_text(MAV_SEVERITY_WARNING, "WEnc: Failed to attach to pin %u", pin); } + pin_value = hal.gpio->read(pin); + } +} - // install interrupt handler on rising or falling edge of gpio for pin b - irq_state[instance].last_gpio_b = get_gpio(last_pin_b); - if (irq_state[instance].last_gpio_b != 0) { - stm32_gpiosetevent(irq_state[instance].last_gpio_b, true, true, false, _state.instance==0 ? irq_handler0_pinb : irq_handler1_pinb); - } +void AP_WheelEncoder_Quadrature::update(void) +{ + update_pin(last_pin_a, get_pin_a(), last_pin_a_value); + update_pin(last_pin_b, get_pin_b(), last_pin_b_value); + static uint32_t last_warn_ms = 0; + const uint32_t now = AP_HAL::millis(); + if (now - last_warn_ms > 1000) { + last_warn_ms = now; } // disable interrupts to prevent race with irq_handler - irqstate_t istate = irqsave(); + void *irqstate = hal.scheduler->disable_interrupts_save(); // copy distance and error count so it is accessible to front end - _state.distance_count = irq_state[instance].distance_count; - _state.total_count = irq_state[instance].total_count; - _state.error_count = irq_state[instance].error_count; - _state.last_reading_ms = irq_state[instance].last_reading_ms; + _state.distance_count = irq_state.distance_count; + _state.total_count = irq_state.total_count; + _state.error_count = irq_state.error_count; + _state.last_reading_ms = irq_state.last_reading_ms; // restore interrupts - irqrestore(istate); -} - -// interrupt handler for instance 0, pin a -int AP_WheelEncoder_Quadrature::irq_handler0_pina(int irq, void *context) -{ - irq_handler(0, true); - return 0; -} - -// interrupt handler for instance 0, pin b -int AP_WheelEncoder_Quadrature::irq_handler0_pinb(int irq, void *context) -{ - irq_handler(0, false); - return 0; -} - -// interrupt handler for instance 1, pin a -int AP_WheelEncoder_Quadrature::irq_handler1_pina(int irq, void *context) -{ - irq_handler(1, true); - return 0; -} - -// interrupt handler for instance 1, pin b -int AP_WheelEncoder_Quadrature::irq_handler1_pinb(int irq, void *context) -{ - irq_handler(1, false); - return 0; -} - -// get gpio id from pin number -uint32_t AP_WheelEncoder_Quadrature::get_gpio(uint8_t pin_number) -{ -#ifdef GPIO_GPIO0_INPUT - switch (pin_number) { - case 50: - return GPIO_GPIO0_INPUT; - case 51: - return GPIO_GPIO1_INPUT; - case 52: - return GPIO_GPIO2_INPUT; - case 53: - return GPIO_GPIO3_INPUT; - case 54: - return GPIO_GPIO4_INPUT; - case 55: - return GPIO_GPIO5_INPUT; - } -#endif - return 0; + hal.scheduler->restore_interrupts(irqstate); } // convert pin a and pin b state to a wheel encoder phase @@ -156,43 +109,45 @@ uint8_t AP_WheelEncoder_Quadrature::pin_ab_to_phase(bool pin_a, bool pin_b) return (uint8_t)pin_a << 1 | (uint8_t)pin_b; } -void AP_WheelEncoder_Quadrature::update_phase_and_error_count(bool pin_a_now, bool pin_b_now, uint8_t &phase, int32_t &distance_count, uint32_t &total_count, uint32_t &error_count) +void AP_WheelEncoder_Quadrature::update_phase_and_error_count() { // convert pin state before and after to phases - uint8_t phase_after = pin_ab_to_phase(pin_a_now, pin_b_now); + uint8_t phase_after = pin_ab_to_phase(last_pin_a_value, last_pin_b_value); // look for invalid changes - uint8_t step_forward = phase < 3 ? phase+1 : 0; - uint8_t step_back = phase > 0 ? phase-1 : 3; + uint8_t step_forward = irq_state.phase < 3 ? irq_state.phase+1 : 0; + uint8_t step_back = irq_state.phase > 0 ? irq_state.phase-1 : 3; if (phase_after == step_forward) { - phase = phase_after; - distance_count++; + irq_state.phase = phase_after; + irq_state.distance_count++; } else if (phase_after == step_back) { - phase = phase_after; - distance_count--; + irq_state.phase = phase_after; + irq_state.distance_count--; } else { - error_count++; + irq_state.error_count++; } - total_count++; + irq_state.total_count++; } -// combined irq handler -void AP_WheelEncoder_Quadrature::irq_handler(uint8_t instance, bool pin_a) +void AP_WheelEncoder_Quadrature::irq_handler(uint8_t pin, + bool pin_value, + uint32_t timestamp) { // sanity check - if (irq_state[instance].last_gpio_a == 0 || irq_state[instance].last_gpio_b == 0) { + if (last_pin_a == 0 || last_pin_b == 0) { return; } - // read value of pin-a and pin-b - bool pin_a_high = stm32_gpioread(irq_state[instance].last_gpio_a); - bool pin_b_high = stm32_gpioread(irq_state[instance].last_gpio_b); - // update distance and error counts - update_phase_and_error_count(pin_a_high, pin_b_high, irq_state[instance].phase, irq_state[instance].distance_count, irq_state[instance].total_count, irq_state[instance].error_count); + if (pin == last_pin_a) { + last_pin_a_value = pin_value; + } else if (pin == last_pin_b) { + last_pin_b_value = pin_value; + } else { + return; + }; + update_phase_and_error_count(); // record update time - irq_state[instance].last_reading_ms = AP_HAL::millis(); + irq_state.last_reading_ms = timestamp; } - -#endif // CONFIG_HAL_BOARD diff --git a/libraries/AP_WheelEncoder/WheelEncoder_Quadrature.h b/libraries/AP_WheelEncoder/WheelEncoder_Quadrature.h index 4e7860a5dc..a29c2e5e57 100644 --- a/libraries/AP_WheelEncoder/WheelEncoder_Quadrature.h +++ b/libraries/AP_WheelEncoder/WheelEncoder_Quadrature.h @@ -30,34 +30,30 @@ public: private: - // gpio interrupt handlers - static int irq_handler0_pina(int irq, void *context); // instance 0's pin_a handler - static int irq_handler0_pinb(int irq, void *context); // instance 0's pin_b handler - static int irq_handler1_pina(int irq, void *context); // instance 1's pin_a handler - static int irq_handler1_pinb(int irq, void *context); // instance 1's pin_b handler - static void irq_handler(uint8_t instance, bool pin_a); // combined irq handler + // check if pin has changed and initialise gpio event callback + void update_pin(uint8_t &pin, uint8_t new_pin, uint8_t &pin_value); - // get gpio id from pin number - static uint32_t get_gpio(uint8_t pin_number); + // gpio interrupt handlers + void irq_handler(uint8_t pin, bool pin_value, uint32_t timestamp); // combined irq handler // convert pin a and b status to phase static uint8_t pin_ab_to_phase(bool pin_a, bool pin_b); // update phase, distance_count and error count using pin a and b's latest state - static void update_phase_and_error_count(bool pin_a_now, bool pin_b_now, uint8_t &phase, int32_t &distance_count, uint32_t &total_count, uint32_t &error_count); + void update_phase_and_error_count(); struct IrqState { - uint32_t last_gpio_a; // gpio used for pin a - uint32_t last_gpio_b; // gpio used for pin b uint8_t phase; // current phase of encoder (from 0 to 3) int32_t distance_count; // distance measured by cumulative steps forward or backwards uint32_t total_count; // total number of successful readings from sensor (used for sensor quality calcs) uint32_t error_count; // total number of errors reading from sensor (used for sensor quality calcs) uint32_t last_reading_ms; // system time of last update from encoder - }; - static struct IrqState irq_state[WHEELENCODER_MAX_INSTANCES]; + } irq_state; // private members - uint8_t last_pin_a; - uint8_t last_pin_b; + uint8_t last_pin_a = -1; + uint8_t last_pin_b = -1; + + uint8_t last_pin_a_value; + uint8_t last_pin_b_value; };