adc & rcontrol work

include/at91_adc.h

@@ -0,0 +1,22 @@
+#ifndef AT91_ADC_H_
+#define AT91_ADC_H_
+
+#include <stdint.h>
+
+#define ADC_GYRO_ROLL	0
+#define ADC_GYRO_NICK	1
+#define ADC_GYRO_YAW	2
+#define ADC_ACC_ROLL	3
+#define ADC_ACC_NICK	4
+#define ADC_ACC_YAW	5
+#define ADC_VOLTAGE	6
+
+#define ADC_CAL_GYRO	1
+#define ADC_CAL_ACC	2
+
+void adc_trigger(void);
+
+void adc_calibrate(uint32_t mode);
+void at91_adc_init(void);
+
+#endif /*AT91_ADC_H_*/

include/at91_tc1.h

@@ -12,6 +12,7 @@ struct rc_values {
 
 uint32_t rcontrol_getvalues(struct rc_values *rc);
 void rcontrol_calibrate(uint32_t mode);
+void rcontrol_print_cal(void);
 
 void at91_tc1_init(void);
 

include/at91_tests.h

@@ -3,7 +3,4 @@
 
 void at91_rttc_test_init(void);
 
-void at91_adc_test_init(void);
-void at91_adc_printresults(void);
-
 #endif /*AT91_TESTS_H_*/

main.c

@@ -20,6 +20,7 @@
 #include "at91_sysc.h"
 #include "at91_dbgu.h"
 #include "at91_pitc.h"
+#include "at91_adc.h"
 #include "at91_tests.h"
 #include "at91_udp.h"
 #include "at91_pio.h"
@@ -48,16 +49,27 @@ static uint32_t pitc_test(struct pitc_timer *timer)
 		printf("%+5d ", rc.chan[j]);
 
 	printf("\r");
-*/
 
+	static uint32_t cnt;
+
+	if (cnt++ == 300) {
+		rcontrol_calibrate(RC_CAL_END);
+		printf("\n\r");
+		rcontrol_print_cal();
+	}
+*/
+/*
 	static uint8_t data[4] = { 0x00, 0x00, 0x00, 0x00 };
 	twi_setpwm(data);
+*/
+
+	adc_trigger();
 
 	return PITC_RESTART_TIMER;
 }
 
 static struct pitc_timer pitc_test_timer = {
-	.interval = 10,
+	.interval = 20,
 	.func = &pitc_test,
 };
 
@@ -83,7 +95,7 @@ int main(void)
 
 	/* twi */
 	at91_twi_init();
-//	at91_twi_test();
+	at91_twi_test();
 
 	/* usb */
 	at91_udp_init();
@@ -91,30 +103,10 @@ int main(void)
 	printf("static alloc: %5ld bytes\n\r", static_alloc_used());
 
 	/* adc, need timer */
-//	at91_adc_test_init();
+	at91_adc_init();
 
-	uint8_t buf[0x80];
+	pitc_schedule_timer(&pitc_test_timer);
-	memset(buf, 0xAA, sizeof(buf));
-
-	uint32_t ret = twi_read_eeprom(0x1234, buf, sizeof(buf));
-	printf("read:%ld\n\r", ret);
-
-	uint32_t i;
-	for (i = 0; i < sizeof(buf); i += 16) {
-		uint32_t j;
-		for (j = 0; j < 16; j++)
-			printf("0x%02x ", buf[i +j]);
-
-		printf("\n\r");
-	}
-
-	for (i = 0; i < sizeof(buf); i++)
-		buf[i] ^= 0xFF;
-
-	ret = twi_write_eeprom(0x1234, buf, sizeof(buf));
-	printf("write:%ld\n\r", ret);
-
-//	pitc_schedule_timer(&pitc_test_timer);
 
+//	rcontrol_calibrate(RC_CAL_START);
 	while (1);
 }

src/at91_adc.c

@@ -21,45 +21,132 @@
 
 #include "AT91SAM7S256.h"
 #include "board.h"
+#include "at91_adc.h"
 #include "at91_pitc.h"
 
-static uint16_t adc_result[7];
+#define ADC_CAL_COUNT	1024
+
+static uint16_t adc_tmp[7];
+static int16_t adc_result[7];
+static uint16_t adc_offset[6];
+
+static uint32_t adc_cal_count;
+volatile static uint32_t adc_cal_mode;
+static uint32_t adc_cal_data[3];
 
 static void at91_adc_isr(void)
 {
 	AT91S_PDC *pdc = AT91C_BASE_PDC_ADC;
-	pdc->PDC_RPR = (uint32_t) &adc_result;
+	pdc->PDC_RPR = (uint32_t) &adc_tmp;
-	pdc->PDC_RCR = ARRAY_SIZE(adc_result);
+	pdc->PDC_RCR = ARRAY_SIZE(adc_tmp);
 	pdc->PDC_PTCR = AT91C_PDC_RXTEN;
 
 	/* clear interrupts */
-	AT91S_ADC *adc = AT91C_BASE_ADC;
+	uint32_t status = *AT91C_ADC_SR;
-	uint32_t status = adc->ADC_SR;
 	status = status;
+
+	uint32_t i;
+	for (i = 0; i < ARRAY_SIZE(adc_offset); i++)
+		adc_result[i] = adc_tmp[i] - adc_offset[i];
+
+	/* (adc / 1024) * 3.3V * (11k / 1k) * 100 */
+	adc_result[ADC_VOLTAGE] = ((uint32_t)adc_tmp[ADC_VOLTAGE] * 3630) / 1024;
 }
 
