diff --git a/src/main/drivers/adc.c b/src/main/drivers/adc.c index 8e9b52d9dc..93978a17a3 100644 --- a/src/main/drivers/adc.c +++ b/src/main/drivers/adc.c @@ -46,13 +46,6 @@ volatile FAST_RAM_ZERO_INIT uint16_t adcValues[ADC_CHANNEL_COUNT]; volatile uint16_t adcValues[ADC_CHANNEL_COUNT]; #endif -#ifdef USE_ADC_INTERNAL -uint16_t adcTSCAL1; -uint16_t adcTSCAL2; -int16_t adcTSSlopeK; -uint16_t adcVREFINTCAL; -#endif - uint8_t adcChannelByTag(ioTag_t ioTag) { for (uint8_t i = 0; i < ARRAYLEN(adcTagMap); i++) { @@ -132,6 +125,29 @@ bool adcVerifyPin(ioTag_t tag, ADCDevice device) return false; } +#ifdef USE_ADC_INTERNAL + +int32_t adcVREFINTCAL; // ADC value (12-bit) of band gap with Vref = VREFINTCAL_VREF +int32_t adcTSCAL1; +int32_t adcTSCAL2; +int32_t adcTSSlopeK; + +uint16_t adcInternalCompensateVref(uint16_t vrefAdcValue) +{ + // This is essentially a tuned version of + // __HAL_ADC_CALC_VREFANALOG_VOLTAGE(vrefAdcValue, ADC_RESOLUTION_12B); + return (uint16_t)((uint32_t)(adcVREFINTCAL * VREFINT_CAL_VREF) / vrefAdcValue); +} + +int16_t adcInternalComputeTemperature(uint16_t tempAdcValue, uint16_t vrefValue) +{ + // This is essentially a tuned version of + // __HAL_ADC_CALC_TEMPERATURE(vrefValue, tempAdcValue, ADC_RESOLUTION_12B); + + return ((((int32_t)((tempAdcValue * vrefValue) / TEMPSENSOR_CAL_VREFANALOG) - adcTSCAL1) * adcTSSlopeK) + 500) / 1000 + TEMPSENSOR_CAL1_TEMP; +} +#endif // USE_ADC_INTERNAL + #else uint16_t adcGetChannel(uint8_t channel) { diff --git a/src/main/drivers/adc.h b/src/main/drivers/adc.h index 48c258c64b..081b136ba6 100644 --- a/src/main/drivers/adc.h +++ b/src/main/drivers/adc.h @@ -91,15 +91,12 @@ void adcInit(const struct adcConfig_s *config); uint16_t adcGetChannel(uint8_t channel); #ifdef USE_ADC_INTERNAL -extern uint16_t adcVREFINTCAL; -extern uint16_t adcTSCAL1; -extern uint16_t adcTSCAL2; -extern int16_t adcTSSlopeK; - bool adcInternalIsBusy(void); void adcInternalStartConversion(void); uint16_t adcInternalReadVrefint(void); uint16_t adcInternalReadTempsensor(void); +uint16_t adcInternalCompensateVref(uint16_t vrefAdcValue); +int16_t adcInternalComputeTemperature(uint16_t tempAdcValue, uint16_t vrefValue); #endif #if !defined(SIMULATOR_BUILD) diff --git a/src/main/drivers/adc_impl.h b/src/main/drivers/adc_impl.h index 30b2169521..1411eb24ef 100644 --- a/src/main/drivers/adc_impl.h +++ b/src/main/drivers/adc_impl.h @@ -80,6 +80,13 @@ typedef struct adcDevice_s { #endif } adcDevice_t; +#ifdef USE_ADC_INTERNAL +extern int32_t adcVREFINTCAL; // ADC value (12-bit) of band gap with Vref = VREFINTCAL_VREF +extern int32_t adcTSCAL1; +extern int32_t adcTSCAL2; +extern int32_t adcTSSlopeK; +#endif + extern const adcDevice_t adcHardware[]; extern const adcTagMap_t adcTagMap[ADC_TAG_MAP_COUNT]; extern adcOperatingConfig_t adcOperatingConfig[ADC_CHANNEL_COUNT]; @@ -92,3 +99,40 @@ 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); + +// +// VREFINT and TEMPSENSOR related definitions +// These are shared among common adc.c and MCU dependent adc_stm32XXX.c +// +#ifdef STM32F7 +// STM32F7 HAL library V1.12.0 defines VREFINT and TEMPSENSOR in stm32f7xx_ll_adc.h, +// which is not included from stm32f7xx_hal_adc.h +// We manually copy required lines here. +// XXX V1.14.0 may solve this problem + +#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) || defined(STM32F765xx) +// F745xx_F746xx and F765xx_F767xx_F769xx +#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 +#endif // STM32F7 + +#ifdef STM32F4 +// STM32F4 stdlib does not define any of these +#define VREFINT_CAL_VREF (3300U) +#define TEMPSENSOR_CAL_VREFANALOG (3300U) +#define TEMPSENSOR_CAL1_TEMP ((int32_t) 30) +#define TEMPSENSOR_CAL2_TEMP ((int32_t) 110) +#endif diff --git a/src/main/drivers/adc_stm32f7xx.c b/src/main/drivers/adc_stm32f7xx.c index cd041b2ea6..cda29a8ec5 100644 --- a/src/main/drivers/adc_stm32f7xx.c +++ b/src/main/drivers/adc_stm32f7xx.c @@ -38,26 +38,6 @@ #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). */ - -// These addresses are incorrectly defined in stm32f7xx_ll_adc.h -#if defined(STM32F745xx) || defined(STM32F746xx) || defined(STM32F765xx) -// F745xx_F746xx and F765xx_F767xx_F769xx -#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, diff --git a/src/main/sensors/adcinternal.c b/src/main/sensors/adcinternal.c index 5e5904e90d..cad059dc89 100644 --- a/src/main/sensors/adcinternal.c +++ b/src/main/sensors/adcinternal.c @@ -87,10 +87,8 @@ void adcInternalProcess(timeUs_t currentTimeUs) adcVrefintValue = updateMovingAverageUint16(&adcVrefintAverageState, vrefintSample); adcTempsensorValue = updateMovingAverageUint16(&adcTempsensorAverageState, tempsensorSample); - vrefMv = 3300 * adcVrefintValue / adcVREFINTCAL; - - int32_t adcTempsensorAdjusted = (int32_t)(adcTempsensorValue * 3300) / vrefMv; - coreTemperature = ((adcTempsensorAdjusted - adcTSCAL1) * adcTSSlopeK + 30 * 1000 + 500) / 1000; + vrefMv = adcInternalCompensateVref(adcVrefintValue); + coreTemperature = adcInternalComputeTemperature(adcTempsensorValue, vrefMv); DEBUG_SET(DEBUG_ADC_INTERNAL, 0, coreTemperature); DEBUG_SET(DEBUG_ADC_INTERNAL, 1, vrefintSample); diff --git a/src/main/target/OMNINXT/hardware_revision.c b/src/main/target/OMNINXT/hardware_revision.c index 32b1e186bd..5e8d2ac137 100644 --- a/src/main/target/OMNINXT/hardware_revision.c +++ b/src/main/target/OMNINXT/hardware_revision.c @@ -150,7 +150,6 @@ static uint16_t adcIDDetectReadVrefint(void) #endif #if defined(OMNINXT7) -#define VREFINT_CAL_ADDR 0x1FF07A2A #include "drivers/adc_impl.h"