Merge branch 'master' of ssh://git.gag.com/scm/git/fw/altos

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

diff --git a/src/drivers/ao_quadrature.c b/src/drivers/ao_quadrature.c
index 59e845181ceea6f5f43f0fe1c854f185e2806076..b50f1bf5c90b4c1e52f0ac73a77ca55a47b26203 100644 (file)
--- a/src/drivers/ao_quadrature.c
+++ b/src/drivers/ao_quadrature.c
@@ -22,7 +22,7 @@
 #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
@@ -73,7 +73,7 @@ ao_quadrature_read(struct stm_gpio *gpio, uint8_t pin_a, uint8_t pin_b, struct a
        a = ao_debounce(a, &debounce_state[0]);
        b = ao_debounce(b, &debounce_state[1]);
 
-       return a | (b << 1);
+       return (uint16_t) (a | (b << 1));
 }
 
 #define _ao_quadrature_get(q)  ao_quadrature_read(port(q), bita(q), bitb(q), ao_debounce_state[q])
@@ -111,7 +111,8 @@ static const struct {
 };
 
 static void
-_ao_quadrature_set(uint8_t q, uint8_t new) {
+_ao_quadrature_set(uint8_t q, uint8_t new)
+{
        uint8_t old;
 
        ao_arch_block_interrupts();
@@ -129,13 +130,21 @@ static void
 ao_quadrature_isr(void)
 {
 #if AO_QUADRATURE_COUNT > 0
-       _ao_quadrature_set(0, _ao_quadrature_get(0));
+       _ao_quadrature_set(0, (uint8_t) _ao_quadrature_get(0));
 #endif
 #if AO_QUADRATURE_COUNT > 1
-       _ao_quadrature_set(1, _ao_quadrature_get(1));
+       _ao_quadrature_set(1, (uint8_t) _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
 ao_quadrature_poll(uint8_t q)
 {
@@ -154,15 +163,14 @@ ao_quadrature_wait(uint8_t q)
 static void
 ao_quadrature_test(void)
 {
-       uint8_t q;
+       uint32_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;
+       q = ao_cmd_decimal();
        if (q >= AO_QUADRATURE_COUNT)
                ao_cmd_status = ao_cmd_syntax_error;
        if (ao_cmd_status != ao_cmd_success)
@@ -183,7 +191,7 @@ ao_quadrature_test(void)
                        t = ao_quadrature_step[q];
                        printf("step %3d ", t);
 #endif
-                       printf ("count %3d state %2x\n", c, s);
+                       printf ("count %3ld state %2x\n", (long) c, s);
                        flush();
                }
        }
@@ -204,9 +212,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, (uint8_t) _ao_quadrature_get(q)); \
        } while (0)
 
 void