From d2bc4e5af5e9a3c95f94d7dd3b88aa398a12c934 Mon Sep 17 00:00:00 2001 From: jflyper Date: Tue, 20 Mar 2018 20:52:37 +0900 Subject: [PATCH] F7 Internal ADC (VREFINT and TEMPSENSOR) (#5322) --- src/main/drivers/adc_stm32f7xx.c | 207 +++++++++++++++++++++++++------ src/main/target/common_fc_post.h | 5 + src/main/target/common_fc_pre.h | 1 + 3 files changed, 174 insertions(+), 39 deletions(-) diff --git a/src/main/drivers/adc_stm32f7xx.c b/src/main/drivers/adc_stm32f7xx.c index 318d566794..f0fc1442c0 100644 --- a/src/main/drivers/adc_stm32f7xx.c +++ b/src/main/drivers/adc_stm32f7xx.c @@ -43,6 +43,31 @@ #define ADC_INSTANCE ADC1 #endif +#ifndef ADC1_DMA_STREAM +#define ADC1_DMA_STREAM DMA2_Stream4 +#endif + +// 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). */ + +// These addresses are incorrectly defined in stm32f7xx_ll_adc.h + +#if defined(STM32F745xx) || defined(STM32F746xx) +// F745xx_F746xx +#define VREFINT_CAL_ADDR ((uint16_t*) (0x1FF0F44A)) +#define TEMPSENSOR_CAL1_ADDR ((uint16_t*) (0x1FF0F44C)) +#define TEMPSENSOR_CAL2_ADDR ((uint16_t*) (0x1FF0F44E)) +#elif defined(STM32F722xx) +// F72x_F73x +#define VREFINT_CAL_ADDR ((uint16_t*) (0x1FF07A2A)) +#define TEMPSENSOR_CAL1_ADDR ((uint16_t*) (0x1FF07A2C)) +#define TEMPSENSOR_CAL2_ADDR ((uint16_t*) (0x1FF07A2E)) +#endif + const adcDevice_t adcHardware[] = { { .ADCx = ADC1, .rccADC = RCC_APB2(ADC1), .DMAy_Streamx = ADC1_DMA_STREAM, .channel = DMA_CHANNEL_0 }, { .ADCx = ADC2, .rccADC = RCC_APB2(ADC2), .DMAy_Streamx = ADC2_DMA_STREAM, .channel = DMA_CHANNEL_1 }, @@ -79,6 +104,113 @@ const adcTagMap_t adcTagMap[] = { { DEFIO_TAG_E__PA7, ADC_DEVICES_12, ADC_CHANNEL_7 }, }; +void adcInitDevice(adcDevice_t *adcdev, int channelCount) +{ + adcdev->ADCHandle.Init.ClockPrescaler = ADC_CLOCK_SYNC_PCLK_DIV8; + adcdev->ADCHandle.Init.ContinuousConvMode = ENABLE; + adcdev->ADCHandle.Init.Resolution = ADC_RESOLUTION_12B; + adcdev->ADCHandle.Init.ExternalTrigConv = ADC_EXTERNALTRIGCONV_T1_CC1; + adcdev->ADCHandle.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE; + adcdev->ADCHandle.Init.DataAlign = ADC_DATAALIGN_RIGHT; + adcdev->ADCHandle.Init.NbrOfConversion = channelCount; +#ifdef USE_ADC_INTERNAL + // Multiple injected channel seems to require scan conversion mode to be + // enabled even if main (non-injected) channel count is 1. + adcdev->ADCHandle.Init.ScanConvMode = ENABLE; +#else + adcdev->ADCHandle.Init.ScanConvMode = channelCount > 1 ? ENABLE : DISABLE; // 1=scan more that one channel in group +#endif + adcdev->ADCHandle.Init.DiscontinuousConvMode = DISABLE; + adcdev->ADCHandle.Init.NbrOfDiscConversion = 0; + adcdev->ADCHandle.Init.DMAContinuousRequests = ENABLE; + adcdev->ADCHandle.Init.EOCSelection = DISABLE; + adcdev->ADCHandle.Instance = adcdev->ADCx; + + if (HAL_ADC_Init(&adcdev->ADCHandle) != HAL_OK) + { + /* Initialization Error */ + } +} + +static adcDevice_t adc; + +#ifdef USE_ADC_INTERNAL + +static adcDevice_t adcInternal; +static ADC_HandleTypeDef *adcInternalHandle; + +void adcInitInternalInjected(adcDevice_t *adcdev) +{ + adcInternalHandle = &adcdev->ADCHandle; + + ADC_InjectionConfTypeDef iConfig; + + iConfig.InjectedChannel = ADC_CHANNEL_VREFINT; + iConfig.InjectedRank = 1; + iConfig.InjectedSamplingTime = ADC_SAMPLETIME_480CYCLES; + iConfig.InjectedOffset = 0; + iConfig.InjectedNbrOfConversion = 2; + iConfig.InjectedDiscontinuousConvMode = DISABLE; + iConfig.AutoInjectedConv = DISABLE; + iConfig.ExternalTrigInjecConv = 0; // Don't care + iConfig.ExternalTrigInjecConvEdge = 0; // Don't care + + if (HAL_ADCEx_InjectedConfigChannel(adcInternalHandle, &iConfig) != HAL_OK) { + /* Channel Configuration Error */ + } + + iConfig.InjectedChannel = ADC_CHANNEL_TEMPSENSOR; + iConfig.InjectedRank = 2; + + if (HAL_ADCEx_InjectedConfigChannel(adcInternalHandle, &iConfig) != HAL_OK) { + /* Channel Configuration Error */ + } + + adcVREFINTCAL = *(uint16_t *)VREFINT_CAL_ADDR; + adcTSCAL1 = *TEMPSENSOR_CAL1_ADDR; + adcTSCAL2 = *TEMPSENSOR_CAL2_ADDR; + adcTSSlopeK = (TEMPSENSOR_CAL2_TEMP - TEMPSENSOR_CAL1_TEMP) * 1000 / (adcTSCAL2 - adcTSCAL1); +} + +// Note on sampling time for temperature sensor and vrefint: +// Both sources have minimum sample time of 10us. +// With prescaler = 8: +// 168MHz : fAPB2 = 84MHz, fADC = 10.5MHz, tcycle = 0.090us, 10us = 105cycle < 144cycle +// 240MHz : fAPB2 = 120MHz, fADC = 15.0MHz, tcycle = 0.067usk 10us = 150cycle < 480cycle +// +// 480cycles@15.0MHz = 32us + +static bool adcInternalConversionInProgress = false; + +bool adcInternalIsBusy(void) +{ + if (adcInternalConversionInProgress) { + if (HAL_ADCEx_InjectedPollForConversion(adcInternalHandle, 0) == HAL_OK) { + adcInternalConversionInProgress = false; + } + } + + return adcInternalConversionInProgress; +} + +void adcInternalStartConversion(void) +{ + HAL_ADCEx_InjectedStart(adcInternalHandle); + + adcInternalConversionInProgress = true; +} + +uint16_t adcInternalReadVrefint(void) +{ + return HAL_ADCEx_InjectedGetValue(adcInternalHandle, ADC_INJECTED_RANK_1); +} + +uint16_t adcInternalReadTempsensor(void) +{ + return HAL_ADCEx_InjectedGetValue(adcInternalHandle, ADC_INJECTED_RANK_2); +} +#endif + void adcInit(const adcConfig_t *config) { uint8_t i; @@ -106,7 +238,7 @@ void adcInit(const adcConfig_t *config) if (device == ADCINVALID) return; - adcDevice_t adc = adcHardware[device]; + adc = adcHardware[device]; bool adcActive = false; for (int i = 0; i < ADC_CHANNEL_COUNT; i++) { @@ -123,32 +255,48 @@ void adcInit(const adcConfig_t *config) adcOperatingConfig[i].enabled = true; } +#ifndef USE_ADC_INTERNAL if (!adcActive) { return; } +#endif RCC_ClockCmd(adc.rccADC, ENABLE); - dmaInit(dmaGetIdentifier(adc.DMAy_Streamx), OWNER_ADC, 0); - adc.ADCHandle.Init.ClockPrescaler = ADC_CLOCK_SYNC_PCLK_DIV8; - adc.ADCHandle.Init.ContinuousConvMode = ENABLE; - adc.ADCHandle.Init.Resolution = ADC_RESOLUTION_12B; - adc.ADCHandle.Init.ExternalTrigConv = ADC_EXTERNALTRIGCONV_T1_CC1; - adc.ADCHandle.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE; - adc.ADCHandle.Init.DataAlign = ADC_DATAALIGN_RIGHT; - adc.ADCHandle.Init.NbrOfConversion = configuredAdcChannels; - adc.ADCHandle.Init.ScanConvMode = configuredAdcChannels > 1 ? ENABLE : DISABLE; // 1=scan more that one channel in group - adc.ADCHandle.Init.DiscontinuousConvMode = DISABLE; - adc.ADCHandle.Init.NbrOfDiscConversion = 0; - adc.ADCHandle.Init.DMAContinuousRequests = ENABLE; - adc.ADCHandle.Init.EOCSelection = DISABLE; - adc.ADCHandle.Instance = adc.ADCx; + adcInitDevice(&adc, configuredAdcChannels); - /*##-1- Configure the ADC peripheral #######################################*/ - if (HAL_ADC_Init(&adc.ADCHandle) != HAL_OK) - { - /* Initialization Error */ +#ifdef USE_ADC_INTERNAL + // If device is not ADC1 or there's no active channel, then initialize ADC1 here. + if (device != ADCDEV_1 || !adcActive) { + adcInternal = adcHardware[ADCDEV_1]; + RCC_ClockCmd(adcInternal.rccADC, ENABLE); + adcInitDevice(&adcInternal, 0); + adcInitInternalInjected(&adcInternal); + } else { + adcInitInternalInjected(&adc); } +#endif + + uint8_t rank = 1; + for (i = 0; i < ADC_CHANNEL_COUNT; i++) { + if (!adcOperatingConfig[i].enabled) { + continue; + } + + ADC_ChannelConfTypeDef sConfig; + + sConfig.Channel = adcOperatingConfig[i].adcChannel; + sConfig.Rank = rank++; + sConfig.SamplingTime = adcOperatingConfig[i].sampleTime; + sConfig.Offset = 0; + + if (HAL_ADC_ConfigChannel(&adc.ADCHandle, &sConfig) != HAL_OK) + { + /* Channel Configuration Error */ + } + } + + dmaInit(dmaGetIdentifier(adc.DMAy_Streamx), OWNER_ADC, 0); adc.DmaHandle.Init.Channel = adc.channel; adc.DmaHandle.Init.Direction = DMA_PERIPH_TO_MEMORY; @@ -164,7 +312,6 @@ void adcInit(const adcConfig_t *config) adc.DmaHandle.Init.PeriphBurst = DMA_PBURST_SINGLE; adc.DmaHandle.Instance = adc.DMAy_Streamx; - /*##-2- Initialize the DMA stream ##########################################*/ if (HAL_DMA_Init(&adc.DmaHandle) != HAL_OK) { /* Initialization Error */ @@ -172,26 +319,8 @@ void adcInit(const adcConfig_t *config) __HAL_LINKDMA(&adc.ADCHandle, DMA_Handle, adc.DmaHandle); - uint8_t rank = 1; - for (i = 0; i < ADC_CHANNEL_COUNT; i++) { - if (!adcOperatingConfig[i].enabled) { - continue; - } - ADC_ChannelConfTypeDef sConfig; - sConfig.Channel = adcOperatingConfig[i].adcChannel; - sConfig.Rank = rank++; - sConfig.SamplingTime = adcOperatingConfig[i].sampleTime; - sConfig.Offset = 0; - - /*##-3- Configure ADC regular channel ######################################*/ - if (HAL_ADC_ConfigChannel(&adc.ADCHandle, &sConfig) != HAL_OK) - { - /* Channel Configuration Error */ - } - } - //HAL_CLEANINVALIDATECACHE((uint32_t*)&adcValues, configuredAdcChannels); - /*##-4- Start the conversion process #######################################*/ + if (HAL_ADC_Start_DMA(&adc.ADCHandle, (uint32_t*)&adcValues, configuredAdcChannels) != HAL_OK) { /* Start Conversation Error */ diff --git a/src/main/target/common_fc_post.h b/src/main/target/common_fc_post.h index 9275dd5f24..1c4708c349 100644 --- a/src/main/target/common_fc_post.h +++ b/src/main/target/common_fc_post.h @@ -97,3 +97,8 @@ #ifndef ENABLE_DSHOT_DMAR #define ENABLE_DSHOT_DMAR false #endif + +// Some target doesn't define USE_ADC which USE_ADC_INTERNAL depends on +#ifndef USE_ADC +#undef USE_ADC_INTERNAL +#endif diff --git a/src/main/target/common_fc_pre.h b/src/main/target/common_fc_pre.h index 83ee843666..f9c98ee04c 100644 --- a/src/main/target/common_fc_pre.h +++ b/src/main/target/common_fc_pre.h @@ -68,6 +68,7 @@ #define I2C4_OVERCLOCK true #define USE_GYRO_DATA_ANALYSE #define USE_OVERCLOCK +#define USE_ADC_INTERNAL #endif #if defined(STM32F4) || defined(STM32F7)