sam7fc/src/at91_adc.c

/***************************************************************************
 *   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 <stdint.h>
#include <stdio.h>
#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<<AT91C_ID_ADC);

	adc_calibrate(ADC_CAL_ACC_LOAD);
	adc_calibrate(ADC_CAL_GYRO);
}