altos: Remove 8051 address space specifiers

[fw/altos] / src / drivers / ao_quadrature.c

diff --git a/src/drivers/ao_quadrature.c b/src/drivers/ao_quadrature.c
index deecfb79422b9c2980507a0a00e9eb3344b64842..20781c407374b9f5a5d41947ba8ea66774951e4d 100644 (file)
--- a/src/drivers/ao_quadrature.c
+++ b/src/drivers/ao_quadrature.c
@@ -22,7 +22,11 @@
 #include <ao_fast_timer.h>
 #include <ao_event.h>
 
-__xdata int32_t ao_quadrature_count[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 {
@@ -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