rc filtering
This commit is contained in:
parent
508ab7c94b
commit
26b594582f
@ -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_*/
|
||||
|
||||
@ -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))
|
||||
|
||||
39
main.c
39
main.c
@ -32,47 +32,40 @@
|
||||
#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);
|
||||
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);
|
||||
}
|
||||
|
||||
173
src/at91_tc1.c
173
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];
|
||||
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);
|
||||
|
||||
@ -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 */
|
||||
|
||||
Loading…
Reference in New Issue
Block a user