🐛 Fix FastPWM calculations (#25343)

Co-authored-by: Scott Lahteine <thinkyhead@users.noreply.github.com>

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);
+                     f_pc_temp   = (F_CPU) / ((p * res_pc_temp) << 1),
-      const int      f_diff      = _MAX(f, f_desired) - _MIN(f, f_desired),
+                     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);

Marlin/src/HAL/ESP32/HAL.cpp

@@ -342,17 +342,17 @@ 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);
   }
 }
-}
 
 int8_t MarlinHAL::set_pwm_frequency(const pin_t pin, const uint32_t f_desired) {
   #if ENABLED(I2S_STEPPER_STREAM)
@@ -360,9 +360,8 @@ 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
@@ -371,7 +370,6 @@ int8_t MarlinHAL::set_pwm_frequency(const pin_t pin, const uint32_t f_desired) {
   }
   return get_pwm_channel(pin, f_desired, PWM_RESOLUTION); // try for new one
 }
-}
 
 // use hardware PWM if avail, if not then ISR
 void analogWrite(const pin_t pin, const uint16_t value, const uint32_t freq/*=PWM_FREQUENCY*/, const uint16_t res/*=8*/) { // always 8 bit resolution!

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