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