diff --git a/libraries/AP_WheelEncoder/AP_WheelEncoder.cpp b/libraries/AP_WheelEncoder/AP_WheelEncoder.cpp
new file mode 100644
index 0000000000..0b1af3093b
--- /dev/null
+++ b/libraries/AP_WheelEncoder/AP_WheelEncoder.cpp
@@ -0,0 +1,239 @@
+/*
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see .
+ */
+
+#include "AP_WheelEncoder.h"
+#include "WheelEncoder_Quadrature.h"
+
+extern const AP_HAL::HAL& hal;
+
+// table of user settable parameters
+const AP_Param::GroupInfo AP_WheelEncoder::var_info[] = {
+ // @Param: _TYPE
+ // @DisplayName: WheelEncoder type
+ // @Description: What type of WheelEncoder is connected
+ // @Values: 0:None,1:Quadrature
+ // @User: Standard
+ AP_GROUPINFO("_TYPE", 0, AP_WheelEncoder, _type[0], 0),
+
+ // @Param: _SCALING
+ // @DisplayName: WheelEncoder scaling
+ // @Description: Scaling factor between sensor reading and measured distance in millimeters
+ // @Increment: 0.001
+ // @User: Standard
+ AP_GROUPINFO("_SCALING", 1, AP_WheelEncoder, _scaling[0], WHEELENCODER_SCALING_DEFAULT),
+
+ // @Param: _POS_X
+ // @DisplayName: WheelEncoder X position
+ // @Description: X position of the first wheel encoder in body frame. Positive X is forward of the origin
+ // @Units: m
+ // @User: Standard
+ AP_GROUPINFO("_POS_X", 2, AP_WheelEncoder, _pos_x[0], 0.0f),
+
+ // @Param: _POS_Y
+ // @DisplayName: WheelEncoder Y position
+ // @Description: Y position of the first wheel encoder accelerometer in body frame. Positive Y is to the right of the origin
+ // @Units: m
+ // @User: Standard
+ AP_GROUPINFO("_POS_Y", 3, AP_WheelEncoder, _pos_y[0], 0.0f),
+
+ // @Param: _PINA
+ // @DisplayName: Input Pin A
+ // @Description: Input Pin A
+ // @Values: -1:Disabled,50:PixhawkAUX1,51:PixhawkAUX2,52:PixhawkAUX3,53:PixhawkAUX4,54:PixhawkAUX5,55:PixhawkAUX6
+ // @User: Standard
+ AP_GROUPINFO("_PINA", 4, AP_WheelEncoder, _pina[0], 55),
+
+ // @Param: _PINB
+ // @DisplayName: Input Pin B
+ // @Description: Input Pin B
+ // @Values: -1:Disabled,50:PixhawkAUX1,51:PixhawkAUX2,52:PixhawkAUX3,53:PixhawkAUX4,54:PixhawkAUX5,55:PixhawkAUX6
+ // @User: Standard
+ AP_GROUPINFO("_PINB", 5, AP_WheelEncoder, _pinb[0], 54),
+
+#if WHEELENCODER_MAX_INSTANCES > 1
+ // @Param: 2_TYPE
+ // @DisplayName: Second WheelEncoder type
+ // @Description: What type of WheelEncoder sensor is connected
+ // @Values: 0:None,1:Quadrature
+ // @User: Standard
+ AP_GROUPINFO("2_TYPE", 6, AP_WheelEncoder, _type[1], 0),
+
+ // @Param: 2_SCALING
+ // @DisplayName: WheelEncoder scaling
+ // @Description: Scaling factor between sensor reading and measured distance in millimeters
+ // @Increment: 0.001
+ // @User: Standard
+ AP_GROUPINFO("2_SCALING",7, AP_WheelEncoder, _scaling[1], WHEELENCODER_SCALING_DEFAULT),
+
+ // @Param: 2_POS_X
+ // @DisplayName: WheelEncoder X position
+ // @Description: X position of the first wheel encoder in body frame. Positive X is forward of the origin
+ // @Units: m
+ // @User: Standard
+ AP_GROUPINFO("2_POS_X", 8, AP_WheelEncoder, _pos_x[1], 0.0f),
+
+ // @Param: _POS_Y
+ // @DisplayName: WheelEncoder Y position
+ // @Description: Y position of the first wheel encoder accelerometer in body frame. Positive Y is to the right of the origin
+ // @Units: m
+ // @User: Standard
+ AP_GROUPINFO("2_POS_Y", 9, AP_WheelEncoder, _pos_y[1], 0.0f),
+
+ // @Param: 2_PINA
+ // @DisplayName: Second Encoder Input Pin A
+ // @Description: Second Encoder Input Pin A
+ // @Values: -1:Disabled,50:PixhawkAUX1,51:PixhawkAUX2,52:PixhawkAUX3,53:PixhawkAUX4,54:PixhawkAUX5,55:PixhawkAUX6
+ // @User: Standard
+ AP_GROUPINFO("2_PINA", 10, AP_WheelEncoder, _pina[1], 53),
+
+ // @Param: 2_PINB
+ // @DisplayName: Second Encoder Input Pin B
+ // @Description: Second Encoder Input Pin B
+ // @Values: -1:Disabled,50:PixhawkAUX1,51:PixhawkAUX2,52:PixhawkAUX3,53:PixhawkAUX4,54:PixhawkAUX5,55:PixhawkAUX6
+ // @User: Standard
+ AP_GROUPINFO("2_PINB", 11, AP_WheelEncoder, _pinb[1], 52),
+#endif
+
+ AP_GROUPEND
+};
+
+AP_WheelEncoder::AP_WheelEncoder(void) :
+ num_instances(0)
+{
+ AP_Param::setup_object_defaults(this, var_info);
+
+ // init state and drivers
+ memset(state,0,sizeof(state));
+ memset(drivers,0,sizeof(drivers));
+}
+
+// initialise the AP_WheelEncoder class.
+void AP_WheelEncoder::init(void)
+{
+ if (num_instances != 0) {
+ // init called a 2nd time?
+ return;
+ }
+ for (uint8_t i=0; iupdate();
+ }
+ }
+}
+
+// check if an instance is healthy
+bool AP_WheelEncoder::healthy(uint8_t instance) const
+{
+ if (instance >= num_instances) {
+ return false;
+ }
+ return true;
+}
+
+// check if an instance is activated
+bool AP_WheelEncoder::enabled(uint8_t instance) const
+{
+ if (instance >= num_instances) {
+ return false;
+ }
+ // if no sensor type is selected, the sensor is not activated.
+ if (_type[instance] == WheelEncoder_TYPE_NONE) {
+ return false;
+ }
+ return true;
+}
+
+// get the total distance travelled in meters
+Vector2f AP_WheelEncoder::get_position(uint8_t instance) const
+{
+ // for invalid instances return zero vector
+ if (instance >= WHEELENCODER_MAX_INSTANCES) {
+ return Vector2f(0.0f, 0.0f);
+ }
+ return Vector2f(_pos_x[instance],_pos_y[instance]);
+}
+
+
+// get the total distance traveled in meters
+float AP_WheelEncoder::get_distance(uint8_t instance) const
+{
+ // for invalid instances return zero
+ if (instance >= WHEELENCODER_MAX_INSTANCES) {
+ return 0.0f;
+ }
+ return _scaling[instance] * state[instance].distance_count * 0.001f;
+}
+
+// get the total number of sensor reading from the encoder
+uint32_t AP_WheelEncoder::get_total_count(uint8_t instance) const
+{
+ // for invalid instances return zero
+ if (instance >= WHEELENCODER_MAX_INSTANCES) {
+ return 0;
+ }
+ return state[instance].total_count;
+}
+
+// get the total distance traveled in meters
+uint32_t AP_WheelEncoder::get_error_count(uint8_t instance) const
+{
+ // for invalid instances return zero
+ if (instance >= WHEELENCODER_MAX_INSTANCES) {
+ return 0;
+ }
+ return state[instance].error_count;
+}
+
+// get the signal quality for a sensor
+float AP_WheelEncoder::get_signal_quality(uint8_t instance) const
+{
+ // protect against divide by zero
+ if (state[instance].total_count == 0) {
+ return 0.0f;
+ }
+ return constrain_float((1.0f - ((float)state[instance].error_count / (float)state[instance].total_count)) * 100.0f, 0.0f, 100.0f);
+}
+
+// get the system time (in milliseconds) of the last update
+uint32_t AP_WheelEncoder::get_last_reading_ms(uint8_t instance) const
+{
+ // for invalid instances return zero
+ if (instance >= WHEELENCODER_MAX_INSTANCES) {
+ return 0;
+ }
+ return state[instance].last_reading_ms;
+}
diff --git a/libraries/AP_WheelEncoder/AP_WheelEncoder.h b/libraries/AP_WheelEncoder/AP_WheelEncoder.h
new file mode 100644
index 0000000000..53b21925b7
--- /dev/null
+++ b/libraries/AP_WheelEncoder/AP_WheelEncoder.h
@@ -0,0 +1,99 @@
+/*
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see .
+ */
+#pragma once
+
+#include
+#include
+#include
+#include
+
+// Maximum number of WheelEncoder measurement instances available on this platform
+#define WHEELENCODER_MAX_INSTANCES 2
+#define WHEELENCODER_SCALING_DEFAULT 0.05f // default scaling between sensor readings and millimeters
+
+class AP_WheelEncoder_Backend;
+
+class AP_WheelEncoder
+{
+public:
+ friend class AP_WheelEncoder_Backend;
+ friend class AP_WheelEncoder_Quadrature;
+
+ AP_WheelEncoder(void);
+
+ // WheelEncoder driver types
+ enum WheelEncoder_Type {
+ WheelEncoder_TYPE_NONE = 0,
+ WheelEncoder_TYPE_QUADRATURE = 1
+ };
+
+ // The WheelEncoder_State structure is filled in by the backend driver
+ struct WheelEncoder_State {
+ uint8_t instance; // the instance number of this WheelEncoder
+ int32_t distance_count; // cumulative number of forward + backwards events received from wheel encoder
+ float distance; // total distance measured
+ 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; // time of last reading
+ };
+
+ // detect and initialise any available rpm sensors
+ void init(void);
+
+ // update state of all sensors. Should be called from main loop
+ void update(void);
+
+ // return the number of wheel encoder sensor instances
+ uint8_t num_sensors(void) const { return num_instances; }
+
+ // return true if healthy
+ bool healthy(uint8_t instance) const;
+
+ // return true if the instance is enabled
+ bool enabled(uint8_t instance) const;
+
+ // get the position of the wheel associated with the wheel encoder
+ Vector2f get_position(uint8_t instance) const;
+
+ // get the total distance traveled in meters
+ float get_distance(uint8_t instance) const;
+
+ // get the total number of sensor reading from the encoder
+ uint32_t get_total_count(uint8_t instance) const;
+
+ // get the total number of errors reading from the encoder
+ uint32_t get_error_count(uint8_t instance) const;
+
+ // get the signal quality for a sensor (0 = extremely poor quality, 100 = extremely good quality)
+ float get_signal_quality(uint8_t instance) const;
+
+ // get the system time (in milliseconds) of the last update
+ uint32_t get_last_reading_ms(uint8_t instance) const;
+
+ static const struct AP_Param::GroupInfo var_info[];
+
+protected:
+ // parameters for each instance
+ AP_Int8 _type[WHEELENCODER_MAX_INSTANCES];
+ AP_Float _scaling[WHEELENCODER_MAX_INSTANCES];
+ AP_Float _pos_x[WHEELENCODER_MAX_INSTANCES];
+ AP_Float _pos_y[WHEELENCODER_MAX_INSTANCES];
+ AP_Int8 _pina[WHEELENCODER_MAX_INSTANCES];
+ AP_Int8 _pinb[WHEELENCODER_MAX_INSTANCES];
+
+ WheelEncoder_State state[WHEELENCODER_MAX_INSTANCES];
+ AP_WheelEncoder_Backend *drivers[WHEELENCODER_MAX_INSTANCES];
+ uint8_t num_instances;
+};
diff --git a/libraries/AP_WheelEncoder/WheelEncoder_Backend.cpp b/libraries/AP_WheelEncoder/WheelEncoder_Backend.cpp
new file mode 100644
index 0000000000..06f281c8c8
--- /dev/null
+++ b/libraries/AP_WheelEncoder/WheelEncoder_Backend.cpp
@@ -0,0 +1,44 @@
+/*
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see .
+ */
+
+#include
+#include
+#include "AP_WheelEncoder.h"
+#include "WheelEncoder_Backend.h"
+
+// base class constructor.
+AP_WheelEncoder_Backend::AP_WheelEncoder_Backend(AP_WheelEncoder &frontend, uint8_t instance, AP_WheelEncoder::WheelEncoder_State &state) :
+ _frontend(frontend),
+ _state(state)
+{
+}
+
+// return pin. returns -1 if pin is not defined for this instance
+int8_t AP_WheelEncoder_Backend::get_pin_a() const
+{
+ if (_state.instance > 1) {
+ return -1;
+ }
+ return _frontend._pina[_state.instance].get();
+}
+
+// return pin. returns -1 if pin is not defined for this instance
+int8_t AP_WheelEncoder_Backend::get_pin_b() const
+{
+ if (_state.instance > 1) {
+ return -1;
+ }
+ return _frontend._pinb[_state.instance].get();
+}
diff --git a/libraries/AP_WheelEncoder/WheelEncoder_Backend.h b/libraries/AP_WheelEncoder/WheelEncoder_Backend.h
new file mode 100644
index 0000000000..b6ea9cd1e2
--- /dev/null
+++ b/libraries/AP_WheelEncoder/WheelEncoder_Backend.h
@@ -0,0 +1,42 @@
+/*
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see .
+ */
+#pragma once
+
+#include
+#include
+#include "AP_WheelEncoder.h"
+
+class AP_WheelEncoder_Backend
+{
+public:
+ // constructor. This incorporates initialisation as well.
+ AP_WheelEncoder_Backend(AP_WheelEncoder &frontend, uint8_t instance, AP_WheelEncoder::WheelEncoder_State &state);
+
+ // we declare a virtual destructor so that WheelEncoder drivers can
+ // override with a custom destructor if need be
+ virtual ~AP_WheelEncoder_Backend(void) {}
+
+ // update the state structure. All backends must implement this.
+ virtual void update() = 0;
+
+protected:
+
+ // return pin number. returns -1 if pin is not defined for this instance
+ int8_t get_pin_a() const;
+ int8_t get_pin_b() const;
+
+ AP_WheelEncoder &_frontend;
+ AP_WheelEncoder::WheelEncoder_State &_state;
+};
diff --git a/libraries/AP_WheelEncoder/WheelEncoder_Quadrature.cpp b/libraries/AP_WheelEncoder/WheelEncoder_Quadrature.cpp
new file mode 100644
index 0000000000..df05c3b674
--- /dev/null
+++ b/libraries/AP_WheelEncoder/WheelEncoder_Quadrature.cpp
@@ -0,0 +1,195 @@
+/*
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see .
+ */
+
+#include
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
+#include
+#include
+#include "WheelEncoder_Quadrature.h"
+#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) :
+ AP_WheelEncoder_Backend(frontend, instance, state)
+{
+}
+
+void AP_WheelEncoder_Quadrature::update(void)
+{
+ 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);
+ }
+
+ }
+
+ // 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 (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;
+ }
+
+ // 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);
+ }
+
+ }
+
+ // disable interrupts to prevent race with irq_handler
+ irqstate_t istate = irqsave();
+
+ // 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 = AP_HAL::millis();
+
+ // 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;
+}
+
+// convert pin a and pin b state to a wheel encoder phase
+uint8_t AP_WheelEncoder_Quadrature::pin_ab_to_phase(bool pin_a, bool pin_b)
+{
+ if (!pin_a) {
+ if (!pin_b) {
+ // A = 0, B = 0
+ return 0;
+ } else {
+ // A = 0, B = 1
+ return 1;
+ }
+ } else {
+ if (!pin_b) {
+ // A = 1, B = 0
+ return 3;
+ } else {
+ // A = 1, B = 1
+ return 2;
+ }
+ }
+ 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)
+{
+ // convert pin state before and after to phases
+ uint8_t phase_after = pin_ab_to_phase(pin_a_now, pin_b_now);
+
+ // look for invalid changes
+ uint8_t step_forward = phase < 3 ? phase+1 : 0;
+ uint8_t step_back = phase > 0 ? phase-1 : 3;
+ if (phase_after == step_forward) {
+ phase = phase_after;
+ distance_count++;
+ } else if (phase_after == step_back) {
+ phase = phase_after;
+ distance_count--;
+ } else {
+ error_count++;
+ }
+ total_count++;
+}
+
+// combined irq handler
+void AP_WheelEncoder_Quadrature::irq_handler(uint8_t instance, bool pin_a)
+{
+ // sanity check
+ if (irq_state[instance].last_gpio_a == 0 || irq_state[instance].last_gpio_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);
+}
+
+#endif // CONFIG_HAL_BOARD
diff --git a/libraries/AP_WheelEncoder/WheelEncoder_Quadrature.h b/libraries/AP_WheelEncoder/WheelEncoder_Quadrature.h
new file mode 100644
index 0000000000..347ca53a65
--- /dev/null
+++ b/libraries/AP_WheelEncoder/WheelEncoder_Quadrature.h
@@ -0,0 +1,62 @@
+/*
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see .
+ */
+#pragma once
+
+#include "AP_WheelEncoder.h"
+#include "WheelEncoder_Backend.h"
+#include
+#include
+
+class AP_WheelEncoder_Quadrature : public AP_WheelEncoder_Backend
+{
+public:
+ // constructor
+ AP_WheelEncoder_Quadrature(AP_WheelEncoder &frontend, uint8_t instance, AP_WheelEncoder::WheelEncoder_State &state);
+
+ // update state
+ void update(void);
+
+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
+
+ // get gpio id from pin number
+ static uint32_t get_gpio(uint8_t pin_number);
+
+ // 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);
+
+ 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)
+ };
+ static struct IrqState irq_state[WHEELENCODER_MAX_INSTANCES];
+
+ // private members
+ uint8_t last_pin_a;
+ uint8_t last_pin_b;
+};