From 8414aeac1529a0d0ea59f3454f77ccf71acbed1c Mon Sep 17 00:00:00 2001 From: Olaf Rempel Date: Mon, 10 Mar 2008 21:37:56 +0100 Subject: [PATCH] adc & rcontrol work --- include/at91_adc.h | 22 ++++++++ include/at91_tc1.h | 1 + include/at91_tests.h | 3 - main.c | 44 ++++++--------- src/at91_adc.c | 129 ++++++++++++++++++++++++++++++++++++------- src/at91_tc1.c | 21 +------ 6 files changed, 151 insertions(+), 69 deletions(-) create mode 100644 include/at91_adc.h diff --git a/include/at91_adc.h b/include/at91_adc.h new file mode 100644 index 0000000..7d9acc9 --- /dev/null +++ b/include/at91_adc.h @@ -0,0 +1,22 @@ +#ifndef AT91_ADC_H_ +#define AT91_ADC_H_ + +#include + +#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_*/ diff --git a/include/at91_tc1.h b/include/at91_tc1.h index 8f0a6f2..5dd21fa 100644 --- a/include/at91_tc1.h +++ b/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); diff --git a/include/at91_tests.h b/include/at91_tests.h index fad3d49..f0c8014 100644 --- a/include/at91_tests.h +++ b/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_*/ diff --git a/main.c b/main.c index 8bc20c7..488cbd1 100644 --- a/main.c +++ b/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]; - 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); + pitc_schedule_timer(&pitc_test_timer); +// rcontrol_calibrate(RC_CAL_START); while (1); } diff --git a/src/at91_adc.c b/src/at91_adc.c index 2b0d34a..cbf2905 100644 --- a/src/at91_adc.c +++ b/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_RCR = ARRAY_SIZE(adc_result); + pdc->PDC_RPR = (uint32_t) &adc_tmp; + pdc->PDC_RCR = ARRAY_SIZE(adc_tmp); pdc->PDC_PTCR = AT91C_PDC_RXTEN; /* clear interrupts */ - AT91S_ADC *adc = AT91C_BASE_ADC; - uint32_t status = adc->ADC_SR; + uint32_t status = *AT91C_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; - for (i = 0; i < ARRAY_SIZE(adc_result); i++) - printf("%+4d ", 0x200 - adc_result[i]); - - printf("\n\r"); - + /* trigger next cycle */ *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 = { - .interval = 100, - .func = &adc_trigger, +static struct pitc_timer adc_cal_timer = { + .interval = 1, + .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_RCR = ARRAY_SIZE(adc_result); + pdc->PDC_RPR = (uint32_t) &adc_tmp; + 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< 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);