diff options
Diffstat (limited to 'src/drivers/ao_quadrature.c')
| -rw-r--r-- | src/drivers/ao_quadrature.c | 102 |
1 files changed, 78 insertions, 24 deletions
diff --git a/src/drivers/ao_quadrature.c b/src/drivers/ao_quadrature.c index deecfb79..250e035f 100644 --- a/src/drivers/ao_quadrature.c +++ b/src/drivers/ao_quadrature.c @@ -23,6 +23,10 @@ #include <ao_event.h> __xdata 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 { @@ -39,20 +43,25 @@ static struct ao_debounce ao_debounce_state[AO_QUADRATURE_COUNT][2]; #define pinb(q) AO_QUADRATURE_ ## q ## _B ## _PIN #define isr(q) ao_quadrature_isr_ ## q -#define DEBOUNCE 10 +#ifndef AO_QUADRATURE_DEBOUNCE +#error must define AO_QUADRATURE_DEBOUNCE +#endif 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; - } +#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 @@ -70,8 +79,21 @@ ao_quadrature_read(struct stm_gpio *gpio, uint8_t pin_a, uint8_t pin_b, struct a #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); @@ -79,17 +101,29 @@ _ao_quadrature_queue(uint8_t q, int8_t step) ao_wakeup(&ao_quadrature_count[q]); } +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 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_set(uint8_t q, uint8_t new) +{ + uint8_t old; + + 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 @@ -103,6 +137,14 @@ ao_quadrature_isr(void) #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 ao_quadrature_poll(uint8_t q) { @@ -124,21 +166,32 @@ 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 || - ao_quadrature_state[q] != s) { +#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(); } @@ -160,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 |