git.gag.com Git - fw/altos/blob - src/drivers/ao_quadrature.c

   1 /*
   2  * Copyright © 2012 Keith Packard <keithp@keithp.com>
   3  *
   4  * This program is free software; you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation; version 2 of the License.
   7  *
   8  * This program is distributed in the hope that it will be useful, but
   9  * WITHOUT ANY WARRANTY; without even the implied warranty of
  10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  11  * General Public License for more details.
  12  *
  13  * You should have received a copy of the GNU General Public License along
  14  * with this program; if not, write to the Free Software Foundation, Inc.,
  15  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
  16  */
  17
  18 #include <ao.h>
  19 #include <ao_quadrature.h>
  20 #include <ao_exti.h>
  21 #include <ao_debounce.h>
  22 #include <ao_event.h>
  23
  24 #define AO_QUADRATURE_DEBOUNCE_HOLD     3
  25
  26 static __xdata struct ao_debounce ao_quadrature_debounce[AO_QUADRATURE_COUNT];
  27
  28 #define debounce_id(d)  ((d) - ao_quadrature_debounce)
  29
  30 __xdata int32_t ao_quadrature_count[AO_QUADRATURE_COUNT];
  31
  32 static uint8_t  ao_quadrature_state[AO_QUADRATURE_COUNT];
  33
  34 #define BIT(a,b)        ((a) | ((b) << 1))
  35 #define STATE(old_a, old_b, new_a, new_b)       (((BIT(old_a, old_b) << 2) | BIT(new_a, new_b)))
  36
  37 #define port(q) AO_QUADRATURE_ ## q ## _PORT
  38 #define bita(q) AO_QUADRATURE_ ## q ## _A
  39 #define bitb(q) AO_QUADRATURE_ ## q ## _B
  40 #define pina(q) AO_QUADRATURE_ ## q ## _A ## _PIN
  41 #define pinb(q) AO_QUADRATURE_ ## q ## _B ## _PIN
  42
  43 #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
  44
  45 uint8_t quad_raw[64];
  46 uint8_t quad_r;
  47
  48 static uint8_t
  49 _ao_quadrature_get(struct ao_debounce *debounce) {
  50         uint8_t q = debounce_id(debounce);
  51         uint8_t v = 0;
  52
  53         switch (q) {
  54 #if AO_QUADRATURE_COUNT > 0
  55                 q_case(0);
  56 #endif
  57 #if AO_QUADRATURE_COUNT > 1
  58                 q_case(1);
  59 #endif
  60         }
  61         if (q == 0) {
  62                 quad_raw[quad_r] = v;
  63                 quad_r = (quad_r + 1) & 63;
  64         }
  65         return v;
  66 }
  67
  68 static void
  69 _ao_quadrature_queue(uint8_t q, int8_t step)
  70 {
  71         ao_quadrature_count[q] += step;
  72 #if AO_EVENT
  73         ao_event_put_isr(AO_EVENT_QUADRATURE, q, step);
  74 #endif
  75         ao_wakeup(&ao_quadrature_count[q]);
  76 }
  77
  78 uint8_t quad_history[64];
  79 uint8_t quad_h;
  80
  81 static void
  82 _ao_quadrature_set(struct ao_debounce *debounce, uint8_t value) {
  83         uint8_t q = debounce_id(debounce);
  84
  85         ao_quadrature_state[q] = ((ao_quadrature_state[q] & 3) << 2);
  86         ao_quadrature_state[q] |= value;
  87
  88         if (q == 0) {
  89                 quad_history[quad_h] = ao_quadrature_state[0];
  90                 quad_h = (quad_h + 1) & 63;
  91         }
  92
  93         switch (ao_quadrature_state[q]) {
  94         case STATE(0, 1, 0, 0):
  95                 _ao_quadrature_queue(q, 1);
  96                 break;
  97         case STATE(1, 0, 0, 0):
  98                 _ao_quadrature_queue(q, -1);
  99                 break;
 100         }
 101 }
 102
 103 static void
 104 ao_quadrature_isr(void)
 105 {
 106         uint8_t q;
 107         for (q = 0; q < AO_QUADRATURE_COUNT; q++)
 108                 _ao_debounce_start(&ao_quadrature_debounce[q]);
 109 }
 110
 111 int32_t
 112 ao_quadrature_poll(uint8_t q)
 113 {
 114         int32_t ret;
 115         ao_arch_critical(ret = ao_quadrature_count[q];);
 116         return ret;
 117 }
 118
 119 int32_t
 120 ao_quadrature_wait(uint8_t q)
 121 {
 122         ao_sleep(&ao_quadrature_count[q]);
 123         return ao_quadrature_poll(q);
 124 }
 125
 126 static void
 127 ao_quadrature_test(void)
 128 {
 129         uint8_t q;
 130
 131         ao_cmd_decimal();
 132         q = ao_cmd_lex_i;
 133         for (;;) {
 134                 int32_t c;
 135                 flush();
 136                 c = ao_quadrature_wait(q);
 137                 printf ("new count %6d\n", c);
 138                 if (c == 100)
 139                         break;
 140         }
 141 }
 142
 143 static const struct ao_cmds ao_quadrature_cmds[] = {
 144         { ao_quadrature_test,   "q <unit>\0Test quadrature" },
 145         { 0, NULL }
 146 };
 147
 148 static void
 149 ao_quadrature_debounce_init(struct ao_debounce *debounce) {
 150         debounce->hold = AO_QUADRATURE_DEBOUNCE_HOLD;
 151         debounce->_get = _ao_quadrature_get;
 152         debounce->_set = _ao_quadrature_set;
 153 }
 154
 155 #define init(q) do {                                                    \
 156                 ao_enable_port(port(q));                                \
 157                 ao_quadrature_debounce_init(&ao_quadrature_debounce[q]); \
 158                 ao_exti_setup(port(q), bita(q),                         \
 159                               AO_QUADRATURE_MODE|AO_EXTI_MODE_FALLING|AO_EXTI_MODE_RISING|AO_EXTI_PRIORITY_MED, \
 160                               ao_quadrature_isr);                       \
 161                 ao_exti_enable(port(q), bita(q));                       \
 162                                                                         \
 163                 ao_exti_setup(port(q), bitb(q),                         \
 164                               AO_QUADRATURE_MODE|AO_EXTI_MODE_FALLING|AO_EXTI_MODE_RISING|AO_EXTI_PRIORITY_MED, \
 165                               ao_quadrature_isr);                       \
 166                 ao_exti_enable(port(q), bitb(q));                       \
 167         } while (0)
 168
 169 void
 170 ao_quadrature_init(void)
 171 {
 172         ao_debounce_init();
 173 #if AO_QUADRATURE_COUNT > 0
 174         init(0);
 175 #endif
 176 #if AO_QUADRATURE_COUNT > 1
 177         init(1);
 178 #endif
 179         ao_cmd_register(&ao_quadrature_cmds[0]);
 180 }