X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=src%2Fdrivers%2Fao_quadrature.c;h=20781c407374b9f5a5d41947ba8ea66774951e4d;hp=d07488d08d4f9307b661a0f690f9447dab4fec73;hb=c6e57291d91f1f6c4de5c54a5cfd3eef66d9f830;hpb=01f8df088759ee7e6bc3900a013e0ea4fafaf984 diff --git a/src/drivers/ao_quadrature.c b/src/drivers/ao_quadrature.c index d07488d0..20781c40 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 @@ -21,12 +22,19 @@ #include #include -__xdata int32_t ao_quadrature_count[AO_QUADRATURE_COUNT]; -static uint8_t ao_quadrature_state[AO_QUADRATURE_COUNT]; -static int8_t ao_quadrature_raw[AO_QUADRATURE_COUNT]; +int32_t ao_quadrature_count[AO_QUADRATURE_COUNT]; +#ifndef AO_QUADRATURE_SINGLE_CODE +static int8_t ao_quadrature_step[AO_QUADRATURE_COUNT]; +#endif + +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 @@ -35,18 +43,57 @@ static int8_t ao_quadrature_raw[AO_QUADRATURE_COUNT]; #define pinb(q) AO_QUADRATURE_ ## q ## _B ## _PIN #define isr(q) ao_quadrature_isr_ ## q -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]); - return ~((((v >> pin_a) & 1) | (((v >> pin_b) & 1) << 1))) & 3; + return a | (b << 1); } -#define _ao_quadrature_get(q) ao_quadrature_read(port(q), bita(q), bitb(q)) +#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_step(uint8_t q, int8_t step) { +#ifndef AO_QUADRATURE_SINGLE_CODE + ao_quadrature_step[q] += step; + if (ao_quadrature_state[q] != 0) + return; + if (ao_quadrature_step[q] >= 4) { + ao_quadrature_step[q] = 0; + step = 1; + } else if (ao_quadrature_step[q] <= -4) { + ao_quadrature_step[q] = 0; + step = -1; + } else + return; +#endif ao_quadrature_count[q] += step; #if AO_EVENT ao_event_put_isr(AO_EVENT_QUADRATURE, q, step); @@ -54,51 +101,48 @@ _ao_quadrature_queue(uint8_t q, int8_t step) ao_wakeup(&ao_quadrature_count[q]); } -static const int8_t step[16] = { - [STATE(0,0,0,0)] = 0, - [STATE(0,0,0,1)] = -1, - [STATE(0,0,1,0)] = 1, - [STATE(0,0,1,1)] = 0, - [STATE(0,1,0,0)] = 1, - [STATE(0,1,1,0)] = 0, - [STATE(0,1,1,1)] = -1, - [STATE(1,0,0,0)] = -1, - [STATE(1,0,0,1)] = 0, - [STATE(1,0,1,0)] = 0, - [STATE(1,0,1,1)] = 1, - [STATE(1,1,0,0)] = 0, - [STATE(1,1,0,1)] = 1, - [STATE(1,1,1,0)] = -1, - [STATE(1,1,1,1)] = 0 +static const struct { + uint8_t prev, next; +} ao_quadrature_steps[4] = { + [0] { .prev = 2, .next = 1 }, + [1] { .prev = 0, .next = 3 }, + [3] { .prev = 1, .next = 2 }, + [2] { .prev = 3, .next = 0 }, }; static void -_ao_quadrature_set(uint8_t q, uint8_t value) { - uint8_t v; - - v = ao_quadrature_state[q] & 3; - value = value & 3; +_ao_quadrature_set(uint8_t q, uint8_t new) +{ + uint8_t old; - if (v == value) - return; - - ao_quadrature_state[q] = (v << 2) | value; - - ao_quadrature_raw[q] += step[ao_quadrature_state[q]]; - if (value == 0) { - if (ao_quadrature_raw[q] == 4) - _ao_quadrature_queue(q, 1); - else if (ao_quadrature_raw[q] == -4) - _ao_quadrature_queue(q, -1); - ao_quadrature_raw[q] = 0; - } + ao_arch_block_interrupts(); + old = ao_quadrature_state[q]; + ao_quadrature_state[q] = new; + ao_arch_release_interrupts(); + + if (new == ao_quadrature_steps[old].next) + _ao_quadrature_step(q, 1); + else if (new == ao_quadrature_steps[old].prev) + _ao_quadrature_step(q, -1); } static void ao_quadrature_isr(void) { +#if AO_QUADRATURE_COUNT > 0 _ao_quadrature_set(0, _ao_quadrature_get(0)); +#endif +#if AO_QUADRATURE_COUNT > 1 _ao_quadrature_set(1, _ao_quadrature_get(1)); +#endif +} + +static void +_ao_quadrature_start_one(uint8_t q, uint8_t new) +{ + ao_arch_block_interrupts(); + ao_quadrature_state[q] = new; + ao_arch_release_interrupts(); } int32_t @@ -120,17 +164,38 @@ static void ao_quadrature_test(void) { uint8_t q; + int32_t c; + uint8_t s; +#ifndef AO_QUADRATURE_SINGLE_CODE + int8_t t = 0; +#endif ao_cmd_decimal(); q = ao_cmd_lex_i; - if (q >= AO_QUADRATURE_COUNT) { + if (q >= AO_QUADRATURE_COUNT) ao_cmd_status = ao_cmd_syntax_error; + if (ao_cmd_status != ao_cmd_success) return; + + c = -10000; + s = 0; + while (ao_quadrature_count[q] != 10) { + if (ao_quadrature_count[q] != c || +#ifndef AO_QUADRATURE_SINGLE_CODE + ao_quadrature_step[q] != t || +#endif + ao_quadrature_state[q] != s) + { + c = ao_quadrature_count[q]; + s = ao_quadrature_state[q]; +#ifndef AO_QUADRATURE_SINGLE_CODE + t = ao_quadrature_step[q]; + printf("step %3d ", t); +#endif + printf ("count %3d state %2x\n", c, s); + flush(); + } } - printf ("count %d state %x raw %d\n", - ao_quadrature_count[q], - ao_quadrature_state[q], - ao_quadrature_raw[q]); #if 0 for (;;) { int32_t c; @@ -148,9 +213,10 @@ static const struct ao_cmds ao_quadrature_cmds[] = { { 0, NULL } }; -#define init(q) do { \ - ao_enable_input(port(q), bita(q), 0); \ - ao_enable_input(port(q), bitb(q), 0); \ +#define init(q) do { \ + ao_enable_input(port(q), bita(q), 0); \ + ao_enable_input(port(q), bitb(q), 0); \ + _ao_quadrature_start_one(q, _ao_quadrature_get(q)); \ } while (0) void