|
|
@ -23,8 +23,7 @@ |
|
|
|
#include "board.h" |
|
|
|
#include "at91_adc.h" |
|
|
|
#include "at91_pitc.h" |
|
|
|
|
|
|
|
#include "fixed.h" |
|
|
|
#include "at91_twi.h" |
|
|
|
|
|
|
|
#define ADC_CAL_COUNT 1024 |
|
|
|
|
|
|
@ -32,6 +31,11 @@ static uint16_t adc_tmp[7]; |
|
|
|
static int16_t adc_result[7]; |
|
|
|
static uint16_t adc_offset[6]; |
|
|
|
|
|
|
|
/* check eeprom parameter size (3x uint16_t) */ |
|
|
|
#if ((3 * 2) != EE_ACC_CAL_DATA_SIZE) |
|
|
|
#error "invalid EE_ACC_CAL_DATA_SIZE" |
|
|
|
#endif |
|
|
|
|
|
|
|
static uint32_t adc_cal_count; |
|
|
|
volatile static uint32_t adc_cal_mode; |
|
|
|
static uint32_t adc_cal_data[3]; |
|
|
@ -82,7 +86,7 @@ static uint32_t adc_calibrate_cb(struct pitc_timer *timer) |
|
|
|
adc_offset[ADC_GYRO_NICK] = adc_cal_data[1]; |
|
|
|
adc_offset[ADC_GYRO_YAW] = adc_cal_data[2]; |
|
|
|
|
|
|
|
// TODO: check for invalid values (not centered) -> return value? |
|
|
|
// TODO: check for invalid values (not centered) |
|
|
|
|
|
|
|
printf("GYRO offsets: %4d/%4d/%4d\n\r", |
|
|
|
adc_offset[ADC_GYRO_ROLL], |
|
|
@ -96,11 +100,12 @@ static uint32_t adc_calibrate_cb(struct pitc_timer *timer) |
|
|
|
// TODO: only 1/2 offset? |
|
|
|
adc_offset[ADC_ACC_YAW] = adc_cal_data[2]; |
|
|
|
|
|
|
|
// TODO: save to eeprom |
|
|
|
printf("ACC offsets: %4d/%4d/%4d\n\r", |
|
|
|
adc_offset[ADC_ACC_ROLL], |
|
|
|
adc_offset[ADC_ACC_NICK], |
|
|
|
adc_offset[ADC_ACC_YAW]); |
|
|
|
|
|
|
|
twi_write_eeprom(EE_ACC_CAL_DATA, (uint8_t *)&(adc_offset[ADC_ACC_ROLL]), EE_ACC_CAL_DATA_SIZE); |
|
|
|
} |
|
|
|
|
|
|
|
adc_cal_mode = ADC_CAL_NONE; |
|
|
@ -146,8 +151,6 @@ void at91_adc_init(void) |
|
|
|
/* enable ADC clock */ |
|
|
|
*AT91C_PMC_PCER = (1 << AT91C_ID_ADC); |
|
|
|
|
|
|
|
// TODO: load calibration data from eeprom / use default values |
|
|
|
|
|
|
|
/* ADC Software reset */ |
|
|
|
AT91S_ADC *adc = AT91C_BASE_ADC; |
|
|
|
adc->ADC_CR = AT91C_ADC_SWRST; |
|
|
@ -181,4 +184,11 @@ void at91_adc_init(void) |
|
|
|
aic->AIC_SMR[AT91C_ID_ADC] = IRQPRIO_ADC | AT91C_AIC_SRCTYPE_INT_HIGH_LEVEL; |
|
|
|
aic->AIC_SVR[AT91C_ID_ADC] = (uint32_t)at91_adc_isr; |
|
|
|
aic->AIC_IECR = (1<<AT91C_ID_ADC); |
|
|
|
|
|
|
|
/* load ACC calibration data */ |
|
|
|
twi_read_eeprom(EE_ACC_CAL_DATA, (uint8_t *)&(adc_offset[ADC_ACC_ROLL]), EE_ACC_CAL_DATA_SIZE); |
|
|
|
printf("ACC offsets: %4d/%4d/%4d\n\r", |
|
|
|
adc_offset[ADC_ACC_ROLL], |
|
|
|
adc_offset[ADC_ACC_NICK], |
|
|
|
adc_offset[ADC_ACC_YAW]); |
|
|
|
} |