/*************************************************************************** * sam7fc - ADC routines / calibration * * * * 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 #include #include "AT91SAM7S256.h" #include "board.h" #include "at91_adc.h" #include "at91_pitc.h" #include "at91_twi.h" #include "telemetrie.h" static uint32_t adc_status; static uint16_t adc_tmp[7]; static uint16_t adc_offset[6]; TDC_PTR(adc_offset0, &adc_offset[ADC_GYRO_NICK], "XADC_GYRO_NICK (offset)", int16_t, TDC_UNSIGNED); TDC_PTR(adc_offset1, &adc_offset[ADC_GYRO_ROLL], "XADC_GYRO_ROLL (offset)", int16_t, TDC_UNSIGNED); TDC_PTR(adc_offset2, &adc_offset[ADC_GYRO_GIER], "XADC_GYRO_GIER (offset)", int16_t, TDC_UNSIGNED); TDC_PTR(adc_offset3, &adc_offset[ADC_ACC_NICK], "XADC_ACC_NICK (offset)", int16_t, TDC_UNSIGNED); TDC_PTR(adc_offset4, &adc_offset[ADC_ACC_ROLL], "XADC_ACC_ROLL (offset)", int16_t, TDC_UNSIGNED); TDC_PTR(adc_offset5, &adc_offset[ADC_ACC_GIER], "XADC_ACC_GIER (offset)", int16_t, TDC_UNSIGNED); #define ADC_CAL_COUNT_MAX 1024 static uint32_t adc_cal_count; static uint32_t adc_cal_data[3]; /* check eeprom parameter size (3x uint16_t) */ #if ((3 * 2) != EE_ACC_CAL_DATA_SIZE) #error "invalid EE_ACC_CAL_DATA_SIZE" #endif static void at91_adc_isr(void) { AT91S_PDC *pdc = AT91C_BASE_PDC_ADC; pdc->PDC_RPR = (uint32_t) &adc_tmp; pdc->PDC_RCR = ARRAY_SIZE(adc_tmp); pdc->PDC_PTCR = AT91C_PDC_RXTEN; /* clear interrupts */ uint32_t dummy = *AT91C_ADC_SR; dummy = dummy; if (!(adc_status & (ADC_CAL_GYRO | ADC_CAL_ACC))) return; if (adc_status & ADC_CAL_GYRO) { adc_cal_data[0] += adc_tmp[ADC_GYRO_NICK]; adc_cal_data[1] += adc_tmp[ADC_GYRO_ROLL]; adc_cal_data[2] += adc_tmp[ADC_GYRO_GIER]; } else { adc_cal_data[0] += adc_tmp[ADC_ACC_NICK]; adc_cal_data[1] += adc_tmp[ADC_ACC_ROLL]; adc_cal_data[2] += adc_tmp[ADC_ACC_GIER]; } adc_cal_count++; } static uint32_t adc_calibrate_cb(struct pitc_timer *timer) { if (adc_cal_count < ADC_CAL_COUNT_MAX) { /* trigger next cycle */ *AT91C_ADC_CR = AT91C_ADC_START; return PITC_RESTART_TIMER; } else { if (adc_status & ADC_CAL_GYRO) { adc_offset[ADC_GYRO_NICK] = adc_cal_data[0] / ADC_CAL_COUNT_MAX; adc_offset[ADC_GYRO_ROLL] = adc_cal_data[1] / ADC_CAL_COUNT_MAX; adc_offset[ADC_GYRO_GIER] = adc_cal_data[2] / ADC_CAL_COUNT_MAX; adc_calibrate(ADC_CAL_GYRO_COMPLETE); } else { adc_offset[ADC_ACC_NICK] = adc_cal_data[0] / ADC_CAL_COUNT_MAX; adc_offset[ADC_ACC_ROLL] = adc_cal_data[1] / ADC_CAL_COUNT_MAX; adc_offset[ADC_ACC_GIER] = adc_cal_data[2] / ADC_CAL_COUNT_MAX; adc_calibrate(ADC_CAL_ACC_COMPLETE); } return PITC_REMOVE_TIMER; } } static struct pitc_timer adc_cal_timer = { .interval = 1, .func = &adc_calibrate_cb, }; void adc_trigger(void) { // TODO: err in retvalue? if (!(adc_status & (ADC_CAL_GYRO | ADC_CAL_ACC))) *AT91C_ADC_CR = AT91C_ADC_START; } void adc_get_results(int16_t *adc_result) { // TODO: err in retvalue? if (!(adc_status & (ADC_CAL_GYRO | ADC_CAL_ACC))) { uint32_t i; for (i = ADC_GYRO_NICK; i <= ADC_GYRO_GIER; i++) adc_result[i] = (int16_t)(adc_offset[i]) - (int16_t)(adc_tmp[i]); for (i = ADC_ACC_NICK; i <= ADC_ACC_GIER; i++) adc_result[i] = (int16_t)(adc_tmp[i]) - (int16_t)(adc_offset[i]); /* (adc / 1024) * 3.3V * (11k / 1k) * 100 */ adc_result[ADC_VOLTAGE] = ((uint32_t)adc_tmp[ADC_VOLTAGE] * 3630) / 1024; } } void adc_calibrate(uint32_t mode) { /* disable interrupt */ *AT91C_ADC_IDR = AT91C_ADC_ENDRX; switch (mode) { case ADC_CAL_GYRO_COMPLETE: adc_status &= ~ADC_CAL_GYRO; break; case ADC_CAL_ACC_COMPLETE: twi_write_eeprom(EE_ACC_CAL_DATA, (uint8_t *)&(adc_offset[ADC_ACC_NICK]), EE_ACC_CAL_DATA_SIZE); adc_status &= ~ADC_CAL_ACC; break; case ADC_CAL_ACC_LOAD: twi_read_eeprom(EE_ACC_CAL_DATA, (uint8_t *)&(adc_offset[ADC_ACC_NICK]), EE_ACC_CAL_DATA_SIZE); break; case ADC_CAL_GYRO: case ADC_CAL_ACC: /* calibration in progress.. */ if (adc_status & (ADC_CAL_GYRO | ADC_CAL_ACC)) break; adc_status |= mode; adc_cal_count = 0; adc_cal_data[0] = 0; adc_cal_data[1] = 0; adc_cal_data[2] = 0; pitc_schedule_timer(&adc_cal_timer); /* trigger next cycle */ *AT91C_ADC_CR = AT91C_ADC_START; break; } if (!(adc_status & (ADC_CAL_GYRO | ADC_CAL_ACC))) { printf("ADC offsets: %d/%d/%d %d/%d/%d\n\r", adc_offset[ADC_GYRO_NICK], adc_offset[ADC_GYRO_ROLL], adc_offset[ADC_GYRO_GIER], adc_offset[ADC_ACC_NICK], adc_offset[ADC_ACC_ROLL], adc_offset[ADC_ACC_GIER]); } /* enable interrupt */ *AT91C_ADC_IER = AT91C_ADC_ENDRX; } void adc_drift_adjust(int16_t nick, int16_t roll, int16_t yaw) { adc_offset[ADC_GYRO_NICK] += nick; adc_offset[ADC_GYRO_ROLL] += roll; adc_offset[ADC_GYRO_GIER] += yaw; } void at91_adc_init(void) { /* enable ADC clock */ *AT91C_PMC_PCER = (1 << AT91C_ID_ADC); /* ADC Software reset */ AT91S_ADC *adc = AT91C_BASE_ADC; adc->ADC_CR = AT91C_ADC_SWRST; /* * ADC config: 10bit, no sleep * 4.8MHz (48MHz / ((4 +1) * 2) = 4.8MHz) * 96 cycles Startup ((11 +1) * 8 / 4.8MHz = 20us) * 3 cycles SH ((2 +1) / 4.8MHz = 625ns) * Conversion time per channel @5MHz ~2us */ adc->ADC_MR = AT91C_ADC_TRGEN_DIS | AT91C_ADC_LOWRES_10_BIT | AT91C_ADC_SLEEP_NORMAL_MODE | (AT91C_ADC_PRESCAL & (4 << 8)) | (AT91C_ADC_STARTUP & (11 << 16)) | (AT91C_ADC_SHTIM & (2 << 24)); /* setup PDC */ AT91S_PDC *pdc = AT91C_BASE_PDC_ADC; pdc->PDC_RPR = (uint32_t) &adc_tmp; pdc->PDC_RCR = ARRAY_SIZE(adc_tmp); pdc->PDC_PTCR = AT91C_PDC_RXTEN; /* enable 7 channels (0-1-2-4-5-6-7), PDC Interrupt */ adc->ADC_CHER = 0xF7; adc->ADC_IER = AT91C_ADC_ENDRX; /* low priority, level triggered, own vector */ AT91S_AIC *aic = AT91C_BASE_AIC; 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<