From 4315298f409cfc769eb8c30d96fdf49e7c521d1a Mon Sep 17 00:00:00 2001 From: Hanya Date: Fri, 3 Sep 2021 15:49:02 +0900 Subject: [PATCH] Fix next channel calculation --- os/hal/ports/RP/LLD/ADCv1/hal_adc_lld.c | 31 ++++++++++++++----- os/hal/ports/RP/LLD/ADCv1/hal_adc_lld.h | 6 +++- testhal/RP/RP2040/RT-RP2040-PICO-ADC/Makefile | 2 +- testhal/RP/RP2040/RT-RP2040-PICO-ADC/main.c | 29 +++++++++++------ 4 files changed, 49 insertions(+), 19 deletions(-) diff --git a/os/hal/ports/RP/LLD/ADCv1/hal_adc_lld.c b/os/hal/ports/RP/LLD/ADCv1/hal_adc_lld.c index 49af16fd..c59b8da2 100644 --- a/os/hal/ports/RP/LLD/ADCv1/hal_adc_lld.c +++ b/os/hal/ports/RP/LLD/ADCv1/hal_adc_lld.c @@ -72,9 +72,10 @@ static void set_channel(ADCDriver *adcp, uint8_t channel) { static uint8_t get_next_channel_number_from_mask(uint8_t mask, uint8_t current) { for (uint8_t i = 0; mask > 0; i++) { if (mask & 0x01) { - if (i > current) { + if (!current) { return i; } + current--; } mask >>= 1U; } @@ -112,16 +113,30 @@ OSAL_IRQ_HANDLER(RP_ADC_IRQ_FIFO_HANDLER) { adcp->current_buffer_position += 1; size_t bufferSize = adcp->depth * adcp->grpp->num_channels; - size_t currentChannel = adcp->current_buffer_position % adcp->grpp->num_channels; - size_t currentIteration = adcp->current_buffer_position / adcp->grpp->num_channels; - - if (adcp->grpp->circular && currentChannel == 0 && currentIteration == adcp->depth / 2) { + + adcp->current_channel += 1; + if (adcp->current_channel >= adcp->grpp->num_channels) { + adcp->current_channel = 0; + adcp->current_iteration += 1; + } + + if (adcp->grpp->circular && adcp->current_channel == 0 && + adcp->current_iteration == adcp->depth / 2) { _adc_isr_half_code(adcp); } if (adcp->current_buffer_position == bufferSize) { _adc_isr_full_code(adcp); + + if (adcp->grpp->circular) { + adcp->current_buffer_position = 0; + adcp->current_channel = 0; + adcp->current_iteration = 0; + set_channel(adcp, get_first_channel(adcp)); + RP_ADC_START_ONCE; + } } else { - set_channel(adcp, get_next_channel_number_from_mask(adcp->grpp->channel_mask, currentChannel)); + set_channel(adcp, get_next_channel_number_from_mask( + adcp->grpp->channel_mask, adcp->current_channel)); RP_ADC_START_ONCE; } @@ -178,6 +193,8 @@ void adc_lld_start(ADCDriver *adcp) { #if RP_ADC_USE_ADC1 == TRUE if (&ADCD1 == adcp) { adcp->current_buffer_position = 0; + adcp->current_channel = 0; + adcp->current_iteration = 0; /* Clear control flags. */ adcp->adc->CS = 0; @@ -187,7 +204,7 @@ void adc_lld_start(ADCDriver *adcp) { ((adcp->config->div_frac << ADC_DIV_FRAC_Pos) & ADC_DIV_FRAC_Msk); /* Enable FIFO. */ - fcs |= ADC_FCS_EN; + fcs = ADC_FCS_EN; /* Set DREQ/IRQ threshold. */ fcs |= 1U << ADC_FCS_THRESH_Pos; diff --git a/os/hal/ports/RP/LLD/ADCv1/hal_adc_lld.h b/os/hal/ports/RP/LLD/ADCv1/hal_adc_lld.h index 61f8b449..fe4f4271 100644 --- a/os/hal/ports/RP/LLD/ADCv1/hal_adc_lld.h +++ b/os/hal/ports/RP/LLD/ADCv1/hal_adc_lld.h @@ -107,7 +107,11 @@ typedef uint32_t adcerror_t; /* ADC register. */ \ ADC_TypeDef *adc; \ /* Current index in the buffer. */ \ - size_t current_buffer_position; + size_t current_buffer_position; \ + /* Current channel index. */ \ + size_t current_channel; \ + /* Current iteration in the depth. */ \ + size_t current_iteration; /** * @brief Low level fields of the ADC configuration structure. diff --git a/testhal/RP/RP2040/RT-RP2040-PICO-ADC/Makefile b/testhal/RP/RP2040/RT-RP2040-PICO-ADC/Makefile index 8fc57fe9..c42ad244 100644 --- a/testhal/RP/RP2040/RT-RP2040-PICO-ADC/Makefile +++ b/testhal/RP/RP2040/RT-RP2040-PICO-ADC/Makefile @@ -5,7 +5,7 @@ # Compiler options here. ifeq ($(USE_OPT),) - USE_OPT = -O0 -ggdb -fomit-frame-pointer -falign-functions=16 + USE_OPT = -O3 -ggdb -fomit-frame-pointer -falign-functions=16 endif # C specific options here (added to USE_OPT). diff --git a/testhal/RP/RP2040/RT-RP2040-PICO-ADC/main.c b/testhal/RP/RP2040/RT-RP2040-PICO-ADC/main.c index 715f7016..6f9f7e5f 100644 --- a/testhal/RP/RP2040/RT-RP2040-PICO-ADC/main.c +++ b/testhal/RP/RP2040/RT-RP2040-PICO-ADC/main.c @@ -25,6 +25,10 @@ #define LED_GREEN_PIN 25U +#define ADC_CH0_PIN 26U +#define ADC_CH1_PIN 27U +#define ADC_CH2_PIN 28U + /* * Green LED blinker thread, times are in milliseconds. */ @@ -44,7 +48,7 @@ static THD_FUNCTION(Thread1, arg) { */ void adc_end_callback(ADCDriver *adcp) { (void)adcp; - chprintf((BaseSequentialStream *)&SIOD0, "ADC end cb\r\n"); + //chprintf((BaseSequentialStream *)&SIOD0, "ADC end cb\r\n"); } /* @@ -80,6 +84,9 @@ int main(void) { */ palSetLineMode(LED_GREEN_PIN, PAL_MODE_OUTPUT_PUSHPULL | PAL_RP_PAD_DRIVE12); + // Set pin to ADC. + palSetLineMode(ADC_CH0_PIN, PAL_MODE_INPUT_ANALOG); + /* * Creates the blinker thread. */ @@ -92,30 +99,32 @@ int main(void) { }; const ADCConversionGroup adcConvGroup = { - true, // circular + false, // circular 2, // num_channels &adc_end_callback, // end_cb &adc_error_callback, // error_cb - RP_ADC_CH0 | RP_ADC_CH1, // channel_mask + RP_ADC_CH0 | RP_ADC_CH4, // channel_mask }; adcStart(&ADCD1, &adcConfig); /* Enable temperature sensor. */ - //adcRPEnableTS(&ADCD1); + adcRPEnableTS(&ADCD1); - adcsample_t buf[2] = {0, 0}; + adcsample_t buf[4] = {0, 0, 0, 0}; - adcStartConversion(&ADCD1, &adcConvGroup, (adcsample_t *)&buf, 1); - - chThdSleepMilliseconds(500); - chprintf((BaseSequentialStream *)&SIOD0, "buf[0]: %d\r\n", buf[0]); - chprintf((BaseSequentialStream *)&SIOD0, "buf[1]: %d\r\n", buf[1]); + chprintf((BaseSequentialStream *)&SIOD0, "--\r\n"); + chThdSleepMilliseconds(100); + adcStartConversion(&ADCD1, &adcConvGroup, (adcsample_t *)&buf, 2); /* * Normal main() thread activity, in this demo it does nothing except * sleeping in a loop. */ while (true) { chThdSleepMilliseconds(500); + chprintf((BaseSequentialStream *)&SIOD0, "buf: %d, %d, %d, %d\r\n", buf[0], buf[1], buf[2], buf[3]); + if (!adcConvGroup.circular) { + adcStartConversion(&ADCD1, &adcConvGroup, (adcsample_t *)&buf, 2); + } } }