FTM: fix resonance test motion and optimize compute

Marlin/src/module/ft_motion/resonance_generator.cpp

@@ -61,7 +61,11 @@ float ResonanceGenerator::fast_sin(float x) {
   // Negative? The truncation is one too high.
   if (y < 0.0f) --k;                // Correct for negatives
 
-  const float r = x - k * M_TAU;    // -π <= r <= π
+  float r = x - k * M_TAU;    // -π <= r <= π
+  if (r > M_PI)
+    r -= M_TAU;
+  else if (r < -M_PI)
+    r += M_TAU;
 
   // Cheap polynomial approximation of sin(r)
   return r * (1.27323954f - 0.405284735f * ABS(r));
@@ -76,19 +80,19 @@ void ResonanceGenerator::fill_stepper_plan_buffer() {
 
   while (!ftMotion.stepping.is_full()) {
     // Calculate current frequency
-    const float freq = getFrequencyFromTimeline();
-    if (freq > rt_params.max_freq) {
+    current_freq *= freq_mul;
+    if (current_freq > rt_params.max_freq) {
       done = true;
       return;
     }
 
     // Amplitude based on a sinusoidal wave : A = accel / (4 * PI^2 * f^2)
     //const float accel_magnitude = rt_params.accel_per_hz * freq;
     //const float amplitude = rt_params.amplitude_correction * accel_magnitude / (4.0f * sq(M_PI) * sq(freq));
-    const float amplitude = amplitude_precalc / freq;
+    const float amplitude = amplitude_precalc / current_freq;
 
     // Phase in radians
-    const float phase = freq * rt_factor;
+    const float phase = current_freq * rt_factor;
 
     // Position Offset : between -A and +A
     const float pos_offset = amplitude * fast_sin(phase);

Marlin/src/module/ft_motion/resonance_generator.h

@@ -55,12 +55,16 @@ class ResonanceGenerator {
       rt_time = t;
       active = true;
       done = false;
+      //Precompute frequency multiplier
+      current_freq = rt_params.min_freq;
+      const float inv_octave_duration = 1.0f / rt_params.octave_duration;
+      freq_mul = exp2f(FTM_TS * inv_octave_duration);
     }
 
     // Return frequency based on timeline
     float getFrequencyFromTimeline() {
       // Logarithmic approach with duration per octave
-      return rt_params.min_freq * 2.0f * exp2f((rt_time / rt_params.octave_duration) - 1);
+      return rt_params.min_freq * exp2f(timeline / rt_params.octave_duration);
     }
 
     void fill_stepper_plan_buffer();                // Fill stepper plan buffer with trajectory points
@@ -76,6 +80,8 @@ class ResonanceGenerator {
   private:
     float fast_sin(float x);  // Fast sine approximation
     static float rt_time;     // Test timer
+    float freq_mul;           // Frequency multiplier for sine sweeping
+    float current_freq;       // Current frequency being generated in sinusoidal motion
     static bool active;       // Resonance test active
     static bool done;         // Resonance test done
 };