flight/pios/stm32f10x/pios_adc.c

   1 /**
   2  ******************************************************************************
   3  * @addtogroup PIOS PIOS Core hardware abstraction layer
   4  * @{
   5  * @addtogroup   PIOS_ADC ADC Functions
   6  * @brief STM32 ADC PIOS interface
   7  * @{
   8  *
   9  * @file       pios_adc.c
  10  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
  11  * @brief      Analog to Digital converstion routines
  12  * @see        The GNU Public License (GPL) Version 3
  13  *****************************************************************************/
  14 /*
  15  * This program is free software; you can redistribute it and/or modify
  16  * it under the terms of the GNU General Public License as published by
  17  * the Free Software Foundation; either version 3 of the License, or
  18  * (at your option) any later version.
  19  *
  20  * This program is distributed in the hope that it will be useful, but
  21  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  22  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  23  * for more details.
  24  *
  25  * You should have received a copy of the GNU General Public License along
  26  * with this program; if not, write to the Free Software Foundation, Inc.,
  27  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  28  */
  29
  30 #include "pios.h"
  31
  32 #ifdef PIOS_INCLUDE_ADC
  33
  34 #include <pios_adc_priv.h>
  35 // Private types
  36
  37 enum pios_adc_dev_magic {
  38     PIOS_ADC_DEV_MAGIC = 0x58375124,
  39 };
  40
  41 struct pios_adc_dev {
  42     const struct pios_adc_cfg *cfg;
  43     ADCCallback      callback_function;
  44 #if defined(PIOS_INCLUDE_FREERTOS)
  45     xQueueHandle     data_queue;
  46 #endif
  47     volatile int16_t *valid_data_buffer;
  48     volatile uint8_t adc_oversample;
  49     volatile uint8_t adc_oversample_block_size;
  50     uint8_t  dma_block_size;
  51     uint16_t dma_half_buffer_size;
  52 #if defined(PIOS_INCLUDE_ADC)
  53     int16_t  fir_coeffs[PIOS_ADC_MAX_SAMPLES + 1]  __attribute__((aligned(4)));
  54     volatile int16_t raw_data_buffer[PIOS_ADC_MAX_SAMPLES]  __attribute__((aligned(4))); // Double buffer that DMA just used
  55     float    downsampled_buffer[PIOS_ADC_NUM_CHANNELS]  __attribute__((aligned(4)));
  56     uint8_t  downsampleStep; // allows to reduce the downsampling buffer by operating in two steps
  57 #endif
  58     enum pios_adc_dev_magic magic;
  59 };
  60
  61 float PIOS_ADC_PinGetVolt(uint32_t pin)
  62 {
  63     return ((float)PIOS_ADC_PinGet(pin)) * PIOS_ADC_VOLTAGE_SCALE;
  64 }
  65
  66 #if defined(PIOS_INCLUDE_FREERTOS)
  67 struct pios_adc_dev *pios_adc_dev;
  68 #endif
  69
  70 // Private functions
  71 void PIOS_ADC_downsample_data();
  72 static struct pios_adc_dev *PIOS_ADC_Allocate();
  73 static bool PIOS_ADC_validate(struct pios_adc_dev *);
  74
  75 /* Local Variables */
  76 static GPIO_TypeDef *ADC_GPIO_PORT[PIOS_ADC_NUM_PINS] = PIOS_ADC_PORTS;
  77 static const uint32_t ADC_GPIO_PIN[PIOS_ADC_NUM_PINS] = PIOS_ADC_PINS;
  78 static const uint32_t ADC_CHANNEL[PIOS_ADC_NUM_PINS]  = PIOS_ADC_CHANNELS;
  79
  80 static ADC_TypeDef *ADC_MAPPING[PIOS_ADC_NUM_PINS]    = PIOS_ADC_MAPPING;
  81 static const uint32_t ADC_CHANNEL_MAPPING[PIOS_ADC_NUM_PINS] = PIOS_ADC_CHANNEL_MAPPING;
  82
  83 static bool PIOS_ADC_validate(struct pios_adc_dev *dev)
  84 {
  85     if (dev == NULL) {
  86         return false;
  87     }
  88
  89     return dev->magic == PIOS_ADC_DEV_MAGIC;
  90 }
  91
  92 #if defined(PIOS_INCLUDE_FREERTOS)
  93 static struct pios_adc_dev *PIOS_ADC_Allocate()
  94 {
  95     struct pios_adc_dev *adc_dev;
  96
  97     adc_dev = (struct pios_adc_dev *)pios_malloc(sizeof(*adc_dev));
  98     if (!adc_dev) {
  99         return NULL;
 100     }
 101
 102     adc_dev->magic = PIOS_ADC_DEV_MAGIC;
 103     return adc_dev;
 104 }
 105 #else
 106 #error Not implemented
 107 #endif
 108
 109 /**
 110  * @brief Initialise the ADC Peripheral, configure to run at the max oversampling
 111  */
 112 int32_t PIOS_ADC_Init(const struct pios_adc_cfg *cfg)
 113 {
 114     pios_adc_dev = PIOS_ADC_Allocate();
 115     if (pios_adc_dev == NULL) {
 116         return -1;
 117     }
 118
 119     pios_adc_dev->cfg = cfg;
 120     pios_adc_dev->callback_function = NULL;
 121
 122 #if defined(PIOS_INCLUDE_FREERTOS)
 123     pios_adc_dev->data_queue = NULL;
 124 #endif
 125
 126     /* Setup analog pins */
 127     GPIO_InitTypeDef GPIO_InitStructure;
 128     GPIO_StructInit(&GPIO_InitStructure);
 129     GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
 130     GPIO_InitStructure.GPIO_Mode  = GPIO_Mode_AIN;
 131
 132     /* Enable each ADC pin in the array */
 133     for (int32_t i = 0; i < PIOS_ADC_NUM_PINS; i++) {
 134         GPIO_InitStructure.GPIO_Pin = ADC_GPIO_PIN[i];
 135         GPIO_Init(ADC_GPIO_PORT[i], &GPIO_InitStructure);
 136     }
 137
 138     PIOS_ADC_Config(PIOS_ADC_MAX_OVERSAMPLING);
 139     return 0;
 140 }
 141
 142 /**
 143  * @brief Configure the ADC to run at a fixed oversampling
 144  * @param[in] oversampling the amount of oversampling to run at
 145  */
 146 void PIOS_ADC_Config(uint32_t oversampling)
 147 {
 148     // Ensure oversample is an even number
 149     PIOS_Assert(!(oversampling & 0x1));
 150     pios_adc_dev->adc_oversample = (oversampling > PIOS_ADC_MAX_OVERSAMPLING) ? PIOS_ADC_MAX_OVERSAMPLING : oversampling;
 151     pios_adc_dev->adc_oversample_block_size = pios_adc_dev->adc_oversample / 2;
 152     ADC_DeInit(ADC1);
 153     ADC_DeInit(ADC2);
 154
 155     /* Disable interrupts */
 156     DMA_ITConfig(pios_adc_dev->cfg->dma.rx.channel, pios_adc_dev->cfg->dma.irq.flags, DISABLE);
 157
 158     /* Enable ADC clocks */
 159     PIOS_ADC_CLOCK_FUNCTION;
 160
 161     /* Map channels to conversion slots depending on the channel selection mask */
 162     for (int32_t i = 0; i < PIOS_ADC_NUM_PINS; i++) {
 163         ADC_RegularChannelConfig(ADC_MAPPING[i], ADC_CHANNEL[i],
 164                                  ADC_CHANNEL_MAPPING[i],
 165                                  PIOS_ADC_SAMPLE_TIME);
 166     }
 167
 168 #if (PIOS_ADC_USE_TEMP_SENSOR)
 169     ADC_TempSensorVrefintCmd(ENABLE);
 170     ADC_RegularChannelConfig(PIOS_ADC_TEMP_SENSOR_ADC, ADC_Channel_16,
 171                              PIOS_ADC_TEMP_SENSOR_ADC_CHANNEL,
 172                              PIOS_ADC_SAMPLE_TIME);
 173 #endif
 174     // return
 175     /* Configure ADCs */
 176     ADC_InitTypeDef ADC_InitStructure;
 177     ADC_StructInit(&ADC_InitStructure);
 178     ADC_InitStructure.ADC_Mode = ADC_Mode_RegSimult;
 179     ADC_InitStructure.ADC_ScanConvMode       = ENABLE;
 180     ADC_InitStructure.ADC_ContinuousConvMode = ENABLE;
 181     ADC_InitStructure.ADC_ExternalTrigConv   = ADC_ExternalTrigConv_None;
 182     ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right;
 183     ADC_InitStructure.ADC_NbrOfChannel       = ((PIOS_ADC_NUM_CHANNELS + 1) >> 1);
 184     ADC_Init(ADC1, &ADC_InitStructure);
 185
 186 #if (PIOS_ADC_USE_ADC2)
 187     ADC_Init(ADC2, &ADC_InitStructure);
 188
 189     /* Enable ADC2 external trigger conversion (to synch with ADC1) */
 190     ADC_ExternalTrigConvCmd(ADC2, ENABLE);
 191 #endif
 192
 193     RCC_ADCCLKConfig(PIOS_ADC_ADCCLK);
 194
 195     /* Enable ADC1->DMA request */
 196     ADC_DMACmd(ADC1, ENABLE);
 197
 198     /* ADC1 calibration */
 199     ADC_Cmd(ADC1, ENABLE);
 200     ADC_ResetCalibration(ADC1);
 201     while (ADC_GetResetCalibrationStatus(ADC1)) {
 202         ;
 203     }
 204     ADC_StartCalibration(ADC1);
 205     while (ADC_GetCalibrationStatus(ADC1)) {
 206         ;
 207     }
 208
 209 #if (PIOS_ADC_USE_ADC2)
 210     /* ADC2 calibration */
 211     ADC_Cmd(ADC2, ENABLE);
 212     ADC_ResetCalibration(ADC2);
 213     while (ADC_GetResetCalibrationStatus(ADC2)) {
 214         ;
 215     }
 216     ADC_StartCalibration(ADC2);
 217     while (ADC_GetCalibrationStatus(ADC2)) {
 218         ;
 219     }
 220 #endif
 221
 222     /* This makes sure we have an even number of transfers if using ADC2 */
 223     pios_adc_dev->dma_block_size = ((PIOS_ADC_NUM_CHANNELS + PIOS_ADC_USE_ADC2) >> PIOS_ADC_USE_ADC2) << PIOS_ADC_USE_ADC2;
 224     pios_adc_dev->dma_half_buffer_size = pios_adc_dev->dma_block_size * pios_adc_dev->adc_oversample_block_size;
 225
 226     /* Configure DMA channel */
 227     DMA_InitTypeDef dma_init = pios_adc_dev->cfg->dma.rx.init;
 228     dma_init.DMA_MemoryBaseAddr = (uint32_t)&pios_adc_dev->raw_data_buffer[0];
 229     dma_init.DMA_MemoryInc = DMA_MemoryInc_Enable;
 230     dma_init.DMA_BufferSize     = pios_adc_dev->dma_half_buffer_size; /* x2 for double buffer /2 for 32-bit xfr */
 231     DMA_Init(pios_adc_dev->cfg->dma.rx.channel, &dma_init);
 232     DMA_Cmd(pios_adc_dev->cfg->dma.rx.channel, ENABLE);
 233
 234     /* Trigger interrupt when for half conversions too to indicate double buffer */
 235     DMA_ITConfig(pios_adc_dev->cfg->dma.rx.channel, DMA_IT_TC, ENABLE);
 236     DMA_ITConfig(pios_adc_dev->cfg->dma.rx.channel, DMA_IT_HT, ENABLE);
 237
 238     /* Configure DMA interrupt */
 239     NVIC_Init(&pios_adc_dev->cfg->dma.irq.init);
 240
 241     /* Finally start initial conversion */
 242     ADC_SoftwareStartConvCmd(ADC1, ENABLE);
 243
 244     /* Use simple averaging filter for now */
 245     for (int32_t i = 0; i < pios_adc_dev->adc_oversample; i++) {
 246         pios_adc_dev->fir_coeffs[i] = 1;
 247     }
 248     pios_adc_dev->fir_coeffs[pios_adc_dev->adc_oversample] = pios_adc_dev->adc_oversample;
 249
 250     /* Enable DMA1 clock */
 251     RCC_AHBPeriphClockCmd(pios_adc_dev->cfg->dma.ahb_clk, ENABLE);
 252 }
 253
 254 /**
 255  * Returns value of an ADC Pin
 256  * \param[in] pin number
 257  * \return ADC pin value - resolution depends on the selected oversampling rate
 258  * \return -1 if pin doesn't exist
 259  */
 260 int32_t PIOS_ADC_PinGet(uint32_t pin)
 261 {
 262     /* Check if pin exists */
 263     if (pin >= PIOS_ADC_NUM_CHANNELS) {
 264         return -1;
 265     }
 266
 267     /* Return last conversion result */
 268     return pios_adc_dev->downsampled_buffer[pin];
 269 }
 270
 271 /**
 272  * @brief Set a callback function that is executed whenever
 273  * the ADC double buffer swaps
 274  */
 275 void PIOS_ADC_SetCallback(ADCCallback new_function)
 276 {
 277     pios_adc_dev->callback_function = new_function;
 278 }
 279
 280 #if defined(PIOS_INCLUDE_FREERTOS)
 281 /**
 282  * @brief Register a queue to add data to when downsampled
 283  */
 284 void PIOS_ADC_SetQueue(xQueueHandle data_queue)
 285 {
 286     pios_adc_dev->data_queue = data_queue;
 287 }
 288 #endif
 289
 290 /**
 291  * @brief Return the address of the downsampled data buffer
 292  */
 293 float *PIOS_ADC_GetBuffer(void)
 294 {
 295     return pios_adc_dev->downsampled_buffer;
 296 }
 297
 298 /**
 299  * @brief Return the address of the raw data data buffer
 300  */
 301 int16_t *PIOS_ADC_GetRawBuffer(void)
 302 {
 303     return (int16_t *)pios_adc_dev->valid_data_buffer;
 304 }
 305
 306 /**
 307  * @brief Return the amount of over sampling
 308  */
 309 uint8_t PIOS_ADC_GetOverSampling(void)
 310 {
 311     return pios_adc_dev->adc_oversample;
 312 }
 313
 314 /**
 315  * @brief Set the fir coefficients.  Takes as many samples as the
 316  * current filter order plus one (normalization)
 317  *
 318  * @param new_filter Array of adc_oversampling floats plus one for the
 319  * filter coefficients
 320  */
 321 void PIOS_ADC_SetFIRCoefficients(float *new_filter)
 322 {
 323     // Less than or equal to get normalization constant
 324     for (int i = 0; i <= pios_adc_dev->adc_oversample; i++) {
 325         pios_adc_dev->fir_coeffs[i] = new_filter[i];
 326     }
 327 }
 328
 329
 330 /**
 331  * @brief Downsample the data for each of the channels then call
 332  * callback function if installed
 333  */
 334 __attribute__((optimize("O3")))
 335 void PIOS_ADC_downsample_data()
 336 {
 337     uint16_t chan;
 338     uint16_t sample;
 339     float *downsampled_buffer = &pios_adc_dev->downsampled_buffer[0];
 340     static int32_t sum[PIOS_ADC_NUM_CHANNELS] = { 0 };
 341
 342     if (pios_adc_dev->downsampleStep == 0) {
 343         for (uint8_t i = 0; i < PIOS_ADC_NUM_CHANNELS; i++) {
 344             sum[i] = 0;
 345         }
 346     }
 347
 348     for (sample = 0; sample < pios_adc_dev->adc_oversample_block_size; sample++) {
 349         const uint16_t *buffer  = (const uint16_t *)&pios_adc_dev->valid_data_buffer[sample * pios_adc_dev->dma_block_size];
 350         const uint16_t firCoeff = pios_adc_dev->fir_coeffs[sample + PIOS_ADC_NUM_CHANNELS * pios_adc_dev->downsampleStep];
 351 #if (PIOS_ADC_USE_TEMP_SENSOR)
 352         for (chan = 1; chan < PIOS_ADC_NUM_CHANNELS; chan++) {
 353 #else
 354         for (chan = 0; chan < PIOS_ADC_NUM_CHANNELS; chan++) {
 355 #endif
 356             sum[chan] += buffer[chan] * firCoeff;
 357         }
 358     }
 359     // Full downsampling + fir filter is a little bit overwhelming for temp sensor.
 360     if (pios_adc_dev->downsampleStep == 0) {
 361         // first part of downsampling done. wait until the second block of samples is acquired
 362         pios_adc_dev->downsampleStep = 1;
 363         return;
 364     }
 365     pios_adc_dev->downsampleStep = 0;
 366 #if (PIOS_ADC_USE_TEMP_SENSOR)
 367     for (chan = 1; chan < PIOS_ADC_NUM_CHANNELS; chan++) {
 368     #else
 369     for (chan = 0; chan < PIOS_ADC_NUM_CHANNELS; chan++) {
 370     #endif
 371         downsampled_buffer[chan] = (float)sum[chan] / (pios_adc_dev->fir_coeffs[pios_adc_dev->adc_oversample]);
 372     }
 373
 374     #if (PIOS_ADC_USE_TEMP_SENSOR)
 375     downsampled_buffer[0] = pios_adc_dev->valid_data_buffer[0];
 376     #endif
 377
 378 #if defined(PIOS_INCLUDE_FREERTOS)
 379     if (pios_adc_dev->data_queue) {
 380         static portBASE_TYPE xHigherPriorityTaskWoken;
 381         xQueueSendFromISR(pios_adc_dev->data_queue, pios_adc_dev->downsampled_buffer, &xHigherPriorityTaskWoken);
 382         portEND_SWITCHING_ISR(xHigherPriorityTaskWoken);
 383     }
 384 #endif
 385     if (pios_adc_dev->callback_function) {
 386         pios_adc_dev->callback_function(pios_adc_dev->downsampled_buffer);
 387     }
 388 }
 389
 390 /**
 391  * @brief Interrupt for half and full buffer transfer
 392  *
 393  * This interrupt handler swaps between the two halfs of the double buffer to make
 394  * sure the ahrs uses the most recent data.  Only swaps data when AHRS is idle, but
 395  * really this is a pretense of a sanity check since the DMA engine is consantly
 396  * running in the background.  Keep an eye on the ekf_too_slow variable to make sure
 397  * it's keeping up.
 398  */
 399 void PIOS_ADC_DMA_Handler(void)
 400 {
 401     if (!PIOS_ADC_validate(pios_adc_dev)) {
 402         return;
 403     }
 404
 405     if (DMA_GetFlagStatus(pios_adc_dev->cfg->full_flag /*DMA1_IT_TC1*/)) { // whole double buffer filled
 406         pios_adc_dev->valid_data_buffer = &pios_adc_dev->raw_data_buffer[pios_adc_dev->dma_half_buffer_size];
 407         DMA_ClearFlag(pios_adc_dev->cfg->full_flag);
 408         PIOS_ADC_downsample_data();
 409     } else if (DMA_GetFlagStatus(pios_adc_dev->cfg->half_flag /*DMA1_IT_HT1*/)) {
 410         pios_adc_dev->valid_data_buffer = &pios_adc_dev->raw_data_buffer[0];
 411         DMA_ClearFlag(pios_adc_dev->cfg->half_flag);
 412         PIOS_ADC_downsample_data();
 413     } else {
 414         // This should not happen, probably due to transfer errors
 415         DMA_ClearFlag(pios_adc_dev->cfg->dma.irq.flags /*DMA1_FLAG_GL1*/);
 416     }
 417 }
 418
 419 #endif /* PIOS_INCLUDE_ADC */
 420
 421 /**
 422  * @}
 423  * @}
 424  */