- for (q = 0; q < AO_QUADRATURE_COUNT; q++) {
- switch (ao_quadrature_state[q]) {
- case STATE(0, 1, 0, 0):
- ao_quadrature_count[q]++;
- break;
- case STATE(1, 0, 0, 0):
- ao_quadrature_count[q]--;
- break;
- default:
- continue;
- }
- ao_quadrature_queue(q);
- 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;