mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-15 12:25:20 +03:00
commit
d3c1ba11ba
8 changed files with 518 additions and 4 deletions
|
@ -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];
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -163,4 +163,9 @@ void adcInit(const adcConfig_t *config)
|
|||
|
||||
ADC_SoftwareStartConvCmd(adc.ADCx, ENABLE);
|
||||
}
|
||||
|
||||
void adcGetChannelValues(void)
|
||||
{
|
||||
// Nothing to do
|
||||
}
|
||||
#endif
|
||||
|
|
|
@ -261,4 +261,9 @@ void adcInit(const adcConfig_t *config)
|
|||
|
||||
ADC_StartConversion(adc.ADCx);
|
||||
}
|
||||
|
||||
void adcGetChannelValues(void)
|
||||
{
|
||||
// Nothing to do
|
||||
}
|
||||
#endif
|
||||
|
|
|
@ -350,4 +350,9 @@ void adcInit(const adcConfig_t *config)
|
|||
|
||||
ADC_SoftwareStartConv(adc.ADCx);
|
||||
}
|
||||
|
||||
void adcGetChannelValues(void)
|
||||
{
|
||||
// Nothing to do
|
||||
}
|
||||
#endif
|
||||
|
|
|
@ -350,4 +350,9 @@ void adcInit(const adcConfig_t *config)
|
|||
/* Start Conversion Error */
|
||||
}
|
||||
}
|
||||
|
||||
void adcGetChannelValues(void)
|
||||
{
|
||||
// Nothing to do
|
||||
}
|
||||
#endif
|
||||
|
|
463
src/main/drivers/adc_stm32h7xx.c
Normal file
463
src/main/drivers/adc_stm32h7xx.c
Normal file
|
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <string.h>
|
||||
|
||||
#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
|
Loading…
Add table
Add a link
Reference in a new issue