From c52139626f09f5545bef4b1c79b507093969d4e3 Mon Sep 17 00:00:00 2001 From: ellensp <530024+ellensp@users.noreply.github.com> Date: Thu, 16 Feb 2023 12:58:51 +1300 Subject: [PATCH] =?UTF-8?q?=F0=9F=90=9B=20Fix=20FastPWM=20calculations=20(?= =?UTF-8?q?#25343)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Scott Lahteine --- Marlin/src/HAL/AVR/fast_pwm.cpp | 27 ++++++++++++++++++----- Marlin/src/HAL/ESP32/HAL.cpp | 34 ++++++++++++++--------------- Marlin/src/HAL/STM32F1/fast_pwm.cpp | 4 ++-- 3 files changed, 40 insertions(+), 25 deletions(-) diff --git a/Marlin/src/HAL/AVR/fast_pwm.cpp b/Marlin/src/HAL/AVR/fast_pwm.cpp index d361aaab38f3..0b2b8fd0b3a4 100644 --- a/Marlin/src/HAL/AVR/fast_pwm.cpp +++ b/Marlin/src/HAL/AVR/fast_pwm.cpp @@ -23,6 +23,10 @@ #include "../../inc/MarlinConfig.h" +//#define DEBUG_AVR_FAST_PWM +#define DEBUG_OUT ENABLED(DEBUG_AVR_FAST_PWM) +#include "../../core/debug_out.h" + struct Timer { volatile uint8_t* TCCRnQ[3]; // max 3 TCCR registers per timer volatile uint16_t* OCRnQ[3]; // max 3 OCR registers per timer @@ -108,12 +112,15 @@ const Timer get_pwm_timer(const pin_t pin) { } void MarlinHAL::set_pwm_frequency(const pin_t pin, const uint16_t f_desired) { + DEBUG_ECHOLNPGM("set_pwm_frequency(pin=", pin, ", freq=", f_desired, ")"); const Timer timer = get_pwm_timer(pin); if (timer.isProtected || !timer.isPWM) return; // Don't proceed if protected timer or not recognized const bool is_timer2 = timer.n == 2; const uint16_t maxtop = is_timer2 ? 0xFF : 0xFFFF; + DEBUG_ECHOLNPGM("maxtop=", maxtop); + uint16_t res = 0xFF; // resolution (TOP value) uint8_t j = CS_NONE; // prescaler index uint8_t wgm = WGM_PWM_PC_8; // waveform generation mode @@ -121,23 +128,29 @@ void MarlinHAL::set_pwm_frequency(const pin_t pin, const uint16_t f_desired) { // Calculating the prescaler and resolution to use to achieve closest frequency if (f_desired != 0) { constexpr uint16_t prescaler[] = { 1, 8, (32), 64, (128), 256, 1024 }; // (*) are Timer 2 only - uint16_t f = (F_CPU) / (2 * 1024 * maxtop) + 1; // Start with the lowest non-zero frequency achievable (1 or 31) + uint16_t f = (F_CPU) / (uint32_t(maxtop) << 11) + 1; // Start with the lowest non-zero frequency achievable (for 16MHz, 1 or 31) + DEBUG_ECHOLNPGM("f=", f); + DEBUG_ECHOLNPGM("(prescaler loop)"); LOOP_L_N(i, COUNT(prescaler)) { // Loop through all prescaler values - const uint16_t p = prescaler[i]; + const uint32_t p = prescaler[i]; // Extend to 32 bits for calculations + DEBUG_ECHOLNPGM("prescaler[", i, "]=", p); uint16_t res_fast_temp, res_pc_temp; if (is_timer2) { #if ENABLED(USE_OCR2A_AS_TOP) // No resolution calculation for TIMER2 unless enabled USE_OCR2A_AS_TOP const uint16_t rft = (F_CPU) / (p * f_desired); res_fast_temp = rft - 1; res_pc_temp = rft / 2; + DEBUG_ECHOLNPGM("(Timer2) res_fast_temp=", res_fast_temp, " res_pc_temp=", res_pc_temp); #else res_fast_temp = res_pc_temp = maxtop; + DEBUG_ECHOLNPGM("(Timer2) res_fast_temp=", maxtop, " res_pc_temp=", maxtop); #endif } else { if (p == 32 || p == 128) continue; // Skip TIMER2 specific prescalers when not TIMER2 const uint16_t rft = (F_CPU) / (p * f_desired); + DEBUG_ECHOLNPGM("(Not Timer 2) F_CPU=" STRINGIFY(F_CPU), " prescaler=", p, " f_desired=", f_desired); res_fast_temp = rft - 1; res_pc_temp = rft / 2; } @@ -147,23 +160,27 @@ void MarlinHAL::set_pwm_frequency(const pin_t pin, const uint16_t f_desired) { // Calculate frequencies of test prescaler and resolution values const uint16_t f_fast_temp = (F_CPU) / (p * (1 + res_fast_temp)), - f_pc_temp = (F_CPU) / (2 * p * res_pc_temp); - const int f_diff = _MAX(f, f_desired) - _MIN(f, f_desired), + f_pc_temp = (F_CPU) / ((p * res_pc_temp) << 1), + f_diff = _MAX(f, f_desired) - _MIN(f, f_desired), f_fast_diff = _MAX(f_fast_temp, f_desired) - _MIN(f_fast_temp, f_desired), f_pc_diff = _MAX(f_pc_temp, f_desired) - _MIN(f_pc_temp, f_desired); + DEBUG_ECHOLNPGM("f_fast_temp=", f_fast_temp, " f_pc_temp=", f_pc_temp, " f_diff=", f_diff, " f_fast_diff=", f_fast_diff, " f_pc_diff=", f_pc_diff); + if (f_fast_diff < f_diff && f_fast_diff <= f_pc_diff) { // FAST values are closest to desired f // Set the Wave Generation Mode to FAST PWM wgm = is_timer2 ? uint8_t(TERN(USE_OCR2A_AS_TOP, WGM2_FAST_PWM_OCR2A, WGM2_FAST_PWM)) : uint8_t(WGM_FAST_PWM_ICRn); // Remember this combination f = f_fast_temp; res = res_fast_temp; j = i + 1; + DEBUG_ECHOLNPGM("(FAST) updated f=", f); } else if (f_pc_diff < f_diff) { // PHASE CORRECT values are closes to desired f // Set the Wave Generation Mode to PWM PHASE CORRECT wgm = is_timer2 ? uint8_t(TERN(USE_OCR2A_AS_TOP, WGM2_PWM_PC_OCR2A, WGM2_PWM_PC)) : uint8_t(WGM_PWM_PC_ICRn); f = f_pc_temp; res = res_pc_temp; j = i + 1; + DEBUG_ECHOLNPGM("(PHASE) updated f=", f); } - } + } // prescaler loop } _SET_WGMnQ(timer, wgm); diff --git a/Marlin/src/HAL/ESP32/HAL.cpp b/Marlin/src/HAL/ESP32/HAL.cpp index 29f3be3c028a..46dd4e761b8c 100644 --- a/Marlin/src/HAL/ESP32/HAL.cpp +++ b/Marlin/src/HAL/ESP32/HAL.cpp @@ -342,16 +342,16 @@ void MarlinHAL::set_pwm_duty(const pin_t pin, const uint16_t v, const uint16_t v } else pindata.pwm_duty_ticks = duty; // PWM duty count = # of 4µs ticks per full PWM cycle + + return; } - else #endif - { - const int8_t cid = get_pwm_channel(pin, PWM_FREQUENCY, PWM_RESOLUTION); - if (cid >= 0) { - const uint32_t duty = map(invert ? v_size - v : v, 0, v_size, 0, _BV(PWM_RESOLUTION)-1); - ledcWrite(cid, duty); - } - } + + const int8_t cid = get_pwm_channel(pin, PWM_FREQUENCY, PWM_RESOLUTION); + if (cid >= 0) { + const uint32_t duty = map(invert ? v_size - v : v, 0, v_size, 0, _BV(PWM_RESOLUTION)-1); + ledcWrite(cid, duty); + } } int8_t MarlinHAL::set_pwm_frequency(const pin_t pin, const uint32_t f_desired) { @@ -360,17 +360,15 @@ int8_t MarlinHAL::set_pwm_frequency(const pin_t pin, const uint32_t f_desired) { pwm_pin_data[pin & 0x7F].pwm_cycle_ticks = 1000000UL / f_desired / 4; // # of 4µs ticks per full PWM cycle return 0; } - else #endif - { - const int8_t cid = channel_for_pin(pin); - if (cid >= 0) { - if (f_desired == ledcReadFreq(cid)) return cid; // no freq change - ledcDetachPin(chan_pin[cid]); - chan_pin[cid] = 0; // remove old freq channel - } - return get_pwm_channel(pin, f_desired, PWM_RESOLUTION); // try for new one - } + + const int8_t cid = channel_for_pin(pin); + if (cid >= 0) { + if (f_desired == ledcReadFreq(cid)) return cid; // no freq change + ledcDetachPin(chan_pin[cid]); + chan_pin[cid] = 0; // remove old freq channel + } + return get_pwm_channel(pin, f_desired, PWM_RESOLUTION); // try for new one } // use hardware PWM if avail, if not then ISR diff --git a/Marlin/src/HAL/STM32F1/fast_pwm.cpp b/Marlin/src/HAL/STM32F1/fast_pwm.cpp index 297804a3ac42..c3f96f0f925f 100644 --- a/Marlin/src/HAL/STM32F1/fast_pwm.cpp +++ b/Marlin/src/HAL/STM32F1/fast_pwm.cpp @@ -39,7 +39,7 @@ inline uint8_t timer_and_index_for_pin(const pin_t pin, timer_dev **timer_ptr) { void MarlinHAL::set_pwm_duty(const pin_t pin, const uint16_t v, const uint16_t v_size/*=255*/, const bool invert/*=false*/) { const uint16_t duty = invert ? v_size - v : v; if (PWM_PIN(pin)) { - timer_dev *timer; UNUSED(timer); + timer_dev *timer; if (timer_freq[timer_and_index_for_pin(pin, &timer)] == 0) set_pwm_frequency(pin, PWM_FREQUENCY); const uint8_t channel = PIN_MAP[pin].timer_channel; @@ -55,7 +55,7 @@ void MarlinHAL::set_pwm_duty(const pin_t pin, const uint16_t v, const uint16_t v void MarlinHAL::set_pwm_frequency(const pin_t pin, const uint16_t f_desired) { if (!PWM_PIN(pin)) return; // Don't proceed if no hardware timer - timer_dev *timer; UNUSED(timer); + timer_dev *timer; timer_freq[timer_and_index_for_pin(pin, &timer)] = f_desired; // Protect used timers