diff --git a/lib/main/AT32F43x/drivers/src/at32f435_437_adc.c b/lib/main/AT32F43x/drivers/src/at32f435_437_adc.c
index de9ceb6666..53b62f1291 100644
--- a/lib/main/AT32F43x/drivers/src/at32f435_437_adc.c
+++ b/lib/main/AT32F43x/drivers/src/at32f435_437_adc.c
@@ -66,17 +66,8 @@ void adc_enable(adc_type *adc_x, confirm_state new_state)
/**
* @brief adc base default para init.
- * @param sequence_mode: set the state of adc sequence mode.
- * this parameter can be:TRUE or FALSE
- * @param repeat_mode: set the state of adc repeat conversion mode.
- * this parameter can be:TRUE or FALSE
- * @param data_align: set the state of adc data alignment.
- * this parameter can be one of the following values:
- * - ADC_RIGHT_ALIGNMENT
- * - ADC_LEFT_ALIGNMENT
- * @param ordinary_channel_length: configure the adc ordinary channel sequence length.
- * this parameter can be:
- * - (0x1~0xf)
+ * Sets sequence FALSE, repeat FALSE, align right, length 1
+ * @param adc_base_struct pointer to the struct to be initialised
* @retval none
*/
void adc_base_default_para_init(adc_base_config_type *adc_base_struct)
diff --git a/src/main/drivers/adc.c b/src/main/drivers/adc.c
index 269b17b63c..14872fcaf8 100644
--- a/src/main/drivers/adc.c
+++ b/src/main/drivers/adc.c
@@ -130,11 +130,25 @@ int32_t adcTSCAL1;
int32_t adcTSCAL2;
int32_t adcTSSlopeK;
-uint16_t adcInternalCompensateVref(uint16_t vrefAdcValue)
+/**
+ * Use a measurement of the fixed internal vref to calculate the external Vref+
+ *
+ * The ADC full range reading equates to Vref+ on the channel. Vref+ is typically
+ * fed from Vcc at 3.3V, but since Vcc isn't a critical value it may be off
+ * by a little due to variation in the regulator. Some chips are provided with a
+ * known internal voltage reference, typically around 1.2V. By measuring this
+ * reference with an internally connected ADC channel we can then calculate a more
+ * accurate value for Vref+ instead of assuming that it is 3.3V
+ *
+ * @param intVRefAdcValue reading from the internal calibration voltage
+ *
+ * @return the calculated value of Vref+
+*/
+uint16_t adcInternalCompensateVref(uint16_t intVRefAdcValue)
{
// 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);
+ return (uint16_t)((uint32_t)(adcVREFINTCAL * VREFINT_CAL_VREF) / intVRefAdcValue);
}
int16_t adcInternalComputeTemperature(uint16_t tempAdcValue, uint16_t vrefValue)
diff --git a/src/main/drivers/adc.h b/src/main/drivers/adc.h
index f122d2dea0..3487c4a65a 100644
--- a/src/main/drivers/adc.h
+++ b/src/main/drivers/adc.h
@@ -76,13 +76,20 @@ typedef enum {
ADC_TEMPSENSOR = 4,
ADC_VREFINT = 5,
ADC_VBAT4 = 6,
+#elif defined(AT32F435)
+ ADC_CHANNEL_INTERNAL_FIRST_ID = 4,
+
+ ADC_TEMPSENSOR = 4,
+ ADC_VREFINT = 5,
+ // ADC_VBAT4 = 6,
+
#endif
ADC_CHANNEL_COUNT
} AdcChannel;
typedef struct adcOperatingConfig_s {
ioTag_t tag;
-#if defined(STM32H7) || defined(STM32G4)
+#if defined(STM32H7) || defined(STM32G4) || defined(AT32F435)
ADCDevice adcDevice; // ADCDEV_x for this input
uint32_t adcChannel; // Channel number for this input. Note that H7 and G4 HAL requires this to be 32-bit encoded number.
#else
diff --git a/src/main/drivers/adc_impl.h b/src/main/drivers/adc_impl.h
index 15844a9cfc..6c315282bb 100644
--- a/src/main/drivers/adc_impl.h
+++ b/src/main/drivers/adc_impl.h
@@ -27,6 +27,12 @@
#if defined(STM32F4) || defined(STM32F7)
#define ADC_TAG_MAP_COUNT 16
+#elif defined(AT32F435)
+#ifdef USE_ADC_INTERNAL
+#define ADC_TAG_MAP_COUNT 18
+#else
+#define ADC_TAG_MAP_COUNT 16
+#endif
#elif defined(STM32H7)
#ifdef USE_ADC_INTERNAL
#define ADC_TAG_MAP_COUNT 30
@@ -47,7 +53,7 @@ typedef struct adcTagMap_s {
ioTag_t tag;
uint8_t devices;
uint32_t channel;
-#if defined(STM32H7) || defined(STM32G4)
+#if defined(STM32H7) || defined(STM32G4) || defined(AT32F435)
uint8_t channelOrdinal;
#endif
} adcTagMap_t;
@@ -139,3 +145,12 @@ void adcGetChannelValues(void);
#define TEMPSENSOR_CAL1_TEMP ((int32_t) 30)
#define TEMPSENSOR_CAL2_TEMP ((int32_t) 110)
#endif
+
+#ifdef AT32F435
+#define VREFINT_EXPECTED (1489U) // The raw ADC reading at 12bit resolution expected for the 1V2 internal ref
+#define VREFINT_CAL_VREF (3300U) // The nominal external Vref+ for the above reading
+#define TEMPSENSOR_CAL_VREFANALOG (3300U)
+#define TEMPSENSOR_CAL1_TEMP (25U)
+#define TEMPSENSOR_CAL1_V (1.27f)
+#define TEMPSENSOR_SLOPE (-4.13f) // mV/C
+#endif
diff --git a/src/main/drivers/at32/adc_at32f43x.c b/src/main/drivers/at32/adc_at32f43x.c
index 4238e9b62c..fef3f0071f 100644
--- a/src/main/drivers/at32/adc_at32f43x.c
+++ b/src/main/drivers/at32/adc_at32f43x.c
@@ -18,9 +18,31 @@
* If not, see .
*/
+/**
+ * Read internal and external analog channels
+ *
+ * Internal channels provide temperature and the internal voltage reference
+ * External channels are for vbat, rssi, current and a generic 'external' inputs
+ *
+ * The ADC is free running and so doesn't require a timer. Samples are moved from
+ * the ADC output register to a buffer by DMA
+ *
+ * The sample rate is kept low to reduce impact on the DMA controller, and the lowest
+ * priority is set for the DMA transfer. It's also recommended to use the highest numbered
+ * DMA channel on the dma controller for ADC, since that is the lowest priority channel
+ * for transfers at the same DMA priority.
+ *
+ * Sample rate is set between 1 and 2kHz by using a long input sampling time and reasonably
+ * high hardware oversampling.
+ *
+ * Note that only ADC1 is used, although the code contains remnants of support for all
+ * three ADC.
+*/
+
#include
#include
#include
+#include
#include "platform.h"
@@ -47,23 +69,27 @@ const adcDevice_t adcHardware[ADCDEV_COUNT] = {
{
.ADCx = ADC1,
.rccADC = RCC_APB2(ADC1),
+#if !defined(USE_DMA_SPEC)
.dmaResource = NULL
+#endif
},
{
.ADCx = ADC2,
.rccADC = RCC_APB2(ADC2),
+#if !defined(USE_DMA_SPEC)
.dmaResource = NULL
+#endif
},
{
.ADCx = ADC3,
.rccADC = RCC_APB2(ADC3),
+#if !defined(USE_DMA_SPEC)
.dmaResource = NULL
+#endif
},
};
-adcDevice_t adcDevice[ADCDEV_COUNT];
-
#define ADC_CHANNEL_VREFINT ADC_CHANNEL_17
#define ADC_CHANNEL_TEMPSENSOR_ADC1 ADC_CHANNEL_16
@@ -93,19 +119,38 @@ const adcTagMap_t adcTagMap[] = {
{ DEFIO_TAG_E__PC5, ADC_DEVICES_12, ADC_CHANNEL_15, 15 },
};
-void adcInitDevice(adcDevice_t *adcdev, int channelCount)
+static volatile DMA_DATA uint32_t adcConversionBuffer[ADC_CHANNEL_COUNT];
+
+/**
+ * Initialise the specified ADC to read multiple channels in repeat mode
+ *
+ * Sets 12 bit resolution, right aligned
+ *
+ * @param dev Specifies the ADC device to use
+ * @param channelCount how many channels to repeat over
+ *
+*/
+void adcInitDevice(const adc_type *dev, const int channelCount)
{
adc_base_config_type adc_base_struct;
- adc_base_default_para_init(&adc_base_struct);
- adc_base_struct.sequence_mode = TRUE;
- adc_base_struct.repeat_mode = TRUE;
+
+ adc_base_default_para_init(&adc_base_struct); // not currently needed, but insurance against the struct being changed in the future
+ adc_base_struct.sequence_mode = TRUE; // reading multiple channels
+ adc_base_struct.repeat_mode = TRUE; // free running, so no need to retrigger
adc_base_struct.data_align = ADC_RIGHT_ALIGNMENT;
adc_base_struct.ordinary_channel_length = channelCount;
- adc_base_config(adcdev->ADCx, &adc_base_struct);
- adc_resolution_set(adcdev->ADCx, ADC_RESOLUTION_12B);
+
+ adc_base_config((adc_type *)dev, &adc_base_struct);
+ adc_resolution_set((adc_type *)dev, ADC_RESOLUTION_12B);
}
-int adcFindTagMapEntry(ioTag_t tag)
+/**
+ * Find a given pin (defined by ioTag) in the map
+ *
+ * @param tag the ioTag to search for
+ * @return the index in adcTagMap corresponding to the given ioTag or -1 if not found
+*/
+int adcFindTagMapEntry(const ioTag_t tag)
{
for (int i = 0; i < ADC_TAG_MAP_COUNT; i++) {
if (adcTagMap[i].tag == tag) {
@@ -115,13 +160,82 @@ int adcFindTagMapEntry(ioTag_t tag)
return -1;
}
-volatile DMA_DATA uint32_t adcConversionBuffer[ADC_CHANNEL_COUNT];
+/**
+ * Setup the scaling offsets and factors used in adc.c
+ * @see src/main/drivers/adc.c
+ * @see src/main/drivers/adc_impl.h
+ *
+ * There are a number of global calibration/scaling factors used in src/main/drivers/adc.c that need to
+ * be set to appropriate values if we want to re-use existing code, e.g. adcInternalComputeTemperature
+ * (the alternative would be to duplicate the code into ST and AT specific versions).
+ * This is made a little confusing since the implementation based on ST datasheets approaches the calculation with
+ * different formula and express the scaling factors in different units compared to the AT datasheets.
+ * The constants are defined in src/main/drivers/adc_impl.h. It seems clearest to use the units from
+ * the datasheet when defining those values, so here we have to convert to what's expected in
+ * adcInternalComputeTemperature.
+*/
+void setScalingFactors(void)
+{
+ // The expected reading for 1.2V internal reference if external vref+ was 3.3V
+ adcVREFINTCAL = VREFINT_EXPECTED;
+ // adcTSCAL1 needs to be the raw ADC value at 25C, but we can't adjust it for VREF+ because we haven't calculated it yet
+ // So this will have to be aproximate (and the use should be fixed at some point)
+ adcTSCAL1 = (TEMPSENSOR_CAL1_V * 4095.0f) / 3.3f;
+
+ // TEMPSENSOR_SLOPE is given in mv/C.
+ // adcTSSlopeK has the opposite sign compared to the AT32 standard because adcInternalComputeTemperature subtracts
+ // the calibration value from the reading where as the AT32 docs subtract the reading from the calibration.
+ // 3300/4095 converts the reading to mV and the factor of 1000 is needed to prevent the slope from rounding to 0.
+ // The intermediate result when this is used is in mC, and adcInternalComputeTemperature then divides by 1000
+ // to get an answer in C
+ adcTSSlopeK = lrintf(-3300.0f*1000.0f/4095.0f/TEMPSENSOR_SLOPE);
+}
+
+
+/**
+ * Setup the ADC so that it's running in the background and ready to
+ * provide channel data
+ *
+ * Notes:
+ * This code only uses ADC1 despite appearances to the contrary, and has not been tested with the other ADCs
+ *
+ * From the RM:
+ * ADCCLK must be less than 80 MHz, while the ADCCLK frequency must be lower than PCLK2
+ *
+ * PCLK2 looks like it's running at 144Mhz, but should be confirmed
+ *
+ * With HCLK of 288, a divider of 4 gives an ADCCLK of 72MHz
+ *
+ * sample time is
+ * ADC_SAMPLE + nbits + 0.5 ADCCLK periods
+ *
+ * So with 12bit samples and ADC_SAMPLE_92_5 that's 105 clks.
+ *
+ * We're using HCLK/4, so 288/4 = 72MHz, each tick is 14ns. Add 5 clks for the interval between conversions,
+ * 110 clks total is 1.54us per channel.
+ *
+ * Max 6 channels is a total of 9.24us, or a basic sample rate of 108kHz per channel
+ *
+ * If we use 64x oversampling we'll get an effective rate of 1.7kHz per channel which should still be plenty.
+ *
+ * (RM and DS mention fast and slow channels, but don't give details on how this affects the above.
+ * It's not relevant to our use case, so ignore the difference for now)
+ *
+ * Called from fc/init.c
+ *
+ * @param config - defines the channels to use for each external input (vbat, rssi, current, external) and also has calibration values for the temperature sensor
+ *
+*/
void adcInit(const adcConfig_t *config)
{
- memset(adcOperatingConfig, 0, sizeof(adcOperatingConfig));
- memcpy(adcDevice, adcHardware, sizeof(adcDevice));
+ uint32_t nChannelsUsed[ADCDEV_COUNT] = {0};
+ setScalingFactors();
+
+ memset(adcOperatingConfig, 0, sizeof(adcOperatingConfig)); // adcOperatingConfig defined in generic adc.c
+
+ // Set channels (ioTags) for enabled ADC inputs
if (config->vbat.enabled) {
adcOperatingConfig[ADC_BATTERY].tag = config->vbat.ioTag;
}
@@ -138,6 +252,8 @@ void adcInit(const adcConfig_t *config)
adcOperatingConfig[ADC_CURRENT].tag = config->current.ioTag;
}
+ // loop over all possible channels and build the adcOperatingConfig to represent
+ // the set of enabled channels
for (int i = 0; i < ADC_CHANNEL_COUNT; i++) {
int map;
int dev;
@@ -158,89 +274,110 @@ void adcInit(const adcConfig_t *config)
continue;
}
- for (dev = 0; dev < ADCDEV_COUNT; dev++) {
-#ifndef USE_DMA_SPEC
- if (!adcDevice[dev].ADCx || !adcDevice[dev].dmaResource) {
- continue;
- }
-#else
- if (!adcDevice[dev].ADCx) {
- continue;
- }
-#endif
+ // Since ADC1 can do all channels this will only ever return adc1 and is unnecessary
- if (adcTagMap[map].devices & (1 << dev)) {
- break;
- }
+ // Find an ADC instance that can be used for the given TagMap index.
+ // for (dev = 0; dev < ADCDEV_COUNT; dev++) {
+ // #ifndef USE_DMA_SPEC
+ // if (!adcDevice[dev].ADCx || !adcDevice[dev].dmaResource) {
+ // continue;
+ // }
+ // #else
+ // if (!adcDevice[dev].ADCx) {
+ // continue;
+ // }
+ // #endif
- if (dev == ADCDEV_COUNT) {
- continue;
- }
- }
+ // if (adcTagMap[map].devices & (1 << dev)) {
+ // break;
+ // }
+ // }
+ dev = ADCDEV_1;
}
adcOperatingConfig[i].adcDevice = dev;
adcOperatingConfig[i].adcChannel = adcTagMap[map].channel;
- adcOperatingConfig[i].sampleTime = ADC_SAMPLING_INTERVAL_5CYCLES;
+ adcOperatingConfig[i].sampleTime = ADC_SAMPLETIME_92_5;
adcOperatingConfig[i].enabled = true;
- adcDevice[dev].channelBits |= (1 << adcTagMap[map].channelOrdinal);
+ nChannelsUsed[dev] += 1; // increase the active channel count for this device
+ // Enable the gpio for analog input
if (adcOperatingConfig[i].tag) {
IOInit(IOGetByTag(adcOperatingConfig[i].tag), OWNER_ADC_BATT + i, 0);
IOConfigGPIO(IOGetByTag(adcOperatingConfig[i].tag), IO_CONFIG(GPIO_MODE_ANALOG,GPIO_DRIVE_STRENGTH_MODERATE, 0, GPIO_PULL_NONE));
}
+ } // for each channel
+
+ adc_reset(); // reset all ADC instances
+
+ // Enable the clock (power up the circuit)
+ // This has to be done before calling adc_common_config for reasons that are unclear
+ // NB only ADC1 will have any channels, so this loop for pedantry only
+ for (int i = 0; i < ADCDEV_COUNT; i++) {
+ if (nChannelsUsed[i] > 0) {
+ RCC_ClockCmd(adcHardware[0].rccADC, ENABLE);
+ }
}
+ // Common config applies equally to all three ADCs, so only apply it once
+ adc_common_config_type adc_common_struct;
+ adc_common_default_para_init(&adc_common_struct);
+ adc_common_struct.combine_mode = ADC_INDEPENDENT_MODE;
+ adc_common_struct.div = ADC_HCLK_DIV_4;
+ adc_common_struct.common_dma_mode = ADC_COMMON_DMAMODE_DISABLE;
+ adc_common_struct.common_dma_request_repeat_state = FALSE;
+ adc_common_struct.sampling_interval = ADC_SAMPLING_INTERVAL_5CYCLES;
+ adc_common_struct.tempervintrv_state = TRUE; // internal channels need to be enabled before use
+
+ adc_common_config(&adc_common_struct);
+
+
+ // Only adc1 will have any channels assigned to it after the code above, so this can be simplified
+
int dmaBufferIndex = 0;
for (int dev = 0; dev < ADCDEV_COUNT; dev++) {
- adcDevice_t *adc = &adcDevice[dev];
+ const adcDevice_t *adc = &adcHardware[dev];
- if (!(adc->ADCx && adc->channelBits)) {
+ // skip this device if it doesn't have any active channels
+ if (nChannelsUsed[dev] == 0) {
continue;
}
- RCC_ClockCmd(adc->rccADC, ENABLE);
+ adcInitDevice(adc->ADCx, nChannelsUsed[dev]);
- adc_reset();
- if (adc->ADCx == ADC1) {
- adc_common_config_type adc_common_struct;
- adc_common_default_para_init(&adc_common_struct);
- adc_common_struct.combine_mode = ADC_INDEPENDENT_MODE;
- adc_common_struct.div = ADC_HCLK_DIV_4;
- adc_common_struct.common_dma_mode = ADC_COMMON_DMAMODE_DISABLE;
- adc_common_struct.common_dma_request_repeat_state = FALSE;
- adc_common_struct.sampling_interval = ADC_SAMPLING_INTERVAL_5CYCLES;
- adc_common_struct.tempervintrv_state = TRUE;
- adc_common_struct.vbat_state = TRUE;
- adc_common_config(&adc_common_struct);
- }
+ // Set the oversampling ratio and matching shift
+ adc_oversample_ratio_shift_set(adc->ADCx, ADC_OVERSAMPLE_RATIO_64, ADC_OVERSAMPLE_SHIFT_6);
- int configuredAdcChannels = BITCOUNT(adc->channelBits);
- adcInitDevice(adc, configuredAdcChannels);
-#ifdef USE_DMA_SPEC
+ #ifdef USE_DMA_SPEC
+
+ // Setup the DMA channel so that data is automatically and continuously transferred from the ADC output register
+ // to the results buffer
+
const dmaChannelSpec_t *dmaSpec = dmaGetChannelSpecByPeripheral(DMA_PERIPH_ADC, dev, config->dmaopt[dev]);
- dmaIdentifier_e dmaIdentifier = dmaGetIdentifier(dmaSpec->ref);
- if (!dmaSpec || !dmaAllocate(dmaIdentifier, OWNER_ADC, RESOURCE_INDEX(dev))) {
+ if (dmaSpec == NULL) {
return;
}
- dmaEnable(dmaIdentifier);
- xDMA_DeInit(dmaSpec->ref);
+ dmaIdentifier_e dmaIdentifier = dmaGetIdentifier(dmaSpec->ref);
+ if ( ! dmaAllocate(dmaIdentifier, OWNER_ADC, RESOURCE_INDEX(dev)) ) {
+ return;
+ }
- adc->dmaResource=dmaSpec->ref;
+ dmaEnable(dmaIdentifier); // enables clock/power
+ xDMA_DeInit(dmaSpec->ref);
dma_init_type dma_init_struct;
dma_default_para_init(&dma_init_struct);
- dma_init_struct.buffer_size = BITCOUNT(adc->channelBits);
+ dma_init_struct.buffer_size = nChannelsUsed[dev];
dma_init_struct.direction = DMA_DIR_PERIPHERAL_TO_MEMORY;
dma_init_struct.memory_data_width = DMA_MEMORY_DATA_WIDTH_WORD;
dma_init_struct.memory_inc_enable = TRUE;
dma_init_struct.peripheral_data_width = DMA_PERIPHERAL_DATA_WIDTH_WORD;
dma_init_struct.peripheral_inc_enable = FALSE;
- dma_init_struct.priority = DMA_PRIORITY_MEDIUM;
- dma_init_struct.loop_mode_enable = TRUE;
+ dma_init_struct.priority = DMA_PRIORITY_LOW;
+ dma_init_struct.loop_mode_enable = TRUE; // operate as a circular buffer, no interrupt handling required
dma_init_struct.memory_base_addr = (uint32_t)&(adcConversionBuffer[dmaBufferIndex]);
dma_init_struct.peripheral_base_addr = (uint32_t)&(adc->ADCx->odt);
@@ -248,55 +385,42 @@ void adcInit(const adcConfig_t *config)
xDMA_Init(dmaSpec->ref, &dma_init_struct);
dmaMuxEnable(dmaIdentifier, dmaSpec->dmaMuxId);
- /* enable dma transfer complete interrupt */
- xDMA_ITConfig(dmaSpec->ref,DMA_IT_TCIF,ENABLE);
- xDMA_Cmd(dmaSpec->ref,ENABLE);
+ xDMA_Cmd(dmaSpec->ref,TRUE); // means dma_channel_enable
adc_dma_mode_enable(adc->ADCx, TRUE);
adc_dma_request_repeat_enable(adc->ADCx, TRUE);
-#endif //end of USE_DMA_SPEC
- for (int adcChan = 0; adcChan < ADC_CHANNEL_COUNT; adcChan++) {
+ #endif //end of USE_DMA_SPEC
- if (!adcOperatingConfig[adcChan].enabled) {
- continue;
+ // set each channel into the auto sequence for this ADC device
+ for (int adcChan = 0; adcChan < ADC_CHANNEL_COUNT; adcChan++)
+ {
+ // only add enabled channels for the current dev (can be simplified if we drop the pretense at handling adc2 and 3)
+ if (adcOperatingConfig[adcChan].enabled && adcOperatingConfig[adcChan].adcDevice == dev)
+ {
+ adcOperatingConfig[adcChan].dmaIndex = dmaBufferIndex++;
+ adc_ordinary_channel_set(adc->ADCx,
+ adcOperatingConfig[adcChan].adcChannel,
+ adcOperatingConfig[adcChan].dmaIndex+1, // This is the sequence number for the adc conversion
+ adcOperatingConfig[adcChan].sampleTime );
}
-
- if (adcOperatingConfig[adcChan].adcDevice != dev) {
- continue;
- }
-
- adcOperatingConfig[adcChan].dmaIndex = dmaBufferIndex++;
- adc_ordinary_channel_set(adcDevice[dev].ADCx,
- adcOperatingConfig[adcChan].adcChannel,
- adcOperatingConfig[adcChan].dmaIndex+1,
- ADC_SAMPLETIME_92_5);
}
-
- if (adc->ADCx==ADC1) {
- adc_voltage_monitor_threshold_value_set(ADC1, 0x100, 0x000);
- adc_voltage_monitor_single_channel_select(ADC1, adcOperatingConfig[ADC_BATTERY].adcChannel);
- adc_voltage_monitor_enable(ADC1, ADC_VMONITOR_SINGLE_ORDINARY);
- }
-
- adc_interrupt_enable(adc->ADCx, ADC_OCCO_INT, TRUE);
-
adc_enable(adc->ADCx, TRUE);
- while (adc_flag_get(adc->ADCx, ADC_RDY_FLAG) == RESET);
-
- dmaBufferIndex += BITCOUNT(adc->channelBits);
+ while (adc_flag_get(adc->ADCx, ADC_RDY_FLAG) == RESET); // wait for ready
adc_calibration_init(adc->ADCx);
while (adc_calibration_init_status_get(adc->ADCx));
adc_calibration_start(adc->ADCx);
while (adc_calibration_status_get(adc->ADCx));
- adc_enable(adc->ADCx, TRUE);
- adc_ordinary_software_trigger_enable(adc->ADCx, TRUE);
+ adc_ordinary_software_trigger_enable(adc->ADCx, TRUE); // start sampling
}
}
+/**
+ * Copies the latest ADC external channel data into adcValues defined in adc.c
+*/
void adcGetChannelValues(void)
{
for (int i = 0; i < ADC_CHANNEL_INTERNAL_FIRST_ID; i++) {
@@ -308,51 +432,59 @@ void adcGetChannelValues(void)
#ifdef USE_ADC_INTERNAL
+/**
+ * This impl is never busy in the sense of being unable to supply data
+*/
bool adcInternalIsBusy(void)
{
return false;
}
+/**
+ * Nop, since the ADC is free running
+*/
void adcInternalStartConversion(void)
{
return;
}
+/**
+ * Reads a given channel from the DMA buffer
+*/
uint16_t adcInternalRead(int channel)
{
- int dmaIndex = adcOperatingConfig[channel].dmaIndex;
+ const int dmaIndex = adcOperatingConfig[channel].dmaIndex;
return adcConversionBuffer[dmaIndex];
}
-int adcPrivateVref = -1;
-int adcPrivateTemp = -1;
-
+/**
+ * Read the internal Vref and return raw value
+ *
+ * The internal Vref is 1.2V and can be used to calculate the external Vref+
+ * External Vref+ determines the scale for the raw ADC readings but since it
+ * is often directly connected to Vdd (approx 3.3V) it isn't accurately controlled.
+ * Calculating the actual value of Vref+ by using measurements of the known 1.2V
+ * internal reference can improve overall accuracy.
+ *
+ * @return the raw ADC reading for the internal voltage reference
+ * @see adcInternalCompensateVref in src/main/drivers/adc.c
+*/
uint16_t adcInternalReadVrefint(void)
{
- uint16_t value = adcInternalRead(ADC_VREFINT);
- adcPrivateVref =((double)1.2 * 4095) / value * 1000;
- return adcPrivateVref;
+ const uint16_t value = adcInternalRead(ADC_VREFINT);
+
+ return value;
}
+/**
+ * Read the internal temperature sensor
+ *
+ * @return the raw ADC reading
+*/
uint16_t adcInternalReadTempsensor(void)
{
- uint16_t value = adcInternalRead(ADC_TEMPSENSOR);
- adcPrivateTemp = (((ADC_TEMP_BASE- value * ADC_VREF / 4096) / ADC_TEMP_SLOPE) + 25);
- return adcPrivateTemp;
-}
-
-void ADC1_2_3_IRQHandler(void)
-{
- for (int dev = 0; dev < ADCDEV_COUNT; dev++) {
- adcDevice_t *adc = &adcDevice[dev];
-
- if (!(adc->ADCx && adc->channelBits)) {
- continue;
- }
- if(adc_flag_get(adc->ADCx, ADC_OCCO_FLAG) != RESET) {
- adc_flag_clear(adc->ADCx, ADC_OCCO_FLAG);
- }
- }
+ const uint16_t value = adcInternalRead(ADC_TEMPSENSOR);
+ return value;
}
#endif // USE_ADC_INTERNAL
diff --git a/src/main/drivers/at32/platform_at32.h b/src/main/drivers/at32/platform_at32.h
index ca8025877c..6688e5207f 100644
--- a/src/main/drivers/at32/platform_at32.h
+++ b/src/main/drivers/at32/platform_at32.h
@@ -48,6 +48,8 @@ typedef enum {DISABLE = 0, ENABLE = !DISABLE} FunctionalState;
#define USE_DMA_SPEC
#define USE_PERSISTENT_OBJECTS
#define USE_CUSTOM_DEFAULTS_ADDRESS
+#define USE_ADC_INTERNAL
+
#define USE_LATE_TASK_STATISTICS
#define TASK_GYROPID_DESIRED_PERIOD 1000 // 1000us = 1kHz
@@ -66,3 +68,4 @@ typedef enum {DISABLE = 0, ENABLE = !DISABLE} FunctionalState;
#define DMA_RAM_W
#define DMA_RAM_RW
+#define USE_LATE_TASK_STATISTICS
diff --git a/src/main/drivers/at32/timer_at32bsp.c b/src/main/drivers/at32/timer_at32bsp.c
index eb9583ae84..8cd0109f62 100644
--- a/src/main/drivers/at32/timer_at32bsp.c
+++ b/src/main/drivers/at32/timer_at32bsp.c
@@ -74,7 +74,9 @@ typedef struct {
channelType_t type;
} timerChannelInfo_t;
+#ifdef TIMER_CHANNEL_COUNT
timerChannelInfo_t timerChannelInfo[TIMER_CHANNEL_COUNT];
+#endif
typedef struct {
uint8_t priority;
@@ -353,6 +355,16 @@ void timerConfigure(const timerHardware_t *timerHardwarePtr, uint16_t period, ui
// allocate and configure timer channel. Timer priority is set to highest priority of its channels
void timerChInit(const timerHardware_t *timHw, channelType_t type, int irqPriority, uint8_t irq)
{
+ #ifndef USE_TIMER_MGMT
+
+ UNUSED(timHw);
+ UNUSED(type);
+ UNUSED(irqPriority);
+ UNUSED(irq);
+ return;
+
+ #else
+
unsigned channel = timHw - TIMER_HARDWARE;
if (channel >= TIMER_CHANNEL_COUNT) {
return;
@@ -371,6 +383,7 @@ void timerChInit(const timerHardware_t *timHw, channelType_t type, int irqPriori
timerInfo[timer].priority = irqPriority;
}
+ #endif
}
void timerChCCHandlerInit(timerCCHandlerRec_t *self, timerCCHandlerCallback *fn)
@@ -666,6 +679,7 @@ void timerInit(void)
GPIO_PinRemapConfig(GPIO_PartialRemap_TIM3, ENABLE);
#endif
+ #ifdef USE_TIMER_MGMT
/* enable the timer peripherals */
for (unsigned i = 0; i < TIMER_CHANNEL_COUNT; i++) {
RCC_ClockCmd(timerRCC(TIMER_HARDWARE[i].tim), ENABLE);
@@ -675,6 +689,7 @@ void timerInit(void)
for (unsigned i = 0; i < TIMER_CHANNEL_COUNT; i++) {
timerChannelInfo[i].type = TYPE_FREE;
}
+ #endif
for (unsigned i = 0; i < USED_TIMER_COUNT; i++) {
timerInfo[i].priority = ~0;
diff --git a/src/main/drivers/timer.h b/src/main/drivers/timer.h
index 83ed3a0fff..586a6b9d83 100644
--- a/src/main/drivers/timer.h
+++ b/src/main/drivers/timer.h
@@ -181,8 +181,10 @@ extern const timerHardware_t fullTimerHardware[];
#else
+#ifdef USABLE_TIMER_CHANNEL_COUNT
#define TIMER_CHANNEL_COUNT USABLE_TIMER_CHANNEL_COUNT
#define TIMER_HARDWARE timerHardware
+#endif
#endif // USE_TIMER_MGMT
diff --git a/src/main/target/AT32F435/target.h b/src/main/target/AT32F435/target.h
index 4a1c4316a1..4239620b25 100644
--- a/src/main/target/AT32F435/target.h
+++ b/src/main/target/AT32F435/target.h
@@ -75,7 +75,7 @@
#define USE_ACCGYRO_BMI160
-//#define USE_USB_DETECT
+#define USE_USB_DETECT
#define USE_VCP
//#define USE_SOFTSERIAL1
//#define USE_SOFTSERIAL2
@@ -83,7 +83,7 @@
#define UNIFIED_SERIAL_PORT_COUNT 1
-//#define USE_ADC
+#define USE_ADC
#define USE_CUSTOM_DEFAULTS
#define USE_PWM_OUTPUT