From 7113e45d1e2e9d3850600492d7b95f8a25047a86 Mon Sep 17 00:00:00 2001 From: Piotr Esden-Tempski Date: Mon, 19 Mar 2012 20:24:17 -0700 Subject: [PATCH] Ported ADC driver to libopencm3. --- sw/airborne/arch/stm32/mcu_periph/adc_arch.c | 247 ++++++++++++------- sw/airborne/boards/lisa_l_1.0.h | 10 +- sw/airborne/boards/lisa_m_1.0.h | 10 +- sw/airborne/lisa/test_adc.c | 3 - 4 files changed, 164 insertions(+), 106 deletions(-) diff --git a/sw/airborne/arch/stm32/mcu_periph/adc_arch.c b/sw/airborne/arch/stm32/mcu_periph/adc_arch.c index 114ede55b7d..88a76b7acb4 100644 --- a/sw/airborne/arch/stm32/mcu_periph/adc_arch.c +++ b/sw/airborne/arch/stm32/mcu_periph/adc_arch.c @@ -84,24 +84,23 @@ */ #include "mcu_periph/adc.h" -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include #include "std.h" #include "led.h" #include BOARD_CONFIG -void adc1_2_irq_handler(void); +void adc1_2_isr(void); uint8_t adc_new_data_trigger; /* Static functions */ -static inline void adc_init_single(ADC_TypeDef * adc_t, +static inline void adc_init_single(uint32_t adc, uint8_t chan1, uint8_t chan2, uint8_t chan3, uint8_t chan4); @@ -129,12 +128,13 @@ static inline void adc_init_irq( void ); */ #ifdef USE_AD1 #ifndef ADC1_GPIO_INIT -#define ADC1_GPIO_INIT(gpio) { \ - (gpio).GPIO_Pin = GPIO_Pin_1 | GPIO_Pin_0; \ - (gpio).GPIO_Mode = GPIO_Mode_AIN; \ - GPIO_Init(GPIOB, (&gpio)); \ - (gpio).GPIO_Pin = GPIO_Pin_5 | GPIO_Pin_3; \ - GPIO_Init(GPIOC, (&gpio)); \ +#define ADC1_GPIO_INIT() { \ + gpio_set_mode(GPIOB, GPIO_MODE_INPUT, \ + GPIO_CNF_INPUT_ANALOG, \ + GPIO1 | GPIO0); \ + gpio_set_mode(GPIOC, GPIO_MODE_INPUT, \ + GPIO_CNF_INPUT_ANALOG, \ + GPIO5 | GPIO3); \ } #endif // ADC1_GPIO_INIT #endif // USE_AD1 @@ -145,15 +145,15 @@ static inline void adc_init_irq( void ); Uses the same GPIOs as ADC1 (lisa specific). */ #ifdef USE_AD2 -#define ADC2_GPIO_INIT(gpio) { \ - (gpio).GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_1; \ - (gpio).GPIO_Mode = GPIO_Mode_AIN; \ - GPIO_Init(GPIOB, (&gpio)); \ - (gpio).GPIO_Pin = GPIO_Pin_3 | GPIO_Pin_5; \ - GPIO_Init(GPIOC, (&gpio)); \ - } #ifndef ADC2_GPIO_INIT -#define ADC2_GPIO_INIT(gpio) { } +#define ADC2_GPIO_INIT() { \ + gpio_set_mode(GPIOB, GPIO_MODE_INPUT, \ + GPIO_CNF_INPUT_ANALOG, \ + GPIO1 | GPIO0); \ + gpio_set_mode(GPIOC, GPIO_MODE_INPUT, \ + GPIO_CNF_INPUT_ANALOG, \ + GPIO5 | GPIO3); \ + } #endif // ADC2_GPIO_INIT #endif // USE_AD2 @@ -186,7 +186,7 @@ static struct adc_buf * adc2_buffers[NB_ADC2_CHANNELS]; Maps integer value x to ADC_InjectedChannel_x, so they can be iterated safely */ -static uint8_t adc_injected_channels[4]; +volatile uint32_t *adc_injected_channels[4]; /* Maps integer value x to ADC_Channel_y, like @@ -215,41 +215,59 @@ void adc_buf_channel(uint8_t adc_channel, static inline void adc_init_rcc( void ) { // {{{ #if defined (USE_AD1) || defined (USE_AD2) - TIM_TypeDef * timer; + uint32_t timer; + volatile uint32_t *rcc_apbenr; uint32_t rcc_apb; #if defined(USE_AD_TIM4) timer = TIM4; - rcc_apb = RCC_APB1Periph_TIM4; + rcc_apbenr = &RCC_APB1ENR; + rcc_apb = RCC_APB1ENR_TIM4EN; #elif defined(USE_AD_TIM1) timer = TIM1; - rcc_apb = RCC_APB2Periph_TIM1; + rcc_apbenr = &RCC_APB2ENR; + rcc_apb = RCC_APB2ENR_TIM1EN; #else timer = TIM2; - rcc_apb = RCC_APB1Periph_TIM2; + rcc_apbenr = &RCC_APB1ENR; + rcc_apb = RCC_APB1ENR_TIM2EN; #endif - TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; - - RCC_ADCCLKConfig(RCC_PCLK2_Div2); - RCC_APB1PeriphClockCmd(rcc_apb, ENABLE); - RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB | - RCC_APB2Periph_GPIOC, ENABLE); + /* + * Historic Note: + * Previously in libstm32 we were setting the ADC clock here. + * It was being set to PCLK2 DIV2 resulting in 36MHz clock on the ADC. I am + * pretty sure that this is wrong as based on the datasheet the ADC clock + * must not exceed 14MHz! Now the clock is being set by the clock init + * routine in libopencm3 so we don't have to set up this clock ourselves any + * more. This comment is here just as a reminder and may be removed in the + * future when we know that everything is working properly. + * (by Esden the historian :D) + */ + + /* Timer peripheral clock enable. */ + rcc_peripheral_enable_clock(rcc_apbenr, rcc_apb); + /* GPIO peripheral clock enable. */ + rcc_peripheral_enable_clock(&RCC_APB2ENR, RCC_APB2ENR_IOPBEN | + RCC_APB2ENR_IOPCEN); + + /* Enable ADC peripheral clocks. */ #ifdef USE_AD1 - RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1, ENABLE); + rcc_peripheral_enable_clock(&RCC_APB2ENR, RCC_APB2ENR_ADC1EN); #endif #ifdef USE_AD2 - RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC2, ENABLE); + rcc_peripheral_enable_clock(&RCC_APB2ENR, RCC_APB2ENR_ADC2EN); #endif /* Time Base configuration */ - TIM_TimeBaseStructInit(&TIM_TimeBaseStructure); - TIM_TimeBaseStructure.TIM_Period = 0xFF; - TIM_TimeBaseStructure.TIM_Prescaler = 0x8; - TIM_TimeBaseStructure.TIM_ClockDivision = 0x0; - TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; - TIM_TimeBaseInit(timer, &TIM_TimeBaseStructure); - TIM_SelectOutputTrigger(timer, TIM_TRGOSource_Update); - TIM_Cmd(timer, ENABLE); + timer_reset(timer); + timer_set_mode(timer, TIM_CR1_CKD_CK_INT, + TIM_CR1_CMS_EDGE, TIM_CR1_DIR_UP); + timer_set_period(timer, 0xFF); + timer_set_prescaler(timer, 0x8); + timer_set_clock_division(timer, 0x0); + /* Generate TRGO on every update. */ + timer_set_master_mode(timer, TIM_CR2_MMS_UPDATE); + timer_enable_counter(timer); #endif // defined (USE_AD1) || defined (USE_AD2) } // }}} @@ -257,12 +275,8 @@ static inline void adc_init_rcc( void ) /* Configure and enable ADC interrupt */ static inline void adc_init_irq( void ) { // {{{ - NVIC_InitTypeDef nvic; - nvic.NVIC_IRQChannel = ADC1_2_IRQn; - nvic.NVIC_IRQChannelPreemptionPriority = 0; - nvic.NVIC_IRQChannelSubPriority = 0; - nvic.NVIC_IRQChannelCmd = ENABLE; - NVIC_Init(&nvic); + nvic_set_priority(NVIC_ADC1_2_IRQ, 0); + nvic_enable_irq(NVIC_ADC1_2_IRQ); } // }}} /* @@ -273,89 +287,132 @@ static inline void adc_init_irq( void ) ... would enable ADC1, enabling channels 1 and 2, but not 3 and 4. */ -static inline void adc_init_single(ADC_TypeDef * adc_t, +static inline void adc_init_single(uint32_t adc, uint8_t chan1, uint8_t chan2, uint8_t chan3, uint8_t chan4) { - GPIO_InitTypeDef gpio; - ADC_InitTypeDef adc; uint8_t num_channels, rank; + uint8_t channels[4]; // Paranoia, must be down for 2+ ADC clock cycles before calibration - ADC_Cmd(adc_t, DISABLE); + adc_off(adc); - /* enable adc_t clock */ - if (adc_t == ADC1) { + /* enable adc clock */ + if (adc == ADC1) { #ifdef USE_AD1 num_channels = NB_ADC1_CHANNELS; - ADC1_GPIO_INIT(gpio); + ADC1_GPIO_INIT(); #endif } - else if (adc_t == ADC2) { + else if (adc == ADC2) { #ifdef USE_AD2 num_channels = NB_ADC2_CHANNELS; - ADC2_GPIO_INIT(gpio); + ADC2_GPIO_INIT(); #endif } /* Configure ADC */ + //adc_reset(adc); - adc.ADC_Mode = ADC_Mode_Independent; - adc.ADC_ScanConvMode = ENABLE; - adc.ADC_ContinuousConvMode = DISABLE; - adc.ADC_ExternalTrigConv = ADC_ExternalTrigConv_None; - adc.ADC_DataAlign = ADC_DataAlign_Right; - adc.ADC_NbrOfChannel = 0; // No. of channels in regular mode - ADC_Init(adc_t, &adc); + /* XXX: This register fiddeling code should be moved to libopencm3! */ + /* Set CR1 register. */ + { + uint32_t tmpreg = ADC_CR1(adc); - ADC_InjectedSequencerLengthConfig(adc_t, num_channels); + /* Clear DUALMOD and SCAN. */ + tmpreg &= ~(ADC_CR1_DUALMOD_MASK | ADC_CR1_SCAN); + + /* Set correct DUALMOD and SCAN. */ + tmpreg |= ADC_CR1_DUALMOD_IND | ADC_CR1_SCAN; + + ADC_CR1(adc) = tmpreg; + } + + /* Set CR2 register. */ + { + uint32_t tmpreg = ADC_CR2(adc); + + /* Clear CONT, ALIGN and EXTSEL. */ + tmpreg &= ~(ADC_CR2_CONT | ADC_CR2_ALIGN | ADC_CR2_EXTSEL_MASK); + + /* Set correct CONT, ALIGN and EXTSEL. */ + tmpreg |= ADC_CR2_EXTSEL_SWSTART; + + ADC_CR2(adc) = tmpreg; + } + + /* Set SQR1 register. */ + { + uint32_t tmpreg = ADC_SQR1(adc); + + /* Clear regular channel sequence length. */ + tmpreg &= ~(0xF); //~(ADC_SQR1_L_MSK); + + /* Set regular channel sequence length. */ + tmpreg |= 0; + + ADC_SQR1(adc) = tmpreg; + } + + /* Set JSQR1 injected sequence length. */ + { + uint32_t tmpreg = ADC_JSQR(adc); + + /* Clear injected channel sequence length. */ + tmpreg &= ~(0x2 << 20); //~(ADC_JSQR_JL_MSK); + + /* Set injected channel sequence length. */ + tmpreg |= (num_channels << ADC_JSQR_JL_LSB); + + ADC_JSQR(adc) = tmpreg; + } rank = 1; if (chan1) { - ADC_InjectedChannelConfig(adc_t, adc_channel_map[0], rank, - ADC_SampleTime_41Cycles5); + adc_set_conversion_time(adc, adc_channel_map[0], ADC_SMPR1_SMP_41DOT5CYC); + channels[rank] = adc_channel_map[0]; rank++; } if (chan2) { - ADC_InjectedChannelConfig(adc_t, adc_channel_map[1], rank, - ADC_SampleTime_41Cycles5); + adc_set_conversion_time(adc, adc_channel_map[1], ADC_SMPR1_SMP_41DOT5CYC); + channels[rank] = adc_channel_map[1]; rank++; } if (chan3) { - ADC_InjectedChannelConfig(adc_t, adc_channel_map[2], rank, - ADC_SampleTime_41Cycles5); + adc_set_conversion_time(adc, adc_channel_map[2], ADC_SMPR1_SMP_41DOT5CYC); + channels[rank] = adc_channel_map[2]; rank++; } if (chan4) { - ADC_InjectedChannelConfig(adc_t, adc_channel_map[3], rank, - ADC_SampleTime_41Cycles5); + adc_set_conversion_time(adc, adc_channel_map[3], ADC_SMPR1_SMP_41DOT5CYC); + channels[rank] = adc_channel_map[3]; } + adc_set_injected_sequence(adc, num_channels, channels); - ADC_ExternalTrigInjectedConvCmd(adc_t, ENABLE); #if defined(USE_AD_TIM4) - ADC_ExternalTrigInjectedConvConfig(adc_t, ADC_ExternalTrigInjecConv_T4_TRGO); + adc_enable_external_trigger_injected(adc, ADC_CR2_JEXTSEL_TIM4_TRGO); #elif defined(USE_AD_TIM1) - ADC_ExternalTrigInjectedConvConfig(adc_t, ADC_ExternalTrigInjecConv_T1_TRGO); + adc_enable_external_trigger_injected(adc, ADC_CR2_JEXTSEL_TIM1_TRGO); #else - ADC_ExternalTrigInjectedConvConfig(adc_t, ADC_ExternalTrigInjecConv_T2_TRGO); + adc_enable_external_trigger_injected(adc, ADC_CR2_JEXTSEL_TIM2_TRGO); #endif /* Enable ADC JEOC interrupt */ - ADC_ITConfig(adc_t, ADC_IT_JEOC, ENABLE); + adc_enable_jeoc_interrupt(adc); /* Enable ADC */ - ADC_Cmd(adc_t, ENABLE); + adc_on(adc); /* Enable ADC reset calibaration register */ - ADC_ResetCalibration(adc_t); + adc_reset_calibration(adc); /* Check the end of ADC reset calibration */ - while (ADC_GetResetCalibrationStatus(adc_t)) ; + while ((ADC_CR2(adc) & ADC_CR2_RSTCAL) != 0); /* Start ADC calibaration */ - ADC_StartCalibration(adc_t); + adc_calibration(adc); /* Check the end of ADC calibration */ - while (ADC_GetCalibrationStatus(adc_t)) ; + while ((ADC_CR2(adc) & ADC_CR2_CAL) != 0); } // adc_init_single @@ -371,17 +428,21 @@ void adc_init( void ) { #ifdef USE_AD1 for(channel = 0; channel < NB_ADC1_CHANNELS; channel++) adc1_buffers[channel] = NULL; + adc_injected_channels[0] = &ADC_JDR1(ADC1); + adc_injected_channels[1] = &ADC_JDR2(ADC1); + adc_injected_channels[2] = &ADC_JDR3(ADC1); + adc_injected_channels[3] = &ADC_JDR4(ADC1); #endif #ifdef USE_AD2 for(channel = 0; channel < NB_ADC2_CHANNELS; channel++) adc2_buffers[channel] = NULL; + adc_injected_channels[0] = &ADC_JDR1(ADC2); + adc_injected_channels[1] = &ADC_JDR2(ADC2); + adc_injected_channels[2] = &ADC_JDR3(ADC2); + adc_injected_channels[3] = &ADC_JDR4(ADC2); #endif adc_new_data_trigger = FALSE; - adc_injected_channels[0] = ADC_InjectedChannel_1; - adc_injected_channels[1] = ADC_InjectedChannel_2; - adc_injected_channels[2] = ADC_InjectedChannel_3; - adc_injected_channels[3] = ADC_InjectedChannel_4; adc_channel_map[0] = BOARD_ADC_CHANNEL_1; adc_channel_map[1] = BOARD_ADC_CHANNEL_2; // FIXME for now we get battery voltage this way @@ -460,7 +521,7 @@ static inline void adc_push_sample(struct adc_buf * buf, uint16_t value) { /** * ADC1+2 interrupt hander */ -void adc1_2_irq_handler(void) +void adc1_2_isr(void) { uint8_t channel = 0; uint16_t value = 0; @@ -468,11 +529,11 @@ void adc1_2_irq_handler(void) #ifdef USE_AD1 // Clear Injected End Of Conversion - ADC_ClearITPendingBit(ADC1, ADC_IT_JEOC); + ADC_SR(ADC1) &= ~ADC_SR_JEOC; for(channel = 0; channel < NB_ADC1_CHANNELS; channel++) { buf = adc1_buffers[channel]; if(buf) { - value = ADC_GetInjectedConversionValue(ADC1, adc_injected_channels[channel]); + value = *adc_injected_channels[channel]; adc_push_sample(buf, value); } } @@ -480,11 +541,11 @@ void adc1_2_irq_handler(void) #endif #ifdef USE_AD2 // Clear Injected End Of Conversion - ADC_ClearITPendingBit(ADC2, ADC_IT_JEOC); + ADC_SR(ADC2) &= ~ADC_SR_JEOC; for(channel = 0; channel < NB_ADC2_CHANNELS; channel++) { buf = adc2_buffers[channel]; if(buf) { - value = ADC_GetInjectedConversionValue(ADC2, adc_injected_channels[channel]); + value = *adc_injected_channels[channel]; adc_push_sample(buf, value); } } diff --git a/sw/airborne/boards/lisa_l_1.0.h b/sw/airborne/boards/lisa_l_1.0.h index b7cb6171fd3..405b38f0528 100644 --- a/sw/airborne/boards/lisa_l_1.0.h +++ b/sw/airborne/boards/lisa_l_1.0.h @@ -27,12 +27,12 @@ #endif #define DefaultVoltageOfAdc(adc) (0.0059*adc) /* Onboard ADCs */ -#define BOARD_ADC_CHANNEL_1 ADC_Channel_8 -#define BOARD_ADC_CHANNEL_2 ADC_Channel_9 +#define BOARD_ADC_CHANNEL_1 8 +#define BOARD_ADC_CHANNEL_2 9 // FIXME - removed for now and used for battery monitoring -//#define BOARD_ADC_CHANNEL_3 ADC_Channel_13 -#define BOARD_ADC_CHANNEL_3 ADC_Channel_0 -#define BOARD_ADC_CHANNEL_4 ADC_Channel_15 +//#define BOARD_ADC_CHANNEL_3 13 +#define BOARD_ADC_CHANNEL_3 0 +#define BOARD_ADC_CHANNEL_4 15 #define BOARD_HAS_BARO 1 diff --git a/sw/airborne/boards/lisa_m_1.0.h b/sw/airborne/boards/lisa_m_1.0.h index 5d4549aa6e2..52ad0837644 100644 --- a/sw/airborne/boards/lisa_m_1.0.h +++ b/sw/airborne/boards/lisa_m_1.0.h @@ -49,12 +49,12 @@ ADC7 PC2/ADC12 BATT PC4/ADC14 */ -#define BOARD_ADC_CHANNEL_1 ADC_Channel_13 -#define BOARD_ADC_CHANNEL_2 ADC_Channel_0 +#define BOARD_ADC_CHANNEL_1 13 +#define BOARD_ADC_CHANNEL_2 0 // FIXME - removed for now and used for battery monitoring -//#define BOARD_ADC_CHANNEL_3 ADC_Channel_10 -#define BOARD_ADC_CHANNEL_3 ADC_Channel_14 -#define BOARD_ADC_CHANNEL_4 ADC_Channel_11 +//#define BOARD_ADC_CHANNEL_3 10 +#define BOARD_ADC_CHANNEL_3 14 +#define BOARD_ADC_CHANNEL_4 11 #define BOARD_HAS_BARO 1 diff --git a/sw/airborne/lisa/test_adc.c b/sw/airborne/lisa/test_adc.c index e92def24ef4..c4be214b386 100644 --- a/sw/airborne/lisa/test_adc.c +++ b/sw/airborne/lisa/test_adc.c @@ -22,9 +22,6 @@ * */ -#include -#include - #include BOARD_CONFIG #include "mcu.h" #include "mcu_periph/sys_time.h"