262 lines
7.5 KiB
C
262 lines
7.5 KiB
C
/***************************************************************************
|
|
* Copyright (C) 01/2008 by Olaf Rempel *
|
|
* razzor@kopf-tisch.de *
|
|
* *
|
|
* 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 *
|
|
* *
|
|
* This program is distributed in the hope that it will be useful, *
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
|
|
* GNU General Public License for more details. *
|
|
* *
|
|
* You should have received a copy of the GNU General Public License *
|
|
* along with this program; if not, write to the *
|
|
* Free Software Foundation, Inc., *
|
|
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. *
|
|
***************************************************************************/
|
|
#include <stdint.h>
|
|
#include <stdio.h>
|
|
#include <stdlib.h> /* abs() */
|
|
#include "AT91SAM7S256.h"
|
|
#include "board.h"
|
|
#include "at91_tc1.h"
|
|
#include "at91_twi.h"
|
|
|
|
/* Hard limits for ISR */
|
|
#define PULSE_MIN 0x0500
|
|
#define PULSE_MAX 0x0D00
|
|
#define PULSE_TIMEOUT 0x0F00
|
|
|
|
#define PULSE_CENTER 0x08C0
|
|
#define PULSE_SWITCH 0x01F0
|
|
|
|
/* 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))
|
|
|
|
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 */
|
|
};
|
|
|
|
/* check eeprom parameter size (uint16_t min/mid/max) */
|
|
#if ((MAX_CHANNELS * 3 * 2) != EE_RC_CAL_DATA_SIZE)
|
|
#error "invalid EE_RC_CAL_DATA_SIZE"
|
|
#endif
|
|
|
|
static struct channel_data ch_data[MAX_CHANNELS];
|
|
static uint32_t count, valid, cal_in_progress;
|
|
|
|
static void ppm_isr(void)
|
|
{
|
|
static uint32_t i;
|
|
|
|
/* RC Compare -> no TIOA1 edge for 2.5ms */
|
|
uint32_t status = *AT91C_TC1_SR;
|
|
|
|
if (status & AT91C_TC_CPCS) {
|
|
/* average channel count */
|
|
count = ((count * 7) + (i << 8)) / 8;
|
|
|
|
/* at least 4 channels and a stable channel count */
|
|
if ((ROUND_DIV256(count) == i) && (i >= 4)) {
|
|
if (valid < 10)
|
|
valid++;
|
|
|
|
} else if (valid > 0) {
|
|
valid--;
|
|
}
|
|
|
|
/* reset index */
|
|
i = 0;
|
|
}
|
|
|
|
/* edge on TIOA1 */
|
|
if (status & AT91C_TC_LDRAS) {
|
|
/* get impulse width */
|
|
uint16_t width = *AT91C_TC1_RA;
|
|
|
|
/* valid range: 1 - 2ms */
|
|
if (width > PULSE_MIN && width < PULSE_MAX) {
|
|
if (i < ARRAY_SIZE(ch_data)) {
|
|
/* calc both filters */
|
|
ch_data[i].width = ((ch_data[i].width * (PULSE_FILTER_FAST -1)) + width) / PULSE_FILTER_FAST;
|
|
ch_data[i].width_slow = ((ch_data[i].width_slow * (PULSE_FILTER_SLOW -1)) + width) / PULSE_FILTER_SLOW;
|
|
|
|
if (cal_in_progress) {
|
|
/* use slow filter values, calc center */
|
|
ch_data[i].min = MIN(ch_data[i].width_slow, ch_data[i].min);
|
|
ch_data[i].max = MAX(ch_data[i].width_slow, ch_data[i].max);
|
|
ch_data[i].mid = (ch_data[i].min + ch_data[i].max) / 2;
|
|
}
|
|
}
|
|
i++;
|
|
}
|
|
}
|
|
}
|
|
|
|
uint32_t rcontrol_getvalues(struct rc_values *rc)
|
|
{
|
|
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;
|
|
|
|
uint16_t width = (filter) ? ch_data[i].width_slow : ch_data[i].width;
|
|
|
|
/* expand the value to +/- VALUE_RANGE */
|
|
int32_t tmp = (uint32_t)(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;
|
|
|
|
// TODO: stick mapping
|
|
rc->chan[i] = tmp;
|
|
}
|
|
return cnt;
|
|
}
|
|
|
|
uint32_t rcontrol_getswitches(struct rc_values *rc)
|
|
{
|
|
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++) {
|
|
if (ch_data[i].width > (PULSE_CENTER + PULSE_SWITCH))
|
|
rc->chan[i] = VALUE_RANGE;
|
|
|
|
else if (ch_data[i].width < (PULSE_CENTER - PULSE_SWITCH))
|
|
rc->chan[i] = -VALUE_RANGE;
|
|
|
|
else
|
|
rc->chan[i] = 0;
|
|
}
|
|
return cnt;
|
|
}
|
|
|
|
void rcontrol_calibrate(uint32_t mode)
|
|
{
|
|
uint32_t i;
|
|
uint8_t buf[EE_RC_CAL_DATA_SIZE];
|
|
uint16_t *ptr = (uint16_t *)buf;
|
|
|
|
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;
|
|
|
|
case RC_CAL_LOAD:
|
|
twi_read_eeprom(EE_RC_CAL_DATA, buf, EE_RC_CAL_DATA_SIZE);
|
|
for (i = 0; i < ARRAY_SIZE(ch_data); i++) {
|
|
ch_data[i].min = *ptr++;
|
|
ch_data[i].mid = *ptr++;
|
|
ch_data[i].min = *ptr++;
|
|
}
|
|
break;
|
|
|
|
case RC_CAL_SAVE:
|
|
for (i = 0; i < ARRAY_SIZE(ch_data); i++) {
|
|
*ptr++ = ch_data[i].min;
|
|
*ptr++ = ch_data[i].mid;
|
|
*ptr++ = ch_data[i].min;
|
|
}
|
|
twi_write_eeprom(EE_RC_CAL_DATA, buf, EE_RC_CAL_DATA_SIZE);
|
|
break;
|
|
}
|
|
}
|
|
|
|
void rcontrol_print_cal(void)
|
|
{
|
|
uint32_t i;
|
|
printf("stick-calibration:\n\r");
|
|
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)
|
|
{
|
|
/* enable TC1 clock */
|
|
*AT91C_PMC_PCER = (1 << AT91C_ID_TC1);
|
|
|
|
/* MCK /32, trigger & capture on falling TIOA1 edge */
|
|
*AT91C_TC1_CMR = AT91C_TC_CLKS_TIMER_DIV3_CLOCK | AT91C_TC_LDRA_FALLING |
|
|
AT91C_TC_ETRGEDG_FALLING | AT91C_TC_ABETRG;
|
|
|
|
/* enable RA load and RC compare interrupt */
|
|
*AT91C_TC1_IER = AT91C_TC_LDRAS | AT91C_TC_CPCS;
|
|
|
|
/* RC Compare Interrupt if no rising Edge on TIOA1 for 2.56ms */
|
|
*AT91C_TC1_RC = PULSE_TIMEOUT;
|
|
|
|
/* enable & trigger the clock */
|
|
*AT91C_TC1_CCR = AT91C_TC_CLKEN | AT91C_TC_SWTRG;
|
|
|
|
/* level triggered, own vector */
|
|
AT91S_AIC *aic = AT91C_BASE_AIC;
|
|
aic->AIC_SMR[AT91C_ID_TC1] = IRQPRIO_TC1 | AT91C_AIC_SRCTYPE_INT_HIGH_LEVEL;
|
|
aic->AIC_SVR[AT91C_ID_TC1] = (uint32_t)ppm_isr;
|
|
aic->AIC_IECR = (1 << AT91C_ID_TC1);
|
|
|
|
rcontrol_calibrate(RC_CAL_LOAD);
|
|
rcontrol_print_cal();
|
|
}
|