From 3f63051df8a8e6df3a10e5539d319a8e7dfb1ce1 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Tue, 1 May 2012 11:11:31 -0500 Subject: [PATCH] PiOS ADC: This could still use a lot of cleanup and needs to be implemented in the non-FreeRTOS case but now the ADC device is allocated dynamically which means CC3D won't waste heap on it. --- flight/CopterControl/System/pios_board.c | 2 +- flight/PiOS/STM32F10x/pios_adc.c | 152 ++++++++++++------ flight/PiOS/inc/pios_adc.h | 4 +- flight/PiOS/inc/pios_adc_priv.h | 21 +-- .../coptercontrol/board_hw_defs.c | 9 -- 5 files changed, 111 insertions(+), 77 deletions(-) diff --git a/flight/CopterControl/System/pios_board.c b/flight/CopterControl/System/pios_board.c index 7b591451f..78e316cb5 100644 --- a/flight/CopterControl/System/pios_board.c +++ b/flight/CopterControl/System/pios_board.c @@ -702,7 +702,7 @@ void PIOS_Board_Init(void) { case 0x01: // Revision 1 with invensense gyros, start the ADC #if defined(PIOS_INCLUDE_ADC) - PIOS_ADC_Init(); + PIOS_ADC_Init(&pios_adc_cfg); #endif #if defined(PIOS_INCLUDE_ADXL345) PIOS_ADXL345_Init(pios_spi_flash_accel_id, 0); diff --git a/flight/PiOS/STM32F10x/pios_adc.c b/flight/PiOS/STM32F10x/pios_adc.c index e8fbe3a85..66eb8f92e 100644 --- a/flight/PiOS/STM32F10x/pios_adc.c +++ b/flight/PiOS/STM32F10x/pios_adc.c @@ -7,8 +7,7 @@ * @{ * * @file pios_adc.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) (tk@midibox.org) + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. * @brief Analog to Digital converstion routines * @see The GNU Public License (GPL) Version 3 *****************************************************************************/ @@ -31,8 +30,38 @@ #include "pios.h" #include +// Private types + +enum pios_adc_dev_magic { + PIOS_ADC_DEV_MAGIC = 0x58375124, +}; + +struct pios_adc_dev { + const struct pios_adc_cfg * cfg; + ADCCallback callback_function; +#if defined(PIOS_INCLUDE_FREERTOS) + xQueueHandle data_queue; +#endif + volatile int16_t *valid_data_buffer; + volatile uint8_t adc_oversample; + uint8_t dma_block_size; + uint16_t dma_half_buffer_size; +#if defined(PIOS_INCLUDE_ADC) + int16_t fir_coeffs[PIOS_ADC_MAX_SAMPLES+1] __attribute__ ((aligned(4))); + volatile int16_t raw_data_buffer[PIOS_ADC_MAX_SAMPLES] __attribute__ ((aligned(4))); // Double buffer that DMA just used + float downsampled_buffer[PIOS_ADC_NUM_CHANNELS] __attribute__ ((aligned(4))); +#endif + enum pios_adc_dev_magic magic; +}; + +#if defined(PIOS_INCLUDE_FREERTOS) +struct pios_adc_dev * pios_adc_dev; +#endif + // Private functions void PIOS_ADC_downsample_data(); +static struct pios_adc_dev * PIOS_ADC_Allocate(); +static bool PIOS_ADC_validate(struct pios_adc_dev *); /* Local Variables */ static GPIO_TypeDef *ADC_GPIO_PORT[PIOS_ADC_NUM_PINS] = PIOS_ADC_PORTS; @@ -42,15 +71,43 @@ static const uint32_t ADC_CHANNEL[PIOS_ADC_NUM_PINS] = PIOS_ADC_CHANNELS; static ADC_TypeDef *ADC_MAPPING[PIOS_ADC_NUM_PINS] = PIOS_ADC_MAPPING; static const uint32_t ADC_CHANNEL_MAPPING[PIOS_ADC_NUM_PINS] = PIOS_ADC_CHANNEL_MAPPING; +static bool PIOS_ADC_validate(struct pios_adc_dev * dev) +{ + if (dev == NULL) + return false; + + return (dev->magic == PIOS_ADC_DEV_MAGIC); +} + +#if defined(PIOS_INCLUDE_FREERTOS) +static struct pios_adc_dev * PIOS_ADC_Allocate() +{ + struct pios_adc_dev * adc_dev; + + adc_dev = (struct pios_adc_dev *)pvPortMalloc(sizeof(*adc_dev)); + if (!adc_dev) return (NULL); + + adc_dev->magic = PIOS_ADC_DEV_MAGIC; + return(adc_dev); +} +#else +#error Not implemented +#endif + /** * @brief Initialise the ADC Peripheral, configure to run at the max oversampling */ -void PIOS_ADC_Init() -{ - pios_adc_devs[0].callback_function = NULL; +int32_t PIOS_ADC_Init(const struct pios_adc_cfg * cfg) +{ + pios_adc_dev = PIOS_ADC_Allocate(); + if (pios_adc_dev == NULL) + return -1; + + pios_adc_dev->cfg = cfg; + pios_adc_dev->callback_function = NULL; #if defined(PIOS_INCLUDE_FREERTOS) - pios_adc_devs[0].data_queue = NULL; + pios_adc_dev->data_queue = NULL; #endif /* Setup analog pins */ @@ -66,6 +123,8 @@ void PIOS_ADC_Init() } PIOS_ADC_Config(PIOS_ADC_MAX_OVERSAMPLING); + + return 0; } /** @@ -74,13 +133,13 @@ void PIOS_ADC_Init() */ void PIOS_ADC_Config(uint32_t oversampling) { - pios_adc_devs[0].adc_oversample = (oversampling > PIOS_ADC_MAX_OVERSAMPLING) ? PIOS_ADC_MAX_OVERSAMPLING : oversampling; + pios_adc_dev->adc_oversample = (oversampling > PIOS_ADC_MAX_OVERSAMPLING) ? PIOS_ADC_MAX_OVERSAMPLING : oversampling; ADC_DeInit(ADC1); ADC_DeInit(ADC2); /* Disable interrupts */ - DMA_ITConfig(pios_adc_devs[0].cfg->dma.rx.channel, pios_adc_devs[0].cfg->dma.irq.flags, DISABLE); + DMA_ITConfig(pios_adc_dev->cfg->dma.rx.channel, pios_adc_dev->cfg->dma.irq.flags, DISABLE); /* Enable ADC clocks */ PIOS_ADC_CLOCK_FUNCTION; @@ -139,34 +198,34 @@ void PIOS_ADC_Config(uint32_t oversampling) #endif /* This makes sure we have an even number of transfers if using ADC2 */ - pios_adc_devs[0].dma_block_size = ((PIOS_ADC_NUM_CHANNELS + PIOS_ADC_USE_ADC2) >> PIOS_ADC_USE_ADC2) << PIOS_ADC_USE_ADC2; - pios_adc_devs[0].dma_half_buffer_size = pios_adc_devs[0].dma_block_size * pios_adc_devs[0].adc_oversample; + pios_adc_dev->dma_block_size = ((PIOS_ADC_NUM_CHANNELS + PIOS_ADC_USE_ADC2) >> PIOS_ADC_USE_ADC2) << PIOS_ADC_USE_ADC2; + pios_adc_dev->dma_half_buffer_size = pios_adc_dev->dma_block_size * pios_adc_dev->adc_oversample; /* Configure DMA channel */ - DMA_InitTypeDef dma_init = pios_adc_devs[0].cfg->dma.rx.init; - dma_init.DMA_MemoryBaseAddr = (uint32_t) &pios_adc_devs[0].raw_data_buffer[0]; + DMA_InitTypeDef dma_init = pios_adc_dev->cfg->dma.rx.init; + dma_init.DMA_MemoryBaseAddr = (uint32_t) &pios_adc_dev->raw_data_buffer[0]; dma_init.DMA_MemoryInc = DMA_MemoryInc_Enable; - dma_init.DMA_BufferSize = pios_adc_devs[0].dma_half_buffer_size; /* x2 for double buffer /2 for 32-bit xfr */ - DMA_Init(pios_adc_devs[0].cfg->dma.rx.channel, &dma_init); - DMA_Cmd(pios_adc_devs[0].cfg->dma.rx.channel, ENABLE); + dma_init.DMA_BufferSize = pios_adc_dev->dma_half_buffer_size; /* x2 for double buffer /2 for 32-bit xfr */ + DMA_Init(pios_adc_dev->cfg->dma.rx.channel, &dma_init); + DMA_Cmd(pios_adc_dev->cfg->dma.rx.channel, ENABLE); /* Trigger interrupt when for half conversions too to indicate double buffer */ - DMA_ITConfig(pios_adc_devs[0].cfg->dma.rx.channel, DMA_IT_TC, ENABLE); - DMA_ITConfig(pios_adc_devs[0].cfg->dma.rx.channel, DMA_IT_HT, ENABLE); + DMA_ITConfig(pios_adc_dev->cfg->dma.rx.channel, DMA_IT_TC, ENABLE); + DMA_ITConfig(pios_adc_dev->cfg->dma.rx.channel, DMA_IT_HT, ENABLE); /* Configure DMA interrupt */ - NVIC_Init(&pios_adc_devs[0].cfg->dma.irq.init); + NVIC_Init(&pios_adc_dev->cfg->dma.irq.init); /* Finally start initial conversion */ ADC_SoftwareStartConvCmd(ADC1, ENABLE); /* Use simple averaging filter for now */ - for (int32_t i = 0; i < pios_adc_devs[0].adc_oversample; i++) - pios_adc_devs[0].fir_coeffs[i] = 1; - pios_adc_devs[0].fir_coeffs[pios_adc_devs[0].adc_oversample] = pios_adc_devs[0].adc_oversample; + for (int32_t i = 0; i < pios_adc_dev->adc_oversample; i++) + pios_adc_dev->fir_coeffs[i] = 1; + pios_adc_dev->fir_coeffs[pios_adc_dev->adc_oversample] = pios_adc_dev->adc_oversample; /* Enable DMA1 clock */ - RCC_AHBPeriphClockCmd(pios_adc_devs[0].cfg->dma.ahb_clk, ENABLE); + RCC_AHBPeriphClockCmd(pios_adc_dev->cfg->dma.ahb_clk, ENABLE); } /** @@ -183,7 +242,7 @@ int32_t PIOS_ADC_PinGet(uint32_t pin) } /* Return last conversion result */ - return pios_adc_devs[0].downsampled_buffer[pin]; + return pios_adc_dev->downsampled_buffer[pin]; } /** @@ -192,7 +251,7 @@ int32_t PIOS_ADC_PinGet(uint32_t pin) */ void PIOS_ADC_SetCallback(ADCCallback new_function) { - pios_adc_devs[0].callback_function = new_function; + pios_adc_dev->callback_function = new_function; } #if defined(PIOS_INCLUDE_FREERTOS) @@ -201,7 +260,7 @@ void PIOS_ADC_SetCallback(ADCCallback new_function) */ void PIOS_ADC_SetQueue(xQueueHandle data_queue) { - pios_adc_devs[0].data_queue = data_queue; + pios_adc_dev->data_queue = data_queue; } #endif @@ -210,7 +269,7 @@ void PIOS_ADC_SetQueue(xQueueHandle data_queue) */ float * PIOS_ADC_GetBuffer(void) { - return pios_adc_devs[0].downsampled_buffer; + return pios_adc_dev->downsampled_buffer; } /** @@ -218,7 +277,7 @@ float * PIOS_ADC_GetBuffer(void) */ int16_t * PIOS_ADC_GetRawBuffer(void) { - return (int16_t *) pios_adc_devs[0].valid_data_buffer; + return (int16_t *) pios_adc_dev->valid_data_buffer; } /** @@ -226,7 +285,7 @@ int16_t * PIOS_ADC_GetRawBuffer(void) */ uint8_t PIOS_ADC_GetOverSampling(void) { - return pios_adc_devs[0].adc_oversample; + return pios_adc_dev->adc_oversample; } /** @@ -239,8 +298,8 @@ uint8_t PIOS_ADC_GetOverSampling(void) void PIOS_ADC_SetFIRCoefficients(float * new_filter) { // Less than or equal to get normalization constant - for(int i = 0; i <= pios_adc_devs[0].adc_oversample; i++) - pios_adc_devs[0].fir_coeffs[i] = new_filter[i]; + for(int i = 0; i <= pios_adc_dev->adc_oversample; i++) + pios_adc_dev->fir_coeffs[i] = new_filter[i]; } /** @@ -251,25 +310,25 @@ void PIOS_ADC_downsample_data() { uint16_t chan; uint16_t sample; - float * downsampled_buffer = &pios_adc_devs[0].downsampled_buffer[0]; + float * downsampled_buffer = &pios_adc_dev->downsampled_buffer[0]; for (chan = 0; chan < PIOS_ADC_NUM_CHANNELS; chan++) { int32_t sum = 0; - for (sample = 0; sample < pios_adc_devs[0].adc_oversample; sample++) { - sum += pios_adc_devs[0].valid_data_buffer[chan + sample * pios_adc_devs[0].dma_block_size] * pios_adc_devs[0].fir_coeffs[sample]; + for (sample = 0; sample < pios_adc_dev->adc_oversample; sample++) { + sum += pios_adc_dev->valid_data_buffer[chan + sample * pios_adc_dev->dma_block_size] * pios_adc_dev->fir_coeffs[sample]; } - downsampled_buffer[chan] = (float) sum / pios_adc_devs[0].fir_coeffs[pios_adc_devs[0].adc_oversample]; + downsampled_buffer[chan] = (float) sum / pios_adc_dev->fir_coeffs[pios_adc_dev->adc_oversample]; } #if defined(PIOS_INCLUDE_FREERTOS) - if(pios_adc_devs[0].data_queue) { + if(pios_adc_dev->data_queue) { static portBASE_TYPE xHigherPriorityTaskWoken; - xQueueSendFromISR(pios_adc_devs[0].data_queue, pios_adc_devs[0].downsampled_buffer, &xHigherPriorityTaskWoken); + xQueueSendFromISR(pios_adc_dev->data_queue, pios_adc_dev->downsampled_buffer, &xHigherPriorityTaskWoken); portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); } #endif - if(pios_adc_devs[0].callback_function) - pios_adc_devs[0].callback_function(pios_adc_devs[0].downsampled_buffer); + if(pios_adc_dev->callback_function) + pios_adc_dev->callback_function(pios_adc_dev->downsampled_buffer); } /** @@ -283,19 +342,22 @@ void PIOS_ADC_downsample_data() */ void PIOS_ADC_DMA_Handler(void) { - if (DMA_GetFlagStatus(pios_adc_devs[0].cfg->full_flag /*DMA1_IT_TC1*/)) { // whole double buffer filled - pios_adc_devs[0].valid_data_buffer = &pios_adc_devs[0].raw_data_buffer[pios_adc_devs[0].dma_half_buffer_size]; - DMA_ClearFlag(pios_adc_devs[0].cfg->full_flag); + if (!PIOS_ADC_validate(pios_adc_dev)) + return; + + if (DMA_GetFlagStatus(pios_adc_dev->cfg->full_flag /*DMA1_IT_TC1*/)) { // whole double buffer filled + pios_adc_dev->valid_data_buffer = &pios_adc_dev->raw_data_buffer[pios_adc_dev->dma_half_buffer_size]; + DMA_ClearFlag(pios_adc_dev->cfg->full_flag); PIOS_ADC_downsample_data(); } - else if (DMA_GetFlagStatus(pios_adc_devs[0].cfg->half_flag /*DMA1_IT_HT1*/)) { - pios_adc_devs[0].valid_data_buffer = &pios_adc_devs[0].raw_data_buffer[0]; - DMA_ClearFlag(pios_adc_devs[0].cfg->half_flag); + else if (DMA_GetFlagStatus(pios_adc_dev->cfg->half_flag /*DMA1_IT_HT1*/)) { + pios_adc_dev->valid_data_buffer = &pios_adc_dev->raw_data_buffer[0]; + DMA_ClearFlag(pios_adc_dev->cfg->half_flag); PIOS_ADC_downsample_data(); } else { // This should not happen, probably due to transfer errors - DMA_ClearFlag(pios_adc_devs[0].cfg->dma.irq.flags /*DMA1_FLAG_GL1*/); + DMA_ClearFlag(pios_adc_dev->cfg->dma.irq.flags /*DMA1_FLAG_GL1*/); } } diff --git a/flight/PiOS/inc/pios_adc.h b/flight/PiOS/inc/pios_adc.h index 9b3aeb396..f72b163ee 100644 --- a/flight/PiOS/inc/pios_adc.h +++ b/flight/PiOS/inc/pios_adc.h @@ -7,8 +7,7 @@ * @{ * * @file pios_adc.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. * @brief ADC functions header. * @see The GNU Public License (GPL) Version 3 * @@ -38,7 +37,6 @@ typedef void (*ADCCallback) (float * data); /* Public Functions */ -void PIOS_ADC_Init(); void PIOS_ADC_Config(uint32_t oversampling); int32_t PIOS_ADC_PinGet(uint32_t pin); int16_t * PIOS_ADC_GetRawBuffer(void); diff --git a/flight/PiOS/inc/pios_adc_priv.h b/flight/PiOS/inc/pios_adc_priv.h index 67176d53b..278dcb8b7 100644 --- a/flight/PiOS/inc/pios_adc_priv.h +++ b/flight/PiOS/inc/pios_adc_priv.h @@ -40,27 +40,10 @@ struct pios_adc_cfg { struct stm32_dma dma; uint32_t half_flag; uint32_t full_flag; + uint16_t max_downsample; }; -struct pios_adc_dev { - const struct pios_adc_cfg *const cfg; - ADCCallback callback_function; -#if defined(PIOS_INCLUDE_FREERTOS) - xQueueHandle data_queue; -#endif - volatile int16_t *valid_data_buffer; - volatile uint8_t adc_oversample; - uint8_t dma_block_size; - uint16_t dma_half_buffer_size; -#if defined(PIOS_INCLUDE_ADC) - int16_t fir_coeffs[PIOS_ADC_MAX_SAMPLES+1] __attribute__ ((aligned(4))); - volatile int16_t raw_data_buffer[PIOS_ADC_MAX_SAMPLES] __attribute__ ((aligned(4))); // Double buffer that DMA just used - float downsampled_buffer[PIOS_ADC_NUM_CHANNELS] __attribute__ ((aligned(4))); -#endif -}; - -extern struct pios_adc_dev pios_adc_devs[]; -extern uint8_t pios_adc_num_devices; +int32_t PIOS_ADC_Init(const struct pios_adc_cfg * cfg); #endif /* PIOS_ADC_PRIV_H */ diff --git a/flight/board_hw_defs/coptercontrol/board_hw_defs.c b/flight/board_hw_defs/coptercontrol/board_hw_defs.c index c17bb32f3..cb1ce745e 100644 --- a/flight/board_hw_defs/coptercontrol/board_hw_defs.c +++ b/flight/board_hw_defs/coptercontrol/board_hw_defs.c @@ -426,15 +426,6 @@ static const struct pios_adc_cfg pios_adc_cfg = { .full_flag = DMA1_IT_TC1, }; -struct pios_adc_dev pios_adc_devs[] = { - { - .cfg = &pios_adc_cfg, - .callback_function = NULL, - }, -}; - -uint8_t pios_adc_num_devices = NELEMENTS(pios_adc_devs); - void PIOS_ADC_handler() { PIOS_ADC_DMA_Handler(); }