X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=src%2Fdrivers%2Fao_quadrature.c;h=d07488d08d4f9307b661a0f690f9447dab4fec73;hp=1f94aa449266dac505411347497fb5edaed3d5e0;hb=988924b51980ad43e39bc4785a625ff25eb16449;hpb=5f7e61c749b02ed16e368502062e39b0471e9257 diff --git a/src/drivers/ao_quadrature.c b/src/drivers/ao_quadrature.c index 1f94aa44..d07488d0 100644 --- a/src/drivers/ao_quadrature.c +++ b/src/drivers/ao_quadrature.c @@ -18,100 +18,151 @@ #include #include #include +#include +#include -__xdata int32_t ao_quadrature_count; - -static uint8_t ao_quadrature_state; +__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]; #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 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); + + return ~((((v >> pin_a) & 1) | (((v >> pin_b) & 1) << 1))) & 3; +} + +#define _ao_quadrature_get(q) ao_quadrature_read(port(q), bita(q), bitb(q)) + 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; - - switch (ao_quadrature_state) { - case STATE(0, 1, 0, 0): - ao_quadrature_count++; - break; - case STATE(1, 0, 0, 0): - ao_quadrature_count--; - break; - default: + 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 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 void +_ao_quadrature_set(uint8_t q, uint8_t value) { + uint8_t v; + + v = ao_quadrature_state[q] & 3; + value = value & 3; + + 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_wakeup(&ao_quadrature_count); +} + +static void +ao_quadrature_isr(void) +{ + _ao_quadrature_set(0, _ao_quadrature_get(0)); + _ao_quadrature_set(1, _ao_quadrature_get(1)); } 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; + if (q >= AO_QUADRATURE_COUNT) { + ao_cmd_status = ao_cmd_syntax_error; + return; + } + 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; 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 } }; +#define init(q) do { \ + ao_enable_input(port(q), bita(q), 0); \ + ao_enable_input(port(q), bitb(q), 0); \ + } 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); +#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]); }