X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=src%2Fdrivers%2Fao_quadrature.c;h=cfa58da662bfbe218e6cdf672b6ec1232c523b00;hp=1f94aa449266dac505411347497fb5edaed3d5e0;hb=72b6c699d355fcd41addb9919d846e63105b9db7;hpb=5f7e61c749b02ed16e368502062e39b0471e9257 diff --git a/src/drivers/ao_quadrature.c b/src/drivers/ao_quadrature.c index 1f94aa44..cfa58da6 100644 --- a/src/drivers/ao_quadrature.c +++ b/src/drivers/ao_quadrature.c @@ -18,100 +18,163 @@ #include #include #include +#include +#include -__xdata int32_t ao_quadrature_count; +#define AO_QUADRATURE_DEBOUNCE_HOLD 3 -static uint8_t ao_quadrature_state; +static __xdata struct ao_debounce ao_quadrature_debounce[AO_QUADRATURE_COUNT]; + +#define debounce_id(d) ((d) - ao_quadrature_debounce) + +__xdata int32_t ao_quadrature_count[AO_QUADRATURE_COUNT]; + +static uint8_t ao_quadrature_state[AO_QUADRATURE_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))) +#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 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; + +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; + } + return v; +} + static void -ao_quadrature_isr(void) +_ao_quadrature_queue(uint8_t q, int8_t step) { - ao_quadrature_state = ((ao_quadrature_state & 3) << 2); - ao_quadrature_state |= ao_gpio_get(AO_QUADRATURE_PORT, AO_QUADRATURE_A, AO_QUADRATURE_A_PIN); - ao_quadrature_state |= ao_gpio_get(AO_QUADRATURE_PORT, AO_QUADRATURE_B, AO_QUADRATURE_B_PIN) << 1; + ao_quadrature_count[q] += step; +#if AO_EVENT + ao_event_put_isr(AO_EVENT_QUADRATURE, q, step); +#endif + ao_wakeup(&ao_quadrature_count[q]); +} + +uint8_t quad_history[64]; +uint8_t quad_h; + +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; - switch (ao_quadrature_state) { + if (q == 0) { + quad_history[quad_h] = ao_quadrature_state[0]; + quad_h = (quad_h + 1) & 63; + } + + switch (ao_quadrature_state[q]) { case STATE(0, 1, 0, 0): - ao_quadrature_count++; + _ao_quadrature_queue(q, 1); break; case STATE(1, 0, 0, 0): - ao_quadrature_count--; + _ao_quadrature_queue(q, -1); break; - default: - return; } - ao_wakeup(&ao_quadrature_count); +} + +static void +ao_quadrature_isr(void) +{ + uint8_t q; + for (q = 0; q < AO_QUADRATURE_COUNT; q++) + _ao_debounce_start(&ao_quadrature_debounce[q]); } int32_t -ao_quadrature_poll(void) +ao_quadrature_poll(uint8_t q) { int32_t ret; - ao_arch_critical(ret = ao_quadrature_count;); + ao_arch_critical(ret = ao_quadrature_count[q];); return ret; } int32_t -ao_quadrature_wait(void) +ao_quadrature_wait(uint8_t q) { - ao_sleep(&ao_quadrature_count); - return ao_quadrature_poll(); + ao_sleep(&ao_quadrature_count[q]); + return ao_quadrature_poll(q); } static void ao_quadrature_test(void) { -#if 1 + uint8_t q; + + ao_cmd_decimal(); + q = ao_cmd_lex_i; for (;;) { int32_t c; flush(); - c = ao_quadrature_wait(); + 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 } }; +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)); \ + } while (0) + void ao_quadrature_init(void) { - ao_quadrature_count = 0; - - ao_enable_port(AO_QUADRATURE_PORT); - ao_exti_setup(AO_QUADRATURE_PORT, AO_QUADRATURE_A, - AO_EXTI_MODE_PULL_UP|AO_EXTI_MODE_FALLING|AO_EXTI_MODE_RISING|AO_EXTI_PRIORITY_MED, - ao_quadrature_isr); - ao_exti_enable(AO_QUADRATURE_PORT, AO_QUADRATURE_A); - ao_exti_setup(AO_QUADRATURE_PORT, AO_QUADRATURE_B, - AO_EXTI_MODE_PULL_UP|AO_EXTI_MODE_FALLING|AO_EXTI_MODE_RISING|AO_EXTI_PRIORITY_MED, - ao_quadrature_isr); - ao_exti_enable(AO_QUADRATURE_PORT, AO_QUADRATURE_B); + ao_debounce_init(); +#if AO_QUADRATURE_COUNT > 0 + init(0); +#endif +#if AO_QUADRATURE_COUNT > 1 + init(1); +#endif ao_cmd_register(&ao_quadrature_cmds[0]); }