|
|
@ -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; |
|
|
|
|
|
|
|
ch++; |
|
|
|
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; |
|
|
|
|
|
|
|
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; |
|
|
|
|
|
|
|
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; |
|
|
|
|
|
|
|
return x / y; |
|
|
|
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); |