AP_WheelEncoder: move to using HAL's attach_interrupt methods

AP_WheelEncoder: add error reporting for attaching of interrupts

AP_WheelEncoder: use detach_interrupt method

AP_WheelEncoder: correct initialisation of wheelencoder instances

AP_WheelEncoder: make update_phase_and_error_count non-static

AP_WheelEncoder: use (uint8_t)-1 in place of 255

libraries/AP_WheelEncoder/AP_WheelEncoder.cpp

@@ -152,20 +152,20 @@ void AP_WheelEncoder::init(void)
         return;
     }
     for (uint8_t i=0; i<WHEELENCODER_MAX_INSTANCES; i++) {
-#if CONFIG_HAL_BOARD == HAL_BOARD_PX4  || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
+#if CONFIG_HAL_BOARD == HAL_BOARD_PX4  || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN || CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
-        uint8_t type = _type[num_instances];
+        switch ((WheelEncoder_Type)_type[i].get()) {
-        uint8_t instance = num_instances;
+        case WheelEncoder_TYPE_QUADRATURE:
-
+            drivers[i] = new AP_WheelEncoder_Quadrature(*this, i, state[i]);
-        if (type == WheelEncoder_TYPE_QUADRATURE) {
+            break;
-            state[instance].instance = instance;
+        case WheelEncoder_TYPE_NONE:
-            drivers[instance] = new AP_WheelEncoder_Quadrature(*this, instance, state[instance]);
+            break;
         }
 #endif
 
         if (drivers[i] != nullptr) {
             // we loaded a driver for this instance, so it must be
             // present (although it may not be healthy)
-            num_instances = i+1;
+            num_instances = i+1;  // num_instances is a high-water-mark
         }
     }
 }

libraries/AP_WheelEncoder/AP_WheelEncoder.h

@@ -39,7 +39,7 @@ public:
     AP_WheelEncoder &operator=(const AP_WheelEncoder&) = delete;
 
     // WheelEncoder driver types
-    enum WheelEncoder_Type {
+    enum WheelEncoder_Type : uint8_t {
         WheelEncoder_TYPE_NONE          = 0,
         WheelEncoder_TYPE_QUADRATURE    = 1
     };

libraries/AP_WheelEncoder/WheelEncoder_Backend.cpp

@@ -23,6 +23,7 @@ AP_WheelEncoder_Backend::AP_WheelEncoder_Backend(AP_WheelEncoder &frontend, uint
         _frontend(frontend),
         _state(state) 
 {
+    state.instance = instance;
 }
 
 // return pin.  returns -1 if pin is not defined for this instance

libraries/AP_WheelEncoder/WheelEncoder_Quadrature.cpp

@@ -15,14 +15,11 @@
 
 #include <AP_HAL/AP_HAL.h>
 
-#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
-#include <AP_BoardConfig/AP_BoardConfig.h>
-#include <board_config.h>
 #include "WheelEncoder_Quadrature.h"
-#include <stdio.h>
+
+#include <GCS_MAVLink/GCS.h>
 
 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;
+    if (new_pin == pin) {
-
+        // no change
-    // check if pin a has changed and initialise gpio event callback
+        return;
-    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);
-        }
-
     }
 
-    // check if pin b has changed and initialise gpio event callback
+    // remove old gpio event callback if present
-    if (last_pin_b != get_pin_b()) {
+    if (pin != (uint8_t)-1 &&
-        last_pin_b = get_pin_b();
+        !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
+    pin = new_pin;
-        if (irq_state[instance].last_gpio_b != 0) {
+
-            stm32_gpiosetevent(irq_state[instance].last_gpio_b, false, false, false, nullptr);
+    // install interrupt handler on rising or falling edge of gpio for pin a
-            irq_state[instance].last_gpio_b = 0;
+    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
+void AP_WheelEncoder_Quadrature::update(void)
-        irq_state[instance].last_gpio_b = get_gpio(last_pin_b);
+{
-        if (irq_state[instance].last_gpio_b != 0) {
+    update_pin(last_pin_a, get_pin_a(), last_pin_a_value);
-            stm32_gpiosetevent(irq_state[instance].last_gpio_b, true, true, false, _state.instance==0 ? irq_handler0_pinb : irq_handler1_pinb);
+    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.distance_count = irq_state.distance_count;
-    _state.total_count = irq_state[instance].total_count;
+    _state.total_count = irq_state.total_count;
-    _state.error_count = irq_state[instance].error_count;
+    _state.error_count = irq_state.error_count;
-    _state.last_reading_ms = irq_state[instance].last_reading_ms;
+    _state.last_reading_ms = irq_state.last_reading_ms;
 
     // restore interrupts
-    irqrestore(istate);
+    hal.scheduler->restore_interrupts(irqstate);
-}
-
-// 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;
 }
 
 // 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_forward = irq_state.phase < 3 ? irq_state.phase+1 : 0;
-    uint8_t step_back = phase > 0 ? phase-1 : 3;
+    uint8_t step_back = irq_state.phase > 0 ? irq_state.phase-1 : 3;
     if (phase_after == step_forward) {
-        phase = phase_after;
+        irq_state.phase = phase_after;
-        distance_count++;
+        irq_state.distance_count++;
     } else if (phase_after == step_back) {
-        phase = phase_after;
+        irq_state.phase = phase_after;
-        distance_count--;
+        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 pin,
-void AP_WheelEncoder_Quadrature::irq_handler(uint8_t instance, bool pin_a)
+                                             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

libraries/AP_WheelEncoder/WheelEncoder_Quadrature.h

@@ -30,34 +30,30 @@ public:
 
 private:
 
-    // gpio interrupt handlers
+    // check if pin has changed and initialise gpio event callback
-    static int irq_handler0_pina(int irq, void *context);   // instance 0's pin_a handler
+    void update_pin(uint8_t &pin, uint8_t new_pin, uint8_t &pin_value);
-    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
 
-    // get gpio id from pin number
+    // gpio interrupt handlers
-    static uint32_t get_gpio(uint8_t pin_number);
+    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
-    };
+    } irq_state;
-    static struct IrqState irq_state[WHEELENCODER_MAX_INSTANCES];
 
     // private members
-    uint8_t last_pin_a;
+    uint8_t last_pin_a = -1;
-    uint8_t last_pin_b;
+    uint8_t last_pin_b = -1;
+
+    uint8_t last_pin_a_value;
+    uint8_t last_pin_b_value;
 };