From 26b594582f212fa4b89a78dc77c4cef66f7bc3c9 Mon Sep 17 00:00:00 2001 From: Olaf Rempel Date: Mon, 4 Feb 2008 18:19:25 +0100 Subject: [PATCH] rc filtering --- include/at91_tc1.h | 10 ++- include/board.h | 2 +- main.c | 39 +++++----- src/at91_tc1.c | 173 +++++++++++++++++++++++++++++++++++++-------- src/at91_udp.c | 2 - 5 files changed, 168 insertions(+), 58 deletions(-) diff --git a/include/at91_tc1.h b/include/at91_tc1.h index ed5388c..8f0a6f2 100644 --- a/include/at91_tc1.h +++ b/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]; - uint32_t count; + int16_t chan[MAX_CHANNELS]; }; +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_*/ diff --git a/include/board.h b/include/board.h index 2a51cc4..2733555 100644 --- a/include/board.h +++ b/include/board.h @@ -33,7 +33,7 @@ #define IRQPRIO_ADC 2 #define IRQPRIO_TWI 7 /* twi must be fast! */ #define IRQPRIO_UDP 4 /* usb */ -#define IRQPRIO_TC1 5 /* ppm capturing */ +#define IRQPRIO_TC1 5 /* ppm capturing */ /* TODO: find better place */ #define ARRAY_SIZE(x) (sizeof(x) / sizeof(*x)) diff --git a/main.c b/main.c index 23ab055..a287fb1 100644 --- a/main.c +++ b/main.c @@ -32,47 +32,40 @@ #include #include -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); + uint32_t count = rcontrol_getvalues(&rc); + + printf("%ld channels: ", count); uint32_t j; - for (j = 0; j < rc.count; j++) - printf("%+3d ", rc.ch[j]); - + for (j = 0; j < count; j++) + printf("%+4d ", rc.chan[j]); + printf("\n\r"); -*/ - return 0; + return PITC_RESTART_TIMER; } 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; /* needed for dbgu */ at91_sysc_init(); - + 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(); @@ -83,16 +76,16 @@ int main(void) /* twi */ at91_twi_init(); - + /* 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()); at91_twi_test(); - + while (1); } diff --git a/src/at91_tc1.c b/src/at91_tc1.c index bb50109..7ec5f62 100644 --- a/src/at91_tc1.c +++ b/src/at91_tc1.c @@ -18,70 +18,167 @@ ***************************************************************************/ #include #include +#include /* 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]; -static uint32_t ch, cnt, valid; +struct channel_data { + 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 (ch < ARRAY_SIZE(ppm_arr)) - ppm_arr[ch] = ((ppm_arr[ch] * 3) + ra) / 4; + if (width > PULSE_MIN && width < PULSE_MAX) { + if (index < ARRAY_SIZE(ch_data)) { + /* 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++; } } } -/* -struct cal_data { - uint16_t min; - uint16_t mid; - uint16_t max; -}; - -int16_t getval(struct cal_data *cal, uint16_t input) +uint32_t rcontrol_getvalues(struct rc_values *rc) { - int32_t x = (input - cal->mid) * 1024; - uint32_t y = (x > 0) ? (cal->max - cal->mid) : (cal->mid - cal->min); + if (valid < 5) + 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); diff --git a/src/at91_udp.c b/src/at91_udp.c index 6bdfb9b..5f89201 100644 --- a/src/at91_udp.c +++ b/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 */