From 777bc842de1e81858985200cf024967036db36ae Mon Sep 17 00:00:00 2001 From: narno2202 <130909513+narno2202@users.noreply.github.com> Date: Mon, 5 Jan 2026 00:17:01 +0100 Subject: [PATCH] =?UTF-8?q?=E2=9A=A1=EF=B8=8F=20Optimize=20FTM=20resonance?= =?UTF-8?q?=20test=20(#28263)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Scott Lahteine --- .../module/ft_motion/resonance_generator.cpp | 43 ++++++++++++++----- .../module/ft_motion/resonance_generator.h | 38 +++++++++------- 2 files changed, 55 insertions(+), 26 deletions(-) diff --git a/Marlin/src/module/ft_motion/resonance_generator.cpp b/Marlin/src/module/ft_motion/resonance_generator.cpp index 2c2297e312..40fba11d67 100644 --- a/Marlin/src/module/ft_motion/resonance_generator.cpp +++ b/Marlin/src/module/ft_motion/resonance_generator.cpp @@ -50,13 +50,33 @@ void ResonanceGenerator::reset() { done = false; } +// Fast sine approximation +float ResonanceGenerator::fast_sin(float x) { + static constexpr float INV_TAU = (1.0f / M_TAU); + + // Reduce the angle to [-π, π] + const float y = x * INV_TAU; // Multiples of 2π + int k = static_cast(y); // Truncates toward zero + + // Negative? The truncation is one too high. + if (y < 0.0f) --k; // Correct for negatives + + const float r = x - k * M_TAU; // -π <= r <= π + + // Cheap polynomial approximation of sin(r) + return r * (1.27323954f - 0.405284735f * ABS(r)); +} + void ResonanceGenerator::fill_stepper_plan_buffer() { - xyze_float_t traj_coords = {}; + xyze_float_t traj_coords = rt_params.start_pos; + + const float amplitude_precalc = (rt_params.amplitude_correction * rt_params.accel_per_hz * 0.25f) / sq(M_PI); + + float rt_factor = rt_time * M_TAU; while (!ftMotion.stepping.is_full()) { // Calculate current frequency - // Logarithmic approach with duration per octave - const float freq = rt_params.min_freq * powf(2.0f, rt_time / rt_params.octave_duration); + const float freq = getFrequencyFromTimeline(); if (freq > rt_params.max_freq) { done = true; return; @@ -65,22 +85,23 @@ void ResonanceGenerator::fill_stepper_plan_buffer() { // 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 = rt_params.amplitude_correction * rt_params.accel_per_hz * 0.25f / (sq(M_PI) * freq); + const float amplitude = amplitude_precalc / freq; // Phase in radians - const float phase = 2.0f * M_PI * freq * rt_time; + const float phase = freq * rt_factor; // Position Offset : between -A and +A - const float pos_offset = amplitude * sinf(phase); + const float pos_offset = amplitude * fast_sin(phase); - // Set base position and apply offset to the test axis in one step for all axes - #define _SET_TRAJ(A) traj_coords.A = rt_params.start_pos.A + (rt_params.axis == A##_AXIS ? pos_offset : 0.0f); - LOGICAL_AXIS_MAP(_SET_TRAJ); + // Resonate the axis being tested + traj_coords[rt_params.axis] = rt_params.start_pos[rt_params.axis] + pos_offset; + + // Increment for the next point (before calling out) + rt_time += FTM_TS; + rt_factor += FTM_TS * M_TAU; // Store in buffer ftMotion.stepping_enqueue(traj_coords); - // Increment time for the next point - rt_time += FTM_TS; } } diff --git a/Marlin/src/module/ft_motion/resonance_generator.h b/Marlin/src/module/ft_motion/resonance_generator.h index 16156693dd..5acc1dc415 100644 --- a/Marlin/src/module/ft_motion/resonance_generator.h +++ b/Marlin/src/module/ft_motion/resonance_generator.h @@ -25,14 +25,18 @@ #include +#ifndef M_TAU + #define M_TAU (2.0f * M_PI) +#endif + typedef struct FTMResonanceTestParams { - AxisEnum axis = NO_AXIS_ENUM; // Axis to test - float min_freq = 5.0f; // Minimum frequency [Hz] - float max_freq = 100.0f; // Maximum frequency [Hz] - float octave_duration = 40.0f; // Octave duration for logarithmic progression - float accel_per_hz = 60.0f; // Acceleration per Hz [mm/sec/Hz] or [g/Hz] - int16_t amplitude_correction = 5; // Amplitude correction factor - xyze_pos_t start_pos; // Initial stepper position + AxisEnum axis = NO_AXIS_ENUM; // Axis to test + float min_freq = 5.0f; // Minimum frequency [Hz] + float max_freq = 100.0f; // Maximum frequency [Hz] + float octave_duration = 40.0f; // Octave duration for logarithmic progression + float accel_per_hz = 60.0f; // Acceleration per Hz [mm/sec/Hz] or [g/Hz] + int16_t amplitude_correction = 5; // Amplitude correction factor + xyze_pos_t start_pos; // Initial stepper position } ftm_resonance_test_params_t; class ResonanceGenerator { @@ -53,21 +57,25 @@ class ResonanceGenerator { done = false; } + // Return frequency based on timeline float getFrequencyFromTimeline() { - return (rt_params.min_freq * powf(2.0f, timeline / rt_params.octave_duration)); // Return frequency based on timeline + // Logarithmic approach with duration per octave + return rt_params.min_freq * 2.0f * exp2f((rt_time / rt_params.octave_duration) - 1); } void fill_stepper_plan_buffer(); // Fill stepper plan buffer with trajectory points + void setActive(const bool state) { active = state; } bool isActive() const { return active; } - bool isDone() const { return done; } - void setActive(bool state) { active = state; } - void setDone(bool state) { done = state; } - void abort(); // Abort resonance test + void setDone(const bool state) { done = state; } + bool isDone() const { return done; } + + void abort(); // Abort resonance test private: - static float rt_time; // Test timer - static bool active; // Resonance test active - static bool done; // Resonance test done + float fast_sin(float x); // Fast sine approximation + static float rt_time; // Test timer + static bool active; // Resonance test active + static bool done; // Resonance test done };