From 965057497165975a501236dc0a185e9a02d952bb Mon Sep 17 00:00:00 2001 From: KienTranCNC Date: Mon, 11 May 2026 16:54:48 +0700 Subject: [PATCH 1/5] =?UTF-8?q?B=E1=BB=95=20sung=20th=C3=AAm=20STM32?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Changed_code_v19.txt | 832 ++++++++++++++++++++++++++++++++++ README.md | 43 ++ extras/StepperPins_stm32.h | 34 ++ library.properties | 8 +- src/fas_arch/arduino_stm32.h | 21 + src/fas_arch/common.h | 5 + src/fas_queue/stepper_queue.h | 2 + src/fas_ramp/RampCalculator.h | 91 ++++ src/pd_stm32/pd_config.h | 46 ++ src/pd_stm32/stm32_queue.cpp | 385 ++++++++++++++++ src/pd_stm32/stm32_queue.h | 127 ++++++ 11 files changed, 1590 insertions(+), 4 deletions(-) create mode 100644 Changed_code_v19.txt create mode 100644 extras/StepperPins_stm32.h create mode 100644 src/fas_arch/arduino_stm32.h create mode 100644 src/pd_stm32/pd_config.h create mode 100644 src/pd_stm32/stm32_queue.cpp create mode 100644 src/pd_stm32/stm32_queue.h diff --git a/Changed_code_v19.txt b/Changed_code_v19.txt new file mode 100644 index 00000000..72d20faa --- /dev/null +++ b/Changed_code_v19.txt @@ -0,0 +1,832 @@ +Changed_code_v19.txt +===================== +All files created or modified for FastAccelStepper STM32 port, plan v.19 +Date: 2026-05-11 + +--- + +=== NEW FILE: src/fas_arch/arduino_stm32.h === + +#ifndef FAS_ARCH_ARDUINO_STM32_H +#define FAS_ARCH_ARDUINO_STM32_H + +#define FAS_STM32 + +#include +#include +#include + +// PRIMASK reentrant-safe interrupt control +// Saves and restores PRIMASK to support nested disable/enable calls +#define fasDisableInterrupts() \ + uint32_t __fas_prim = __get_PRIMASK(); __disable_irq() +#define fasEnableInterrupts() \ + __set_PRIMASK(__fas_prim) + +#define FAS_PSTR(s) (s) +#define PIN_UNDEFINED 0xFF +#define PIN_EXTERNAL_FLAG 128 + +#endif /* FAS_ARCH_ARDUINO_STM32_H */ + +end of file src/fas_arch/arduino_stm32.h + +--- + +=== NEW FILE: src/pd_stm32/pd_config.h === + +#ifndef PD_STM32_CONFIG_H +#define PD_STM32_CONFIG_H + +#include + +// ==================================================================== +// Compile-time TICKS_PER_S +// +// User MUST define TICKS_PER_S matching their board's TIM2 counter clock. +// TIM2 counter clock = PCLK1 * (APB1_prescaler==1 ? 1 : 2) +// +// Examples: +// STM32F103 @72MHz: TICKS_PER_S = 72000000 (APB1=36MHz ×2) +// STM32F407 @168MHz: TICKS_PER_S = 84000000 (APB1=42MHz ×2) +// STM32G0 @64MHz: TICKS_PER_S = 64000000 (APB1=64MHz ×1) +// STM32H743 @480MHz: TICKS_PER_S = 240000000 (APB1=120MHz ×2) +// ==================================================================== +#ifndef TICKS_PER_S +#define TICKS_PER_S 72000000UL +#endif + +// ---- Queue topology ---- +#define MAX_STEPPER 4 +#define NUM_QUEUES 4 +#define QUEUE_LEN 32 + +// ---- Pulse width (configurable) ---- +#ifndef STEP_PULSE_WIDTH_US +#define STEP_PULSE_WIDTH_US 6 +#endif +#define STEP_PULSE_WIDTH_TICKS ((uint32_t)(STEP_PULSE_WIDTH_US * (TICKS_PER_S / 1000000L))) + +// ---- Timing constants ---- +#define MIN_CMD_TICKS (TICKS_PER_S / 5000) +#define MIN_DIR_DELAY_US 200 +#define MAX_DIR_DELAY_US (65535 / (TICKS_PER_S / 1000000)) +#define DELAY_MS_BASE 2 +#define CYCLIC_INTERVAL_MS 3 + +// ---- Feature flags ---- +#define SUPPORT_QUEUE_ENTRY_END_POS_U16 +#define NEED_GENERIC_GET_CURRENT_POSITION +#define noop_or_wait __NOP() +#define DEBUG_LED_HALF_PERIOD 50 + +#endif /* PD_STM32_CONFIG_H */ + +//end of file src/pd_stm32/pd_config.h + +--- + +=== NEW FILE: src/pd_stm32/stm32_queue.h === + +#ifndef PD_STM32_QUEUE_H +#define PD_STM32_QUEUE_H + +#include "FastAccelStepper.h" +#include "fas_queue/base.h" +#include "fas_arch/result_codes.h" + +// ---- Default pin mapping (overridable in sketch) ---- +#ifndef STEP_PIN_STEPPER_0 +#define STEP_PIN_STEPPER_0 PA0 +#endif +#ifndef STEP_PIN_STEPPER_1 +#define STEP_PIN_STEPPER_1 PA1 +#endif +#ifndef STEP_PIN_STEPPER_2 +#define STEP_PIN_STEPPER_2 PA2 +#endif +#ifndef STEP_PIN_STEPPER_3 +#define STEP_PIN_STEPPER_3 PA3 +#endif + +// ---- CC interrupt bit helpers ---- +#define CCXIE_BIT(ch) (TIM_DIER_CC1IE << (ch)) +#define CCXIF_BIT(ch) (TIM_SR_CC1IF << (ch)) + +// ---- BSRR/BRR detection ---- +// portClearRegister returns &BSRR on F2/F4/F7, &BRR elsewhere +// When BSRR: write mask << 16 for reset +// When BRR: write mask directly +#if defined(STM32F2xx) || defined(STM32F4xx) || defined(STM32F7xx) +#define STM32_BSRR_CLEAR_SHIFT 1 +#else +#define STM32_BSRR_CLEAR_SHIFT 0 +#endif + +// ==================================================================== +// StepperQueue class — STM32-specific implementation +// ==================================================================== +class StepperQueue : public StepperQueueBase { + public: +#include "../fas_queue/protocol.h" + + volatile bool _isRunning; + bool _initialized; + + // Step pin GPIO + uint8_t _step_pin; + GPIO_TypeDef* _step_port; + uint32_t _step_set_mask; // BSRR set mask (low 16 bits) + uint32_t _step_clr_mask; // Clear mask (BRR or BSRR<<16) + + // Direction pin (atomic via BSRR/BRR) + volatile uint32_t* _dir_bsrr; // &GPIOx->BSRR + uint32_t _dir_set_mask; // BSRR low bits = set + uint32_t _dir_clr_mask; // BSRR high bits = reset, or BRR + + // Timer + volatile uint32_t* _ccr_reg; // &TIM2->CCR1/2/3/4 + uint8_t _timer_ch; // 0..3 + + // Pulse tracking + volatile bool _pulse_high; + volatile bool _dir_delay_active; // Direction settling in progress + + // Channel-to-queue mapping (static) + static StepperQueue* _ch_to_queue[4]; + + // ---- Inline methods ---- + inline void _pd_initVars() { + _step_pin = PIN_UNDEFINED; + _step_port = NULL; + _step_set_mask = 0; + _step_clr_mask = 0; + _dir_bsrr = NULL; + _dir_set_mask = 0; + _dir_clr_mask = 0; + _ccr_reg = NULL; + _timer_ch = 0; + _isRunning = false; + _initialized = false; + _pulse_high = false; + _dir_delay_active = false; + max_speed_in_ticks = STEP_PULSE_WIDTH_TICKS * 4; + } + + inline bool isRunning() const { return _isRunning; } + inline bool isReadyForCommands() const { return true; } + + void setDirPin(uint8_t dir_pin, bool _dirHighCountsUp) { + dirPin = dir_pin; + dirHighCountsUp = _dirHighCountsUp; + if ((dir_pin != PIN_UNDEFINED) && ((dir_pin & PIN_EXTERNAL_FLAG) == 0)) { + GPIO_TypeDef* port = digitalPinToPort(dir_pin); + uint32_t mask = digitalPinToBitMask(dir_pin); + _dir_bsrr = &port->BSRR; + _dir_set_mask = mask; +#if STM32_BSRR_CLEAR_SHIFT + _dir_clr_mask = mask << 16; // BSRR high half = reset +#else + _dir_clr_mask = mask; // BRR register direct +#endif + } + } + + void adjustSpeedToStepperCount(uint8_t steppers); + + private: + static StepperQueue* allocateSlot(uint8_t step_pin, uint8_t timer_ch); +}; + +// ---- Direction pin: atomic via BSRR/BRR ---- +#define SET_DIRECTION_PIN_STATE(q, high) \ + do { \ + if ((q)->_dir_bsrr) { \ + *(q->_dir_bsrr) = (high) ? (q)->_dir_set_mask \ + : (q)->_dir_clr_mask; \ + } \ + } while (0) + +// ---- Enable pin: simple digitalWrite ---- +#define SET_ENABLE_PIN_STATE(q, pin, high) \ + digitalWrite((pin), (high) ? HIGH : LOW) + +// ---- Direction-to-pulse delay ---- +#define AFTER_SET_DIR_PIN_DELAY_US 30 + +#endif /* PD_STM32_QUEUE_H */ + +end of file src/pd_stm32/stm32_queue.h + +--- + +=== NEW FILE: src/pd_stm32/stm32_queue.cpp === + +#include "fas_queue/stepper_queue.h" +#include "log2/Log2Representation.h" +#include "fas_ramp/RampControl.h" + +#if defined(ARDUINO_ARCH_STM32) + +// ==================================================================== +// Static data +// ==================================================================== +static FastAccelStepperEngine* fas_engine = NULL; +static uint8_t stepper_allocated_mask = 0; +static volatile bool _cyclic_pending = false; +static uint32_t _last_cyclic_uwtick = 0; +uint8_t fas_stm32_clock_error = 0; +StepperQueue* StepperQueue::_ch_to_queue[4] = {NULL, NULL, NULL, NULL}; + +// ==================================================================== +// Log2 timer frequency variables (for SUPPORT_LOG2_TIMER_FREQ_VARIABLES) +// Defined here when RampCalculator.h selects runtime fallback path. +// ==================================================================== +#ifdef SUPPORT_LOG2_TIMER_FREQ_VARIABLES +static log2_value_t log2_timer_freq; +static log2_value_t log2_timer_freq_div_sqrt_of_2; +static log2_value_t log2_timer_freq_square_div_2; +#endif + +// ==================================================================== +// TIM2 clock detection +// +// TIM2 counter clock = PCLK1 * (APB1_prescaler==1 ? 1 : 2) +// +// The APB1 prescaler is in RCC->CFGR.PPRE on most STM32 families. +// On H7 it is in RCC->D2CFGR.D2PPRE1 (D2 domain). +// ==================================================================== +static uint32_t getTim2Clock(void) { + uint32_t pclk1 = HAL_RCC_GetPCLK1Freq(); +#if defined(STM32H7xx) + // H7 series: D2 domain, D2CFGR register + uint32_t dppre1 = (RCC->D2CFGR & RCC_D2CFGR_D2PPRE1) >> RCC_D2CFGR_D2PPRE1_Pos; + if (dppre1 > 1) pclk1 *= 2; +#else + // All other STM32 families + uint32_t apb1_pre = (RCC->CFGR & RCC_CFGR_PPRE) >> RCC_CFGR_PPRE_Pos; + if (apb1_pre > 1) pclk1 *= 2; +#endif + return pclk1; +} + +// ==================================================================== +// TIM2 initialization (step generation timer) +// Called once when first stepper is initialized. +// ==================================================================== +static void initStepTimer(void) { + static bool initialized = false; + if (initialized) return; + initialized = true; + + // Enable TIM2 clock (portable across all STM32 families) + __HAL_RCC_TIM2_CLK_ENABLE(); + + // Compute prescaler from actual TIM2 clock and desired TICKS_PER_S + uint32_t tim2_clk = getTim2Clock(); + uint32_t psc; + + if (TICKS_PER_S > tim2_clk) { + // Cannot achieve TICKS_PER_S at this clock frequency. + // Run at maximum rate (PSC=0). All timing will be incorrect. + fas_stm32_clock_error = 1; + psc = 0; + } else { + psc = (tim2_clk / TICKS_PER_S) - 1; + if (psc > 65535) psc = 65535; + } + + // Configure TIM2 + TIM2->CR1 = 0; + TIM2->PSC = psc; + TIM2->ARR = 0xFFFFFFFF; // 32-bit auto-reload + TIM2->EGR |= TIM_EGR_UG; // Reload shadow registers (PSC, ARR) + TIM2->DIER = 0; // All interrupts disabled initially + + // Force LOW all channels (OCxM = 100 = Force Inactive) + // This prevents spurious pulses during initialization. + TIM2->CCMR1 = (4 << 4) | (4 << 12); // OC1M=100, OC2M=100 + TIM2->CCMR2 = (4 << 4) | (4 << 12); // OC3M=100, OC4M=100 + + // NVIC configuration + NVIC_SetPriority(TIM2_IRQn, 0); // Highest priority for step timing + NVIC_EnableIRQ(TIM2_IRQn); + + // Start timer + TIM2->CR1 |= TIM_CR1_CEN; +} + +// ==================================================================== +// Dynamic slot allocation — supports ANY GPIO pin for step +// ==================================================================== +static int8_t findFreeSlot(void) { + for (int i = 0; i < MAX_STEPPER; i++) { + if (!(stepper_allocated_mask & (1 << i))) { + return i; + } + } + return -1; // All slots used +} + +// ==================================================================== +// Queue initialization +// ==================================================================== +void StepperQueue::init(uint8_t queue_num, uint8_t step_pin) { + static const uint8_t ch_map[4] = {0, 1, 2, 3}; + _timer_ch = ch_map[queue_num]; + + // Ensure TIM2 is initialized + initStepTimer(); + + // Step pin GPIO configuration + _step_pin = step_pin; + _step_port = digitalPinToPort(step_pin); + uint32_t mask = digitalPinToBitMask(step_pin); + _step_set_mask = mask; + + // Configure clear mask based on BSRR/BRR architecture +#if STM32_BSRR_CLEAR_SHIFT + _step_clr_mask = mask << 16; // BSRR high half = reset bits +#else + _step_clr_mask = mask; // BRR register (direct) +#endif + + // Configure pin as OUTPUT, initial LOW + pinMode(step_pin, OUTPUT); + digitalWrite(step_pin, LOW); + + // Store CCR register pointer for fast ISR access + volatile uint32_t* ccr[] = {&TIM2->CCR1, &TIM2->CCR2, &TIM2->CCR3, &TIM2->CCR4}; + _ccr_reg = ccr[_timer_ch]; + + // Register channel-to-queue mapping + _ch_to_queue[_timer_ch] = this; + _initialized = true; + _isRunning = false; +} + +// ==================================================================== +// Start / Stop +// ==================================================================== +void StepperQueue::startQueue(void) { + _isRunning = true; + _pulse_high = false; + _dir_delay_active = false; + + // Write CCR first, then barrier, then enable interrupt + // This ensures the compare value is visible before the IRQ can fire. + *_ccr_reg = TIM2->CNT + (TICKS_PER_S / 1000000); // ~1µs offset + __DMB(); + + // DIER RMW: disable interrupts to prevent race with ISR on other channels + __disable_irq(); + TIM2->DIER |= CCXIE_BIT(_timer_ch); + __enable_irq(); +} + +void StepperQueue::forceStop(void) { + // Disable CC interrupt (atomic) + __disable_irq(); + TIM2->DIER &= ~CCXIE_BIT(_timer_ch); + _isRunning = false; + read_idx = next_write_idx; // Discard remaining queue entries + __enable_irq(); + + // Ensure step pin is LOW (use BSRR/BRR appropriately) + if (_step_port) { +#if STM32_BSRR_CLEAR_SHIFT + _step_port->BSRR = _step_clr_mask; +#else + _step_port->BRR = _step_clr_mask; +#endif + } +} + +void StepperQueue::connect(void) {} +void StepperQueue::disconnect(void) {} + +// ==================================================================== +// Speed adjustment based on number of active steppers +// ==================================================================== +void StepperQueue::adjustSpeedToStepperCount(uint8_t steppers) { + if (steppers == 1) + max_speed_in_ticks = STEP_PULSE_WIDTH_TICKS * 2; + else if (steppers == 2) + max_speed_in_ticks = STEP_PULSE_WIDTH_TICKS * 3; + else + max_speed_in_ticks = STEP_PULSE_WIDTH_TICKS * 4; + + if (max_speed_in_ticks < MIN_CMD_TICKS) + max_speed_in_ticks = MIN_CMD_TICKS; +} + +// ==================================================================== +// getActualTicksWithDirection — retrieve current step rate +// ==================================================================== +bool StepperQueue::getActualTicksWithDirection( + struct actual_ticks_s* speed) const { + fasDisableInterrupts(); + speed->count_up = queue_end.count_up; + speed->ticks = _last_command_ticks; + fasEnableInterrupts(); + inject_fill_interrupt(0); + return true; +} + +// ==================================================================== +// Cyclic PendSV trigger +// Called at end of TIM2_IRQHandler every ~3ms. +// Triggers PendSV exception to fill queues without consuming ISR time. +// ==================================================================== +static void cyclic_check_and_pend(void) { + uint32_t now = uwTick; + if ((now - _last_cyclic_uwtick) >= CYCLIC_INTERVAL_MS) { + _last_cyclic_uwtick = now; + if (!_cyclic_pending) { + _cyclic_pending = true; + SCB->ICSR = SCB_ICSR_PENDSVSET_Msk; + } + } +} + +// ==================================================================== +// TIM2_IRQHandler — Step pulse generation (all 4 channels) +// +// State machine: +// Phase 1 (_pulse_high=false): Set step pin HIGH, schedule LOW +// Phase 2 (_pulse_high=true): Set step pin LOW, process queue +// DirSettle (_dir_delay_active): Direction settling complete → start pulse +// +// SR handling: +// Snapshot SR at entry. Build ch_processed mask of all handled flags. +// Clear all at end by writing ~ch_processed (rc_w0 behavior). +// ==================================================================== +void TIM2_IRQHandler(void) { + uint32_t sr = TIM2->SR; + uint32_t ch_processed = 0; + + for (uint8_t ch = 0; ch < 4; ch++) { + uint32_t ccif = CCXIF_BIT(ch); + if (!(sr & ccif)) continue; + + // Always mark for clear — prevents infinite loop from spurious IRQs + ch_processed |= ccif; + + StepperQueue* q = StepperQueue::_ch_to_queue[ch]; + if (!q || !q->_isRunning) continue; + + if (q->_pulse_high) { + // ====== Phase 2: pulse end ====== + // The step pin was HIGH; bring it LOW. + q->_pulse_high = false; + +#if STM32_BSRR_CLEAR_SHIFT + q->_step_port->BSRR = q->_step_clr_mask; +#else + q->_step_port->BRR = q->_step_clr_mask; +#endif + + // Read queue entry + uint8_t rp = q->read_idx; + uint8_t wp = q->next_write_idx; + + if (rp == wp) { + // Queue empty — stop this channel + TIM2->DIER &= ~CCXIE_BIT(ch); + q->_isRunning = false; + continue; + } + + struct queue_entry* e = &q->entry[rp & QUEUE_LEN_MASK]; + q->_last_command_ticks = e->ticks; + + if (e->steps > 1) { + // Multi-step command: reduce step count, continue with same period + e->steps--; + *q->_ccr_reg = TIM2->CNT + (uint32_t)e->ticks; + } else { + // Single step complete — advance to next entry + rp++; + q->read_idx = rp; + + if (rp == wp) { + TIM2->DIER &= ~CCXIE_BIT(ch); + q->_isRunning = false; + continue; + } + + e = &q->entry[rp & QUEUE_LEN_MASK]; + + // Handle direction change (BSRR atomic — NOT ODR XOR) + if (e->toggle_dir && q->_dir_bsrr) { + if (e->dirPinState) { + *q->_dir_bsrr = q->_dir_set_mask; + } else { + *q->_dir_bsrr = q->_dir_clr_mask; + } + e->toggle_dir = 0; // Clear flag — prevents double-toggle + + // Insert direction settling delay + uint32_t dd = AFTER_SET_DIR_PIN_DELAY_US * (TICKS_PER_S / 1000000UL); + if (dd < MIN_CMD_TICKS) dd = MIN_CMD_TICKS; + *q->_ccr_reg = TIM2->CNT + dd; + q->_dir_delay_active = true; + continue; + } + + // No direction change — schedule next step pulse + *q->_ccr_reg = TIM2->CNT + (uint32_t)e->ticks; + } + } else if (q->_dir_delay_active) { + // ====== Direction settling complete ====== + // The settling delay has elapsed. Now start the first step pulse + // in the new direction (set pin HIGH). + q->_dir_delay_active = false; + q->_pulse_high = true; + q->_step_port->BSRR = q->_step_set_mask; + *q->_ccr_reg = TIM2->CNT + STEP_PULSE_WIDTH_TICKS; + } else { + // ====== Phase 1: pulse start ====== + // Set step pin HIGH, schedule LOW after STEP_PULSE_WIDTH_TICKS. + q->_pulse_high = true; + q->_step_port->BSRR = q->_step_set_mask; + *q->_ccr_reg = TIM2->CNT + STEP_PULSE_WIDTH_TICKS; + } + } + + // Clear all processed flags at once (rc_w0: bits set to 1 are ignored) + TIM2->SR = ~ch_processed; + + // Trigger cyclic queue fill + cyclic_check_and_pend(); +} + +// ==================================================================== +// PendSV_Handler — Deferred queue fill +// +// Weak attribute allows FreeRTOS to override this handler. +// Define DISABLE_FAS_PENDSV to skip installation entirely. +// ==================================================================== +#if !defined(DISABLE_FAS_PENDSV) +__attribute__((weak)) void PendSV_Handler(void) { + _cyclic_pending = false; + __DMB(); + if (fas_engine) { + fas_engine->manageSteppers(); + } +} +#endif + +// ==================================================================== +// Allocation — dynamic slot assignment for any GPIO step pin +// ==================================================================== +StepperQueue* StepperQueue::tryAllocateQueue( + FastAccelStepperEngine* engine, uint8_t step_pin) { + (void)engine; + int8_t idx = findFreeSlot(); + if (idx < 0) return nullptr; + + fas_queue[idx]._initVars(); + fas_queue[idx].init((uint8_t)idx, step_pin); + stepper_allocated_mask |= (1 << idx); + return &fas_queue[idx]; +} + +// ==================================================================== +// Engine initialization +// ==================================================================== +void fas_init_engine(FastAccelStepperEngine* engine) { + fas_engine = engine; + + // Initialize Log2 timer frequency variables if using runtime fallback + // (SUPPORT_LOG2_TIMER_FREQ_VARIABLES path in RampCalculator.h) + init_ramp_module(); + + // PendSV at lowest priority — avoids blocking higher-priority interrupts + NVIC_SetPriority(PendSV_IRQn, 0xFF); +} + +#endif /* ARDUINO_ARCH_STM32 */ + +end of file src/pd_stm32/stm32_queue.cpp + +--- + +=== NEW FILE: extras/StepperPins_stm32.h === + +#ifndef STEPPERPINS_STM32_H +#define STEPPERPINS_STM32_H + +// ==================================================================== +// Default step pin mapping for STM32 platforms +// +// These defaults map steppers 0-3 to PA0-PA3 (TIM2 channels 1-4). +// Override by defining before including this header: +// +// #define STEP_PIN_STEPPER_0 PB0 +// #include "StepperPins_stm32.h" +// +// Note: On STM32, any GPIO pin can be used as a step pin. +// PA0-PA3 are only a convention — the timer is used only for +// interrupt timing, not direct pin output. +// ==================================================================== + +#ifndef STEP_PIN_STEPPER_0 +#define STEP_PIN_STEPPER_0 PA0 +#endif + +#ifndef STEP_PIN_STEPPER_1 +#define STEP_PIN_STEPPER_1 PA1 +#endif + +#ifndef STEP_PIN_STEPPER_2 +#define STEP_PIN_STEPPER_2 PA2 +#endif + +#ifndef STEP_PIN_STEPPER_3 +#define STEP_PIN_STEPPER_3 PA3 +#endif + +#endif /* STEPPERPINS_STM32_H */ + +end of file extras/StepperPins_stm32.h + +--- + +=== MODIFIED FILE: src/fas_arch/common.h (ADDED STM32 DISPATCH) === + +// Added after line 95 (PICO_RP2040 branch): +// The new branch: + +#elif defined(ARDUINO_ARCH_STM32) +// STM32 family (STM32duino core) +#include "fas_arch/arduino_stm32.h" +#include "pd_stm32/pd_config.h" + +// This was inserted BEFORE #else #error "Unsupported devices" + +end of file src/fas_arch/common.h (modification shown) + +--- + +=== MODIFIED FILE: src/fas_queue/stepper_queue.h (ADDED STM32 BRANCH) === + +// Added after line 21 (SUPPORT_ESP32 branch): +// The new branch: + +#elif defined(FAS_STM32) +#include "pd_stm32/stm32_queue.h" + +// This was inserted BEFORE #else #error "Unsupported architecture" + +end of file src/fas_queue/stepper_queue.h (modification shown) + +--- + +=== MODIFIED FILE: src/fas_ramp/RampCalculator.h (ADDED 13 STM32 FREQUENCY BRANCHES) === + +// Added after line 20 (TICKS_PER_S == 21000000L branch): +// The 13 new STM32 frequency branches: + +#elif (TICKS_PER_S == 32000000L) +// STM32L0, RP2040 +#define LOG2_TICKS_PER_S ((log2_value_t)0x31dd) +#define LOG2_TICKS_PER_S_DIV_SQRT_OF_2 ((log2_value_t)0x30dd) +#define LOG2_ACCEL_FACTOR ((log2_value_t)0x61ba) +#define US_TO_TICKS(u32) ((u32) * 32) +#define TICKS_TO_US(u32) ((u32) / 32) + +#elif (TICKS_PER_S == 48000000L) +// STM32F0/G0/WL +#define LOG2_TICKS_PER_S ((log2_value_t)0x3308) +#define LOG2_TICKS_PER_S_DIV_SQRT_OF_2 ((log2_value_t)0x3208) +#define LOG2_ACCEL_FACTOR ((log2_value_t)0x6411) +#define US_TO_TICKS(u32) ((u32) * 48) +#define TICKS_TO_US(u32) ((u32) / 48) + +#elif (TICKS_PER_S == 64000000L) +// STM32G0/WB +#define LOG2_TICKS_PER_S ((log2_value_t)0x33dd) +#define LOG2_TICKS_PER_S_DIV_SQRT_OF_2 ((log2_value_t)0x32dd) +#define LOG2_ACCEL_FACTOR ((log2_value_t)0x65ba) +#define US_TO_TICKS(u32) ((u32) * 64) +#define TICKS_TO_US(u32) ((u32) / 64) + +#elif (TICKS_PER_S == 72000000L) +// STM32F1/L1 (most common) +#define LOG2_TICKS_PER_S ((log2_value_t)0x3434) +#define LOG2_TICKS_PER_S_DIV_SQRT_OF_2 ((log2_value_t)0x3334) +#define LOG2_ACCEL_FACTOR ((log2_value_t)0x6668) +#define US_TO_TICKS(u32) ((u32) * 72) +#define TICKS_TO_US(u32) ((u32) / 72) + +#elif (TICKS_PER_S == 80000000L) +// STM32L4 +#define LOG2_TICKS_PER_S ((log2_value_t)0x3482) +#define LOG2_TICKS_PER_S_DIV_SQRT_OF_2 ((log2_value_t)0x3382) +#define LOG2_ACCEL_FACTOR ((log2_value_t)0x6704) +#define US_TO_TICKS(u32) ((u32) * 80) +#define TICKS_TO_US(u32) ((u32) / 80) + +#elif (TICKS_PER_S == 84000000L) +// STM32F401/411 +#define LOG2_TICKS_PER_S ((log2_value_t)0x34a6) +#define LOG2_TICKS_PER_S_DIV_SQRT_OF_2 ((log2_value_t)0x33a6) +#define LOG2_ACCEL_FACTOR ((log2_value_t)0x674c) +#define US_TO_TICKS(u32) ((u32) * 84) +#define TICKS_TO_US(u32) ((u32) / 84) + +#elif (TICKS_PER_S == 100000000L) +// STM32F411/746 +#define LOG2_TICKS_PER_S ((log2_value_t)0x3527) +#define LOG2_TICKS_PER_S_DIV_SQRT_OF_2 ((log2_value_t)0x3427) +#define LOG2_ACCEL_FACTOR ((log2_value_t)0x684d) +#define US_TO_TICKS(u32) ((u32) * 100) +#define TICKS_TO_US(u32) ((u32) / 100) + +#elif (TICKS_PER_S == 120000000L) +// STM32L4+/F4 +#define LOG2_TICKS_PER_S ((log2_value_t)0x35ad) +#define LOG2_TICKS_PER_S_DIV_SQRT_OF_2 ((log2_value_t)0x34ad) +#define LOG2_ACCEL_FACTOR ((log2_value_t)0x695a) +#define US_TO_TICKS(u32) ((u32) * 120) +#define TICKS_TO_US(u32) ((u32) / 120) + +#elif (TICKS_PER_S == 168000000L) +// STM32F405/407 +#define LOG2_TICKS_PER_S ((log2_value_t)0x36a6) +#define LOG2_TICKS_PER_S_DIV_SQRT_OF_2 ((log2_value_t)0x35a6) +#define LOG2_ACCEL_FACTOR ((log2_value_t)0x6b4c) +#define US_TO_TICKS(u32) ((u32) * 168) +#define TICKS_TO_US(u32) ((u32) / 168) + +#elif (TICKS_PER_S == 170000000L) +// STM32F3/G4 +#define LOG2_TICKS_PER_S ((log2_value_t)0x36af) +#define LOG2_TICKS_PER_S_DIV_SQRT_OF_2 ((log2_value_t)0x35af) +#define LOG2_ACCEL_FACTOR ((log2_value_t)0x6b5e) +#define US_TO_TICKS(u32) ((u32) * 170) +#define TICKS_TO_US(u32) ((u32) / 170) + +#elif (TICKS_PER_S == 216000000L) +// STM32F7 +#define LOG2_TICKS_PER_S ((log2_value_t)0x375f) +#define LOG2_TICKS_PER_S_DIV_SQRT_OF_2 ((log2_value_t)0x365f) +#define LOG2_ACCEL_FACTOR ((log2_value_t)0x6cbe) +#define US_TO_TICKS(u32) ((u32) * 216) +#define TICKS_TO_US(u32) ((u32) / 216) + +#elif (TICKS_PER_S == 480000000L) +// STM32H7 (default) +#define LOG2_TICKS_PER_S ((log2_value_t)0x39ad) +#define LOG2_TICKS_PER_S_DIV_SQRT_OF_2 ((log2_value_t)0x38ad) +#define LOG2_ACCEL_FACTOR ((log2_value_t)0x715a) +#define US_TO_TICKS(u32) ((u32) * 480) +#define TICKS_TO_US(u32) ((u32) / 480) + +#elif (TICKS_PER_S == 550000000L) +// STM32H7 (overclock) +#define LOG2_TICKS_PER_S ((log2_value_t)0x3a12) +#define LOG2_TICKS_PER_S_DIV_SQRT_OF_2 ((log2_value_t)0x3912) +#define LOG2_ACCEL_FACTOR ((log2_value_t)0x7224) +#define US_TO_TICKS(u32) ((u32) * 550) +#define TICKS_TO_US(u32) ((u32) / 550) + +end of file src/fas_ramp/RampCalculator.h (modification shown) + +--- + +=== MODIFIED FILE: library.properties === + +name=FastAccelStepper +version=1.2.5 +license=MIT +author=Jochen Kiemes +maintainer=Jochen Kiemes +sentence=A high speed stepper library for Atmega 168/168p/328/328p (nano), 32u4 (leonardo), 2560, ESP32, ESP32S2, ESP32S3, ESP32C3, ESP32C6, Atmel SAM Due, Raspberry pi pico and pico 2, and STM32 +paragraph=Drive stepper motors with acceleration/deceleration profile up to 50 kSteps/s (Atmega) and 200kSteps/s (esp32). Supports STM32F1/F4/G0/H7 series via STM32duino core. +url=https://github.com/gin66/FastAccelStepper +repository=https://github.com/gin66/FastAccelStepper.git +architectures=avr,esp32,sam,rp2040,rp2350,stm32 +category=Device Control +dot_a_linkage=true + +Changes from original: +- sentence: added ", and STM32" +- paragraph: added "Supports STM32F1/F4/G0/H7 series via STM32duino core." +- architectures: added ",stm32" + +end of file library.properties + +--- + +=== MODIFIED FILE: README.md (ADDED STM32 SECTION) === + +Added new section "STM32 Arduino Support" before "## Contribution" at end of file. +See the full content in the repository. + +end of file README.md (modification shown) + +--- + +=== END OF Changed_code_v19.txt === \ No newline at end of file diff --git a/README.md b/README.md index a5d50aaa..cebbe41e 100644 --- a/README.md +++ b/README.md @@ -639,6 +639,49 @@ Found on youtube: As mentioned by kthod861 in [Issue #110](https://github.com/gin66/FastAccelStepper/issues/110): * [22 01 2021 Stepper POC3](https://youtu.be/fm2_VkUG10k) +## STM32 Arduino Support + +FastAccelStepper supports STM32 microcontrollers via the official +[Arduino Core STM32](https://github.com/stm32duino/Arduino_Core_STM32). + +### Architecture + +- **Step generation**: TIM2 CC interrupt + BSRR/BRR GPIO (push-pull OUTPUT) +- **Steppers**: Up to 4 (any GPIO pins, dynamic slot allocation) +- **Pulse width**: 6 µs (configurable via `STEP_PULSE_WIDTH_US`) +- **Cyclic fill**: PendSV exception, triggered from TIM2 ISR every 3ms via uwTick +- **GPIO mode**: Standard push-pull OUTPUT (any GPIO pin works) +- **Direction**: Atomic BSRR set/reset — not ODR XOR (race-free) +- **Direction settling**: `_dir_delay_active` state machine, 30µs delay +- **TIM2 clock**: Auto-detection with APB1 prescaler ×2 correction +- **Interrupt safety**: PRIMASK save/restore (reentrant) +- **SR handling**: Snapshot → clear all processed at once (rc_w0) +- **PendSV**: `__attribute__((weak))` — FreeRTOS compatible + +### Default Pin Mapping + +| Stepper | Default Pin | Notes | +|---------|-------------|-------| +| 0 | PA0 | Any GPIO pin can be used | +| 1 | PA1 | (PA0-PA3 are conventional only) | +| 2 | PA2 | | +| 3 | PA3 | | + +### ⚠ Warnings + +1. **No FreeRTOS** — PendSV is overridden (weak, FreeRTOS can override). +2. **TIM2 is reserved** — Do not use TIM2 elsewhere. +3. **HAL timebase must be SysTick** (default). uwTick is used for cyclic fill. +4. **Do not call HAL_Delay() with steppers running** — TIM2 priority 0 may preempt SysTick. Use millis() polling. +5. **TICKS_PER_S must match TIM2 counter clock** — Define before library includes: + ```cpp + #define TICKS_PER_S 72000000UL // STM32F103 @72MHz + #include + ``` + See `pd_stm32/pd_config.h` for examples for each board. +6. **Clock error**: After `engine.init()`, check `fas_stm32_clock_error`: + if non-zero, `TICKS_PER_S` exceeds actual TIM2 clock. + ## Contribution - Thanks ixil for pull request (https://github.com/gin66/FastAccelStepper/pull/19) for ATmega2560 diff --git a/extras/StepperPins_stm32.h b/extras/StepperPins_stm32.h new file mode 100644 index 00000000..b80e1d10 --- /dev/null +++ b/extras/StepperPins_stm32.h @@ -0,0 +1,34 @@ +#ifndef STEPPERPINS_STM32_H +#define STEPPERPINS_STM32_H + +// ==================================================================== +// Default step pin mapping for STM32 platforms +// +// These defaults map steppers 0-3 to PA0-PA3 (TIM2 channels 1-4). +// Override by defining before including this header: +// +// #define STEP_PIN_STEPPER_0 PB0 +// #include "StepperPins_stm32.h" +// +// Note: On STM32, any GPIO pin can be used as a step pin. +// PA0-PA3 are only a convention — the timer is used only for +// interrupt timing, not direct pin output. +// ==================================================================== + +#ifndef STEP_PIN_STEPPER_0 +#define STEP_PIN_STEPPER_0 PA0 +#endif + +#ifndef STEP_PIN_STEPPER_1 +#define STEP_PIN_STEPPER_1 PA1 +#endif + +#ifndef STEP_PIN_STEPPER_2 +#define STEP_PIN_STEPPER_2 PA2 +#endif + +#ifndef STEP_PIN_STEPPER_3 +#define STEP_PIN_STEPPER_3 PA3 +#endif + +#endif /* STEPPERPINS_STM32_H */ \ No newline at end of file diff --git a/library.properties b/library.properties index bf955629..ea4e6834 100644 --- a/library.properties +++ b/library.properties @@ -3,10 +3,10 @@ version=1.2.5 license=MIT author=Jochen Kiemes maintainer=Jochen Kiemes -sentence=A high speed stepper library for Atmega 168/168p/328/328p (nano), 32u4 (leonardo), 2560, ESP32, ESP32S2, ESP32S3, ESP32C3, ESP32C6, Atmel SAM Due, Raspberry pi pico and pico 2 -paragraph=Drive stepper motors with acceleration/deceleration profile up to 50 kSteps/s (Atmega) and 200kSteps/s (esp32). +sentence=A high speed stepper library for Atmega 168/168p/328/328p (nano), 32u4 (leonardo), 2560, ESP32, ESP32S2, ESP32S3, ESP32C3, ESP32C6, Atmel SAM Due, Raspberry pi pico and pico 2, and STM32 +paragraph=Drive stepper motors with acceleration/deceleration profile up to 50 kSteps/s (Atmega) and 200kSteps/s (esp32). Supports STM32F1/F4/G0/H7 series via STM32duino core. url=https://github.com/gin66/FastAccelStepper repository=https://github.com/gin66/FastAccelStepper.git -architectures=avr,esp32,sam,rp2040,rp2350 +architectures=avr,esp32,sam,rp2040,rp2350,stm32 category=Device Control -dot_a_linkage=true +dot_a_linkage=true \ No newline at end of file diff --git a/src/fas_arch/arduino_stm32.h b/src/fas_arch/arduino_stm32.h new file mode 100644 index 00000000..006e4059 --- /dev/null +++ b/src/fas_arch/arduino_stm32.h @@ -0,0 +1,21 @@ +#ifndef FAS_ARCH_ARDUINO_STM32_H +#define FAS_ARCH_ARDUINO_STM32_H + +#define FAS_STM32 + +#include +#include +#include + +// PRIMASK reentrant-safe interrupt control +// Saves and restores PRIMASK to support nested disable/enable calls +#define fasDisableInterrupts() \ + uint32_t __fas_prim = __get_PRIMASK(); __disable_irq() +#define fasEnableInterrupts() \ + __set_PRIMASK(__fas_prim) + +#define FAS_PSTR(s) (s) +#define PIN_UNDEFINED 0xFF +#define PIN_EXTERNAL_FLAG 128 + +#endif /* FAS_ARCH_ARDUINO_STM32_H */ \ No newline at end of file diff --git a/src/fas_arch/common.h b/src/fas_arch/common.h index 2fb4c914..09f76349 100644 --- a/src/fas_arch/common.h +++ b/src/fas_arch/common.h @@ -94,6 +94,11 @@ struct queue_end_s { #include "fas_arch/arduino_rp_pico.h" #include "pd_pico/pd_config.h" +#elif defined(ARDUINO_ARCH_STM32) +// STM32 family (STM32duino core) +#include "fas_arch/arduino_stm32.h" +#include "pd_stm32/pd_config.h" + #else #error "Unsupported devices" #endif diff --git a/src/fas_queue/stepper_queue.h b/src/fas_queue/stepper_queue.h index 31133b0d..a704fc18 100644 --- a/src/fas_queue/stepper_queue.h +++ b/src/fas_queue/stepper_queue.h @@ -19,6 +19,8 @@ #include "pd_pico/pico_queue.h" #elif defined(SUPPORT_ESP32) #include "pd_esp32/esp32_queue.h" +#elif defined(FAS_STM32) +#include "pd_stm32/stm32_queue.h" #else #error "Unsupported architecture" #endif diff --git a/src/fas_ramp/RampCalculator.h b/src/fas_ramp/RampCalculator.h index 52c2f564..c60a3c41 100644 --- a/src/fas_ramp/RampCalculator.h +++ b/src/fas_ramp/RampCalculator.h @@ -18,6 +18,97 @@ #define LOG2_ACCEL_FACTOR LOG2_CONST_2205E11 #define US_TO_TICKS(u32) ((u32) * 21) #define TICKS_TO_US(u32) ((u32) / 21) +#elif (TICKS_PER_S == 32000000L) +// STM32L0, RP2040 +#define LOG2_TICKS_PER_S ((log2_value_t)0x31dd) +#define LOG2_TICKS_PER_S_DIV_SQRT_OF_2 ((log2_value_t)0x30dd) +#define LOG2_ACCEL_FACTOR ((log2_value_t)0x61ba) +#define US_TO_TICKS(u32) ((u32) * 32) +#define TICKS_TO_US(u32) ((u32) / 32) +#elif (TICKS_PER_S == 48000000L) +// STM32F0/G0/WL +#define LOG2_TICKS_PER_S ((log2_value_t)0x3308) +#define LOG2_TICKS_PER_S_DIV_SQRT_OF_2 ((log2_value_t)0x3208) +#define LOG2_ACCEL_FACTOR ((log2_value_t)0x6411) +#define US_TO_TICKS(u32) ((u32) * 48) +#define TICKS_TO_US(u32) ((u32) / 48) +#elif (TICKS_PER_S == 64000000L) +// STM32G0/WB +#define LOG2_TICKS_PER_S ((log2_value_t)0x33dd) +#define LOG2_TICKS_PER_S_DIV_SQRT_OF_2 ((log2_value_t)0x32dd) +#define LOG2_ACCEL_FACTOR ((log2_value_t)0x65ba) +#define US_TO_TICKS(u32) ((u32) * 64) +#define TICKS_TO_US(u32) ((u32) / 64) +#elif (TICKS_PER_S == 72000000L) +// STM32F1/L1 (most common) +#define LOG2_TICKS_PER_S ((log2_value_t)0x3434) +#define LOG2_TICKS_PER_S_DIV_SQRT_OF_2 ((log2_value_t)0x3334) +#define LOG2_ACCEL_FACTOR ((log2_value_t)0x6668) +#define US_TO_TICKS(u32) ((u32) * 72) +#define TICKS_TO_US(u32) ((u32) / 72) +#elif (TICKS_PER_S == 80000000L) +// STM32L4 +#define LOG2_TICKS_PER_S ((log2_value_t)0x3482) +#define LOG2_TICKS_PER_S_DIV_SQRT_OF_2 ((log2_value_t)0x3382) +#define LOG2_ACCEL_FACTOR ((log2_value_t)0x6704) +#define US_TO_TICKS(u32) ((u32) * 80) +#define TICKS_TO_US(u32) ((u32) / 80) +#elif (TICKS_PER_S == 84000000L) +// STM32F401/411 +#define LOG2_TICKS_PER_S ((log2_value_t)0x34a6) +#define LOG2_TICKS_PER_S_DIV_SQRT_OF_2 ((log2_value_t)0x33a6) +#define LOG2_ACCEL_FACTOR ((log2_value_t)0x674c) +#define US_TO_TICKS(u32) ((u32) * 84) +#define TICKS_TO_US(u32) ((u32) / 84) +#elif (TICKS_PER_S == 100000000L) +// STM32F411/746 +#define LOG2_TICKS_PER_S ((log2_value_t)0x3527) +#define LOG2_TICKS_PER_S_DIV_SQRT_OF_2 ((log2_value_t)0x3427) +#define LOG2_ACCEL_FACTOR ((log2_value_t)0x684d) +#define US_TO_TICKS(u32) ((u32) * 100) +#define TICKS_TO_US(u32) ((u32) / 100) +#elif (TICKS_PER_S == 120000000L) +// STM32L4+/F4 +#define LOG2_TICKS_PER_S ((log2_value_t)0x35ad) +#define LOG2_TICKS_PER_S_DIV_SQRT_OF_2 ((log2_value_t)0x34ad) +#define LOG2_ACCEL_FACTOR ((log2_value_t)0x695a) +#define US_TO_TICKS(u32) ((u32) * 120) +#define TICKS_TO_US(u32) ((u32) / 120) +#elif (TICKS_PER_S == 168000000L) +// STM32F405/407 +#define LOG2_TICKS_PER_S ((log2_value_t)0x36a6) +#define LOG2_TICKS_PER_S_DIV_SQRT_OF_2 ((log2_value_t)0x35a6) +#define LOG2_ACCEL_FACTOR ((log2_value_t)0x6b4c) +#define US_TO_TICKS(u32) ((u32) * 168) +#define TICKS_TO_US(u32) ((u32) / 168) +#elif (TICKS_PER_S == 170000000L) +// STM32F3/G4 +#define LOG2_TICKS_PER_S ((log2_value_t)0x36af) +#define LOG2_TICKS_PER_S_DIV_SQRT_OF_2 ((log2_value_t)0x35af) +#define LOG2_ACCEL_FACTOR ((log2_value_t)0x6b5e) +#define US_TO_TICKS(u32) ((u32) * 170) +#define TICKS_TO_US(u32) ((u32) / 170) +#elif (TICKS_PER_S == 216000000L) +// STM32F7 +#define LOG2_TICKS_PER_S ((log2_value_t)0x375f) +#define LOG2_TICKS_PER_S_DIV_SQRT_OF_2 ((log2_value_t)0x365f) +#define LOG2_ACCEL_FACTOR ((log2_value_t)0x6cbe) +#define US_TO_TICKS(u32) ((u32) * 216) +#define TICKS_TO_US(u32) ((u32) / 216) +#elif (TICKS_PER_S == 480000000L) +// STM32H7 (default) +#define LOG2_TICKS_PER_S ((log2_value_t)0x39ad) +#define LOG2_TICKS_PER_S_DIV_SQRT_OF_2 ((log2_value_t)0x38ad) +#define LOG2_ACCEL_FACTOR ((log2_value_t)0x715a) +#define US_TO_TICKS(u32) ((u32) * 480) +#define TICKS_TO_US(u32) ((u32) / 480) +#elif (TICKS_PER_S == 550000000L) +// STM32H7 (overclock) +#define LOG2_TICKS_PER_S ((log2_value_t)0x3a12) +#define LOG2_TICKS_PER_S_DIV_SQRT_OF_2 ((log2_value_t)0x3912) +#define LOG2_ACCEL_FACTOR ((log2_value_t)0x7224) +#define US_TO_TICKS(u32) ((u32) * 550) +#define TICKS_TO_US(u32) ((u32) / 550) #else #define SUPPORT_LOG2_TIMER_FREQ_VARIABLES #define LOG2_TICKS_PER_S log2_timer_freq diff --git a/src/pd_stm32/pd_config.h b/src/pd_stm32/pd_config.h new file mode 100644 index 00000000..b5645688 --- /dev/null +++ b/src/pd_stm32/pd_config.h @@ -0,0 +1,46 @@ +#ifndef PD_STM32_CONFIG_H +#define PD_STM32_CONFIG_H + +#include + +// ==================================================================== +// Compile-time TICKS_PER_S +// +// User MUST define TICKS_PER_S matching their board's TIM2 counter clock. +// TIM2 counter clock = PCLK1 * (APB1_prescaler==1 ? 1 : 2) +// +// Examples: +// STM32F103 @72MHz: TICKS_PER_S = 72000000 (APB1=36MHz ×2) +// STM32F407 @168MHz: TICKS_PER_S = 84000000 (APB1=42MHz ×2) +// STM32G0 @64MHz: TICKS_PER_S = 64000000 (APB1=64MHz ×1) +// STM32H743 @480MHz: TICKS_PER_S = 240000000 (APB1=120MHz ×2) +// ==================================================================== +#ifndef TICKS_PER_S +#define TICKS_PER_S 72000000UL +#endif + +// ---- Queue topology ---- +#define MAX_STEPPER 4 +#define NUM_QUEUES 4 +#define QUEUE_LEN 32 + +// ---- Pulse width (configurable) ---- +#ifndef STEP_PULSE_WIDTH_US +#define STEP_PULSE_WIDTH_US 6 +#endif +#define STEP_PULSE_WIDTH_TICKS ((uint32_t)(STEP_PULSE_WIDTH_US * (TICKS_PER_S / 1000000L))) + +// ---- Timing constants ---- +#define MIN_CMD_TICKS (TICKS_PER_S / 5000) +#define MIN_DIR_DELAY_US 200 +#define MAX_DIR_DELAY_US (65535 / (TICKS_PER_S / 1000000)) +#define DELAY_MS_BASE 2 +#define CYCLIC_INTERVAL_MS 3 + +// ---- Feature flags ---- +#define SUPPORT_QUEUE_ENTRY_END_POS_U16 +#define NEED_GENERIC_GET_CURRENT_POSITION +#define noop_or_wait __NOP() +#define DEBUG_LED_HALF_PERIOD 50 + +#endif /* PD_STM32_CONFIG_H */ \ No newline at end of file diff --git a/src/pd_stm32/stm32_queue.cpp b/src/pd_stm32/stm32_queue.cpp new file mode 100644 index 00000000..9998928d --- /dev/null +++ b/src/pd_stm32/stm32_queue.cpp @@ -0,0 +1,385 @@ +#include "fas_queue/stepper_queue.h" +#include "log2/Log2Representation.h" +#include "fas_ramp/RampControl.h" + +#if defined(ARDUINO_ARCH_STM32) + +// ==================================================================== +// Static data +// ==================================================================== +static FastAccelStepperEngine* fas_engine = NULL; +static uint8_t stepper_allocated_mask = 0; +static volatile bool _cyclic_pending = false; +static uint32_t _last_cyclic_uwtick = 0; +uint8_t fas_stm32_clock_error = 0; +StepperQueue* StepperQueue::_ch_to_queue[4] = {NULL, NULL, NULL, NULL}; + +// ==================================================================== +// Log2 timer frequency variables (for SUPPORT_LOG2_TIMER_FREQ_VARIABLES) +// Defined here when RampCalculator.h selects runtime fallback path. +// ==================================================================== +#ifdef SUPPORT_LOG2_TIMER_FREQ_VARIABLES +static log2_value_t log2_timer_freq; +static log2_value_t log2_timer_freq_div_sqrt_of_2; +static log2_value_t log2_timer_freq_square_div_2; +#endif + +// ==================================================================== +// TIM2 clock detection +// +// TIM2 counter clock = PCLK1 * (APB1_prescaler==1 ? 1 : 2) +// +// The APB1 prescaler is in RCC->CFGR.PPRE on most STM32 families. +// On H7 it is in RCC->D2CFGR.D2PPRE1 (D2 domain). +// ==================================================================== +static uint32_t getTim2Clock(void) { + uint32_t pclk1 = HAL_RCC_GetPCLK1Freq(); +#if defined(STM32H7xx) + // H7 series: D2 domain, D2CFGR register + uint32_t dppre1 = (RCC->D2CFGR & RCC_D2CFGR_D2PPRE1) >> RCC_D2CFGR_D2PPRE1_Pos; + if (dppre1 > 1) pclk1 *= 2; +#else + // All other STM32 families + uint32_t apb1_pre = (RCC->CFGR & RCC_CFGR_PPRE) >> RCC_CFGR_PPRE_Pos; + if (apb1_pre > 1) pclk1 *= 2; +#endif + return pclk1; +} + +// ==================================================================== +// TIM2 initialization (step generation timer) +// Called once when first stepper is initialized. +// ==================================================================== +static void initStepTimer(void) { + static bool initialized = false; + if (initialized) return; + initialized = true; + + // Enable TIM2 clock (portable across all STM32 families) + __HAL_RCC_TIM2_CLK_ENABLE(); + + // Compute prescaler from actual TIM2 clock and desired TICKS_PER_S + uint32_t tim2_clk = getTim2Clock(); + uint32_t psc; + + if (TICKS_PER_S > tim2_clk) { + // Cannot achieve TICKS_PER_S at this clock frequency. + // Run at maximum rate (PSC=0). All timing will be incorrect. + fas_stm32_clock_error = 1; + psc = 0; + } else { + psc = (tim2_clk / TICKS_PER_S) - 1; + if (psc > 65535) psc = 65535; + } + + // Configure TIM2 + TIM2->CR1 = 0; + TIM2->PSC = psc; + TIM2->ARR = 0xFFFFFFFF; // 32-bit auto-reload + TIM2->EGR |= TIM_EGR_UG; // Reload shadow registers (PSC, ARR) + TIM2->DIER = 0; // All interrupts disabled initially + + // Force LOW all channels (OCxM = 100 = Force Inactive) + // This prevents spurious pulses during initialization. + TIM2->CCMR1 = (4 << 4) | (4 << 12); // OC1M=100, OC2M=100 + TIM2->CCMR2 = (4 << 4) | (4 << 12); // OC3M=100, OC4M=100 + + // NVIC configuration + NVIC_SetPriority(TIM2_IRQn, 0); // Highest priority for step timing + NVIC_EnableIRQ(TIM2_IRQn); + + // Start timer + TIM2->CR1 |= TIM_CR1_CEN; +} + +// ==================================================================== +// Dynamic slot allocation — supports ANY GPIO pin for step +// ==================================================================== +static int8_t findFreeSlot(void) { + for (int i = 0; i < MAX_STEPPER; i++) { + if (!(stepper_allocated_mask & (1 << i))) { + return i; + } + } + return -1; // All slots used +} + +// ==================================================================== +// Queue initialization +// ==================================================================== +void StepperQueue::init(uint8_t queue_num, uint8_t step_pin) { + static const uint8_t ch_map[4] = {0, 1, 2, 3}; + _timer_ch = ch_map[queue_num]; + + // Ensure TIM2 is initialized + initStepTimer(); + + // Step pin GPIO configuration + _step_pin = step_pin; + _step_port = digitalPinToPort(step_pin); + uint32_t mask = digitalPinToBitMask(step_pin); + _step_set_mask = mask; + + // Configure clear mask based on BSRR/BRR architecture +#if STM32_BSRR_CLEAR_SHIFT + _step_clr_mask = mask << 16; // BSRR high half = reset bits +#else + _step_clr_mask = mask; // BRR register (direct) +#endif + + // Configure pin as OUTPUT, initial LOW + pinMode(step_pin, OUTPUT); + digitalWrite(step_pin, LOW); + + // Store CCR register pointer for fast ISR access + volatile uint32_t* ccr[] = {&TIM2->CCR1, &TIM2->CCR2, &TIM2->CCR3, &TIM2->CCR4}; + _ccr_reg = ccr[_timer_ch]; + + // Register channel-to-queue mapping + _ch_to_queue[_timer_ch] = this; + _initialized = true; + _isRunning = false; +} + +// ==================================================================== +// Start / Stop +// ==================================================================== +void StepperQueue::startQueue(void) { + _isRunning = true; + _pulse_high = false; + _dir_delay_active = false; + + // Write CCR first, then barrier, then enable interrupt + // This ensures the compare value is visible before the IRQ can fire. + *_ccr_reg = TIM2->CNT + (TICKS_PER_S / 1000000); // ~1µs offset + __DMB(); + + // DIER RMW: disable interrupts to prevent race with ISR on other channels + __disable_irq(); + TIM2->DIER |= CCXIE_BIT(_timer_ch); + __enable_irq(); +} + +void StepperQueue::forceStop(void) { + // Disable CC interrupt (atomic) + __disable_irq(); + TIM2->DIER &= ~CCXIE_BIT(_timer_ch); + _isRunning = false; + read_idx = next_write_idx; // Discard remaining queue entries + __enable_irq(); + + // Ensure step pin is LOW (use BSRR/BRR appropriately) + if (_step_port) { +#if STM32_BSRR_CLEAR_SHIFT + _step_port->BSRR = _step_clr_mask; +#else + _step_port->BRR = _step_clr_mask; +#endif + } +} + +void StepperQueue::connect(void) {} +void StepperQueue::disconnect(void) {} + +// ==================================================================== +// Speed adjustment based on number of active steppers +// ==================================================================== +void StepperQueue::adjustSpeedToStepperCount(uint8_t steppers) { + if (steppers == 1) + max_speed_in_ticks = STEP_PULSE_WIDTH_TICKS * 2; + else if (steppers == 2) + max_speed_in_ticks = STEP_PULSE_WIDTH_TICKS * 3; + else + max_speed_in_ticks = STEP_PULSE_WIDTH_TICKS * 4; + + if (max_speed_in_ticks < MIN_CMD_TICKS) + max_speed_in_ticks = MIN_CMD_TICKS; +} + +// ==================================================================== +// getActualTicksWithDirection — retrieve current step rate +// ==================================================================== +bool StepperQueue::getActualTicksWithDirection( + struct actual_ticks_s* speed) const { + fasDisableInterrupts(); + speed->count_up = queue_end.count_up; + speed->ticks = _last_command_ticks; + fasEnableInterrupts(); + inject_fill_interrupt(0); + return true; +} + +// ==================================================================== +// Cyclic PendSV trigger +// Called at end of TIM2_IRQHandler every ~3ms. +// Triggers PendSV exception to fill queues without consuming ISR time. +// ==================================================================== +static void cyclic_check_and_pend(void) { + uint32_t now = uwTick; + if ((now - _last_cyclic_uwtick) >= CYCLIC_INTERVAL_MS) { + _last_cyclic_uwtick = now; + if (!_cyclic_pending) { + _cyclic_pending = true; + SCB->ICSR = SCB_ICSR_PENDSVSET_Msk; + } + } +} + +// ==================================================================== +// TIM2_IRQHandler — Step pulse generation (all 4 channels) +// +// State machine: +// Phase 1 (_pulse_high=false): Set step pin HIGH, schedule LOW +// Phase 2 (_pulse_high=true): Set step pin LOW, process queue +// DirSettle (_dir_delay_active): Direction settling complete → start pulse +// +// SR handling: +// Snapshot SR at entry. Build ch_processed mask of all handled flags. +// Clear all at end by writing ~ch_processed (rc_w0 behavior). +// ==================================================================== +void TIM2_IRQHandler(void) { + uint32_t sr = TIM2->SR; + uint32_t ch_processed = 0; + + for (uint8_t ch = 0; ch < 4; ch++) { + uint32_t ccif = CCXIF_BIT(ch); + if (!(sr & ccif)) continue; + + // Always mark for clear — prevents infinite loop from spurious IRQs + ch_processed |= ccif; + + StepperQueue* q = StepperQueue::_ch_to_queue[ch]; + if (!q || !q->_isRunning) continue; + + if (q->_pulse_high) { + // ====== Phase 2: pulse end ====== + // The step pin was HIGH; bring it LOW. + q->_pulse_high = false; + +#if STM32_BSRR_CLEAR_SHIFT + q->_step_port->BSRR = q->_step_clr_mask; +#else + q->_step_port->BRR = q->_step_clr_mask; +#endif + + // Read queue entry + uint8_t rp = q->read_idx; + uint8_t wp = q->next_write_idx; + + if (rp == wp) { + // Queue empty — stop this channel + TIM2->DIER &= ~CCXIE_BIT(ch); + q->_isRunning = false; + continue; + } + + struct queue_entry* e = &q->entry[rp & QUEUE_LEN_MASK]; + q->_last_command_ticks = e->ticks; + + if (e->steps > 1) { + // Multi-step command: reduce step count, continue with same period + e->steps--; + *q->_ccr_reg = TIM2->CNT + (uint32_t)e->ticks; + } else { + // Single step complete — advance to next entry + rp++; + q->read_idx = rp; + + if (rp == wp) { + TIM2->DIER &= ~CCXIE_BIT(ch); + q->_isRunning = false; + continue; + } + + e = &q->entry[rp & QUEUE_LEN_MASK]; + + // Handle direction change (BSRR atomic — NOT ODR XOR) + if (e->toggle_dir && q->_dir_bsrr) { + if (e->dirPinState) { + *q->_dir_bsrr = q->_dir_set_mask; + } else { + *q->_dir_bsrr = q->_dir_clr_mask; + } + e->toggle_dir = 0; // Clear flag — prevents double-toggle + + // Insert direction settling delay + uint32_t dd = AFTER_SET_DIR_PIN_DELAY_US * (TICKS_PER_S / 1000000UL); + if (dd < MIN_CMD_TICKS) dd = MIN_CMD_TICKS; + *q->_ccr_reg = TIM2->CNT + dd; + q->_dir_delay_active = true; + continue; + } + + // No direction change — schedule next step pulse + *q->_ccr_reg = TIM2->CNT + (uint32_t)e->ticks; + } + } else if (q->_dir_delay_active) { + // ====== Direction settling complete ====== + // The settling delay has elapsed. Now start the first step pulse + // in the new direction (set pin HIGH). + q->_dir_delay_active = false; + q->_pulse_high = true; + q->_step_port->BSRR = q->_step_set_mask; + *q->_ccr_reg = TIM2->CNT + STEP_PULSE_WIDTH_TICKS; + } else { + // ====== Phase 1: pulse start ====== + // Set step pin HIGH, schedule LOW after STEP_PULSE_WIDTH_TICKS. + q->_pulse_high = true; + q->_step_port->BSRR = q->_step_set_mask; + *q->_ccr_reg = TIM2->CNT + STEP_PULSE_WIDTH_TICKS; + } + } + + // Clear all processed flags at once (rc_w0: bits set to 1 are ignored) + TIM2->SR = ~ch_processed; + + // Trigger cyclic queue fill + cyclic_check_and_pend(); +} + +// ==================================================================== +// PendSV_Handler — Deferred queue fill +// +// Weak attribute allows FreeRTOS to override this handler. +// Define DISABLE_FAS_PENDSV to skip installation entirely. +// ==================================================================== +#if !defined(DISABLE_FAS_PENDSV) +__attribute__((weak)) void PendSV_Handler(void) { + _cyclic_pending = false; + __DMB(); + if (fas_engine) { + fas_engine->manageSteppers(); + } +} +#endif + +// ==================================================================== +// Allocation — dynamic slot assignment for any GPIO step pin +// ==================================================================== +StepperQueue* StepperQueue::tryAllocateQueue( + FastAccelStepperEngine* engine, uint8_t step_pin) { + (void)engine; + int8_t idx = findFreeSlot(); + if (idx < 0) return nullptr; + + fas_queue[idx]._initVars(); + fas_queue[idx].init((uint8_t)idx, step_pin); + stepper_allocated_mask |= (1 << idx); + return &fas_queue[idx]; +} + +// ==================================================================== +// Engine initialization +// ==================================================================== +void fas_init_engine(FastAccelStepperEngine* engine) { + fas_engine = engine; + + // Initialize Log2 timer frequency variables if using runtime fallback + // (SUPPORT_LOG2_TIMER_FREQ_VARIABLES path in RampCalculator.h) + init_ramp_module(); + + // PendSV at lowest priority — avoids blocking higher-priority interrupts + NVIC_SetPriority(PendSV_IRQn, 0xFF); +} + +#endif /* ARDUINO_ARCH_STM32 */ \ No newline at end of file diff --git a/src/pd_stm32/stm32_queue.h b/src/pd_stm32/stm32_queue.h new file mode 100644 index 00000000..b0db2cce --- /dev/null +++ b/src/pd_stm32/stm32_queue.h @@ -0,0 +1,127 @@ +#ifndef PD_STM32_QUEUE_H +#define PD_STM32_QUEUE_H + +#include "FastAccelStepper.h" +#include "fas_queue/base.h" +#include "fas_arch/result_codes.h" + +// ---- Default pin mapping (overridable in sketch) ---- +#ifndef STEP_PIN_STEPPER_0 +#define STEP_PIN_STEPPER_0 PA0 +#endif +#ifndef STEP_PIN_STEPPER_1 +#define STEP_PIN_STEPPER_1 PA1 +#endif +#ifndef STEP_PIN_STEPPER_2 +#define STEP_PIN_STEPPER_2 PA2 +#endif +#ifndef STEP_PIN_STEPPER_3 +#define STEP_PIN_STEPPER_3 PA3 +#endif + +// ---- CC interrupt bit helpers ---- +#define CCXIE_BIT(ch) (TIM_DIER_CC1IE << (ch)) +#define CCXIF_BIT(ch) (TIM_SR_CC1IF << (ch)) + +// ---- BSRR/BRR detection ---- +// portClearRegister returns &BSRR on F2/F4/F7, &BRR elsewhere +// When BSRR: write mask << 16 for reset +// When BRR: write mask directly +#if defined(STM32F2xx) || defined(STM32F4xx) || defined(STM32F7xx) +#define STM32_BSRR_CLEAR_SHIFT 1 +#else +#define STM32_BSRR_CLEAR_SHIFT 0 +#endif + +// ==================================================================== +// StepperQueue class — STM32-specific implementation +// ==================================================================== +class StepperQueue : public StepperQueueBase { + public: +#include "../fas_queue/protocol.h" + + volatile bool _isRunning; + bool _initialized; + + // Step pin GPIO + uint8_t _step_pin; + GPIO_TypeDef* _step_port; + uint32_t _step_set_mask; // BSRR set mask (low 16 bits) + uint32_t _step_clr_mask; // Clear mask (BRR or BSRR<<16) + + // Direction pin (atomic via BSRR/BRR) + volatile uint32_t* _dir_bsrr; // &GPIOx->BSRR + uint32_t _dir_set_mask; // BSRR low bits = set + uint32_t _dir_clr_mask; // BSRR high bits = reset, or BRR + + // Timer + volatile uint32_t* _ccr_reg; // &TIM2->CCR1/2/3/4 + uint8_t _timer_ch; // 0..3 + + // Pulse tracking + volatile bool _pulse_high; + volatile bool _dir_delay_active; // Direction settling in progress + + // Channel-to-queue mapping (static) + static StepperQueue* _ch_to_queue[4]; + + // ---- Inline methods ---- + inline void _pd_initVars() { + _step_pin = PIN_UNDEFINED; + _step_port = NULL; + _step_set_mask = 0; + _step_clr_mask = 0; + _dir_bsrr = NULL; + _dir_set_mask = 0; + _dir_clr_mask = 0; + _ccr_reg = NULL; + _timer_ch = 0; + _isRunning = false; + _initialized = false; + _pulse_high = false; + _dir_delay_active = false; + max_speed_in_ticks = STEP_PULSE_WIDTH_TICKS * 4; + } + + inline bool isRunning() const { return _isRunning; } + inline bool isReadyForCommands() const { return true; } + + void setDirPin(uint8_t dir_pin, bool _dirHighCountsUp) { + dirPin = dir_pin; + dirHighCountsUp = _dirHighCountsUp; + if ((dir_pin != PIN_UNDEFINED) && ((dir_pin & PIN_EXTERNAL_FLAG) == 0)) { + GPIO_TypeDef* port = digitalPinToPort(dir_pin); + uint32_t mask = digitalPinToBitMask(dir_pin); + _dir_bsrr = &port->BSRR; + _dir_set_mask = mask; +#if STM32_BSRR_CLEAR_SHIFT + _dir_clr_mask = mask << 16; // BSRR high half = reset +#else + _dir_clr_mask = mask; // BRR register direct +#endif + } + } + + void adjustSpeedToStepperCount(uint8_t steppers); + + private: + static StepperQueue* allocateSlot(uint8_t step_pin, uint8_t timer_ch); +}; + +// ---- Direction pin: atomic via BSRR/BRR ---- +#define SET_DIRECTION_PIN_STATE(q, high) \ + do { \ + if ((q)->_dir_bsrr) { \ + *(q->_dir_bsrr) = (high) ? (q)->_dir_set_mask \ + : (q)->_dir_clr_mask; \ + } \ + } while (0) + +// ---- Enable pin: simple digitalWrite ---- +#define SET_ENABLE_PIN_STATE(q, pin, high) \ + digitalWrite((pin), (high) ? HIGH : LOW) + +// ---- Direction-to-pulse delay ---- +#define AFTER_SET_DIR_PIN_DELAY_US 30 + +#endif /* PD_STM32_QUEUE_H */ \ No newline at end of file From f2cba8d33185474b8cfedc9e6127e1fd8e217e47 Mon Sep 17 00:00:00 2001 From: KienTranCNC Date: Mon, 11 May 2026 17:51:03 +0700 Subject: [PATCH 2/5] =?UTF-8?q?Fix=20l=E1=BB=97i?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/pd_stm32/stm32_queue.cpp | 70 ++++++++++++++++-------------------- src/pd_stm32/stm32_queue.h | 30 ++++++---------- 2 files changed, 42 insertions(+), 58 deletions(-) diff --git a/src/pd_stm32/stm32_queue.cpp b/src/pd_stm32/stm32_queue.cpp index 9998928d..d300990d 100644 --- a/src/pd_stm32/stm32_queue.cpp +++ b/src/pd_stm32/stm32_queue.cpp @@ -14,34 +14,30 @@ static uint32_t _last_cyclic_uwtick = 0; uint8_t fas_stm32_clock_error = 0; StepperQueue* StepperQueue::_ch_to_queue[4] = {NULL, NULL, NULL, NULL}; -// ==================================================================== -// Log2 timer frequency variables (for SUPPORT_LOG2_TIMER_FREQ_VARIABLES) -// Defined here when RampCalculator.h selects runtime fallback path. -// ==================================================================== -#ifdef SUPPORT_LOG2_TIMER_FREQ_VARIABLES -static log2_value_t log2_timer_freq; -static log2_value_t log2_timer_freq_div_sqrt_of_2; -static log2_value_t log2_timer_freq_square_div_2; -#endif - // ==================================================================== // TIM2 clock detection // // TIM2 counter clock = PCLK1 * (APB1_prescaler==1 ? 1 : 2) // -// The APB1 prescaler is in RCC->CFGR.PPRE on most STM32 families. -// On H7 it is in RCC->D2CFGR.D2PPRE1 (D2 domain). +// The APB1 prescaler is in: +// - H7: RCC->D2CFGR.D2PPRE1 (D2 domain) +// - G0/G4: RCC->CFGR.PPRE (single field) +// - F1/F4/F7/L1/L4: RCC->CFGR.PPRE1 // ==================================================================== static uint32_t getTim2Clock(void) { uint32_t pclk1 = HAL_RCC_GetPCLK1Freq(); #if defined(STM32H7xx) - // H7 series: D2 domain, D2CFGR register + // H7 series: D2 domain uint32_t dppre1 = (RCC->D2CFGR & RCC_D2CFGR_D2PPRE1) >> RCC_D2CFGR_D2PPRE1_Pos; if (dppre1 > 1) pclk1 *= 2; +#elif defined(STM32G0xx) || defined(STM32G4xx) + // G0/G4: single PPRE field (no PPRE1/PPRE2) + uint32_t pp = (RCC->CFGR & RCC_CFGR_PPRE) >> RCC_CFGR_PPRE_Pos; + if (pp > 1) pclk1 *= 2; #else - // All other STM32 families - uint32_t apb1_pre = (RCC->CFGR & RCC_CFGR_PPRE) >> RCC_CFGR_PPRE_Pos; - if (apb1_pre > 1) pclk1 *= 2; + // F1/F4/F7/L1/L4: PPRE1 field + uint32_t pp = (RCC->CFGR & RCC_CFGR_PPRE1) >> RCC_CFGR_PPRE1_Pos; + if (pp > 1) pclk1 *= 2; #endif return pclk1; } @@ -114,18 +110,16 @@ void StepperQueue::init(uint8_t queue_num, uint8_t step_pin) { // Ensure TIM2 is initialized initStepTimer(); - // Step pin GPIO configuration + // Step pin GPIO configuration — validate port first _step_pin = step_pin; _step_port = digitalPinToPort(step_pin); + if (!_step_port) return; // Invalid pin — init fails silently, ISR skips + uint32_t mask = digitalPinToBitMask(step_pin); _step_set_mask = mask; - // Configure clear mask based on BSRR/BRR architecture -#if STM32_BSRR_CLEAR_SHIFT - _step_clr_mask = mask << 16; // BSRR high half = reset bits -#else - _step_clr_mask = mask; // BRR register (direct) -#endif + // Clear mask: BSRR high half = reset (mask << 16 works on ALL families) + _step_clr_mask = mask << 16; // Configure pin as OUTPUT, initial LOW pinMode(step_pin, OUTPUT); @@ -150,31 +144,28 @@ void StepperQueue::startQueue(void) { _dir_delay_active = false; // Write CCR first, then barrier, then enable interrupt - // This ensures the compare value is visible before the IRQ can fire. *_ccr_reg = TIM2->CNT + (TICKS_PER_S / 1000000); // ~1µs offset __DMB(); - // DIER RMW: disable interrupts to prevent race with ISR on other channels + // Save/restore PRIMASK for reentrant-safe IRQ disable + uint32_t prim = __get_PRIMASK(); __disable_irq(); TIM2->DIER |= CCXIE_BIT(_timer_ch); - __enable_irq(); + if (!prim) __enable_irq(); } void StepperQueue::forceStop(void) { - // Disable CC interrupt (atomic) + // Save/restore PRIMASK for reentrant-safe IRQ disable + uint32_t prim = __get_PRIMASK(); __disable_irq(); TIM2->DIER &= ~CCXIE_BIT(_timer_ch); _isRunning = false; read_idx = next_write_idx; // Discard remaining queue entries - __enable_irq(); + if (!prim) __enable_irq(); - // Ensure step pin is LOW (use BSRR/BRR appropriately) + // Ensure step pin is LOW (BSRR high-half clear works on ALL families) if (_step_port) { -#if STM32_BSRR_CLEAR_SHIFT _step_port->BSRR = _step_clr_mask; -#else - _step_port->BRR = _step_clr_mask; -#endif } } @@ -253,14 +244,9 @@ void TIM2_IRQHandler(void) { if (q->_pulse_high) { // ====== Phase 2: pulse end ====== - // The step pin was HIGH; bring it LOW. + // The step pin was HIGH; bring it LOW (BSRR high-half clear). q->_pulse_high = false; - -#if STM32_BSRR_CLEAR_SHIFT q->_step_port->BSRR = q->_step_clr_mask; -#else - q->_step_port->BRR = q->_step_clr_mask; -#endif // Read queue entry uint8_t rp = q->read_idx; @@ -359,6 +345,12 @@ __attribute__((weak)) void PendSV_Handler(void) { StepperQueue* StepperQueue::tryAllocateQueue( FastAccelStepperEngine* engine, uint8_t step_pin) { (void)engine; + + // Validate step pin before any hardware access + if (step_pin == PIN_UNDEFINED) return nullptr; + if ((step_pin & PIN_EXTERNAL_FLAG)) return nullptr; // External pins not supported + if (!digitalPinToPort(step_pin)) return nullptr; // Invalid pin → NULL port + int8_t idx = findFreeSlot(); if (idx < 0) return nullptr; diff --git a/src/pd_stm32/stm32_queue.h b/src/pd_stm32/stm32_queue.h index b0db2cce..87ecdd92 100644 --- a/src/pd_stm32/stm32_queue.h +++ b/src/pd_stm32/stm32_queue.h @@ -23,15 +23,11 @@ #define CCXIE_BIT(ch) (TIM_DIER_CC1IE << (ch)) #define CCXIF_BIT(ch) (TIM_SR_CC1IF << (ch)) -// ---- BSRR/BRR detection ---- -// portClearRegister returns &BSRR on F2/F4/F7, &BRR elsewhere -// When BSRR: write mask << 16 for reset -// When BRR: write mask directly -#if defined(STM32F2xx) || defined(STM32F4xx) || defined(STM32F7xx) -#define STM32_BSRR_CLEAR_SHIFT 1 -#else -#define STM32_BSRR_CLEAR_SHIFT 0 -#endif +// BSRR register layout (identical on ALL STM32 families): +// Bits [0:15] = set bits (write 1 → pin HIGH) +// Bits [16:31] = reset bits (write 1 → pin LOW) +// Clear is always done via BSRR high-half (mask << 16). +// Separate BRR register is NOT used — BSRR reset-half works everywhere. // ==================================================================== // StepperQueue class — STM32-specific implementation @@ -46,13 +42,13 @@ class StepperQueue : public StepperQueueBase { // Step pin GPIO uint8_t _step_pin; GPIO_TypeDef* _step_port; - uint32_t _step_set_mask; // BSRR set mask (low 16 bits) - uint32_t _step_clr_mask; // Clear mask (BRR or BSRR<<16) + uint32_t _step_set_mask; // BSRR set mask (write to BSRR low = set HIGH) + uint32_t _step_clr_mask; // BSRR clear mask (write to BSRR high = set LOW) = mask<<16 - // Direction pin (atomic via BSRR/BRR) + // Direction pin (atomic via BSRR) volatile uint32_t* _dir_bsrr; // &GPIOx->BSRR - uint32_t _dir_set_mask; // BSRR low bits = set - uint32_t _dir_clr_mask; // BSRR high bits = reset, or BRR + uint32_t _dir_set_mask; // BSRR low bits = set HIGH + uint32_t _dir_clr_mask; // BSRR high bits = set LOW (mask << 16) // Timer volatile uint32_t* _ccr_reg; // &TIM2->CCR1/2/3/4 @@ -94,11 +90,7 @@ class StepperQueue : public StepperQueueBase { uint32_t mask = digitalPinToBitMask(dir_pin); _dir_bsrr = &port->BSRR; _dir_set_mask = mask; -#if STM32_BSRR_CLEAR_SHIFT - _dir_clr_mask = mask << 16; // BSRR high half = reset -#else - _dir_clr_mask = mask; // BRR register direct -#endif + _dir_clr_mask = mask << 16; // BSRR high half = reset (works on ALL families) } } From 7203034160c2dcd64739e8a84147b7197e48bd8c Mon Sep 17 00:00:00 2001 From: KienTranCNC Date: Fri, 15 May 2026 23:45:55 +0700 Subject: [PATCH 3/5] debug round 1 --- Changed_code_v19.txt | 832 ----------------------------------- src/fas_arch/arduino_stm32.h | 4 +- src/pd_stm32/pd_config.h | 6 + src/pd_stm32/stm32_queue.cpp | 309 ++++++++++--- src/pd_stm32/stm32_queue.h | 18 +- 5 files changed, 272 insertions(+), 897 deletions(-) delete mode 100644 Changed_code_v19.txt diff --git a/Changed_code_v19.txt b/Changed_code_v19.txt deleted file mode 100644 index 72d20faa..00000000 --- a/Changed_code_v19.txt +++ /dev/null @@ -1,832 +0,0 @@ -Changed_code_v19.txt -===================== -All files created or modified for FastAccelStepper STM32 port, plan v.19 -Date: 2026-05-11 - ---- - -=== NEW FILE: src/fas_arch/arduino_stm32.h === - -#ifndef FAS_ARCH_ARDUINO_STM32_H -#define FAS_ARCH_ARDUINO_STM32_H - -#define FAS_STM32 - -#include -#include -#include - -// PRIMASK reentrant-safe interrupt control -// Saves and restores PRIMASK to support nested disable/enable calls -#define fasDisableInterrupts() \ - uint32_t __fas_prim = __get_PRIMASK(); __disable_irq() -#define fasEnableInterrupts() \ - __set_PRIMASK(__fas_prim) - -#define FAS_PSTR(s) (s) -#define PIN_UNDEFINED 0xFF -#define PIN_EXTERNAL_FLAG 128 - -#endif /* FAS_ARCH_ARDUINO_STM32_H */ - -end of file src/fas_arch/arduino_stm32.h - ---- - -=== NEW FILE: src/pd_stm32/pd_config.h === - -#ifndef PD_STM32_CONFIG_H -#define PD_STM32_CONFIG_H - -#include - -// ==================================================================== -// Compile-time TICKS_PER_S -// -// User MUST define TICKS_PER_S matching their board's TIM2 counter clock. -// TIM2 counter clock = PCLK1 * (APB1_prescaler==1 ? 1 : 2) -// -// Examples: -// STM32F103 @72MHz: TICKS_PER_S = 72000000 (APB1=36MHz ×2) -// STM32F407 @168MHz: TICKS_PER_S = 84000000 (APB1=42MHz ×2) -// STM32G0 @64MHz: TICKS_PER_S = 64000000 (APB1=64MHz ×1) -// STM32H743 @480MHz: TICKS_PER_S = 240000000 (APB1=120MHz ×2) -// ==================================================================== -#ifndef TICKS_PER_S -#define TICKS_PER_S 72000000UL -#endif - -// ---- Queue topology ---- -#define MAX_STEPPER 4 -#define NUM_QUEUES 4 -#define QUEUE_LEN 32 - -// ---- Pulse width (configurable) ---- -#ifndef STEP_PULSE_WIDTH_US -#define STEP_PULSE_WIDTH_US 6 -#endif -#define STEP_PULSE_WIDTH_TICKS ((uint32_t)(STEP_PULSE_WIDTH_US * (TICKS_PER_S / 1000000L))) - -// ---- Timing constants ---- -#define MIN_CMD_TICKS (TICKS_PER_S / 5000) -#define MIN_DIR_DELAY_US 200 -#define MAX_DIR_DELAY_US (65535 / (TICKS_PER_S / 1000000)) -#define DELAY_MS_BASE 2 -#define CYCLIC_INTERVAL_MS 3 - -// ---- Feature flags ---- -#define SUPPORT_QUEUE_ENTRY_END_POS_U16 -#define NEED_GENERIC_GET_CURRENT_POSITION -#define noop_or_wait __NOP() -#define DEBUG_LED_HALF_PERIOD 50 - -#endif /* PD_STM32_CONFIG_H */ - -//end of file src/pd_stm32/pd_config.h - ---- - -=== NEW FILE: src/pd_stm32/stm32_queue.h === - -#ifndef PD_STM32_QUEUE_H -#define PD_STM32_QUEUE_H - -#include "FastAccelStepper.h" -#include "fas_queue/base.h" -#include "fas_arch/result_codes.h" - -// ---- Default pin mapping (overridable in sketch) ---- -#ifndef STEP_PIN_STEPPER_0 -#define STEP_PIN_STEPPER_0 PA0 -#endif -#ifndef STEP_PIN_STEPPER_1 -#define STEP_PIN_STEPPER_1 PA1 -#endif -#ifndef STEP_PIN_STEPPER_2 -#define STEP_PIN_STEPPER_2 PA2 -#endif -#ifndef STEP_PIN_STEPPER_3 -#define STEP_PIN_STEPPER_3 PA3 -#endif - -// ---- CC interrupt bit helpers ---- -#define CCXIE_BIT(ch) (TIM_DIER_CC1IE << (ch)) -#define CCXIF_BIT(ch) (TIM_SR_CC1IF << (ch)) - -// ---- BSRR/BRR detection ---- -// portClearRegister returns &BSRR on F2/F4/F7, &BRR elsewhere -// When BSRR: write mask << 16 for reset -// When BRR: write mask directly -#if defined(STM32F2xx) || defined(STM32F4xx) || defined(STM32F7xx) -#define STM32_BSRR_CLEAR_SHIFT 1 -#else -#define STM32_BSRR_CLEAR_SHIFT 0 -#endif - -// ==================================================================== -// StepperQueue class — STM32-specific implementation -// ==================================================================== -class StepperQueue : public StepperQueueBase { - public: -#include "../fas_queue/protocol.h" - - volatile bool _isRunning; - bool _initialized; - - // Step pin GPIO - uint8_t _step_pin; - GPIO_TypeDef* _step_port; - uint32_t _step_set_mask; // BSRR set mask (low 16 bits) - uint32_t _step_clr_mask; // Clear mask (BRR or BSRR<<16) - - // Direction pin (atomic via BSRR/BRR) - volatile uint32_t* _dir_bsrr; // &GPIOx->BSRR - uint32_t _dir_set_mask; // BSRR low bits = set - uint32_t _dir_clr_mask; // BSRR high bits = reset, or BRR - - // Timer - volatile uint32_t* _ccr_reg; // &TIM2->CCR1/2/3/4 - uint8_t _timer_ch; // 0..3 - - // Pulse tracking - volatile bool _pulse_high; - volatile bool _dir_delay_active; // Direction settling in progress - - // Channel-to-queue mapping (static) - static StepperQueue* _ch_to_queue[4]; - - // ---- Inline methods ---- - inline void _pd_initVars() { - _step_pin = PIN_UNDEFINED; - _step_port = NULL; - _step_set_mask = 0; - _step_clr_mask = 0; - _dir_bsrr = NULL; - _dir_set_mask = 0; - _dir_clr_mask = 0; - _ccr_reg = NULL; - _timer_ch = 0; - _isRunning = false; - _initialized = false; - _pulse_high = false; - _dir_delay_active = false; - max_speed_in_ticks = STEP_PULSE_WIDTH_TICKS * 4; - } - - inline bool isRunning() const { return _isRunning; } - inline bool isReadyForCommands() const { return true; } - - void setDirPin(uint8_t dir_pin, bool _dirHighCountsUp) { - dirPin = dir_pin; - dirHighCountsUp = _dirHighCountsUp; - if ((dir_pin != PIN_UNDEFINED) && ((dir_pin & PIN_EXTERNAL_FLAG) == 0)) { - GPIO_TypeDef* port = digitalPinToPort(dir_pin); - uint32_t mask = digitalPinToBitMask(dir_pin); - _dir_bsrr = &port->BSRR; - _dir_set_mask = mask; -#if STM32_BSRR_CLEAR_SHIFT - _dir_clr_mask = mask << 16; // BSRR high half = reset -#else - _dir_clr_mask = mask; // BRR register direct -#endif - } - } - - void adjustSpeedToStepperCount(uint8_t steppers); - - private: - static StepperQueue* allocateSlot(uint8_t step_pin, uint8_t timer_ch); -}; - -// ---- Direction pin: atomic via BSRR/BRR ---- -#define SET_DIRECTION_PIN_STATE(q, high) \ - do { \ - if ((q)->_dir_bsrr) { \ - *(q->_dir_bsrr) = (high) ? (q)->_dir_set_mask \ - : (q)->_dir_clr_mask; \ - } \ - } while (0) - -// ---- Enable pin: simple digitalWrite ---- -#define SET_ENABLE_PIN_STATE(q, pin, high) \ - digitalWrite((pin), (high) ? HIGH : LOW) - -// ---- Direction-to-pulse delay ---- -#define AFTER_SET_DIR_PIN_DELAY_US 30 - -#endif /* PD_STM32_QUEUE_H */ - -end of file src/pd_stm32/stm32_queue.h - ---- - -=== NEW FILE: src/pd_stm32/stm32_queue.cpp === - -#include "fas_queue/stepper_queue.h" -#include "log2/Log2Representation.h" -#include "fas_ramp/RampControl.h" - -#if defined(ARDUINO_ARCH_STM32) - -// ==================================================================== -// Static data -// ==================================================================== -static FastAccelStepperEngine* fas_engine = NULL; -static uint8_t stepper_allocated_mask = 0; -static volatile bool _cyclic_pending = false; -static uint32_t _last_cyclic_uwtick = 0; -uint8_t fas_stm32_clock_error = 0; -StepperQueue* StepperQueue::_ch_to_queue[4] = {NULL, NULL, NULL, NULL}; - -// ==================================================================== -// Log2 timer frequency variables (for SUPPORT_LOG2_TIMER_FREQ_VARIABLES) -// Defined here when RampCalculator.h selects runtime fallback path. -// ==================================================================== -#ifdef SUPPORT_LOG2_TIMER_FREQ_VARIABLES -static log2_value_t log2_timer_freq; -static log2_value_t log2_timer_freq_div_sqrt_of_2; -static log2_value_t log2_timer_freq_square_div_2; -#endif - -// ==================================================================== -// TIM2 clock detection -// -// TIM2 counter clock = PCLK1 * (APB1_prescaler==1 ? 1 : 2) -// -// The APB1 prescaler is in RCC->CFGR.PPRE on most STM32 families. -// On H7 it is in RCC->D2CFGR.D2PPRE1 (D2 domain). -// ==================================================================== -static uint32_t getTim2Clock(void) { - uint32_t pclk1 = HAL_RCC_GetPCLK1Freq(); -#if defined(STM32H7xx) - // H7 series: D2 domain, D2CFGR register - uint32_t dppre1 = (RCC->D2CFGR & RCC_D2CFGR_D2PPRE1) >> RCC_D2CFGR_D2PPRE1_Pos; - if (dppre1 > 1) pclk1 *= 2; -#else - // All other STM32 families - uint32_t apb1_pre = (RCC->CFGR & RCC_CFGR_PPRE) >> RCC_CFGR_PPRE_Pos; - if (apb1_pre > 1) pclk1 *= 2; -#endif - return pclk1; -} - -// ==================================================================== -// TIM2 initialization (step generation timer) -// Called once when first stepper is initialized. -// ==================================================================== -static void initStepTimer(void) { - static bool initialized = false; - if (initialized) return; - initialized = true; - - // Enable TIM2 clock (portable across all STM32 families) - __HAL_RCC_TIM2_CLK_ENABLE(); - - // Compute prescaler from actual TIM2 clock and desired TICKS_PER_S - uint32_t tim2_clk = getTim2Clock(); - uint32_t psc; - - if (TICKS_PER_S > tim2_clk) { - // Cannot achieve TICKS_PER_S at this clock frequency. - // Run at maximum rate (PSC=0). All timing will be incorrect. - fas_stm32_clock_error = 1; - psc = 0; - } else { - psc = (tim2_clk / TICKS_PER_S) - 1; - if (psc > 65535) psc = 65535; - } - - // Configure TIM2 - TIM2->CR1 = 0; - TIM2->PSC = psc; - TIM2->ARR = 0xFFFFFFFF; // 32-bit auto-reload - TIM2->EGR |= TIM_EGR_UG; // Reload shadow registers (PSC, ARR) - TIM2->DIER = 0; // All interrupts disabled initially - - // Force LOW all channels (OCxM = 100 = Force Inactive) - // This prevents spurious pulses during initialization. - TIM2->CCMR1 = (4 << 4) | (4 << 12); // OC1M=100, OC2M=100 - TIM2->CCMR2 = (4 << 4) | (4 << 12); // OC3M=100, OC4M=100 - - // NVIC configuration - NVIC_SetPriority(TIM2_IRQn, 0); // Highest priority for step timing - NVIC_EnableIRQ(TIM2_IRQn); - - // Start timer - TIM2->CR1 |= TIM_CR1_CEN; -} - -// ==================================================================== -// Dynamic slot allocation — supports ANY GPIO pin for step -// ==================================================================== -static int8_t findFreeSlot(void) { - for (int i = 0; i < MAX_STEPPER; i++) { - if (!(stepper_allocated_mask & (1 << i))) { - return i; - } - } - return -1; // All slots used -} - -// ==================================================================== -// Queue initialization -// ==================================================================== -void StepperQueue::init(uint8_t queue_num, uint8_t step_pin) { - static const uint8_t ch_map[4] = {0, 1, 2, 3}; - _timer_ch = ch_map[queue_num]; - - // Ensure TIM2 is initialized - initStepTimer(); - - // Step pin GPIO configuration - _step_pin = step_pin; - _step_port = digitalPinToPort(step_pin); - uint32_t mask = digitalPinToBitMask(step_pin); - _step_set_mask = mask; - - // Configure clear mask based on BSRR/BRR architecture -#if STM32_BSRR_CLEAR_SHIFT - _step_clr_mask = mask << 16; // BSRR high half = reset bits -#else - _step_clr_mask = mask; // BRR register (direct) -#endif - - // Configure pin as OUTPUT, initial LOW - pinMode(step_pin, OUTPUT); - digitalWrite(step_pin, LOW); - - // Store CCR register pointer for fast ISR access - volatile uint32_t* ccr[] = {&TIM2->CCR1, &TIM2->CCR2, &TIM2->CCR3, &TIM2->CCR4}; - _ccr_reg = ccr[_timer_ch]; - - // Register channel-to-queue mapping - _ch_to_queue[_timer_ch] = this; - _initialized = true; - _isRunning = false; -} - -// ==================================================================== -// Start / Stop -// ==================================================================== -void StepperQueue::startQueue(void) { - _isRunning = true; - _pulse_high = false; - _dir_delay_active = false; - - // Write CCR first, then barrier, then enable interrupt - // This ensures the compare value is visible before the IRQ can fire. - *_ccr_reg = TIM2->CNT + (TICKS_PER_S / 1000000); // ~1µs offset - __DMB(); - - // DIER RMW: disable interrupts to prevent race with ISR on other channels - __disable_irq(); - TIM2->DIER |= CCXIE_BIT(_timer_ch); - __enable_irq(); -} - -void StepperQueue::forceStop(void) { - // Disable CC interrupt (atomic) - __disable_irq(); - TIM2->DIER &= ~CCXIE_BIT(_timer_ch); - _isRunning = false; - read_idx = next_write_idx; // Discard remaining queue entries - __enable_irq(); - - // Ensure step pin is LOW (use BSRR/BRR appropriately) - if (_step_port) { -#if STM32_BSRR_CLEAR_SHIFT - _step_port->BSRR = _step_clr_mask; -#else - _step_port->BRR = _step_clr_mask; -#endif - } -} - -void StepperQueue::connect(void) {} -void StepperQueue::disconnect(void) {} - -// ==================================================================== -// Speed adjustment based on number of active steppers -// ==================================================================== -void StepperQueue::adjustSpeedToStepperCount(uint8_t steppers) { - if (steppers == 1) - max_speed_in_ticks = STEP_PULSE_WIDTH_TICKS * 2; - else if (steppers == 2) - max_speed_in_ticks = STEP_PULSE_WIDTH_TICKS * 3; - else - max_speed_in_ticks = STEP_PULSE_WIDTH_TICKS * 4; - - if (max_speed_in_ticks < MIN_CMD_TICKS) - max_speed_in_ticks = MIN_CMD_TICKS; -} - -// ==================================================================== -// getActualTicksWithDirection — retrieve current step rate -// ==================================================================== -bool StepperQueue::getActualTicksWithDirection( - struct actual_ticks_s* speed) const { - fasDisableInterrupts(); - speed->count_up = queue_end.count_up; - speed->ticks = _last_command_ticks; - fasEnableInterrupts(); - inject_fill_interrupt(0); - return true; -} - -// ==================================================================== -// Cyclic PendSV trigger -// Called at end of TIM2_IRQHandler every ~3ms. -// Triggers PendSV exception to fill queues without consuming ISR time. -// ==================================================================== -static void cyclic_check_and_pend(void) { - uint32_t now = uwTick; - if ((now - _last_cyclic_uwtick) >= CYCLIC_INTERVAL_MS) { - _last_cyclic_uwtick = now; - if (!_cyclic_pending) { - _cyclic_pending = true; - SCB->ICSR = SCB_ICSR_PENDSVSET_Msk; - } - } -} - -// ==================================================================== -// TIM2_IRQHandler — Step pulse generation (all 4 channels) -// -// State machine: -// Phase 1 (_pulse_high=false): Set step pin HIGH, schedule LOW -// Phase 2 (_pulse_high=true): Set step pin LOW, process queue -// DirSettle (_dir_delay_active): Direction settling complete → start pulse -// -// SR handling: -// Snapshot SR at entry. Build ch_processed mask of all handled flags. -// Clear all at end by writing ~ch_processed (rc_w0 behavior). -// ==================================================================== -void TIM2_IRQHandler(void) { - uint32_t sr = TIM2->SR; - uint32_t ch_processed = 0; - - for (uint8_t ch = 0; ch < 4; ch++) { - uint32_t ccif = CCXIF_BIT(ch); - if (!(sr & ccif)) continue; - - // Always mark for clear — prevents infinite loop from spurious IRQs - ch_processed |= ccif; - - StepperQueue* q = StepperQueue::_ch_to_queue[ch]; - if (!q || !q->_isRunning) continue; - - if (q->_pulse_high) { - // ====== Phase 2: pulse end ====== - // The step pin was HIGH; bring it LOW. - q->_pulse_high = false; - -#if STM32_BSRR_CLEAR_SHIFT - q->_step_port->BSRR = q->_step_clr_mask; -#else - q->_step_port->BRR = q->_step_clr_mask; -#endif - - // Read queue entry - uint8_t rp = q->read_idx; - uint8_t wp = q->next_write_idx; - - if (rp == wp) { - // Queue empty — stop this channel - TIM2->DIER &= ~CCXIE_BIT(ch); - q->_isRunning = false; - continue; - } - - struct queue_entry* e = &q->entry[rp & QUEUE_LEN_MASK]; - q->_last_command_ticks = e->ticks; - - if (e->steps > 1) { - // Multi-step command: reduce step count, continue with same period - e->steps--; - *q->_ccr_reg = TIM2->CNT + (uint32_t)e->ticks; - } else { - // Single step complete — advance to next entry - rp++; - q->read_idx = rp; - - if (rp == wp) { - TIM2->DIER &= ~CCXIE_BIT(ch); - q->_isRunning = false; - continue; - } - - e = &q->entry[rp & QUEUE_LEN_MASK]; - - // Handle direction change (BSRR atomic — NOT ODR XOR) - if (e->toggle_dir && q->_dir_bsrr) { - if (e->dirPinState) { - *q->_dir_bsrr = q->_dir_set_mask; - } else { - *q->_dir_bsrr = q->_dir_clr_mask; - } - e->toggle_dir = 0; // Clear flag — prevents double-toggle - - // Insert direction settling delay - uint32_t dd = AFTER_SET_DIR_PIN_DELAY_US * (TICKS_PER_S / 1000000UL); - if (dd < MIN_CMD_TICKS) dd = MIN_CMD_TICKS; - *q->_ccr_reg = TIM2->CNT + dd; - q->_dir_delay_active = true; - continue; - } - - // No direction change — schedule next step pulse - *q->_ccr_reg = TIM2->CNT + (uint32_t)e->ticks; - } - } else if (q->_dir_delay_active) { - // ====== Direction settling complete ====== - // The settling delay has elapsed. Now start the first step pulse - // in the new direction (set pin HIGH). - q->_dir_delay_active = false; - q->_pulse_high = true; - q->_step_port->BSRR = q->_step_set_mask; - *q->_ccr_reg = TIM2->CNT + STEP_PULSE_WIDTH_TICKS; - } else { - // ====== Phase 1: pulse start ====== - // Set step pin HIGH, schedule LOW after STEP_PULSE_WIDTH_TICKS. - q->_pulse_high = true; - q->_step_port->BSRR = q->_step_set_mask; - *q->_ccr_reg = TIM2->CNT + STEP_PULSE_WIDTH_TICKS; - } - } - - // Clear all processed flags at once (rc_w0: bits set to 1 are ignored) - TIM2->SR = ~ch_processed; - - // Trigger cyclic queue fill - cyclic_check_and_pend(); -} - -// ==================================================================== -// PendSV_Handler — Deferred queue fill -// -// Weak attribute allows FreeRTOS to override this handler. -// Define DISABLE_FAS_PENDSV to skip installation entirely. -// ==================================================================== -#if !defined(DISABLE_FAS_PENDSV) -__attribute__((weak)) void PendSV_Handler(void) { - _cyclic_pending = false; - __DMB(); - if (fas_engine) { - fas_engine->manageSteppers(); - } -} -#endif - -// ==================================================================== -// Allocation — dynamic slot assignment for any GPIO step pin -// ==================================================================== -StepperQueue* StepperQueue::tryAllocateQueue( - FastAccelStepperEngine* engine, uint8_t step_pin) { - (void)engine; - int8_t idx = findFreeSlot(); - if (idx < 0) return nullptr; - - fas_queue[idx]._initVars(); - fas_queue[idx].init((uint8_t)idx, step_pin); - stepper_allocated_mask |= (1 << idx); - return &fas_queue[idx]; -} - -// ==================================================================== -// Engine initialization -// ==================================================================== -void fas_init_engine(FastAccelStepperEngine* engine) { - fas_engine = engine; - - // Initialize Log2 timer frequency variables if using runtime fallback - // (SUPPORT_LOG2_TIMER_FREQ_VARIABLES path in RampCalculator.h) - init_ramp_module(); - - // PendSV at lowest priority — avoids blocking higher-priority interrupts - NVIC_SetPriority(PendSV_IRQn, 0xFF); -} - -#endif /* ARDUINO_ARCH_STM32 */ - -end of file src/pd_stm32/stm32_queue.cpp - ---- - -=== NEW FILE: extras/StepperPins_stm32.h === - -#ifndef STEPPERPINS_STM32_H -#define STEPPERPINS_STM32_H - -// ==================================================================== -// Default step pin mapping for STM32 platforms -// -// These defaults map steppers 0-3 to PA0-PA3 (TIM2 channels 1-4). -// Override by defining before including this header: -// -// #define STEP_PIN_STEPPER_0 PB0 -// #include "StepperPins_stm32.h" -// -// Note: On STM32, any GPIO pin can be used as a step pin. -// PA0-PA3 are only a convention — the timer is used only for -// interrupt timing, not direct pin output. -// ==================================================================== - -#ifndef STEP_PIN_STEPPER_0 -#define STEP_PIN_STEPPER_0 PA0 -#endif - -#ifndef STEP_PIN_STEPPER_1 -#define STEP_PIN_STEPPER_1 PA1 -#endif - -#ifndef STEP_PIN_STEPPER_2 -#define STEP_PIN_STEPPER_2 PA2 -#endif - -#ifndef STEP_PIN_STEPPER_3 -#define STEP_PIN_STEPPER_3 PA3 -#endif - -#endif /* STEPPERPINS_STM32_H */ - -end of file extras/StepperPins_stm32.h - ---- - -=== MODIFIED FILE: src/fas_arch/common.h (ADDED STM32 DISPATCH) === - -// Added after line 95 (PICO_RP2040 branch): -// The new branch: - -#elif defined(ARDUINO_ARCH_STM32) -// STM32 family (STM32duino core) -#include "fas_arch/arduino_stm32.h" -#include "pd_stm32/pd_config.h" - -// This was inserted BEFORE #else #error "Unsupported devices" - -end of file src/fas_arch/common.h (modification shown) - ---- - -=== MODIFIED FILE: src/fas_queue/stepper_queue.h (ADDED STM32 BRANCH) === - -// Added after line 21 (SUPPORT_ESP32 branch): -// The new branch: - -#elif defined(FAS_STM32) -#include "pd_stm32/stm32_queue.h" - -// This was inserted BEFORE #else #error "Unsupported architecture" - -end of file src/fas_queue/stepper_queue.h (modification shown) - ---- - -=== MODIFIED FILE: src/fas_ramp/RampCalculator.h (ADDED 13 STM32 FREQUENCY BRANCHES) === - -// Added after line 20 (TICKS_PER_S == 21000000L branch): -// The 13 new STM32 frequency branches: - -#elif (TICKS_PER_S == 32000000L) -// STM32L0, RP2040 -#define LOG2_TICKS_PER_S ((log2_value_t)0x31dd) -#define LOG2_TICKS_PER_S_DIV_SQRT_OF_2 ((log2_value_t)0x30dd) -#define LOG2_ACCEL_FACTOR ((log2_value_t)0x61ba) -#define US_TO_TICKS(u32) ((u32) * 32) -#define TICKS_TO_US(u32) ((u32) / 32) - -#elif (TICKS_PER_S == 48000000L) -// STM32F0/G0/WL -#define LOG2_TICKS_PER_S ((log2_value_t)0x3308) -#define LOG2_TICKS_PER_S_DIV_SQRT_OF_2 ((log2_value_t)0x3208) -#define LOG2_ACCEL_FACTOR ((log2_value_t)0x6411) -#define US_TO_TICKS(u32) ((u32) * 48) -#define TICKS_TO_US(u32) ((u32) / 48) - -#elif (TICKS_PER_S == 64000000L) -// STM32G0/WB -#define LOG2_TICKS_PER_S ((log2_value_t)0x33dd) -#define LOG2_TICKS_PER_S_DIV_SQRT_OF_2 ((log2_value_t)0x32dd) -#define LOG2_ACCEL_FACTOR ((log2_value_t)0x65ba) -#define US_TO_TICKS(u32) ((u32) * 64) -#define TICKS_TO_US(u32) ((u32) / 64) - -#elif (TICKS_PER_S == 72000000L) -// STM32F1/L1 (most common) -#define LOG2_TICKS_PER_S ((log2_value_t)0x3434) -#define LOG2_TICKS_PER_S_DIV_SQRT_OF_2 ((log2_value_t)0x3334) -#define LOG2_ACCEL_FACTOR ((log2_value_t)0x6668) -#define US_TO_TICKS(u32) ((u32) * 72) -#define TICKS_TO_US(u32) ((u32) / 72) - -#elif (TICKS_PER_S == 80000000L) -// STM32L4 -#define LOG2_TICKS_PER_S ((log2_value_t)0x3482) -#define LOG2_TICKS_PER_S_DIV_SQRT_OF_2 ((log2_value_t)0x3382) -#define LOG2_ACCEL_FACTOR ((log2_value_t)0x6704) -#define US_TO_TICKS(u32) ((u32) * 80) -#define TICKS_TO_US(u32) ((u32) / 80) - -#elif (TICKS_PER_S == 84000000L) -// STM32F401/411 -#define LOG2_TICKS_PER_S ((log2_value_t)0x34a6) -#define LOG2_TICKS_PER_S_DIV_SQRT_OF_2 ((log2_value_t)0x33a6) -#define LOG2_ACCEL_FACTOR ((log2_value_t)0x674c) -#define US_TO_TICKS(u32) ((u32) * 84) -#define TICKS_TO_US(u32) ((u32) / 84) - -#elif (TICKS_PER_S == 100000000L) -// STM32F411/746 -#define LOG2_TICKS_PER_S ((log2_value_t)0x3527) -#define LOG2_TICKS_PER_S_DIV_SQRT_OF_2 ((log2_value_t)0x3427) -#define LOG2_ACCEL_FACTOR ((log2_value_t)0x684d) -#define US_TO_TICKS(u32) ((u32) * 100) -#define TICKS_TO_US(u32) ((u32) / 100) - -#elif (TICKS_PER_S == 120000000L) -// STM32L4+/F4 -#define LOG2_TICKS_PER_S ((log2_value_t)0x35ad) -#define LOG2_TICKS_PER_S_DIV_SQRT_OF_2 ((log2_value_t)0x34ad) -#define LOG2_ACCEL_FACTOR ((log2_value_t)0x695a) -#define US_TO_TICKS(u32) ((u32) * 120) -#define TICKS_TO_US(u32) ((u32) / 120) - -#elif (TICKS_PER_S == 168000000L) -// STM32F405/407 -#define LOG2_TICKS_PER_S ((log2_value_t)0x36a6) -#define LOG2_TICKS_PER_S_DIV_SQRT_OF_2 ((log2_value_t)0x35a6) -#define LOG2_ACCEL_FACTOR ((log2_value_t)0x6b4c) -#define US_TO_TICKS(u32) ((u32) * 168) -#define TICKS_TO_US(u32) ((u32) / 168) - -#elif (TICKS_PER_S == 170000000L) -// STM32F3/G4 -#define LOG2_TICKS_PER_S ((log2_value_t)0x36af) -#define LOG2_TICKS_PER_S_DIV_SQRT_OF_2 ((log2_value_t)0x35af) -#define LOG2_ACCEL_FACTOR ((log2_value_t)0x6b5e) -#define US_TO_TICKS(u32) ((u32) * 170) -#define TICKS_TO_US(u32) ((u32) / 170) - -#elif (TICKS_PER_S == 216000000L) -// STM32F7 -#define LOG2_TICKS_PER_S ((log2_value_t)0x375f) -#define LOG2_TICKS_PER_S_DIV_SQRT_OF_2 ((log2_value_t)0x365f) -#define LOG2_ACCEL_FACTOR ((log2_value_t)0x6cbe) -#define US_TO_TICKS(u32) ((u32) * 216) -#define TICKS_TO_US(u32) ((u32) / 216) - -#elif (TICKS_PER_S == 480000000L) -// STM32H7 (default) -#define LOG2_TICKS_PER_S ((log2_value_t)0x39ad) -#define LOG2_TICKS_PER_S_DIV_SQRT_OF_2 ((log2_value_t)0x38ad) -#define LOG2_ACCEL_FACTOR ((log2_value_t)0x715a) -#define US_TO_TICKS(u32) ((u32) * 480) -#define TICKS_TO_US(u32) ((u32) / 480) - -#elif (TICKS_PER_S == 550000000L) -// STM32H7 (overclock) -#define LOG2_TICKS_PER_S ((log2_value_t)0x3a12) -#define LOG2_TICKS_PER_S_DIV_SQRT_OF_2 ((log2_value_t)0x3912) -#define LOG2_ACCEL_FACTOR ((log2_value_t)0x7224) -#define US_TO_TICKS(u32) ((u32) * 550) -#define TICKS_TO_US(u32) ((u32) / 550) - -end of file src/fas_ramp/RampCalculator.h (modification shown) - ---- - -=== MODIFIED FILE: library.properties === - -name=FastAccelStepper -version=1.2.5 -license=MIT -author=Jochen Kiemes -maintainer=Jochen Kiemes -sentence=A high speed stepper library for Atmega 168/168p/328/328p (nano), 32u4 (leonardo), 2560, ESP32, ESP32S2, ESP32S3, ESP32C3, ESP32C6, Atmel SAM Due, Raspberry pi pico and pico 2, and STM32 -paragraph=Drive stepper motors with acceleration/deceleration profile up to 50 kSteps/s (Atmega) and 200kSteps/s (esp32). Supports STM32F1/F4/G0/H7 series via STM32duino core. -url=https://github.com/gin66/FastAccelStepper -repository=https://github.com/gin66/FastAccelStepper.git -architectures=avr,esp32,sam,rp2040,rp2350,stm32 -category=Device Control -dot_a_linkage=true - -Changes from original: -- sentence: added ", and STM32" -- paragraph: added "Supports STM32F1/F4/G0/H7 series via STM32duino core." -- architectures: added ",stm32" - -end of file library.properties - ---- - -=== MODIFIED FILE: README.md (ADDED STM32 SECTION) === - -Added new section "STM32 Arduino Support" before "## Contribution" at end of file. -See the full content in the repository. - -end of file README.md (modification shown) - ---- - -=== END OF Changed_code_v19.txt === \ No newline at end of file diff --git a/src/fas_arch/arduino_stm32.h b/src/fas_arch/arduino_stm32.h index 006e4059..67b59524 100644 --- a/src/fas_arch/arduino_stm32.h +++ b/src/fas_arch/arduino_stm32.h @@ -15,7 +15,7 @@ __set_PRIMASK(__fas_prim) #define FAS_PSTR(s) (s) -#define PIN_UNDEFINED 0xFF -#define PIN_EXTERNAL_FLAG 128 +// PIN_UNDEFINED (255) and PIN_EXTERNAL_FLAG (128) are defined in +// FastAccelStepper.h. Do NOT redefine here to avoid -Wmacro-redefined. #endif /* FAS_ARCH_ARDUINO_STM32_H */ \ No newline at end of file diff --git a/src/pd_stm32/pd_config.h b/src/pd_stm32/pd_config.h index b5645688..e4fef21a 100644 --- a/src/pd_stm32/pd_config.h +++ b/src/pd_stm32/pd_config.h @@ -37,6 +37,12 @@ #define DELAY_MS_BASE 2 #define CYCLIC_INTERVAL_MS 3 +// ==================================================================== +// NOTE: STM32F1 TIM2 is 16-bit only. +// ARR = 0xFFFFFFFF is masked to 0xFFFF by F1 hardware. +// Minimum speed = TICKS_PER_S / 65536 ≈ 1098 steps/s @72MHz. +// ==================================================================== + // ---- Feature flags ---- #define SUPPORT_QUEUE_ENTRY_END_POS_U16 #define NEED_GENERIC_GET_CURRENT_POSITION diff --git a/src/pd_stm32/stm32_queue.cpp b/src/pd_stm32/stm32_queue.cpp index d300990d..17f5cacf 100644 --- a/src/pd_stm32/stm32_queue.cpp +++ b/src/pd_stm32/stm32_queue.cpp @@ -4,6 +4,68 @@ #if defined(ARDUINO_ARCH_STM32) +// ==================================================================== +// FAS_DMB — Data Memory Barrier wrapper +// +// ARMv6-M (M0/M0+) does not have __DMB(). Use __DSB() instead. +// ARMv7-M (M3/M4/M7) and ARMv8-M.main (M33) have __DMB(). +// ARMv8-M.base (M23) does not have __DMB() — reserved, use __DSB(). +// +// Affected STM32 families: +// __DMB() OK: F1, F4, F7, H7, G4, L4, WB, WL, L5, U5, H5 +// __DSB() needed: G0, F0, L0, C0 (ARMv6-M, __ARM_ARCH_6M__) +// Reserved: M23 (ARMv8-M.base, __ARM_ARCH_8M_BASE__) +// +// GCC and ARMCC define __ARM_ARCH_6M__ automatically when compiling +// with -mcpu=cortex-m0 or -mcpu=cortex-m0plus. +// ==================================================================== +#if defined(__ARM_ARCH_6M__) + // M0/M0+ (G0, F0, L0, C0): không có __DMB() + #define FAS_DMB() __DSB() +#elif defined(__ARM_ARCH_7M__) || defined(__ARM_ARCH_7EM__) || \ + defined(__ARM_ARCH_8M_MAIN__) + // M3/M4/M7/M33: có __DMB() + #define FAS_DMB() __DMB() + // reserved: __ARM_ARCH_8M_BASE__ (M23) → dùng __DSB() nếu cần +#else + #define FAS_DMB() __DSB() // fallback an toàn +#endif + +// ==================================================================== +// Timer selection — STM32C0 does NOT have TIM2 +// +// STM32C0 series (e.g. STM32C031) only has TIM1, TIM3, TIM14, TIM16, TIM17. +// We use TIM3 on C0. TIM3 is a 16-bit timer (ARR=0xFFFF). +// All other STM32 families use TIM2 (32-bit on most, 16-bit on F1). +// +// TIM3 on C0 supports up to 4 channels (CCR1-CCR4) via TIM3->CCR1-4, +// which matches the 4 stepper channels expected by the code. +// +// Macros: +// FAS_TIMER — timer peripheral (TIM2 or TIM3) +// FAS_TIMER_IRQn — NVIC IRQ number +// FAS_TIMER_RCC_ENABLE — HAL macro to enable timer clock +// FAS_TIMER_ARR_MAX — auto-reload max (0xFFFF for 16-bit, 0xFFFFFFFF for 32-bit) +// FAS_TIM_IS_16BIT — defined if timer is 16-bit (needs wrap handling) +// ==================================================================== +#if defined(STM32C0xx) + #define FAS_TIMER TIM3 + #define FAS_TIMER_IRQn TIM3_IRQn + #define FAS_TIMER_RCC_ENABLE __HAL_RCC_TIM3_CLK_ENABLE() + #define FAS_TIM_IS_16BIT + #define FAS_TIMER_ARR_MAX 0xFFFF +#else + #define FAS_TIMER TIM2 + #define FAS_TIMER_IRQn TIM2_IRQn + #define FAS_TIMER_RCC_ENABLE __HAL_RCC_TIM2_CLK_ENABLE() + #if defined(STM32F1xx) + #define FAS_TIM_IS_16BIT + #define FAS_TIMER_ARR_MAX 0xFFFF + #else + #define FAS_TIMER_ARR_MAX 0xFFFFFFFF + #endif +#endif + // ==================================================================== // Static data // ==================================================================== @@ -12,80 +74,129 @@ static uint8_t stepper_allocated_mask = 0; static volatile bool _cyclic_pending = false; static uint32_t _last_cyclic_uwtick = 0; uint8_t fas_stm32_clock_error = 0; +uint32_t fas_stm32_clock_tim_clk = 0; // Cached timer clock for warning output StepperQueue* StepperQueue::_ch_to_queue[4] = {NULL, NULL, NULL, NULL}; // ==================================================================== -// TIM2 clock detection +// Timer clock detection // -// TIM2 counter clock = PCLK1 * (APB1_prescaler==1 ? 1 : 2) +// Timer counter clock = PCLK1 * (APB1_prescaler==1 ? 1 : 2) // // The APB1 prescaler is in: -// - H7: RCC->D2CFGR.D2PPRE1 (D2 domain) -// - G0/G4: RCC->CFGR.PPRE (single field) -// - F1/F4/F7/L1/L4: RCC->CFGR.PPRE1 +// - H7: RCC->D2CFGR.D2PPRE1 (D2 domain, encoding: 0-3=÷1, 4=÷2, 5=÷4, 6=÷8, 7=÷16) +// - G0/C0: RCC->CFGR.PPRE (single APB bus, same encoding) +// - Others F1/F4/F7/L1/L4/G4/WB: RCC->CFGR.PPRE1 (same encoding) +// +// NOTE: APB prescaler field encoding (all STM32 families): +// 0-3 = ÷1 (0,1 valid; 2,3 reserved) +// 4 = ÷2 +// 5 = ÷4 +// 6 = ÷8 +// 7 = ÷16 +// Condition for clock ×2: prescaler > 1 ↔ field >= 4 // ==================================================================== -static uint32_t getTim2Clock(void) { +static uint32_t getTimClock(void) { uint32_t pclk1 = HAL_RCC_GetPCLK1Freq(); #if defined(STM32H7xx) - // H7 series: D2 domain + // H7 series: D2 domain, D2CFGR register uint32_t dppre1 = (RCC->D2CFGR & RCC_D2CFGR_D2PPRE1) >> RCC_D2CFGR_D2PPRE1_Pos; - if (dppre1 > 1) pclk1 *= 2; -#elif defined(STM32G0xx) || defined(STM32G4xx) - // G0/G4: single PPRE field (no PPRE1/PPRE2) + if (dppre1 >= 4) pclk1 *= 2; +#elif defined(STM32G0xx) || defined(STM32C0xx) + // G0/C0: single APB bus, uses RCC_CFGR_PPRE uint32_t pp = (RCC->CFGR & RCC_CFGR_PPRE) >> RCC_CFGR_PPRE_Pos; - if (pp > 1) pclk1 *= 2; + if (pp >= 4) pclk1 *= 2; #else - // F1/F4/F7/L1/L4: PPRE1 field + // F1/F4/F7/L1/L4/G4/WB...: uses RCC_CFGR_PPRE1 uint32_t pp = (RCC->CFGR & RCC_CFGR_PPRE1) >> RCC_CFGR_PPRE1_Pos; - if (pp > 1) pclk1 *= 2; + if (pp >= 4) pclk1 *= 2; #endif return pclk1; } // ==================================================================== -// TIM2 initialization (step generation timer) +// fas_tim_set_ccr — Write CCR with 16-bit wrap handling (F1, C0) +// +// On 16-bit timers (F1 TIM2, C0 TIM3), (cnt + delay) may exceed 0xFFFF. +// The & 0xFFFF mask correctly handles the wrap: +// target = (cnt + delay) & 0xFFFF = cnt + delay - 65536 (if overflow) +// Actual ticks = (0xFFFF - cnt + 1) + target +// = (65536 - cnt) + (cnt + delay - 65536) = delay ✓ +// +// Example: cnt=65520, delay=432 +// target = (65520+432) & 0xFFFF = 416 +// ticks = (65536-65520) + 416 = 16 + 416 = 432 = delay ✓ +// +// On 32-bit timers, simple addition is safe (no overflow in practice). +// ==================================================================== +static inline void fas_tim_set_ccr(volatile uint32_t* ccr, uint32_t delay) { +#if defined(STM32F1xx) || defined(STM32C0xx) + uint32_t cnt = FAS_TIMER->CNT; + // 16-bit timer: (cnt + delay) & 0xFFFF xử lý wrap chính xác. + // Xem toán học ở comment function. + *ccr = (cnt + delay) & 0xFFFF; +#else + *ccr = FAS_TIMER->CNT + delay; +#endif +} + +// ==================================================================== +// Step timer initialization // Called once when first stepper is initialized. +// Uses FAS_TIMER macros to support TIM2 (all families) or TIM3 (C0). // ==================================================================== static void initStepTimer(void) { static bool initialized = false; if (initialized) return; initialized = true; - // Enable TIM2 clock (portable across all STM32 families) - __HAL_RCC_TIM2_CLK_ENABLE(); + // Enable timer clock (TIM2 on most, TIM3 on C0) + FAS_TIMER_RCC_ENABLE(); - // Compute prescaler from actual TIM2 clock and desired TICKS_PER_S - uint32_t tim2_clk = getTim2Clock(); - uint32_t psc; + // Cache the actual timer clock for later warning output + fas_stm32_clock_tim_clk = getTimClock(); - if (TICKS_PER_S > tim2_clk) { + // ---- Clock validation ---- + if (TICKS_PER_S == 0 || TICKS_PER_S > fas_stm32_clock_tim_clk) { // Cannot achieve TICKS_PER_S at this clock frequency. // Run at maximum rate (PSC=0). All timing will be incorrect. fas_stm32_clock_error = 1; - psc = 0; + } else if (fas_stm32_clock_tim_clk % TICKS_PER_S != 0) { + // Non-integer prescaler — timer tick rate will deviate. + // Example: 84MHz TIM2 with TICKS_PER_S=72MHz → psc=(84/72)-1=0 → timer at 84MHz, +16.7% error. + // (Addition in v8: error code 2 was not present in original code.) + fas_stm32_clock_error = 2; + } + + // Compute prescaler + uint32_t psc; + if (fas_stm32_clock_error == 1) { + psc = 0; // Run at maximum rate (timing will be wrong) } else { - psc = (tim2_clk / TICKS_PER_S) - 1; - if (psc > 65535) psc = 65535; + psc = (fas_stm32_clock_tim_clk / TICKS_PER_S) - 1; + if (psc > 65535) { + psc = 65535; + fas_stm32_clock_error = 1; // Prescaler clamped — timing will be wrong + } } - // Configure TIM2 - TIM2->CR1 = 0; - TIM2->PSC = psc; - TIM2->ARR = 0xFFFFFFFF; // 32-bit auto-reload - TIM2->EGR |= TIM_EGR_UG; // Reload shadow registers (PSC, ARR) - TIM2->DIER = 0; // All interrupts disabled initially + // Configure timer + FAS_TIMER->CR1 = 0; + FAS_TIMER->PSC = psc; + FAS_TIMER->ARR = FAS_TIMER_ARR_MAX; + FAS_TIMER->EGR |= TIM_EGR_UG; // Reload shadow registers (PSC, ARR) + FAS_TIMER->DIER = 0; // All interrupts disabled initially // Force LOW all channels (OCxM = 100 = Force Inactive) // This prevents spurious pulses during initialization. - TIM2->CCMR1 = (4 << 4) | (4 << 12); // OC1M=100, OC2M=100 - TIM2->CCMR2 = (4 << 4) | (4 << 12); // OC3M=100, OC4M=100 + FAS_TIMER->CCMR1 = (4 << 4) | (4 << 12); // OC1M=100, OC2M=100 + FAS_TIMER->CCMR2 = (4 << 4) | (4 << 12); // OC3M=100, OC4M=100 // NVIC configuration - NVIC_SetPriority(TIM2_IRQn, 0); // Highest priority for step timing - NVIC_EnableIRQ(TIM2_IRQn); + NVIC_SetPriority(FAS_TIMER_IRQn, 0); // Highest priority for step timing + NVIC_EnableIRQ(FAS_TIMER_IRQn); // Start timer - TIM2->CR1 |= TIM_CR1_CEN; + FAS_TIMER->CR1 |= TIM_CR1_CEN; } // ==================================================================== @@ -126,12 +237,19 @@ void StepperQueue::init(uint8_t queue_num, uint8_t step_pin) { digitalWrite(step_pin, LOW); // Store CCR register pointer for fast ISR access + // Must match the timer type: TIM2 on most families, TIM3 on C0 + // This is the only place where the concrete timer register is referenced. + // All other CCR writes go through _ccr_reg (fast pointer) or fas_tim_set_ccr(). +#if defined(STM32C0xx) + volatile uint32_t* ccr[] = {&FAS_TIMER->CCR1, &FAS_TIMER->CCR2, &FAS_TIMER->CCR3, &FAS_TIMER->CCR4}; +#else volatile uint32_t* ccr[] = {&TIM2->CCR1, &TIM2->CCR2, &TIM2->CCR3, &TIM2->CCR4}; +#endif _ccr_reg = ccr[_timer_ch]; // Register channel-to-queue mapping _ch_to_queue[_timer_ch] = this; - _initialized = true; + // _initialized = true was removed (fix_plan_v3 FIX #7) _isRunning = false; } @@ -144,13 +262,14 @@ void StepperQueue::startQueue(void) { _dir_delay_active = false; // Write CCR first, then barrier, then enable interrupt - *_ccr_reg = TIM2->CNT + (TICKS_PER_S / 1000000); // ~1µs offset - __DMB(); + // Use fas_tim_set_ccr for 16-bit safe CCR write (handles F1/C0 wrap) + fas_tim_set_ccr(_ccr_reg, TICKS_PER_S / 1000000); // ~1µs offset + FAS_DMB(); // Save/restore PRIMASK for reentrant-safe IRQ disable uint32_t prim = __get_PRIMASK(); __disable_irq(); - TIM2->DIER |= CCXIE_BIT(_timer_ch); + FAS_TIMER->DIER |= CCXIE_BIT(_timer_ch); if (!prim) __enable_irq(); } @@ -158,7 +277,7 @@ void StepperQueue::forceStop(void) { // Save/restore PRIMASK for reentrant-safe IRQ disable uint32_t prim = __get_PRIMASK(); __disable_irq(); - TIM2->DIER &= ~CCXIE_BIT(_timer_ch); + FAS_TIMER->DIER &= ~CCXIE_BIT(_timer_ch); _isRunning = false; read_idx = next_write_idx; // Discard remaining queue entries if (!prim) __enable_irq(); @@ -217,7 +336,51 @@ static void cyclic_check_and_pend(void) { } // ==================================================================== -// TIM2_IRQHandler — Step pulse generation (all 4 channels) +// Clock error reporting — prints warning to Serial if clock mismatch +// +// Call site: engine.init() → FastAccelStepperEngine::init() → fas_init_engine() +// → fas_stm32_report_clock_error() +// +// ⚠️ User MUST call Serial.begin() BEFORE engine.init(). +// If Serial is not initialized, the warning is silently dropped +// (no crash, but user won't see the error). +// +// Error codes: +// 0 = OK (no error) +// 1 = TICKS_PER_S > actual timer clock, OR prescaler clamped at 65535 +// (timing will be wrong — define correct TICKS_PER_S in build_flags) +// 2 = Non-integer prescaler (tim2_clk % TICKS_PER_S != 0) +// (timing will be slightly off — define correct TICKS_PER_S in build_flags) +// ==================================================================== +static void fas_stm32_report_clock_error(void) { + if (fas_stm32_clock_error == 0) return; + if (!Serial) return; // Serial not initialized → silent, no crash + + Serial.print("[FAS] WARNING: Step timer clock error (code="); + Serial.print(fas_stm32_clock_error); + Serial.println(")"); + + switch (fas_stm32_clock_error) { + case 1: + Serial.println(" Cause: TICKS_PER_S > actual timer clock, or prescaler > 65535."); + break; + case 2: + Serial.println(" Cause: Non-integer prescaler (timer clock not divisible by TICKS_PER_S)."); + break; + } + Serial.print(" TICKS_PER_S="); + Serial.print(TICKS_PER_S); + Serial.print(" Timer_CLK="); + Serial.println(fas_stm32_clock_tim_clk); + Serial.println(" Fix: add -DTICKS_PER_S=xxx in platformio.ini (see debug_plan_v8.md appendix B)."); +} + +// ==================================================================== +// FAS_TIMER_IRQHandler — Step pulse generation (all 4 channels) +// +// The ISR name depends on which timer is used: +// - C0: TIM3_IRQHandler (handles TIM3->SR, TIM3->DIER, TIM3->CNT) +// - Others: TIM2_IRQHandler (handles TIM2->SR, TIM2->DIER, TIM2->CNT) // // State machine: // Phase 1 (_pulse_high=false): Set step pin HIGH, schedule LOW @@ -227,9 +390,15 @@ static void cyclic_check_and_pend(void) { // SR handling: // Snapshot SR at entry. Build ch_processed mask of all handled flags. // Clear all at end by writing ~ch_processed (rc_w0 behavior). +// +// IMPORTANT: All CCR writes use fas_tim_set_ccr() for 16-bit wrap safety. // ==================================================================== +#if defined(STM32C0xx) +void TIM3_IRQHandler(void) { +#else void TIM2_IRQHandler(void) { - uint32_t sr = TIM2->SR; +#endif + uint32_t sr = FAS_TIMER->SR; uint32_t ch_processed = 0; for (uint8_t ch = 0; ch < 4; ch++) { @@ -254,7 +423,7 @@ void TIM2_IRQHandler(void) { if (rp == wp) { // Queue empty — stop this channel - TIM2->DIER &= ~CCXIE_BIT(ch); + FAS_TIMER->DIER &= ~CCXIE_BIT(ch); q->_isRunning = false; continue; } @@ -265,14 +434,14 @@ void TIM2_IRQHandler(void) { if (e->steps > 1) { // Multi-step command: reduce step count, continue with same period e->steps--; - *q->_ccr_reg = TIM2->CNT + (uint32_t)e->ticks; + fas_tim_set_ccr(q->_ccr_reg, e->ticks); } else { // Single step complete — advance to next entry rp++; q->read_idx = rp; if (rp == wp) { - TIM2->DIER &= ~CCXIE_BIT(ch); + FAS_TIMER->DIER &= ~CCXIE_BIT(ch); q->_isRunning = false; continue; } @@ -291,33 +460,53 @@ void TIM2_IRQHandler(void) { // Insert direction settling delay uint32_t dd = AFTER_SET_DIR_PIN_DELAY_US * (TICKS_PER_S / 1000000UL); if (dd < MIN_CMD_TICKS) dd = MIN_CMD_TICKS; - *q->_ccr_reg = TIM2->CNT + dd; + fas_tim_set_ccr(q->_ccr_reg, dd); q->_dir_delay_active = true; continue; } // No direction change — schedule next step pulse - *q->_ccr_reg = TIM2->CNT + (uint32_t)e->ticks; + fas_tim_set_ccr(q->_ccr_reg, e->ticks); } } else if (q->_dir_delay_active) { // ====== Direction settling complete ====== - // The settling delay has elapsed. Now start the first step pulse - // in the new direction (set pin HIGH). + // The settling delay has elapsed. Check if the current entry + // has steps>0 before emitting a pulse. q->_dir_delay_active = false; - q->_pulse_high = true; - q->_step_port->BSRR = q->_step_set_mask; - *q->_ccr_reg = TIM2->CNT + STEP_PULSE_WIDTH_TICKS; + + uint8_t rp = q->read_idx; + struct queue_entry* e = &q->entry[rp & QUEUE_LEN_MASK]; + if (e->steps > 0) { + // Real step: start pulse (set pin HIGH) + q->_pulse_high = true; + q->_step_port->BSRR = q->_step_set_mask; + fas_tim_set_ccr(q->_ccr_reg, STEP_PULSE_WIDTH_TICKS); + } else { + // Pure pause (steps=0): skip pulse, schedule entry ticks + q->_pulse_high = false; + fas_tim_set_ccr(q->_ccr_reg, e->ticks); + } } else { // ====== Phase 1: pulse start ====== - // Set step pin HIGH, schedule LOW after STEP_PULSE_WIDTH_TICKS. - q->_pulse_high = true; - q->_step_port->BSRR = q->_step_set_mask; - *q->_ccr_reg = TIM2->CNT + STEP_PULSE_WIDTH_TICKS; + uint8_t rp = q->read_idx; + struct queue_entry* e = &q->entry[rp & QUEUE_LEN_MASK]; + if (e->steps > 0) { + // Set step pin HIGH, schedule LOW after STEP_PULSE_WIDTH_TICKS. + q->_pulse_high = true; + q->_step_port->BSRR = q->_step_set_mask; + fas_tim_set_ccr(q->_ccr_reg, STEP_PULSE_WIDTH_TICKS); + } else { + // steps=0: skip pulse, schedule entry ticks directly. + // On next ISR, _pulse_high=false and _dir_delay_active=false, + // so it will re-enter Phase 1 and advance to next entry. + q->_pulse_high = false; + fas_tim_set_ccr(q->_ccr_reg, e->ticks); + } } } // Clear all processed flags at once (rc_w0: bits set to 1 are ignored) - TIM2->SR = ~ch_processed; + FAS_TIMER->SR = ~ch_processed; // Trigger cyclic queue fill cyclic_check_and_pend(); @@ -332,7 +521,7 @@ void TIM2_IRQHandler(void) { #if !defined(DISABLE_FAS_PENDSV) __attribute__((weak)) void PendSV_Handler(void) { _cyclic_pending = false; - __DMB(); + FAS_DMB(); if (fas_engine) { fas_engine->manageSteppers(); } @@ -370,8 +559,12 @@ void fas_init_engine(FastAccelStepperEngine* engine) { // (SUPPORT_LOG2_TIMER_FREQ_VARIABLES path in RampCalculator.h) init_ramp_module(); + // Print clock error warning (if any) — user must have called Serial.begin() + // before engine.init() for this to appear. + fas_stm32_report_clock_error(); + // PendSV at lowest priority — avoids blocking higher-priority interrupts NVIC_SetPriority(PendSV_IRQn, 0xFF); } -#endif /* ARDUINO_ARCH_STM32 */ \ No newline at end of file +#endif /* ARDUINO_ARCH_STM32 */ diff --git a/src/pd_stm32/stm32_queue.h b/src/pd_stm32/stm32_queue.h index 87ecdd92..0794836d 100644 --- a/src/pd_stm32/stm32_queue.h +++ b/src/pd_stm32/stm32_queue.h @@ -37,7 +37,7 @@ class StepperQueue : public StepperQueueBase { #include "../fas_queue/protocol.h" volatile bool _isRunning; - bool _initialized; + // bool _initialized was removed — set but never read (fix_plan_v3 FIX #7) // Step pin GPIO uint8_t _step_pin; @@ -73,7 +73,7 @@ class StepperQueue : public StepperQueueBase { _ccr_reg = NULL; _timer_ch = 0; _isRunning = false; - _initialized = false; + // _initialized = false was removed (fix_plan_v3 FIX #7) _pulse_high = false; _dir_delay_active = false; max_speed_in_ticks = STEP_PULSE_WIDTH_TICKS * 4; @@ -82,11 +82,19 @@ class StepperQueue : public StepperQueueBase { inline bool isRunning() const { return _isRunning; } inline bool isReadyForCommands() const { return true; } + // setDirPin — configure direction pin for atomic BSRR access + // Validates digitalPinToPort() before dereferencing. + // If port is NULL (invalid pin), _dir_bsrr stays NULL → SET_DIRECTION_PIN_STATE + // will be a no-op (safe, queued direction change fails silently). void setDirPin(uint8_t dir_pin, bool _dirHighCountsUp) { dirPin = dir_pin; dirHighCountsUp = _dirHighCountsUp; if ((dir_pin != PIN_UNDEFINED) && ((dir_pin & PIN_EXTERNAL_FLAG) == 0)) { GPIO_TypeDef* port = digitalPinToPort(dir_pin); + if (!port) { // Invalid pin → BSRR stays NULL, SET_DIRECTION_PIN_STATE becomes no-op + _dir_bsrr = NULL; + return; + } uint32_t mask = digitalPinToBitMask(dir_pin); _dir_bsrr = &port->BSRR; _dir_set_mask = mask; @@ -95,9 +103,6 @@ class StepperQueue : public StepperQueueBase { } void adjustSpeedToStepperCount(uint8_t steppers); - - private: - static StepperQueue* allocateSlot(uint8_t step_pin, uint8_t timer_ch); }; // ---- Direction pin: atomic via BSRR/BRR ---- @@ -114,6 +119,9 @@ class StepperQueue : public StepperQueueBase { digitalWrite((pin), (high) ? HIGH : LOW) // ---- Direction-to-pulse delay ---- +// Guard allows override from build_flags (-DAFTER_SET_DIR_PIN_DELAY_US=50) +#ifndef AFTER_SET_DIR_PIN_DELAY_US #define AFTER_SET_DIR_PIN_DELAY_US 30 +#endif #endif /* PD_STM32_QUEUE_H */ \ No newline at end of file From 9f219788f74e964102701d0066fbd6fa42b5093f Mon Sep 17 00:00:00 2001 From: KienTranCNC Date: Sat, 16 May 2026 17:00:29 +0700 Subject: [PATCH 4/5] debug round 2 --- .../build_arduino_examples_matrix.yml | 9 ++--- .../workflows/build_idf_examples_matrix.yml | 4 --- extras/ci/build_matrix.yaml | 36 +++++++++++++++++++ extras/ci/platformio.ini | 35 ++++++++++++++++++ src/pd_stm32/pd_config.h | 9 +++-- src/pd_stm32/stm32_queue.cpp | 2 +- src/pd_stm32/stm32_queue.h | 2 +- 7 files changed, 84 insertions(+), 13 deletions(-) diff --git a/.github/workflows/build_arduino_examples_matrix.yml b/.github/workflows/build_arduino_examples_matrix.yml index 190a39c2..cd66d5d1 100644 --- a/.github/workflows/build_arduino_examples_matrix.yml +++ b/.github/workflows/build_arduino_examples_matrix.yml @@ -10,10 +10,6 @@ on: pull_request: branches: [ master ] -concurrency: - group: ${{ github.workflow }}-${{ github.ref }} - cancel-in-progress: true - jobs: build: strategy: @@ -80,6 +76,11 @@ jobs: - atmelsam - rpipico - rpipico2 + - bluepill_f103c8 + - nucleo_g070rb + - blackpill_f401cc + - nucleo_h743zi + - nucleo_l476rg runs-on: ubuntu-latest diff --git a/.github/workflows/build_idf_examples_matrix.yml b/.github/workflows/build_idf_examples_matrix.yml index fda5b6e8..a5976fc5 100644 --- a/.github/workflows/build_idf_examples_matrix.yml +++ b/.github/workflows/build_idf_examples_matrix.yml @@ -15,10 +15,6 @@ on: pull_request: branches: [ master ] -concurrency: - group: ${{ github.workflow }}-${{ github.ref }} - cancel-in-progress: true - jobs: build: strategy: diff --git a/extras/ci/build_matrix.yaml b/extras/ci/build_matrix.yaml index 505b3900..f64d20af 100644 --- a/extras/ci/build_matrix.yaml +++ b/extras/ci/build_matrix.yaml @@ -152,6 +152,12 @@ templates: framework: arduino lib_extra_dirs: . + stm32: + platform: ststm32 + framework: arduino + build_flags: ["-Wall"] + lib_extra_dirs: . + environments: esp32: template: esp32_arduino @@ -285,6 +291,31 @@ environments: board_upload_psram_length: 1048576 build_flags: ["-D__FREERTOS=1"] + bluepill_f103c8: + template: stm32 + board: bluepill_f103c8 + build_flags_extra: ["-DTICKS_PER_S=72000000UL"] + + nucleo_g070rb: + template: stm32 + board: nucleo_g070rb + build_flags_extra: ["-DTICKS_PER_S=64000000UL"] + + blackpill_f401cc: + template: stm32 + board: blackpill_f401cc + build_flags_extra: ["-DTICKS_PER_S=84000000UL"] + + nucleo_h743zi: + template: stm32 + board: nucleo_h743zi + build_flags_extra: ["-DTICKS_PER_S=240000000UL"] + + nucleo_l476rg: + template: stm32 + board: nucleo_l476rg + build_flags_extra: ["-DTICKS_PER_S=80000000UL"] + versioned_environments: esp32: prefix: esp32 @@ -429,6 +460,11 @@ workflows: - atmelsam - rpipico - rpipico2 + - bluepill_f103c8 + - nucleo_g070rb + - blackpill_f401cc + - nucleo_h743zi + - nucleo_l476rg script: build-platformio.sh idf: diff --git a/extras/ci/platformio.ini b/extras/ci/platformio.ini index b2b7e132..4c2a98fb 100644 --- a/extras/ci/platformio.ini +++ b/extras/ci/platformio.ini @@ -195,6 +195,41 @@ build_flags = -D__FREERTOS=1 lib_extra_dirs = . board_upload.psram_length = 1048576 +[env:bluepill_f103c8] +platform = ststm32 +board = bluepill_f103c8 +framework = arduino +build_flags = -Wall -DTICKS_PER_S=72000000UL +lib_extra_dirs = . + +[env:nucleo_g070rb] +platform = ststm32 +board = nucleo_g070rb +framework = arduino +build_flags = -Wall -DTICKS_PER_S=64000000UL +lib_extra_dirs = . + +[env:blackpill_f401cc] +platform = ststm32 +board = blackpill_f401cc +framework = arduino +build_flags = -Wall -DTICKS_PER_S=84000000UL +lib_extra_dirs = . + +[env:nucleo_h743zi] +platform = ststm32 +board = nucleo_h743zi +framework = arduino +build_flags = -Wall -DTICKS_PER_S=240000000UL +lib_extra_dirs = . + +[env:nucleo_l476rg] +platform = ststm32 +board = nucleo_l476rg +framework = arduino +build_flags = -Wall -DTICKS_PER_S=80000000UL +lib_extra_dirs = . + [env:esp32_V6_13_0] platform = espressif32 @ 6.13.0 board = esp32dev diff --git a/src/pd_stm32/pd_config.h b/src/pd_stm32/pd_config.h index e4fef21a..b9dd8589 100644 --- a/src/pd_stm32/pd_config.h +++ b/src/pd_stm32/pd_config.h @@ -9,11 +9,14 @@ // User MUST define TICKS_PER_S matching their board's TIM2 counter clock. // TIM2 counter clock = PCLK1 * (APB1_prescaler==1 ? 1 : 2) // -// Examples: +// Examples (all with PLL enabled): // STM32F103 @72MHz: TICKS_PER_S = 72000000 (APB1=36MHz ×2) // STM32F407 @168MHz: TICKS_PER_S = 84000000 (APB1=42MHz ×2) -// STM32G0 @64MHz: TICKS_PER_S = 64000000 (APB1=64MHz ×1) -// STM32H743 @480MHz: TICKS_PER_S = 240000000 (APB1=120MHz ×2) +// STM32G0 @64MHz (with PLL): TICKS_PER_S = 64000000 (APB1=64MHz ×1) +// STM32H743 @480MHz (with PLL): TICKS_PER_S = 240000000 (APB1=120MHz ×2) +// STM32C031 @48MHz (with PLL): TICKS_PER_S = 48000000 (APB1=48MHz ×1, TIM3) +// STM32F091 @48MHz (with PLL): TICKS_PER_S = 48000000 (APB1=48MHz ×1, TIM2) +// STM32L073 @32MHz (with PLL): TICKS_PER_S = 32000000 (APB1=32MHz ×1, TIM2) // ==================================================================== #ifndef TICKS_PER_S #define TICKS_PER_S 72000000UL diff --git a/src/pd_stm32/stm32_queue.cpp b/src/pd_stm32/stm32_queue.cpp index 17f5cacf..fd18ca39 100644 --- a/src/pd_stm32/stm32_queue.cpp +++ b/src/pd_stm32/stm32_queue.cpp @@ -218,7 +218,7 @@ void StepperQueue::init(uint8_t queue_num, uint8_t step_pin) { static const uint8_t ch_map[4] = {0, 1, 2, 3}; _timer_ch = ch_map[queue_num]; - // Ensure TIM2 is initialized + // Ensure step timer is initialized (TIM2 / TIM3 on C0) initStepTimer(); // Step pin GPIO configuration — validate port first diff --git a/src/pd_stm32/stm32_queue.h b/src/pd_stm32/stm32_queue.h index 0794836d..88d342f5 100644 --- a/src/pd_stm32/stm32_queue.h +++ b/src/pd_stm32/stm32_queue.h @@ -51,7 +51,7 @@ class StepperQueue : public StepperQueueBase { uint32_t _dir_clr_mask; // BSRR high bits = set LOW (mask << 16) // Timer - volatile uint32_t* _ccr_reg; // &TIM2->CCR1/2/3/4 + volatile uint32_t* _ccr_reg; // &FAS_TIMER->CCR1/2/3/4 (TIM2 or TIM3 on C0) uint8_t _timer_ch; // 0..3 // Pulse tracking From b8aa3089a975c3e8f98590700e9f1ea94dfa5c00 Mon Sep 17 00:00:00 2001 From: KienTranCNC Date: Sat, 16 May 2026 21:13:21 +0700 Subject: [PATCH 5/5] bug fix round 3 --- src/pd_stm32/pd_config.h | 9 +++++---- src/pd_stm32/stm32_queue.cpp | 3 ++- 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/src/pd_stm32/pd_config.h b/src/pd_stm32/pd_config.h index b9dd8589..2bf5a456 100644 --- a/src/pd_stm32/pd_config.h +++ b/src/pd_stm32/pd_config.h @@ -6,8 +6,8 @@ // ==================================================================== // Compile-time TICKS_PER_S // -// User MUST define TICKS_PER_S matching their board's TIM2 counter clock. -// TIM2 counter clock = PCLK1 * (APB1_prescaler==1 ? 1 : 2) +// User MUST define TICKS_PER_S matching their board's TIM2 (or TIM3 on C0) counter clock. +// Timer counter clock = PCLK1 * (APB1_prescaler==1 ? 1 : 2) // // Examples (all with PLL enabled): // STM32F103 @72MHz: TICKS_PER_S = 72000000 (APB1=36MHz ×2) @@ -31,17 +31,18 @@ #ifndef STEP_PULSE_WIDTH_US #define STEP_PULSE_WIDTH_US 6 #endif -#define STEP_PULSE_WIDTH_TICKS ((uint32_t)(STEP_PULSE_WIDTH_US * (TICKS_PER_S / 1000000L))) +#define STEP_PULSE_WIDTH_TICKS ((uint32_t)(STEP_PULSE_WIDTH_US * (TICKS_PER_S / 1000000UL))) // ---- Timing constants ---- #define MIN_CMD_TICKS (TICKS_PER_S / 5000) #define MIN_DIR_DELAY_US 200 -#define MAX_DIR_DELAY_US (65535 / (TICKS_PER_S / 1000000)) +#define MAX_DIR_DELAY_US (65535 / (TICKS_PER_S / 1000000UL)) #define DELAY_MS_BASE 2 #define CYCLIC_INTERVAL_MS 3 // ==================================================================== // NOTE: STM32F1 TIM2 is 16-bit only. +// C0 TIM3 is also 16-bit (ARR=0xFFFF). Min speed @48MHz ≈ 733 steps/s. // ARR = 0xFFFFFFFF is masked to 0xFFFF by F1 hardware. // Minimum speed = TICKS_PER_S / 65536 ≈ 1098 steps/s @72MHz. // ==================================================================== diff --git a/src/pd_stm32/stm32_queue.cpp b/src/pd_stm32/stm32_queue.cpp index fd18ca39..c7f5d9a5 100644 --- a/src/pd_stm32/stm32_queue.cpp +++ b/src/pd_stm32/stm32_queue.cpp @@ -243,6 +243,7 @@ void StepperQueue::init(uint8_t queue_num, uint8_t step_pin) { #if defined(STM32C0xx) volatile uint32_t* ccr[] = {&FAS_TIMER->CCR1, &FAS_TIMER->CCR2, &FAS_TIMER->CCR3, &FAS_TIMER->CCR4}; #else + // non-C0 branch: FAS_TIMER resolves to TIM2, so hardcoding TIM2 is intentional. volatile uint32_t* ccr[] = {&TIM2->CCR1, &TIM2->CCR2, &TIM2->CCR3, &TIM2->CCR4}; #endif _ccr_reg = ccr[_timer_ch]; @@ -321,7 +322,7 @@ bool StepperQueue::getActualTicksWithDirection( // ==================================================================== // Cyclic PendSV trigger -// Called at end of TIM2_IRQHandler every ~3ms. +// Called at end of FAS_TIMER_IRQHandler every ~3ms. // Triggers PendSV exception to fill queues without consuming ISR time. // ==================================================================== static void cyclic_check_and_pend(void) {