From 5eb9139287eeac3f02553ba0b306be6d26c25407 Mon Sep 17 00:00:00 2001 From: Josh Pieper Date: Wed, 15 Mar 2023 17:17:12 -0400 Subject: [PATCH] Make ADC initialization occur at fixed phasing relative to their prescaler In a398d0c4fde08ea5a585bbf0d53da6be422e0915 the initialization sequence was changed to improve performance. While that helped at the time, and I believed made the initialization robust it seems that it was possible for other phasing issues to cause problems. Notably, if ADC3 was initialized at some relative phases w.r.t. ADC1, then the ADC1 conversions would have undesired noise at some frequencies. To resolve this, we add some inline assembly that ensures that the ADC enable bit gets flipped at the exact same clock cycle relative to the prescaler every time. --- fw/BUILD | 1 + fw/bldc_servo.cc | 11 ++-- fw/moteus.cc | 4 +- fw/moteus_hw.cc | 2 +- fw/stm32g4_adc.cc | 138 ++++++++++++++++++++++++++++++++++++++++++++++ fw/stm32g4_adc.h | 25 +-------- 6 files changed, 150 insertions(+), 31 deletions(-) create mode 100644 fw/stm32g4_adc.cc diff --git a/fw/BUILD b/fw/BUILD index b7e1c37e..8543979c 100644 --- a/fw/BUILD +++ b/fw/BUILD @@ -115,6 +115,7 @@ MOTEUS_SOURCES = [ "motor_driver.h", "stm32_dma.h", "stm32g4_adc.h", + "stm32g4_adc.cc", "stm32g4_dma_uart.h", "stm32_bitbang_spi.h", "stm32_gpio_interrupt_in.h", diff --git a/fw/bldc_servo.cc b/fw/bldc_servo.cc index dbc61a1d..3c20cb9c 100644 --- a/fw/bldc_servo.cc +++ b/fw/bldc_servo.cc @@ -664,11 +664,12 @@ class BldcServo::Impl { (map_adc_prescale(config_.adc_prescale) << ADC_CCR_PRESC_Pos) | (1 << ADC_CCR_DUAL_Pos); // dual mode, regular + injected - EnableAdc(ms_timer_, ADC1); - EnableAdc(ms_timer_, ADC2); - EnableAdc(ms_timer_, ADC3); - EnableAdc(ms_timer_, ADC4); - EnableAdc(ms_timer_, ADC5); + + EnableAdc(ms_timer_, ADC1, config_.adc_prescale); + EnableAdc(ms_timer_, ADC2, config_.adc_prescale); + EnableAdc(ms_timer_, ADC3, config_.adc_prescale); + EnableAdc(ms_timer_, ADC4, config_.adc_prescale); + EnableAdc(ms_timer_, ADC5, config_.adc_prescale); if (family0_) { adc1_sqr_ = ADC1->SQR1 = diff --git a/fw/moteus.cc b/fw/moteus.cc index 01ed0289..0b05b2c8 100644 --- a/fw/moteus.cc +++ b/fw/moteus.cc @@ -179,15 +179,13 @@ int main(void) { mbed_die(); } - // To enable cycle counting. -#ifdef MOTEUS_PERFORMANCE_MEASURE + // We require cycle counting be enabled for some things. { ITM->LAR = 0xC5ACCE55; CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk; DWT->CYCCNT = 0; DWT->CTRL |= DWT_CTRL_CYCCNTENA_Msk; } -#endif // Turn on our power light. DigitalOut power_led(g_hw_pins.power_led, 0); diff --git a/fw/moteus_hw.cc b/fw/moteus_hw.cc index 86b36a92..5640da36 100644 --- a/fw/moteus_hw.cc +++ b/fw/moteus_hw.cc @@ -147,7 +147,7 @@ FamilyAndVersion DetectMoteusFamily(MillisecondTimer* timer) { (17 << ADC_SQR1_SQ1_Pos) | // IN17 (0 << ADC_SQR1_L_Pos); // length 1 - EnableAdc(timer, ADC2); + EnableAdc(timer, ADC2, 16); ADC2->CR |= ADC_CR_ADSTART; while ((ADC2->ISR & ADC_ISR_EOC) == 0); diff --git a/fw/stm32g4_adc.cc b/fw/stm32g4_adc.cc new file mode 100644 index 00000000..2b4bf77a --- /dev/null +++ b/fw/stm32g4_adc.cc @@ -0,0 +1,138 @@ +// Copyright 2018-2023 Josh Pieper, jjp@pobox.com. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "fw/stm32g4_adc.h" + +namespace moteus { + +void EnableAdc(MillisecondTimer* timer, ADC_TypeDef* adc, int prescaler) { + // 20.4.6: ADC Deep power-down mode startup procedure + adc->CR &= ~ADC_CR_DEEPPWD; + adc->CR |= ADC_CR_ADVREGEN; + + // tADCREG_S = 20us per STM32G474xB datasheet + timer->wait_us(20); + + adc->CR |= ADC_CR_ADCAL; + while (adc->CR & ADC_CR_ADCAL); + timer->wait_us(1); + + // 20.4.9: Software procedure to enable the ADC + adc->ISR |= ADC_ISR_ADRDY; + + + + // Note: The STM32G4 has many ADCs, ADC1/2 share a prescaler as do + // 3/4. When the 3/4 post-prescaler clocks are aligned at some + // phases, the ADC1/2 results can be corrupted (assuming ADC3/4 have + // their sampling started immediately after 1/2). To make this more + // repeatable, we ensure that the ADC is enabled at a known phase + // with respect to the global cycle counter as measured by the DWT. + + // To do this, we use the DWT's cycle counter to delay until it hits + // a value that modulo the prescaler equals zero exactly. We first + // calculate this, then jump through a bunch of hoops to reliably + // get the STM32G4 to have a cycle accurate time. + const uint32_t initial_cyccnt = DWT->CYCCNT; + const uint32_t desired_cyccnt = + static_cast(initial_cyccnt / prescaler) * prescaler + + prescaler * 32; + + // Clear the instruction and data cache, to help get the same + // results every time. + FLASH->ACR &= ~(FLASH_ACR_ICEN | FLASH_ACR_DCEN); + FLASH->ACR |= FLASH_ACR_ICRST | FLASH_ACR_DCRST; + FLASH->ACR &= ~(FLASH_ACR_ICRST | FLASH_ACR_DCRST); + FLASH->ACR |= (FLASH_ACR_ICEN | FLASH_ACR_DCEN); + + uint8_t buf[16]; + uint32_t tmp; + uint32_t loop_count; + uint32_t remainder; + asm volatile ( + // Align ourselves to improve robustness. + ".balign 4;" + + // Capture the current CYCCNT, subtract it from our desired and + // a hand-tuned extra amount. + "ldr %[tmp], [%[cyccnt], #0];" + "sub %[tmp], %[desired], %[tmp];" + "sub %[tmp], %[tmp], #16;" + + // Our ultimate delay loop takes 4 cycles per iteration. + "asr %[loop_count], %[tmp], #2;" + + // Which means we may have up to 3 extra NOPs to insert. This + // part of the code requires the function to be placed in CCM + // memory, otherwise the FLASH ART destroys our ability to add + // single cycle delays. + "and %[remainder], %[tmp], #3;" + + "cmp %[remainder], #0;" + "ble 10f;" + "mov %[tmp], #0;" + "mov %[tmp], #0;" + "10:;" + + "cmp %[remainder], #1;" + "ble 11f;" + "mov %[tmp], #0;" + "mov %[tmp], #0;" + "11:;" + + "cmp %[remainder], #2;" + "ble 12f;" + "mov %[tmp], #0;" + "mov %[tmp], #0;" + "12:;" + + // Make sure we are at the beginning of any pre-fetch lines. + ".balign 4;" + + // Our main loop, which takes 4 cycles. + "1:;" + "subs %[loop_count], %[loop_count], #1;" + "nop;" + "bne 1b;" + + "nop;" + + // To verify this result, use a debugger and set a breakpoint at + // the next instruction after this inline block. At that point, + // compare MyDWT->CYCCNT to desired_cycnt. Note that you cannot + // single step or use any breakpoints at any intermediate point, + // or the results will not be meaningful. + : [tmp]"=&r"(tmp), + [loop_count]"=&r"(loop_count), + [remainder]"=&r"(remainder) + : [cyccnt]"r"(&DWT->CYCCNT), + [desired]"r"(desired_cyccnt), + [buf]"r"(buf) + : + ); + + adc->CR |= ADC_CR_ADEN; + + // This is just to force the above line to not be re-ordered. + asm volatile ( + "nop;" + ); + + while (! (adc->ISR & ADC_ISR_ADRDY)); + adc->ISR |= ADC_ISR_ADRDY; + + adc->CFGR &= ~(ADC_CFGR_CONT); +} + +} diff --git a/fw/stm32g4_adc.h b/fw/stm32g4_adc.h index 4707c097..4722bd1d 100644 --- a/fw/stm32g4_adc.h +++ b/fw/stm32g4_adc.h @@ -16,6 +16,7 @@ #include "mbed.h" +#include "fw/ccm.h" #include "fw/millisecond_timer.h" namespace moteus { @@ -27,26 +28,6 @@ inline void DisableAdc(ADC_TypeDef* adc) { } } -inline void EnableAdc(MillisecondTimer* timer, ADC_TypeDef* adc) { - // 20.4.6: ADC Deep power-down mode startup procedure - adc->CR &= ~ADC_CR_DEEPPWD; - adc->CR |= ADC_CR_ADVREGEN; - - // tADCREG_S = 20us per STM32G474xB datasheet - timer->wait_us(20); - - adc->CR |= ADC_CR_ADCAL; - while (adc->CR & ADC_CR_ADCAL); - timer->wait_us(1); - - // 20.4.9: Software procedure to enable the ADC - adc->ISR |= ADC_ISR_ADRDY; - adc->CR |= ADC_CR_ADEN; - - while (! (adc->ISR & ADC_ISR_ADRDY)); - adc->ISR |= ADC_ISR_ADRDY; - - adc->CFGR &= ~(ADC_CFGR_CONT); -} - +// This function needs to be in CCM memory to achieve deterministic timings. +void EnableAdc(MillisecondTimer* timer, ADC_TypeDef* adc, int prescaler) MOTEUS_CCM_ATTRIBUTE; }