-static uint32_t adc_trigger(struct pitc_timer *timer)
+static uint32_t adc_calibrate_cb(struct pitc_timer *timer)
 {
-	uint32_t i;
+	/* trigger next cycle */
-	for (i = 0; i < ARRAY_SIZE(adc_result); i++)
-		printf("%+4d ", 0x200 - adc_result[i]);
-
-	printf("\n\r");
-
 	*AT91C_ADC_CR = AT91C_ADC_START;
+
+	if (adc_cal_mode == ADC_CAL_GYRO) {
+		adc_cal_data[0] += adc_tmp[ADC_GYRO_ROLL];
+		adc_cal_data[1] += adc_tmp[ADC_GYRO_NICK];
+		adc_cal_data[2] += adc_tmp[ADC_GYRO_YAW];
+
+	} else {
+		adc_cal_data[0] += adc_tmp[ADC_ACC_ROLL];
+		adc_cal_data[1] += adc_tmp[ADC_ACC_NICK];
+		adc_cal_data[2] += adc_tmp[ADC_ACC_YAW];
+	}
+
+	adc_cal_count--;
+	if (adc_cal_count == 0) {
+		adc_cal_data[0] /= ADC_CAL_COUNT;
+		adc_cal_data[1] /= ADC_CAL_COUNT;
+		adc_cal_data[2] /= ADC_CAL_COUNT;
+
+		adc_cal_mode = 0;
+
+		return PITC_REMOVE_TIMER;
+	}
+
 	return PITC_RESTART_TIMER;
 }
 
-static struct pitc_timer adc_timer = {
+static struct pitc_timer adc_cal_timer = {
-	.interval = 100,
+	.interval = 1,
-	.func = &adc_trigger,
+	.func = &adc_calibrate_cb,
 };
 
-void at91_adc_test_init(void)
+void adc_trigger(void)
+{
+	printf("R:%4d N:%4d Y:%4d  R:%4d N:%4d Y:%4d  U:%4d\n\r",
+		adc_result[ADC_GYRO_ROLL], adc_result[ADC_GYRO_NICK], adc_result[ADC_GYRO_YAW],
+		adc_result[ADC_ACC_ROLL], adc_result[ADC_ACC_NICK], adc_result[ADC_ACC_YAW],
+		adc_result[ADC_VOLTAGE]);
+
+	*AT91C_ADC_CR = AT91C_ADC_START;
+}
+
+void adc_calibrate(uint32_t mode)
+{
+	if (mode == ADC_CAL_GYRO || mode == ADC_CAL_ACC) {
+
+		adc_cal_count = ADC_CAL_COUNT;
+		adc_cal_mode = mode;
+		adc_cal_data[0] = 0;
+		adc_cal_data[1] = 0;
+		adc_cal_data[2] = 0;
+		pitc_schedule_timer(&adc_cal_timer);
+
+		// wait (1024 * 1ms ~> 1s)
+		while (adc_cal_mode);
+
+		if (mode == ADC_CAL_GYRO) {
+			adc_offset[ADC_GYRO_ROLL] = adc_cal_data[0];
+			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?
+
+			printf("GYRO offsets: %4d/%4d/%4d\n\r",
+				adc_offset[ADC_GYRO_ROLL],
+				adc_offset[ADC_GYRO_NICK],
+				adc_offset[ADC_GYRO_YAW]);
+
+		} else {
+			adc_offset[ADC_ACC_ROLL] = adc_cal_data[0];
+			adc_offset[ADC_ACC_NICK] = adc_cal_data[1];
+
+			// 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]);
+		}
+	}
+}
+
+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;
@@ -80,11 +167,11 @@ void at91_adc_test_init(void)
 
 	/* setup PDC */
 	AT91S_PDC *pdc = AT91C_BASE_PDC_ADC;
-	pdc->PDC_RPR = (uint32_t) &adc_result;
+	pdc->PDC_RPR = (uint32_t) &adc_tmp;
-	pdc->PDC_RCR = ARRAY_SIZE(adc_result);
+	pdc->PDC_RCR = ARRAY_SIZE(adc_tmp);
 	pdc->PDC_PTCR = AT91C_PDC_RXTEN;
 
-	/* enable 4 channels, PDC Interrupt */
+	/* enable 7 channels (0-1-2-4-5-6-7), PDC Interrupt */
 	adc->ADC_CHER = 0xF7;
 	adc->ADC_IER = AT91C_ADC_ENDRX;
 
@@ -94,5 +181,7 @@ void at91_adc_test_init(void)
 	aic->AIC_SVR[AT91C_ID_ADC] = (uint32_t)at91_adc_isr;
 	aic->AIC_IECR = (1<<AT91C_ID_ADC);
 
-	pitc_schedule_timer(&adc_timer);
+	// TODO: trigger via rc
+	adc_calibrate(ADC_CAL_GYRO);
+	adc_calibrate(ADC_CAL_ACC);
 }

src/at91_tc1.c

@@ -22,7 +22,6 @@
 #include "AT91SAM7S256.h"
 #include "board.h"
 #include "at91_tc1.h"
-#include "at91_pio.h"
 
 /* Hard limits for ISR */
 #define PULSE_MIN	0x0500
@@ -123,7 +122,7 @@ uint32_t rcontrol_getvalues(struct rc_values *rc)
 		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;
+		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 */
@@ -207,21 +206,3 @@ 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);