rc filtering

include/at91_tc1.h

@@ -3,12 +3,16 @@
 
 #define MAX_CHANNELS	8
 
+#define RC_CAL_START	0
+#define RC_CAL_END	1
+
 struct rc_values {
-	int16_t ch[MAX_CHANNELS];
+	int16_t chan[MAX_CHANNELS];
-	uint32_t count;
 };
 
+uint32_t rcontrol_getvalues(struct rc_values *rc);
+void rcontrol_calibrate(uint32_t mode);
+
 void at91_tc1_init(void);
-uint32_t rc_ppm_getvalues(struct rc_values *rc);
 
 #endif /*RC_PPM_H_*/

main.c

@@ -32,37 +32,27 @@
 #include <stdio.h>
 #include <string.h>
 
-AT91S_AIC *aic = AT91C_BASE_AIC;
-AT91S_TWI *twi = AT91C_BASE_TWI;
-AT91S_UDP *udp = AT91C_BASE_UDP;
-
 static uint32_t pitc_test(struct pitc_timer *timer)
 {
 	static uint32_t i;
 	*AT91C_PIOA_SODR = i;
 	i = i ^ LED_GREEN;
 	*AT91C_PIOA_CODR = i;
-/*	
-	struct rc_values rc;
-	uint32_t valid = rc_ppm_getvalues(&rc);
-	if (!valid)
-		rc.count = 0;
 
-	printf("%d channels: ", rc.count);
+	struct rc_values rc;
+	uint32_t count = rcontrol_getvalues(&rc);
+
+	printf("%ld channels: ", count);
 	uint32_t j;
-	for (j = 0; j < rc.count; j++)
+	for (j = 0; j < count; j++)
-		printf("%+3d ", rc.ch[j]);
+		printf("%+4d ", rc.chan[j]);
 
 	printf("\n\r");
-*/
+	return PITC_RESTART_TIMER;
-	return 0;
 }
 
 int main(void)
 {
-	/* trigger pinchange-isrs */
-	at91_pio_init();
-
 	/* LED outputs */
 	*AT91C_PIOA_PER = LED_GREEN | LED_ORANGE;
 	*AT91C_PIOA_OER = LED_GREEN | LED_ORANGE;
@@ -73,6 +63,9 @@ int main(void)
 	at91_dbgu_init();
 	at91_dbgu_puts("==========================================================\n\rGood morning Dave\n\r");
 
+	/* triggers pinchange-isrs */
+	at91_pio_init();
+
 	/* timer */
 	at91_pitc_init();
 	at91_rttc_test_init();
@@ -87,7 +80,7 @@ int main(void)
 	/* usb */
 	at91_udp_init();
 
-	struct pitc_timer *pitc_test_timer = alloc_pitc_timer(10, &pitc_test, NULL);
+	struct pitc_timer *pitc_test_timer = alloc_pitc_timer(1, &pitc_test, NULL);
 	pitc_schedule_timer(pitc_test_timer);
 
 	printf("static alloc: %5ld bytes\n\r", static_alloc_used());

src/at91_tc1.c

@@ -18,70 +18,167 @@
  ***************************************************************************/
 #include <stdint.h>
 #include <stdio.h>
+#include <stdlib.h>		/* abs() */
 #include "AT91SAM7S256.h"
 #include "board.h"
 #include "at91_tc1.h"
+#include "at91_pio.h"
+
+/* Hard limits for ISR */
+#define PULSE_MIN	0x0500
+#define PULSE_MAX	0x0D00
+#define PULSE_TIMEOUT	0x0F00
+#define PULSE_CENTER	0x08C0
+
+/* moving average filters */
+#define PULSE_FILTER_FAST	(1<<2)
+#define PULSE_FILTER_SLOW	(1<<4)
+#define PULSE_FILTER_DIFF	16		/* point to switch filters */
+#define PULSE_MID_DIFF		50		/* minimum diff to center */
+
+#define VALUE_RANGE		256
 
 #define ROUND_DIV256(x) ((x >> 8) + ((x & 0x80) ? 1 : 0))
 
-static uint32_t ppm_arr[MAX_CHANNELS];
+struct channel_data {
-static uint32_t ch, cnt, valid;
+	uint16_t width;
+	uint16_t width_slow;
+	uint16_t filter;	/* 0 - fast filter, 1 - slow filter */
+	uint16_t min;		/* minimum value during calibration */
+	uint16_t mid;		/* center value */
+	uint16_t max;		/* maximum value */
+};
+
+static struct channel_data ch_data[MAX_CHANNELS];
+static uint32_t index, count, valid, cal_in_progress;
 
 static void ppm_isr(void)
 {
 	/* RC Compare -> no TIOA1 edge for 2.5ms */
 	uint32_t status = *AT91C_TC1_SR;
-	if (status & AT91C_TC_CPCS) {
 
+	if (status & AT91C_TC_CPCS) {
 		/* average channel count */
-		cnt = ((cnt * 7) + (ch << 8)) / 8;
+		count = ((count * 7) + (index << 8)) / 8;
 
 		/* at least 4 channels and a stable channel count */
-		valid = (ROUND_DIV256(cnt) == ch) && (ch >= 4);
+		if ((ROUND_DIV256(count) == index) && (index >= 4)) {
+			if (valid < 10)
+				valid++;
+
+		} else if (valid > 0) {
+			valid--;
+		}
 
 		/* reset index */
-		ch = 0;
+		index = 0;
 	}
 
 	/* edge on TIOA1 */
 	if (status & AT91C_TC_LDRAS) {
 		/* get impulse width */
-		uint16_t ra = *AT91C_TC1_RA;
+		uint16_t width = *AT91C_TC1_RA;
 
 		/* valid range: 1 - 2ms */
-		if (ra > 0x0500 && ra < 0x0D00) {
+		if (width > PULSE_MIN && width < PULSE_MAX) {
-			if (ch < ARRAY_SIZE(ppm_arr))
+			if (index < ARRAY_SIZE(ch_data)) {
-				ppm_arr[ch] = ((ppm_arr[ch] * 3) + ra) / 4;
+				/* calc both filters */
+				ch_data[index].width = ((ch_data[index].width * (PULSE_FILTER_FAST -1)) + width) / PULSE_FILTER_FAST;
+				ch_data[index].width_slow = ((ch_data[index].width_slow * (PULSE_FILTER_SLOW -1)) + width) / PULSE_FILTER_SLOW;
 
-			ch++;
+				if (cal_in_progress) {
+					/* use slow filter values, calc center */
+					ch_data[index].min = MIN(ch_data[index].width_slow, ch_data[index].min);
+					ch_data[index].max = MAX(ch_data[index].width_slow, ch_data[index].max);
+					ch_data[index].mid = (ch_data[index].min + ch_data[index].max) / 2;
+				}
+			}
+			index++;
 		}
 	}
 }
 
-/*
+uint32_t rcontrol_getvalues(struct rc_values *rc)
-struct cal_data {
-	uint16_t min;
-	uint16_t mid;
-	uint16_t max;
-};
-
-int16_t getval(struct cal_data *cal, uint16_t input)
 {
-	int32_t x = (input - cal->mid) * 1024;
+	if (valid < 5)
-	uint32_t y = (x > 0) ? (cal->max - cal->mid) : (cal->mid - cal->min);
+		return 0;
 
-	return x / y;
+	uint32_t i;
+	uint32_t cnt = MIN(ROUND_DIV256(count), ARRAY_SIZE(ch_data));
+	for (i = 0; i < cnt; i++) {
+		/* switch between fast and slow filter */
+		uint16_t filter = (abs(ch_data[i].width - ch_data[i].width_slow) < PULSE_FILTER_DIFF);
+
+		/*
+		 * transition fast -> slow filter
+		 * slow filter is lagging behind, so give it a boost
+		 */
+		if (filter && !ch_data[i].filter && !cal_in_progress)
+			ch_data[i].width_slow = ch_data[i].width;
+
+		ch_data[i].filter = filter;
+
+		uint16_t width = (filter) ? ch_data[i].width_slow : ch_data[i].width;
+
+		/* expand the value to +/- VALUE_RANGE */
+		int32_t tmp = (width - ch_data[i].mid) * VALUE_RANGE;
+		tmp = tmp / ((tmp > 0) ? (ch_data[i].max - ch_data[i].mid) : (ch_data[i].mid - ch_data[i].min));
+
+		/* keep result in range */
+		if (tmp > VALUE_RANGE)
+			tmp = VALUE_RANGE;
+
+		if (tmp < -VALUE_RANGE)
+			tmp = -VALUE_RANGE;
+
+		rc->chan[i] = tmp;
+	}
+	return cnt;
 }
-*/
 
-uint32_t rc_ppm_getvalues(struct rc_values *rc)
+void rcontrol_calibrate(uint32_t mode)
 {
 	uint32_t i;
-	rc->count = MIN(ROUND_DIV256(cnt), ARRAY_SIZE(ppm_arr));
-	for (i = 0; i < rc->count; i++)
-		rc->ch[i] = (ppm_arr[i] - 0x8C0);
 
-	return valid;
+	switch (mode) {
+	case RC_CAL_START:
+		cal_in_progress = 1;
+		for (i = 0; i < ARRAY_SIZE(ch_data); i++) {
+			/* use hard limits as hint */
+			ch_data[i].max = PULSE_MIN;
+			ch_data[i].mid = (PULSE_MIN + PULSE_MAX) / 2;
+			ch_data[i].min = PULSE_MAX;
+		}
+		break;
+
+	case RC_CAL_END:
+		cal_in_progress = 0;
+		for (i = 0; i < ARRAY_SIZE(ch_data); i++) {
+			/* treat current position as center */
+			ch_data[i].mid = ch_data[i].width_slow;
+
+			/* if center is near minimum, clamp output to 0..+1024 */
+			if (ch_data[i].mid - ch_data[i].min < PULSE_MID_DIFF)
+				ch_data[i].mid = ch_data[i].min;
+
+			/* if center is near maximum, clamp output to -1024..0 */
+			if (ch_data[i].max - ch_data[i].mid < PULSE_MID_DIFF)
+				ch_data[i].mid = ch_data[i].max;
+		}
+		break;
+	}
+}
+
+void rcontrol_print_cal(void)
+{
+	uint32_t i;
+	for (i = 0; i < ARRAY_SIZE(ch_data); i++) {
+		printf("%ld: %d(%+d) - %d(0) - %d(%+d)\n\r", i,
+			ch_data[i].min, ch_data[i].min - ch_data[i].mid,
+			ch_data[i].mid,
+			ch_data[i].max, ch_data[i].max - ch_data[i].mid
+			);
+	}
 }
 
 void at91_tc1_init(void)
@@ -97,7 +194,7 @@ void at91_tc1_init(void)
 	*AT91C_TC1_IER = AT91C_TC_LDRAS | AT91C_TC_CPCS;
 
 	/* RC Compare Interrupt if no rising Edge on TIOA1 for 2.56ms */
-	*AT91C_TC1_RC = 0x0F00;
+	*AT91C_TC1_RC = PULSE_TIMEOUT;
 
 	/* enable & trigger the clock */
 	*AT91C_TC1_CCR = AT91C_TC_CLKEN | AT91C_TC_SWTRG;
@@ -108,3 +205,21 @@ void at91_tc1_init(void)
 	aic->AIC_SVR[AT91C_ID_TC1] = (uint32_t)ppm_isr;
 	aic->AIC_IECR = (1 << AT91C_ID_TC1);
 }
+
+static void tast_monitor(uint32_t status, uint32_t input)
+{
+	if (!(input & TAST1) && (cal_in_progress == 0)) {
+		printf("start calibration\n\r");
+
+		rcontrol_calibrate(RC_CAL_START);
+
+
+	} else if (!(input & TAST2) && (cal_in_progress == 1)) {
+		printf("end calibration\n\r");
+
+		rcontrol_calibrate(RC_CAL_END);
+		rcontrol_print_cal();
+	}
+}
+
+PIO_PINCHANGE_ISR(TAST1 | TAST2, tast_monitor);

src/at91_udp.c

@@ -428,8 +428,6 @@ static void udp_isr(void)
 	uint32_t isr = *AT91C_UDP_ISR;
 
 	if (isr & AT91C_UDP_ENDBUSRES) {
-		printf("usb reset\n\r");
-
 		AT91S_UDP *udp = AT91C_BASE_UDP;
 
 		/* reset all endpoints */