X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=src%2Fdrivers%2Fao_quadrature.c;h=20781c407374b9f5a5d41947ba8ea66774951e4d;hp=cfa58da662bfbe218e6cdf672b6ec1232c523b00;hb=c6e57291d91f1f6c4de5c54a5cfd3eef66d9f830;hpb=72b6c699d355fcd41addb9919d846e63105b9db7 diff --git a/src/drivers/ao_quadrature.c b/src/drivers/ao_quadrature.c index cfa58da6..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 @@ -18,56 +19,81 @@ #include #include #include -#include +#include #include -#define AO_QUADRATURE_DEBOUNCE_HOLD 3 - -static __xdata struct ao_debounce ao_quadrature_debounce[AO_QUADRATURE_COUNT]; - -#define debounce_id(d) ((d) - ao_quadrature_debounce) +int32_t ao_quadrature_count[AO_QUADRATURE_COUNT]; +#ifndef AO_QUADRATURE_SINGLE_CODE +static int8_t ao_quadrature_step[AO_QUADRATURE_COUNT]; +#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 q_case(q) case q: v = (!ao_gpio_get(port(q), bita(q), pina(q))) | ((!ao_gpio_get(port(q), bitb(q), pinb(q))) << 1); break - -uint8_t quad_raw[64]; -uint8_t quad_r; +#ifndef AO_QUADRATURE_DEBOUNCE +#error must define AO_QUADRATURE_DEBOUNCE +#endif static uint8_t -_ao_quadrature_get(struct ao_debounce *debounce) { - uint8_t q = debounce_id(debounce); - uint8_t v = 0; - - switch (q) { -#if AO_QUADRATURE_COUNT > 0 - q_case(0); -#endif -#if AO_QUADRATURE_COUNT > 1 - q_case(1); -#endif - } - if (q == 0) { - quad_raw[quad_r] = v; - quad_r = (quad_r + 1) & 63; +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 v; + 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 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_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); @@ -75,37 +101,48 @@ _ao_quadrature_queue(uint8_t q, int8_t step) ao_wakeup(&ao_quadrature_count[q]); } -uint8_t quad_history[64]; -uint8_t quad_h; +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(struct ao_debounce *debounce, uint8_t value) { - uint8_t q = debounce_id(debounce); - - ao_quadrature_state[q] = ((ao_quadrature_state[q] & 3) << 2); - ao_quadrature_state[q] |= value; - - if (q == 0) { - quad_history[quad_h] = ao_quadrature_state[0]; - quad_h = (quad_h + 1) & 63; - } +_ao_quadrature_set(uint8_t q, uint8_t new) +{ + uint8_t old; - switch (ao_quadrature_state[q]) { - case STATE(0, 1, 0, 0): - _ao_quadrature_queue(q, 1); - break; - case STATE(1, 0, 0, 0): - _ao_quadrature_queue(q, -1); - break; - } + 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) { - uint8_t q; - for (q = 0; q < AO_QUADRATURE_COUNT; q++) - _ao_debounce_start(&ao_quadrature_debounce[q]); +#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 @@ -127,9 +164,39 @@ 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) + 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(); + } + } +#if 0 for (;;) { int32_t c; flush(); @@ -138,6 +205,7 @@ ao_quadrature_test(void) if (c == 100) break; } +#endif } static const struct ao_cmds ao_quadrature_cmds[] = { @@ -145,36 +213,22 @@ static const struct ao_cmds ao_quadrature_cmds[] = { { 0, NULL } }; -static void -ao_quadrature_debounce_init(struct ao_debounce *debounce) { - debounce->hold = AO_QUADRATURE_DEBOUNCE_HOLD; - debounce->_get = _ao_quadrature_get; - debounce->_set = _ao_quadrature_set; -} - #define init(q) do { \ - ao_enable_port(port(q)); \ - ao_quadrature_debounce_init(&ao_quadrature_debounce[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)); \ + 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 ao_quadrature_init(void) { - ao_debounce_init(); #if AO_QUADRATURE_COUNT > 0 init(0); #endif #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]); }