-static inline uint16_t
-ao_quadrature_read(struct stm_gpio *gpio, uint8_t pin_a, uint8_t pin_b) {
- uint16_t v = stm_gpio_get_all(gpio);
+#ifndef AO_QUADRATURE_DEBOUNCE
+#error must define AO_QUADRATURE_DEBOUNCE
+#endif
+
+static uint8_t
+ao_debounce(uint8_t cur, struct ao_debounce *debounce)
+{
+#if AO_QUADRATURE_DEBOUNCE > 0
+ if (debounce->count > 0) {
+ debounce->count--;
+ } else if (cur != debounce->state) {
+ debounce->state = cur;
+ debounce->count = AO_QUADRATURE_DEBOUNCE;
+ }
+ return debounce->state;
+#else
+ (void) debounce;
+ return cur;
+#endif
+}
+
+static uint16_t
+ao_quadrature_read(struct stm_gpio *gpio, uint8_t pin_a, uint8_t pin_b, struct ao_debounce debounce_state[2]) {
+ uint16_t v = ~stm_gpio_get_all(gpio);
+ uint8_t a = (v >> pin_a) & 1;
+ uint8_t b = (v >> pin_b) & 1;
+
+ a = ao_debounce(a, &debounce_state[0]);
+ b = ao_debounce(b, &debounce_state[1]);