X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=src%2Fdrivers%2Fao_quadrature.c;h=20781c407374b9f5a5d41947ba8ea66774951e4d;hp=66a77dfaa997b22824805cbbdc4bda9c63b5618d;hb=c6e57291d91f1f6c4de5c54a5cfd3eef66d9f830;hpb=8d9c79f5c162e07d57d42c6ba5825a3327a911d5

diff --git a/src/drivers/ao_quadrature.c b/src/drivers/ao_quadrature.c
index 66a77dfa..20781c40 100644
--- a/src/drivers/ao_quadrature.c
+++ b/src/drivers/ao_quadrature.c
@@ -3,7 +3,8 @@
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; version 2 of the License.
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
  *
  * This program is distributed in the hope that it will be useful, but
  * WITHOUT ANY WARRANTY; without even the implied warranty of
@@ -21,9 +22,20 @@
 #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 {
+	uint8_t	state;
+	uint8_t	count;
+};
+
+static struct ao_debounce ao_debounce_state[AO_QUADRATURE_COUNT][2];
+
 #define port(q)	AO_QUADRATURE_ ## q ## _PORT
 #define bita(q) AO_QUADRATURE_ ## q ## _A
 #define bitb(q) AO_QUADRATURE_ ## q ## _B
@@ -31,18 +43,57 @@ static uint8_t  ao_quadrature_state[AO_QUADRATURE_COUNT];
 #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);
+#ifndef AO_QUADRATURE_DEBOUNCE
+#error must define AO_QUADRATURE_DEBOUNCE
+#endif
 
-	return ~((((v >> pin_a) & 1) | (((v >> pin_b) & 1) << 1))) & 3;
+static uint8_t
+ao_debounce(uint8_t cur, struct ao_debounce *debounce)
+{
+#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
 }
 
-#define _ao_quadrature_get(q)	ao_quadrature_read(port(q), bita(q), bitb(q))
+static uint16_t
+ao_quadrature_read(struct stm_gpio *gpio, uint8_t pin_a, uint8_t pin_b, struct ao_debounce debounce_state[2]) {
+	uint16_t	v = ~stm_gpio_get_all(gpio);
+	uint8_t		a = (v >> pin_a) & 1;
+	uint8_t		b = (v >> pin_b) & 1;
+
+	a = ao_debounce(a, &debounce_state[0]);
+	b = ao_debounce(b, &debounce_state[1]);
+
+	return a | (b << 1);
+}
+
+#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);
@@ -50,18 +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
@@ -75,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)
 {
@@ -96,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();
 		}
@@ -132,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