diff --git a/make/source.mk b/make/source.mk index dda6cba09..348d8ee92 100644 --- a/make/source.mk +++ b/make/source.mk @@ -71,6 +71,7 @@ COMMON_SRC = \ pg/sdcard.c \ pg/vcd.c \ scheduler/scheduler.c \ + sensors/adcinternal.c \ sensors/battery.c \ sensors/current.c \ sensors/voltage.c \ diff --git a/src/main/build/debug.c b/src/main/build/debug.c index 204e1f2e4..d32c94f86 100644 --- a/src/main/build/debug.c +++ b/src/main/build/debug.c @@ -55,4 +55,5 @@ const char * const debugModeNames[DEBUG_COUNT] = { "RANGEFINDER", "RANGEFINDER_QUALITY", "LIDAR_TF", + "CORE_TEMP", }; diff --git a/src/main/build/debug.h b/src/main/build/debug.h index 210ed54f4..072fdcde0 100644 --- a/src/main/build/debug.h +++ b/src/main/build/debug.h @@ -73,6 +73,7 @@ typedef enum { DEBUG_RANGEFINDER, DEBUG_RANGEFINDER_QUALITY, DEBUG_LIDAR_TF, + DEBUG_CORE_TEMP, DEBUG_COUNT } debugType_e; diff --git a/src/main/drivers/adc.c b/src/main/drivers/adc.c index 67c13ebb9..904faa68c 100644 --- a/src/main/drivers/adc.c +++ b/src/main/drivers/adc.c @@ -19,11 +19,10 @@ #include #include "platform.h" +#include "common/utils.h" #ifdef USE_ADC -#include "common/utils.h" - #include "build/build_config.h" #include "build/debug.h" @@ -34,12 +33,18 @@ #include "adc.h" - //#define DEBUG_ADC_CHANNELS adcOperatingConfig_t adcOperatingConfig[ADC_CHANNEL_COUNT]; volatile uint16_t adcValues[ADC_CHANNEL_COUNT]; +#ifdef USE_ADC_INTERNAL +uint16_t adcTSCAL1; +uint16_t adcTSCAL2; +uint16_t adcTSSlopeK; +uint16_t adcVREFINTCAL; +#endif + uint8_t adcChannelByTag(ioTag_t ioTag) { for (uint8_t i = 0; i < ARRAYLEN(adcTagMap); i++) { @@ -116,4 +121,11 @@ bool adcVerifyPin(ioTag_t tag, ADCDevice device) return false; } + +#else +uint16_t adcGetChannel(uint8_t channel) +{ + UNUSED(channel); + return 0; +} #endif diff --git a/src/main/drivers/adc.h b/src/main/drivers/adc.h index dd7309c43..2a8f88ee6 100644 --- a/src/main/drivers/adc.h +++ b/src/main/drivers/adc.h @@ -20,6 +20,7 @@ #include #include "drivers/io_types.h" +#include "drivers/time.h" #ifndef ADC_INSTANCE #define ADC_INSTANCE ADC1 @@ -75,6 +76,18 @@ struct adcConfig_s; 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 uint16_t adcTSSlopeK; + +bool adcInternalIsBusy(void); +void adcInternalStartConversion(void); +uint16_t adcInternalReadVrefint(void); +uint16_t adcInternalReadTempsensor(void); +#endif + #ifndef SITL ADCDevice adcDeviceByInstance(ADC_TypeDef *instance); #endif diff --git a/src/main/drivers/adc_stm32f4xx.c b/src/main/drivers/adc_stm32f4xx.c index 51cf5a9ba..ca4ac4a08 100644 --- a/src/main/drivers/adc_stm32f4xx.c +++ b/src/main/drivers/adc_stm32f4xx.c @@ -23,6 +23,8 @@ #ifdef USE_ADC +#include "build/debug.h" + #include "drivers/accgyro/accgyro.h" #include "drivers/system.h" @@ -96,11 +98,84 @@ const adcTagMap_t adcTagMap[] = { #endif }; -void adcInit(const adcConfig_t *config) +#define VREFINT_CAL_ADDR 0x1FFF7A2A +#define TS_CAL1_ADDR 0x1FFF7A2C +#define TS_CAL2_ADDR 0x1FFF7A2E + +void adcInitDevice(ADC_TypeDef *adcdev, int channelCount) { ADC_InitTypeDef ADC_InitStructure; - DMA_InitTypeDef DMA_InitStructure; + ADC_StructInit(&ADC_InitStructure); + + ADC_InitStructure.ADC_ContinuousConvMode = ENABLE; + ADC_InitStructure.ADC_Resolution = ADC_Resolution_12b; + ADC_InitStructure.ADC_ExternalTrigConv = ADC_ExternalTrigConv_T1_CC1; + ADC_InitStructure.ADC_ExternalTrigConvEdge = ADC_ExternalTrigConvEdge_None; + ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right; + ADC_InitStructure.ADC_NbrOfConversion = channelCount; + ADC_InitStructure.ADC_ScanConvMode = channelCount > 1 ? ENABLE : DISABLE; // 1=scan more that one channel in group + + ADC_Init(adcdev, &ADC_InitStructure); +} + +#ifdef USE_ADC_INTERNAL +void adcInitInternalInjected(void) +{ + ADC_TempSensorVrefintCmd(ENABLE); + ADC_InjectedDiscModeCmd(ADC1, DISABLE); + ADC_InjectedSequencerLengthConfig(ADC1, 2); + ADC_InjectedChannelConfig(ADC1, ADC_Channel_Vrefint, 1, ADC_SampleTime_480Cycles); + ADC_InjectedChannelConfig(ADC1, ADC_Channel_TempSensor, 2, ADC_SampleTime_480Cycles); + + adcVREFINTCAL = *(uint16_t *)VREFINT_CAL_ADDR; + adcTSCAL1 = *(uint16_t *)TS_CAL1_ADDR; + adcTSCAL2 = *(uint16_t *)TS_CAL2_ADDR; + adcTSSlopeK = (110 - 30) * 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 (ADC_GetFlagStatus(ADC1, ADC_FLAG_JEOC) != RESET) { + adcInternalConversionInProgress = false; + } + } + + return adcInternalConversionInProgress; +} + +void adcInternalStartConversion(void) +{ + ADC_ClearFlag(ADC1, ADC_FLAG_JEOC); + ADC_SoftwareStartInjectedConv(ADC1); + + adcInternalConversionInProgress = true; +} + +uint16_t adcInternalReadVrefint(void) +{ + return ADC_GetInjectedConversionValue(ADC1, ADC_InjectedChannel_1); +} + +uint16_t adcInternalReadTempsensor(void) +{ + return ADC_GetInjectedConversionValue(ADC1, ADC_InjectedChannel_2); +} +#endif + +void adcInit(const adcConfig_t *config) +{ uint8_t i; uint8_t configuredAdcChannels = 0; @@ -149,10 +224,47 @@ void adcInit(const adcConfig_t *config) RCC_ClockCmd(adc.rccADC, ENABLE); + ADC_CommonInitTypeDef ADC_CommonInitStructure; + + ADC_CommonStructInit(&ADC_CommonInitStructure); + ADC_CommonInitStructure.ADC_Mode = ADC_Mode_Independent; + ADC_CommonInitStructure.ADC_Prescaler = ADC_Prescaler_Div8; + ADC_CommonInitStructure.ADC_DMAAccessMode = ADC_DMAAccessMode_Disabled; + ADC_CommonInitStructure.ADC_TwoSamplingDelay = ADC_TwoSamplingDelay_5Cycles; + ADC_CommonInit(&ADC_CommonInitStructure); + + adcInitDevice(adc.ADCx, configuredAdcChannels); + + uint8_t rank = 1; + for (i = 0; i < ADC_CHANNEL_COUNT; i++) { + if (!adcOperatingConfig[i].enabled) { + continue; + } + ADC_RegularChannelConfig(adc.ADCx, adcOperatingConfig[i].adcChannel, rank++, adcOperatingConfig[i].sampleTime); + } + ADC_DMARequestAfterLastTransferCmd(adc.ADCx, ENABLE); + + ADC_DMACmd(adc.ADCx, ENABLE); + ADC_Cmd(adc.ADCx, ENABLE); + +#ifdef USE_ADC_INTERNAL + // If device is not ADC1, then initialize ADC1 separately + if (device != ADCDEV_1) { + RCC_ClockCmd(adcHardware[ADCDEV_1].rccADC, ENABLE); + adcInitDevice(ADC1, 2); + ADC_Cmd(ADC1, ENABLE); + } + + // Initialize for injected conversion + adcInitInternalInjected(); +#endif + dmaInit(dmaGetIdentifier(adc.DMAy_Streamx), OWNER_ADC, 0); DMA_DeInit(adc.DMAy_Streamx); + DMA_InitTypeDef DMA_InitStructure; + DMA_StructInit(&DMA_InitStructure); DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&adc.ADCx->DR; DMA_InitStructure.DMA_Channel = adc.channel; @@ -169,39 +281,6 @@ void adcInit(const adcConfig_t *config) DMA_Cmd(adc.DMAy_Streamx, ENABLE); - ADC_CommonInitTypeDef ADC_CommonInitStructure; - - ADC_CommonStructInit(&ADC_CommonInitStructure); - ADC_CommonInitStructure.ADC_Mode = ADC_Mode_Independent; - ADC_CommonInitStructure.ADC_Prescaler = ADC_Prescaler_Div8; - ADC_CommonInitStructure.ADC_DMAAccessMode = ADC_DMAAccessMode_Disabled; - ADC_CommonInitStructure.ADC_TwoSamplingDelay = ADC_TwoSamplingDelay_5Cycles; - ADC_CommonInit(&ADC_CommonInitStructure); - - ADC_StructInit(&ADC_InitStructure); - - ADC_InitStructure.ADC_ContinuousConvMode = ENABLE; - ADC_InitStructure.ADC_Resolution = ADC_Resolution_12b; - ADC_InitStructure.ADC_ExternalTrigConv = ADC_ExternalTrigConv_T1_CC1; - ADC_InitStructure.ADC_ExternalTrigConvEdge = ADC_ExternalTrigConvEdge_None; - ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right; - ADC_InitStructure.ADC_NbrOfConversion = configuredAdcChannels; - ADC_InitStructure.ADC_ScanConvMode = configuredAdcChannels > 1 ? ENABLE : DISABLE; // 1=scan more that one channel in group - - ADC_Init(adc.ADCx, &ADC_InitStructure); - - uint8_t rank = 1; - for (i = 0; i < ADC_CHANNEL_COUNT; i++) { - if (!adcOperatingConfig[i].enabled) { - continue; - } - ADC_RegularChannelConfig(adc.ADCx, adcOperatingConfig[i].adcChannel, rank++, adcOperatingConfig[i].sampleTime); - } - ADC_DMARequestAfterLastTransferCmd(adc.ADCx, ENABLE); - - ADC_DMACmd(adc.ADCx, ENABLE); - ADC_Cmd(adc.ADCx, ENABLE); - ADC_SoftwareStartConv(adc.ADCx); } #endif diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index 2033c556b..bd2fe15b1 100644 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -71,6 +71,7 @@ #include "rx/rx.h" #include "sensors/acceleration.h" +#include "sensors/adcinternal.h" #include "sensors/barometer.h" #include "sensors/battery.h" #include "sensors/compass.h" @@ -324,6 +325,9 @@ void fcTasksInit(void) #ifdef USE_ESC_SENSOR setTaskEnabled(TASK_ESC_SENSOR, feature(FEATURE_ESC_SENSOR)); #endif +#ifdef USE_ADC_INTERNAL + setTaskEnabled(TASK_ADC_INTERNAL, true); +#endif #ifdef USE_CMS #ifdef USE_MSP_DISPLAYPORT setTaskEnabled(TASK_CMS, true); @@ -595,5 +599,14 @@ cfTask_t cfTasks[TASK_COUNT] = { .staticPriority = TASK_PRIORITY_IDLE }, #endif + +#ifdef USE_ADC_INTERNAL + [TASK_ADC_INTERNAL] = { + .taskName = "ADCINTERNAL", + .taskFunc = adcInternalProcess, + .desiredPeriod = TASK_PERIOD_HZ(1), + .staticPriority = TASK_PRIORITY_IDLE + }, +#endif #endif }; diff --git a/src/main/interface/cli.c b/src/main/interface/cli.c index ad3dcd2eb..b3c08652a 100644 --- a/src/main/interface/cli.c +++ b/src/main/interface/cli.c @@ -130,6 +130,7 @@ extern uint8_t __config_end; #include "scheduler/scheduler.h" #include "sensors/acceleration.h" +#include "sensors/adcinternal.h" #include "sensors/barometer.h" #include "sensors/battery.h" #include "sensors/boardalignment.h" @@ -3011,6 +3012,12 @@ static void cliStatus(char *cmdline) cliPrintf("CPU Clock=%dMHz", (SystemCoreClock / 1000000)); +#ifdef USE_ADC_INTERNAL + uint16_t vrefintMv = getVrefMv(); + uint16_t coretemp = getCoreTemperatureCelsius(); + cliPrintf(", Vref=%d.%2dV, Core temp=%ddegC", vrefintMv / 1000, (vrefintMv % 1000) / 10, coretemp); +#endif + #if defined(USE_SENSOR_NAMES) const uint32_t detectedSensorsMask = sensorsMask(); for (uint32_t i = 0; ; i++) { diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h index 5045a5c55..1f8167691 100644 --- a/src/main/scheduler/scheduler.h +++ b/src/main/scheduler/scheduler.h @@ -118,6 +118,10 @@ typedef enum { TASK_RCDEVICE, #endif +#ifdef USE_ADC_INTERNAL + TASK_ADC_INTERNAL, +#endif + /* Count of real tasks */ TASK_COUNT, diff --git a/src/main/sensors/adcinternal.c b/src/main/sensors/adcinternal.c new file mode 100644 index 000000000..a8ed9de90 --- /dev/null +++ b/src/main/sensors/adcinternal.c @@ -0,0 +1,114 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it 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 is distributed in the hope that it 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 Cleanflight. If not, see . + */ + + +#include +#include + +#include "platform.h" + +#if defined(USE_ADC) && defined(USE_ADC_INTERNAL) + +#include "build/debug.h" + +#include "common/utils.h" + +#include "drivers/adc.h" + +typedef struct movingAverageStateUint16_s { + uint32_t sum; + uint16_t *values; + uint8_t size; + uint8_t pos; +} movingAverageStateUint16_t; + +uint16_t updateMovingAverageUint16(movingAverageStateUint16_t *state, uint16_t newValue) +{ + state->sum -= state->values[state->pos]; + state->values[state->pos] = newValue; + state->sum += newValue; + state->pos = (state->pos + 1) % state->size; + + return state->sum / state->size; +} + +static uint16_t adcVrefintValue; +static uint16_t adcVrefintValues[8]; +movingAverageStateUint16_t adcVrefintAverageState = { 0, adcVrefintValues, 8, 0 } ; + +static uint16_t adcTempsensorValue; +static uint16_t adcTempsensorValues[8]; +movingAverageStateUint16_t adcTempsensorAverageState = { 0, adcTempsensorValues, 8, 0 } ; + +static int16_t coreTemperature; + +uint16_t getVrefMv(void) +{ +#ifdef ADC_VOLTAGE_REFERENCE_MV + return ADC_VOLTAGE_REFERENCE_MV; +#else + return 3300 * adcVrefintValue / adcVREFINTCAL; +#endif +} + +uint16_t getCoreTemperatureCelsius(void) +{ + return coreTemperature; +} + +void adcInternalProcess(timeUs_t currentTimeUs) +{ + UNUSED(currentTimeUs); + + if (adcInternalIsBusy()) { + return; + } + + uint16_t vrefintSample = adcInternalReadVrefint(); + uint16_t tempsensorSample = adcInternalReadTempsensor(); + + adcVrefintValue = updateMovingAverageUint16(&adcVrefintAverageState, vrefintSample); + adcTempsensorValue = updateMovingAverageUint16(&adcTempsensorAverageState, tempsensorSample); + + int32_t adcTempsensorAdjusted = (int32_t)adcTempsensorValue * 3300 / getVrefMv(); + coreTemperature = ((adcTempsensorAdjusted - adcTSCAL1) * adcTSSlopeK + 30 * 1000 + 500) / 1000; + + DEBUG_SET(DEBUG_CORE_TEMP, 0, coreTemperature); + + adcInternalStartConversion(); // Start next conversion +} + +void adcInternalInit(void) +{ + // Call adcInternalProcess repeatedly to fill moving average array + for (int i = 0 ; i < 9 ; i++) { + while (adcInternalIsBusy()) { + // empty + } + adcInternalProcess(0); + } +} +#else +uint16_t getVrefMv(void) +{ +#ifdef ADC_VOLTAGE_REFERENCE_MV + return ADC_VOLTAGE_REFERENCE_MV; +#else + return 3300; +#endif +} +#endif diff --git a/src/main/sensors/adcinternal.h b/src/main/sensors/adcinternal.h new file mode 100644 index 000000000..f0c0e3cc2 --- /dev/null +++ b/src/main/sensors/adcinternal.h @@ -0,0 +1,25 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it 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 is distributed in the hope that it 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 Cleanflight. If not, see . + */ + +#pragma once + +#include "drivers/time.h" + +void adcInternalInit(void); +void adcInternalProcess(timeUs_t currentTimeUs); +uint16_t getCoreTemperatureCelsius(void); +uint16_t getVrefMv(void); diff --git a/src/main/sensors/current.c b/src/main/sensors/current.c index 99710f7bd..fbb314a51 100644 --- a/src/main/sensors/current.c +++ b/src/main/sensors/current.c @@ -32,6 +32,7 @@ #include "pg/pg_ids.h" #include "config/config_reset.h" +#include "sensors/adcinternal.h" #include "sensors/current.h" #include "sensors/esc_sensor.h" @@ -77,8 +78,6 @@ void currentMeterReset(currentMeter_t *meter) // ADC/Virtual shared // -#define ADCVREF 3300 // in mV - #define IBAT_LPF_FREQ 0.4f static biquadFilter_t adciBatFilter; @@ -106,7 +105,7 @@ static int32_t currentMeterADCToCentiamps(const uint16_t src) const currentSensorADCConfig_t *config = currentSensorADCConfig(); - int32_t millivolts = ((uint32_t)src * ADCVREF) / 4096; + int32_t millivolts = ((uint32_t)src * getVrefMv()) / 4096; // y=x/m+b m is scale in (mV/10A) and b is offset in (mA) int32_t centiAmps = (millivolts * 10000 / (int32_t)config->scale + (int32_t)config->offset) / 10; diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index 028cd7796..3862c690e 100644 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -31,6 +31,7 @@ #include "fc/runtime_config.h" #include "sensors/sensors.h" +#include "sensors/adcinternal.h" #include "sensors/acceleration.h" #include "sensors/barometer.h" #include "sensors/gyro.h" @@ -65,5 +66,9 @@ bool sensorsAutodetect(void) rangefinderInit(); #endif +#ifdef USE_ADC_INTERNAL + adcInternalInit(); +#endif + return gyroDetected; } diff --git a/src/main/sensors/voltage.c b/src/main/sensors/voltage.c index c482559d8..e26b6a55f 100644 --- a/src/main/sensors/voltage.c +++ b/src/main/sensors/voltage.c @@ -33,6 +33,7 @@ #include "pg/pg_ids.h" #include "config/config_reset.h" +#include "sensors/adcinternal.h" #include "sensors/voltage.h" #include "sensors/esc_sensor.h" @@ -138,7 +139,7 @@ STATIC_UNIT_TESTED uint16_t voltageAdcToVoltage(const uint16_t src, const voltag { // calculate battery voltage based on ADC reading // result is Vbatt in 0.1V steps. 3.3V = ADC Vref, 0xFFF = 12bit adc, 110 = 10:1 voltage divider (10k:1k) * 10 for 0.1V - return ((((uint32_t)src * config->vbatscale * 33 + (0xFFF * 5)) / (0xFFF * config->vbatresdivval)) / config->vbatresdivmultiplier); + return ((((uint32_t)src * config->vbatscale * getVrefMv() / 100 + (0xFFF * 5)) / (0xFFF * config->vbatresdivval)) / config->vbatresdivmultiplier); } void voltageMeterADCRefresh(void) diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h index ee91dafb7..af10194fa 100644 --- a/src/main/target/OMNIBUSF4/target.h +++ b/src/main/target/OMNIBUSF4/target.h @@ -237,6 +237,8 @@ #define USE_ADC #define ADC_INSTANCE ADC2 +//#define ADC_INSTANCE ADC1 + #define CURRENT_METER_ADC_PIN PC1 // Direct from CRNT pad (part of onboard sensor for Pro) #define VBAT_ADC_PIN PC2 // 11:1 (10K + 1K) divider #ifdef DYSF4PRO diff --git a/src/main/target/common_fc_pre.h b/src/main/target/common_fc_pre.h index 8a1e02129..7ee3066d6 100644 --- a/src/main/target/common_fc_pre.h +++ b/src/main/target/common_fc_pre.h @@ -50,6 +50,8 @@ #define USE_ESC_SENSOR #define I2C3_OVERCLOCK true #define USE_GYRO_DATA_ANALYSE +#define USE_ADC +#define USE_ADC_INTERNAL #endif #ifdef STM32F722xx