From 1b49b33e00e511249241107a91f403acdd51ecec Mon Sep 17 00:00:00 2001 From: jflyper Date: Mon, 6 May 2019 11:21:47 +0900 Subject: [PATCH] [H7] Enable ADC - Initial cut without internal sensors - Full DMA including internal sensors spanning multiple ADC peripherals - Skip ADC initialisation when ADC instance is unused to avoid crash. - Add volatile & DMA_RAM attribute to ADC DMA buffer - Revised DMA coherency handling - adcGetChannelValues - added comment and moved def to adc_impl.h - Changes per PR comment (indentation fix) --- src/main/drivers/adc.c | 2 + src/main/drivers/adc.h | 13 +- src/main/drivers/adc_impl.h | 24 +- src/main/drivers/adc_stm32f10x.c | 5 + src/main/drivers/adc_stm32f30x.c | 5 + src/main/drivers/adc_stm32f4xx.c | 5 + src/main/drivers/adc_stm32f7xx.c | 5 + src/main/drivers/adc_stm32h7xx.c | 463 +++++++++++++++++++++++++++++++ 8 files changed, 518 insertions(+), 4 deletions(-) create mode 100644 src/main/drivers/adc_stm32h7xx.c diff --git a/src/main/drivers/adc.c b/src/main/drivers/adc.c index e66cc6a6b9..8e9b52d9dc 100644 --- a/src/main/drivers/adc.c +++ b/src/main/drivers/adc.c @@ -89,6 +89,8 @@ ADCDevice adcDeviceByInstance(ADC_TypeDef *instance) uint16_t adcGetChannel(uint8_t channel) { + adcGetChannelValues(); + #ifdef DEBUG_ADC_CHANNELS if (adcOperatingConfig[0].enabled) { debug[0] = adcValues[adcOperatingConfig[0].dmaIndex]; diff --git a/src/main/drivers/adc.h b/src/main/drivers/adc.h index 860a0bff32..c9b658a0d3 100644 --- a/src/main/drivers/adc.h +++ b/src/main/drivers/adc.h @@ -46,7 +46,7 @@ typedef enum ADCDevice { ADCINVALID = -1, ADCDEV_1 = 0, -#if defined(STM32F3) || defined(STM32F4) || defined(STM32F7) +#if defined(STM32F3) || defined(STM32F4) || defined(STM32F7) || defined(STM32H7) ADCDEV_2, ADCDEV_3, #endif @@ -64,12 +64,21 @@ typedef enum { ADC_CURRENT = 1, ADC_EXTERNAL1 = 2, ADC_RSSI = 3, +#ifdef STM32H7 + // On STM32H7, internal sensors are treated in the similar fashion as regular ADC inputs + ADC_CHANNEL_INTERNAL = 4, + ADC_TEMPSENSOR = 4, + ADC_VREFINT = 5, +#endif ADC_CHANNEL_COUNT } AdcChannel; typedef struct adcOperatingConfig_s { ioTag_t tag; - uint8_t adcChannel; // ADC1_INxx channel number +#ifdef STM32H7 + ADCDevice adcDevice; // ADCDEV_x for this input +#endif + uint8_t adcChannel; // ADCy_INxx channel number for this input uint8_t dmaIndex; // index into DMA buffer in case of sparse channels bool enabled; uint8_t sampleTime; diff --git a/src/main/drivers/adc_impl.h b/src/main/drivers/adc_impl.h index 34786f6b1c..b2321c81bb 100644 --- a/src/main/drivers/adc_impl.h +++ b/src/main/drivers/adc_impl.h @@ -26,6 +26,12 @@ #if defined(STM32F4) || defined(STM32F7) #define ADC_TAG_MAP_COUNT 16 +#elif defined(STM32H7) +#ifdef USE_ADC_INTERNAL +#define ADC_TAG_MAP_COUNT 30 +#else +#define ADC_TAG_MAP_COUNT 28 +#endif #elif defined(STM32F3) #define ADC_TAG_MAP_COUNT 39 #else @@ -37,7 +43,10 @@ typedef struct adcTagMap_s { #if !defined(STM32F1) // F1 pins have uniform connection to ADC instances uint8_t devices; #endif - uint8_t channel; + uint32_t channel; +#if defined(STM32H7) + uint8_t channelOrdinal; +#endif } adcTagMap_t; // Encoding for adcTagMap_t.devices @@ -57,14 +66,21 @@ typedef struct adcDevice_s { #if defined(STM32F4) || defined(STM32F7) DMA_Stream_TypeDef* DMAy_Streamx; uint32_t channel; +#elif defined(STM32H7) + DMA_Stream_TypeDef* DMAy_Streamx; + uint32_t request; #else DMA_Channel_TypeDef* DMAy_Channelx; #endif #endif -#if defined(STM32F7) +#if defined(STM32F7) || defined(STM32H7) ADC_HandleTypeDef ADCHandle; DMA_HandleTypeDef DmaHandle; #endif +#if defined(STM32H7) + uint8_t irq; + uint32_t channelBits; +#endif } adcDevice_t; extern const adcDevice_t adcHardware[]; @@ -75,3 +91,7 @@ extern volatile uint16_t adcValues[ADC_CHANNEL_COUNT]; uint8_t adcChannelByTag(ioTag_t ioTag); ADCDevice adcDeviceByInstance(ADC_TypeDef *instance); bool adcVerifyPin(ioTag_t tag, ADCDevice device); + +// Marshall values in DMA instance/channel based order to adcChannel based order. +// Required for multi DMA instance implementation +void adcGetChannelValues(void); diff --git a/src/main/drivers/adc_stm32f10x.c b/src/main/drivers/adc_stm32f10x.c index e107f70cb6..a8824a907b 100644 --- a/src/main/drivers/adc_stm32f10x.c +++ b/src/main/drivers/adc_stm32f10x.c @@ -163,4 +163,9 @@ void adcInit(const adcConfig_t *config) ADC_SoftwareStartConvCmd(adc.ADCx, ENABLE); } + +void adcGetChannelValues(void) +{ + // Nothing to do +} #endif diff --git a/src/main/drivers/adc_stm32f30x.c b/src/main/drivers/adc_stm32f30x.c index 56ec90dace..3c32543116 100644 --- a/src/main/drivers/adc_stm32f30x.c +++ b/src/main/drivers/adc_stm32f30x.c @@ -261,4 +261,9 @@ void adcInit(const adcConfig_t *config) ADC_StartConversion(adc.ADCx); } + +void adcGetChannelValues(void) +{ + // Nothing to do +} #endif diff --git a/src/main/drivers/adc_stm32f4xx.c b/src/main/drivers/adc_stm32f4xx.c index edc7a46121..af80e90524 100644 --- a/src/main/drivers/adc_stm32f4xx.c +++ b/src/main/drivers/adc_stm32f4xx.c @@ -350,4 +350,9 @@ void adcInit(const adcConfig_t *config) ADC_SoftwareStartConv(adc.ADCx); } + +void adcGetChannelValues(void) +{ + // Nothing to do +} #endif diff --git a/src/main/drivers/adc_stm32f7xx.c b/src/main/drivers/adc_stm32f7xx.c index 5cbbc78f09..b1ebff706b 100644 --- a/src/main/drivers/adc_stm32f7xx.c +++ b/src/main/drivers/adc_stm32f7xx.c @@ -350,4 +350,9 @@ void adcInit(const adcConfig_t *config) /* Start Conversion Error */ } } + +void adcGetChannelValues(void) +{ + // Nothing to do +} #endif diff --git a/src/main/drivers/adc_stm32h7xx.c b/src/main/drivers/adc_stm32h7xx.c new file mode 100644 index 0000000000..fc4a06e829 --- /dev/null +++ b/src/main/drivers/adc_stm32h7xx.c @@ -0,0 +1,463 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#include +#include +#include + +#include "platform.h" + +#ifdef USE_ADC + +#include "build/debug.h" + +#include "drivers/accgyro/accgyro.h" +#include "drivers/system.h" + +#include "drivers/io.h" +#include "drivers/io_impl.h" +#include "drivers/rcc.h" +#include "drivers/resource.h" +#include "drivers/dma.h" + +#include "drivers/sensor.h" + +#include "drivers/adc.h" +#include "drivers/adc_impl.h" + +#include "pg/adc.h" + +// Copied from stm32f7xx_ll_adc.h + +#define VREFINT_CAL_VREF ( 3300U) /* Analog voltage reference (Vref+) value with which temperature sensor has been calibrated in production (tolerance: +-10 mV) (unit: mV). */ +#define TEMPSENSOR_CAL1_TEMP (( int32_t) 30) /* Internal temperature sensor, temperature at which temperature sensor has been calibrated in production for data into TEMPSENSOR_CAL1_ADDR (tolerance: +-5 DegC) (unit: DegC). */ +#define TEMPSENSOR_CAL2_TEMP (( int32_t) 110) /* Internal temperature sensor, temperature at which temperature sensor has been calibrated in production for data into TEMPSENSOR_CAL2_ADDR (tolerance: +-5 DegC) (unit: DegC). */ +#define TEMPSENSOR_CAL_VREFANALOG ( 3300U) /* Analog voltage reference (Vref+) voltage with which temperature sensor has been calibrated in production (+-10 mV) (unit: mV). */ + +// Calibration data address for STM32H743. +// Source: STM32H743xI Datasheet DS12110 Rev 5 +// p.104 Table.28 Internal reference voltage calibration values +// p.170 Table 91. Temperature sensor calibration values +// Note that values are in 16-bit resolution, unlike 12-bit used in F-Series. +#define VREFINT_CAL_ADDR ((uint16_t*) (0x1FF1E860)) +#define TEMPSENSOR_CAL1_ADDR ((uint16_t*) (0x1FF1E820)) +#define TEMPSENSOR_CAL2_ADDR ((uint16_t*) (0x1FF1E840)) + +// XXX Instance and DMA stream defs will be gone in unified target + +#ifndef ADC1_INSTANCE +#define ADC1_INSTANCE NULL +#endif +#ifndef ADC2_INSTANCE +#define ADC2_INSTANCE NULL +#endif +#ifndef ADC3_INSTANCE +#define ADC3_INSTANCE NULL +#endif +#ifndef ADC1_DMA_STREAM +#define ADC1_DMA_STREAM NULL +#endif +#ifndef ADC2_DMA_STREAM +#define ADC2_DMA_STREAM NULL +#endif +#ifndef ADC3_DMA_STREAM +#define ADC3_DMA_STREAM NULL +#endif + +const adcDevice_t adcHardware[ADCDEV_COUNT] = { + { + .ADCx = ADC1_INSTANCE, + .rccADC = RCC_AHB1(ADC12), + .DMAy_Streamx = ADC1_DMA_STREAM, + .request = DMA_REQUEST_ADC1, + }, + { .ADCx = ADC2_INSTANCE, + .rccADC = RCC_AHB1(ADC12), + .DMAy_Streamx = ADC2_DMA_STREAM, + .request = DMA_REQUEST_ADC2, + }, + // ADC3 can be serviced by BDMA also, but we settle for DMA1 or 2 (for now). + { + .ADCx = ADC3_INSTANCE, + .rccADC = RCC_AHB4(ADC3), + .DMAy_Streamx = ADC3_DMA_STREAM, + .request = DMA_REQUEST_ADC3, + } +}; + +adcDevice_t adcDevice[ADCDEV_COUNT]; + +/* note these could be packed up for saving space */ +const adcTagMap_t adcTagMap[] = { +#ifdef USE_ADC_INTERNAL + // Pseudo entries for internal sensor. + // Keep these at the beginning for easy indexing by ADC_TAG_MAP_{VREFINT,TEMPSENSOR} +#define ADC_TAG_MAP_VREFINT 0 +#define ADC_TAG_MAP_TEMPSENSOR 1 + { DEFIO_TAG_E__NONE, ADC_DEVICES_3, ADC_CHANNEL_VREFINT, 18 }, + { DEFIO_TAG_E__NONE, ADC_DEVICES_3, ADC_CHANNEL_TEMPSENSOR, 19 }, +#endif + // Inputs available for all packages + { DEFIO_TAG_E__PC0, ADC_DEVICES_123, ADC_CHANNEL_10, 10 }, + { DEFIO_TAG_E__PC1, ADC_DEVICES_123, ADC_CHANNEL_11, 11 }, + { DEFIO_TAG_E__PC2, ADC_DEVICES_3, ADC_CHANNEL_0, 0 }, + { DEFIO_TAG_E__PC3, ADC_DEVICES_3, ADC_CHANNEL_1, 1 }, + { DEFIO_TAG_E__PC4, ADC_DEVICES_12, ADC_CHANNEL_4, 4 }, + { DEFIO_TAG_E__PC5, ADC_DEVICES_12, ADC_CHANNEL_8, 8 }, + { DEFIO_TAG_E__PB0, ADC_DEVICES_12, ADC_CHANNEL_9, 9 }, + { DEFIO_TAG_E__PB1, ADC_DEVICES_12, ADC_CHANNEL_5, 5 }, + { DEFIO_TAG_E__PA0, ADC_DEVICES_1, ADC_CHANNEL_16, 16 }, + { DEFIO_TAG_E__PA1, ADC_DEVICES_1, ADC_CHANNEL_17, 17 }, + { DEFIO_TAG_E__PA2, ADC_DEVICES_12, ADC_CHANNEL_14, 14 }, + { DEFIO_TAG_E__PA3, ADC_DEVICES_12, ADC_CHANNEL_15, 15 }, + { DEFIO_TAG_E__PA4, ADC_DEVICES_12, ADC_CHANNEL_18, 18 }, + { DEFIO_TAG_E__PA5, ADC_DEVICES_12, ADC_CHANNEL_19, 19 }, + { DEFIO_TAG_E__PA6, ADC_DEVICES_12, ADC_CHANNEL_3, 3 }, + { DEFIO_TAG_E__PA7, ADC_DEVICES_12, ADC_CHANNEL_7, 7 }, + +#if 0 + // Inputs available for packages larger than LQFP144 + { DEFIO_TAG_E__PF3, ADC_DEVICES_3, ADC_CHANNEL_5, 5 }, + { DEFIO_TAG_E__PF4, ADC_DEVICES_3, ADC_CHANNEL_9, 9 }, + { DEFIO_TAG_E__PF5, ADC_DEVICES_3, ADC_CHANNEL_4, 4 }, + { DEFIO_TAG_E__PF6, ADC_DEVICES_3, ADC_CHANNEL_8, 8 }, + { DEFIO_TAG_E__PF7, ADC_DEVICES_3, ADC_CHANNEL_3, 3 }, + { DEFIO_TAG_E__PF8, ADC_DEVICES_3, ADC_CHANNEL_7, 7 }, + { DEFIO_TAG_E__PF9, ADC_DEVICES_3, ADC_CHANNEL_2, 2 }, + { DEFIO_TAG_E__PF10, ADC_DEVICES_3, ADC_CHANNEL_6, 6 }, + { DEFIO_TAG_E__PF11, ADC_DEVICES_1, ADC_CHANNEL_2, 2 }, + { DEFIO_TAG_E__PF12, ADC_DEVICES_1, ADC_CHANNEL_6, 6 }, + { DEFIO_TAG_E__PF13, ADC_DEVICES_2, ADC_CHANNEL_2, 2 }, + { DEFIO_TAG_E__PF14, ADC_DEVICES_2, ADC_CHANNEL_6, 6 }, +#endif +}; + +static void Error_Handler(void) { while (1) { } } + +// Note on sampling time. +// Temperature sensor has minimum sample time of 9us. +// With prescaler = 4 at 200MHz (AHB1), fADC = 50MHz (tcycle = 0.02us), 9us = 450cycles < 810 + +void adcInitDevice(adcDevice_t *adcdev, int channelCount) +{ + ADC_HandleTypeDef *hadc = &adcdev->ADCHandle; // For clarity + + hadc->Instance = adcdev->ADCx; + + if (HAL_ADC_DeInit(hadc) != HAL_OK) + { + // ADC de-initialization Error + Error_Handler(); + } + + hadc->Init.ClockPrescaler = ADC_CLOCK_SYNC_PCLK_DIV4; + hadc->Init.Resolution = ADC_RESOLUTION_12B; + hadc->Init.ScanConvMode = ENABLE; // Works with single channel, too + hadc->Init.EOCSelection = ADC_EOC_SINGLE_CONV; + hadc->Init.LowPowerAutoWait = DISABLE; + hadc->Init.ContinuousConvMode = ENABLE; + hadc->Init.NbrOfConversion = channelCount; + hadc->Init.DiscontinuousConvMode = DISABLE; + hadc->Init.NbrOfDiscConversion = 1; // Don't care + hadc->Init.ExternalTrigConv = ADC_SOFTWARE_START; + hadc->Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE; // Don't care + hadc->Init.ConversionDataManagement = ADC_CONVERSIONDATA_DMA_CIRCULAR; + hadc->Init.Overrun = ADC_OVR_DATA_OVERWRITTEN; + hadc->Init.OversamplingMode = DISABLE; + hadc->Init.BoostMode = ENABLE; // Enable Boost mode as ADC clock frequency is bigger than 20 MHz */ + + // Initialize this ADC peripheral + + if (HAL_ADC_Init(hadc) != HAL_OK) { + Error_Handler(); + } + + // Execute calibration + + if (HAL_ADCEx_Calibration_Start(hadc, ADC_CALIB_OFFSET, ADC_SINGLE_ENDED) != HAL_OK) { + Error_Handler(); + } +} + +int adcFindTagMapEntry(ioTag_t tag) +{ + for (int i = 0; i < ADC_TAG_MAP_COUNT; i++) { + if (adcTagMap[i].tag == tag) { + return i; + } + } + return -1; +} + +void adcInitCalibrationValues(void) +{ + adcVREFINTCAL = *(uint16_t *)VREFINT_CAL_ADDR >> 4; + adcTSCAL1 = *TEMPSENSOR_CAL1_ADDR >> 4; + adcTSCAL2 = *TEMPSENSOR_CAL2_ADDR >> 4; + adcTSSlopeK = (TEMPSENSOR_CAL2_TEMP - TEMPSENSOR_CAL1_TEMP) * 1000 / (adcTSCAL2 - adcTSCAL1); +} + +// ADC conversion result DMA buffer +// Need this separate from the main adcValue[] array, because channels are numbered +// by ADC instance order that is different from ADC_xxx numbering. + +static volatile DMA_RAM uint16_t adcConversionBuffer[ADC_CHANNEL_COUNT] __attribute__((aligned(32))); + +void adcInit(const adcConfig_t *config) +{ + memset(adcOperatingConfig, 0, sizeof(adcOperatingConfig)); + memcpy(adcDevice, adcHardware, sizeof(adcDevice)); + + if (config->vbat.enabled) { + adcOperatingConfig[ADC_BATTERY].tag = config->vbat.ioTag; + } + + if (config->rssi.enabled) { + adcOperatingConfig[ADC_RSSI].tag = config->rssi.ioTag; //RSSI_ADC_CHANNEL; + } + + if (config->external1.enabled) { + adcOperatingConfig[ADC_EXTERNAL1].tag = config->external1.ioTag; //EXTERNAL1_ADC_CHANNEL; + } + + if (config->current.enabled) { + adcOperatingConfig[ADC_CURRENT].tag = config->current.ioTag; //CURRENT_METER_ADC_CHANNEL; + } + +#ifdef USE_ADC_INTERNAL + adcInitCalibrationValues(); +#endif + + for (int i = 0; i < ADC_CHANNEL_COUNT; i++) { + int map; + int dev; + + if (i == ADC_TEMPSENSOR) { + map = ADC_TAG_MAP_TEMPSENSOR; + dev = ADCDEV_3; + } else if (i == ADC_VREFINT) { + map = ADC_TAG_MAP_VREFINT; + dev = ADCDEV_3; + } else { + if (!adcOperatingConfig[i].tag) { + continue; + } + + map = adcFindTagMapEntry(adcOperatingConfig[i].tag); + if (map < 0) { + continue; + } + + // Found a tag map entry for this input pin + // Find an ADC device that can handle this input pin + + for (dev = 0; dev < ADCDEV_COUNT; dev++) { + if (!(adcDevice[dev].ADCx && adcDevice[dev].DMAy_Streamx)) { + // Instance not activated + continue; + } + if (adcTagMap[map].devices & (1 << dev)) { + // Found an activated ADC instance for this input pin + break; + } + } + + if (dev == ADCDEV_COUNT) { + // No valid device found, go next channel. + continue; + } + } + + // At this point, map is an entry for the input pin and dev is a valid ADCx for the pin for input i + + adcOperatingConfig[i].adcDevice = dev; + adcOperatingConfig[i].adcChannel = adcTagMap[map].channel; + adcOperatingConfig[i].sampleTime = ADC_SAMPLETIME_810CYCLES_5; + adcOperatingConfig[i].enabled = true; + + adcDevice[dev].channelBits |= (1 << adcTagMap[map].channelOrdinal); + + // Configure a pin for ADC + if (adcOperatingConfig[i].tag) { + IOInit(IOGetByTag(adcOperatingConfig[i].tag), OWNER_ADC_BATT + i, 0); + IOConfigGPIO(IOGetByTag(adcOperatingConfig[i].tag), IO_CONFIG(GPIO_MODE_ANALOG, 0, GPIO_NOPULL)); + } + } + + // Configure ADCx with inputs + + int dmaBufferIndex = 0; + + for (int dev = 0; dev < ADCDEV_COUNT; dev++) { + adcDevice_t *adc = &adcDevice[dev]; + + if (!(adc->ADCx && adc->channelBits)) { + continue; + } + + RCC_ClockCmd(adc->rccADC, ENABLE); + +#ifdef USE_ADC3_DIRECT_HAL_INIT + // XXX (Only) ADC3 (sometimes) fails to self calibrate without these? Need to verify + + // ADC Periph clock enable + __HAL_RCC_ADC3_CLK_ENABLE(); + + // ADC Periph interface clock configuration + __HAL_RCC_ADC_CONFIG(RCC_ADCCLKSOURCE_CLKP); +#endif + + int configuredAdcChannels = BITCOUNT(adc->channelBits); + + adcInitDevice(adc, configuredAdcChannels); + + // Configure channels + + int rank = 1; + + for (int adcChan = 0; adcChan < ADC_CHANNEL_COUNT; adcChan++) { + + if (!adcOperatingConfig[adcChan].enabled) { + continue; + } + + if (adcOperatingConfig[adcChan].adcDevice != dev) { + continue; + } + + adcOperatingConfig[adcChan].dmaIndex = dmaBufferIndex++; + + ADC_ChannelConfTypeDef sConfig; + + sConfig.Channel = adcOperatingConfig[adcChan].adcChannel; /* Sampled channel number */ + sConfig.Rank = rank++; /* Rank of sampled channel number ADCx_CHANNEL */ + sConfig.SamplingTime = ADC_SAMPLETIME_387CYCLES_5; /* Sampling time (number of clock cycles unit) */ + sConfig.SingleDiff = ADC_SINGLE_ENDED; /* Single-ended input channel */ + sConfig.OffsetNumber = ADC_OFFSET_NONE; /* No offset subtraction */ + sConfig.Offset = 0; /* Parameter discarded because offset correction is disabled */ + + if (HAL_ADC_ConfigChannel(&adc->ADCHandle, &sConfig) != HAL_OK) { + Error_Handler(); + } + } + + // Configure DMA for this ADC peripheral + + dmaIdentifier_e dmaIdentifier = dmaGetIdentifier(adc->DMAy_Streamx); + dmaInit(dmaIdentifier, OWNER_ADC, RESOURCE_INDEX(dev)); + + adc->DmaHandle.Instance = adc->DMAy_Streamx; + adc->DmaHandle.Init.Request = adc->request; + adc->DmaHandle.Init.Direction = DMA_PERIPH_TO_MEMORY; + adc->DmaHandle.Init.PeriphInc = DMA_PINC_DISABLE; + adc->DmaHandle.Init.MemInc = DMA_MINC_ENABLE; + adc->DmaHandle.Init.PeriphDataAlignment = DMA_PDATAALIGN_HALFWORD; + adc->DmaHandle.Init.MemDataAlignment = DMA_MDATAALIGN_HALFWORD; + adc->DmaHandle.Init.Mode = DMA_CIRCULAR; + adc->DmaHandle.Init.Priority = DMA_PRIORITY_MEDIUM; + + // Deinitialize & Initialize the DMA for new transfer + + HAL_DMA_DeInit(&adc->DmaHandle); + HAL_DMA_Init(&adc->DmaHandle); + + // Associate the DMA handle + + __HAL_LINKDMA(&adc->ADCHandle, DMA_Handle, adc->DmaHandle); + +#ifdef USE_ADC_INTERRUPT + // XXX No interrupt used, so we can skip this. + // If interrupt is needed in any case, use dmaXXX facility instead, + // using dmaIdentifier obtained above. + + // NVIC configuration for DMA Input data interrupt + + HAL_NVIC_SetPriority(DMA1_Stream1_IRQn, 1, 0); + HAL_NVIC_EnableIRQ(DMA1_Stream1_IRQn); +#endif + } + + // Start channels. + // This must be done after channel configuration is complete, as HAL_ADC_ConfigChannel + // throws an error when configuring internal channels if ADC1 or ADC2 are already enabled. + + dmaBufferIndex = 0; + + for (int dev = 0; dev < ADCDEV_COUNT; dev++) { + + adcDevice_t *adc = &adcDevice[dev]; + + if (!(adc->ADCx && adc->channelBits)) { + continue; + } + + // Start conversion in DMA mode + + if (HAL_ADC_Start_DMA(&adc->ADCHandle, (uint32_t *)&adcConversionBuffer[dmaBufferIndex], BITCOUNT(adc->channelBits)) != HAL_OK) { + Error_Handler(); + } + + dmaBufferIndex += BITCOUNT(adc->channelBits); + } +} + +void adcGetChannelValues(void) +{ + // Transfer values in conversion buffer into adcValues[] + // Cache coherency should be maintained by MPU facility + + for (int i = 0; i < ADC_CHANNEL_INTERNAL; i++) { + adcValues[i] = adcConversionBuffer[adcOperatingConfig[i].dmaIndex]; + } +} + +#ifdef USE_ADC_INTERNAL + +bool adcInternalIsBusy(void) +{ + return false; +} + +void adcInternalStartConversion(void) +{ + return; +} + +uint16_t adcInternalRead(int channel) +{ + int dmaIndex = adcOperatingConfig[channel].dmaIndex; + + return adcConversionBuffer[dmaIndex]; +} + +uint16_t adcInternalReadVrefint(void) +{ + uint16_t value = adcInternalRead(ADC_VREFINT); + return value; +} + +uint16_t adcInternalReadTempsensor(void) +{ + uint16_t value = adcInternalRead(ADC_TEMPSENSOR); + return value; +} +#endif // USE_ADC_INTERNAL + +#endif // USE_ADC