X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=src%2Fdrivers%2Fao_quadrature.c;h=deecfb79422b9c2980507a0a00e9eb3344b64842;hp=6a2c1bba1af0e468831342b6f2cb130c81ce70a9;hb=1085ec5d57e0ed5d132f2bbdac1a0b6a32c0ab4a;hpb=bd02349111ae0f39b320e6a10a330051ddc39fdf diff --git a/src/drivers/ao_quadrature.c b/src/drivers/ao_quadrature.c index 6a2c1bba..deecfb79 100644 --- a/src/drivers/ao_quadrature.c +++ b/src/drivers/ao_quadrature.c @@ -3,7 +3,8 @@ * * 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; version 2 of the License. + * the Free Software Foundation; either version 2 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 @@ -18,56 +19,88 @@ #include #include #include -#if AO_EVENT +#include #include -#define ao_quadrature_queue(q) ao_event_put_isr(AO_EVENT_QUADRATURE, q, ao_quadrature_count[q]) -#else -#define ao_quadrature_queue(q) -#endif __xdata int32_t ao_quadrature_count[AO_QUADRATURE_COUNT]; +static uint8_t ao_quadrature_state[AO_QUADRATURE_COUNT]; -static uint8_t ao_quadrature_state[AO_QUADRATURE_COUNT]; +struct ao_debounce { + uint8_t state; + uint8_t count; +}; -#define BIT(a,b) ((a) | ((b) << 1)) -#define STATE(old_a, old_b, new_a, new_b) (((BIT(old_a, old_b) << 2) | BIT(new_a, new_b))) +static struct ao_debounce ao_debounce_state[AO_QUADRATURE_COUNT][2]; #define port(q) AO_QUADRATURE_ ## q ## _PORT #define bita(q) AO_QUADRATURE_ ## q ## _A #define bitb(q) AO_QUADRATURE_ ## q ## _B +#define pina(q) AO_QUADRATURE_ ## q ## _A ## _PIN +#define pinb(q) AO_QUADRATURE_ ## q ## _B ## _PIN +#define isr(q) ao_quadrature_isr_ ## q -#define ao_quadrature_update(q) do { \ - ao_quadrature_state[q] = ((ao_quadrature_state[q] & 3) << 2); \ - ao_quadrature_state[q] |= ao_gpio_get(port(q), bita(q), 0); \ - ao_quadrature_state[q] |= ao_gpio_get(port(q), bitb(q), 0) << 1; \ - } while (0) - +#define DEBOUNCE 10 + +static uint8_t +ao_debounce(uint8_t cur, struct ao_debounce *debounce) +{ + if (cur == debounce->state) + debounce->count = 0; + else { + if (++debounce->count == DEBOUNCE) { + debounce->state = cur; + debounce->count = 0; + } + } + return debounce->state; +} + +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]); + + return a | (b << 1); +} + +#define _ao_quadrature_get(q) ao_quadrature_read(port(q), bita(q), bitb(q), ao_debounce_state[q]) + +static void +_ao_quadrature_queue(uint8_t q, int8_t step) +{ + ao_quadrature_count[q] += step; +#if AO_EVENT + ao_event_put_isr(AO_EVENT_QUADRATURE, q, step); +#endif + ao_wakeup(&ao_quadrature_count[q]); +} + +static void +_ao_quadrature_set(uint8_t q, uint8_t new) { + uint8_t old = ao_quadrature_state[q]; + + if (old != new && new == 0) { + if (old & 2) + _ao_quadrature_queue(q, 1); + else if (old & 1) + _ao_quadrature_queue(q, -1); + } + ao_quadrature_state[q] = new; +} static void ao_quadrature_isr(void) { - uint8_t q; #if AO_QUADRATURE_COUNT > 0 - ao_quadrature_update(0); + _ao_quadrature_set(0, _ao_quadrature_get(0)); #endif #if AO_QUADRATURE_COUNT > 1 - ao_quadrature_update(1); + _ao_quadrature_set(1, _ao_quadrature_get(1)); #endif - - for (q = 0; q < AO_QUADRATURE_COUNT; q++) { - switch (ao_quadrature_state[q]) { - case STATE(0, 1, 0, 0): - ao_quadrature_count[q]++; - break; - case STATE(1, 0, 0, 0): - ao_quadrature_count[q]--; - break; - default: - continue; - } - ao_quadrature_queue(q); - ao_wakeup(&ao_quadrature_count[q]); - } } int32_t @@ -88,53 +121,48 @@ ao_quadrature_wait(uint8_t q) static void ao_quadrature_test(void) { -#if 1 + uint8_t q; + int32_t c; + uint8_t s; + + ao_cmd_decimal(); + q = ao_cmd_lex_i; + if (q >= AO_QUADRATURE_COUNT) { + ao_cmd_status = ao_cmd_syntax_error; + return; + } + + c = -10000; + s = 0; + while (ao_quadrature_count[q] != 10) { + if (ao_quadrature_count[q] != c || + ao_quadrature_state[q] != s) { + c = ao_quadrature_count[q]; + s = ao_quadrature_state[q]; + printf ("count %3d state %2x\n", c, s); + flush(); + } + } +#if 0 for (;;) { int32_t c; flush(); - c = ao_quadrature_wait(0); + c = ao_quadrature_wait(q); printf ("new count %6d\n", c); if (c == 100) break; } #endif -#if 0 - uint8_t a, old_a, b, old_b; - - old_a = 2; old_b = 2; - for (;;) { - a = ao_gpio_get(AO_QUADRATURE_PORT, AO_QUADRATURE_A, AO_QUADRATURE_A_PIN); - b = ao_gpio_get(AO_QUADRATURE_PORT, AO_QUADRATURE_B, AO_QUADRATURE_B_PIN); - if (a != old_a || b != old_b) { - printf ("A %d B %d count %ld\n", a, b, ao_quadrature_count); - flush(); - ao_yield(); - old_a = a; - old_b = b; - } - if (ao_stdin_ready) - break; - } -#endif } static const struct ao_cmds ao_quadrature_cmds[] = { - { ao_quadrature_test, "q\0Test quadrature" }, + { ao_quadrature_test, "q \0Test quadrature" }, { 0, NULL } }; -#define init(q) do { \ - ao_enable_port(port(q)); \ - \ - ao_exti_setup(port(q), bita(q), \ - AO_QUADRATURE_MODE|AO_EXTI_MODE_FALLING|AO_EXTI_MODE_RISING|AO_EXTI_PRIORITY_MED, \ - ao_quadrature_isr); \ - ao_exti_enable(port(q), bita(q)); \ - \ - ao_exti_setup(port(q), bitb(q), \ - AO_QUADRATURE_MODE|AO_EXTI_MODE_FALLING|AO_EXTI_MODE_RISING|AO_EXTI_PRIORITY_MED, \ - ao_quadrature_isr); \ - ao_exti_enable(port(q), bitb(q)); \ +#define init(q) do { \ + ao_enable_input(port(q), bita(q), 0); \ + ao_enable_input(port(q), bitb(q), 0); \ } while (0) void @@ -146,5 +174,7 @@ ao_quadrature_init(void) #if AO_QUADRATURE_COUNT > 1 init(1); #endif + ao_fast_timer_init(); + ao_fast_timer_on(ao_quadrature_isr); ao_cmd_register(&ao_quadrature_cmds[0]); }