diff --git a/Marlin/2Configuration.h b/Marlin/2Configuration.h index 75f5fac735..a6122eba66 100644 --- a/Marlin/2Configuration.h +++ b/Marlin/2Configuration.h @@ -2908,7 +2908,7 @@ // // Note: Usually sold with a white PCB. // -//#define REPRAP_DISCOUNT_SMART_CONTROLLER +#define REPRAP_DISCOUNT_SMART_CONTROLLER // // GT2560 (YHCB2004) LCD Display @@ -3320,7 +3320,7 @@ * * :[ 'ORIGIN', 'FYSETC', 'HYPRECY', 'MKS', 'RELOADED', 'IA_CREALITY', 'E3S1PRO' ] */ -#define DGUS_LCD_UI MKS +//#define DGUS_LCD_UI MKS #if DGUS_UI_IS(MKS) #define USE_MKS_GREEN_UI #elif DGUS_UI_IS(IA_CREALITY) diff --git a/Marlin/src/feature/resonance/resonance_generator.cpp b/Marlin/src/feature/resonance/resonance_generator.cpp index 1488c0f1a2..36f38159f3 100644 --- a/Marlin/src/feature/resonance/resonance_generator.cpp +++ b/Marlin/src/feature/resonance/resonance_generator.cpp @@ -33,12 +33,9 @@ resonance_test_params_t ResonanceGenerator::rt_params; // Resonance test parameters -bool ResonanceGenerator::active = false; // Resonance test active -bool ResonanceGenerator::done = false; // Resonance test done -float ResonanceGenerator::rt_time; // Resonance test timer -float ResonanceGenerator::amplitude_precalc; -float ResonanceGenerator::freq_mul; -float ResonanceGenerator::phase = 0.0f; +bool ResonanceGenerator::active = false; // Resonance test active +bool ResonanceGenerator::done = false; // Resonance test done +int32_t ResonanceGenerator::freq_to_phase_fp; #if HAS_STANDARD_MOTION block_t ResonanceGenerator::block; @@ -50,41 +47,32 @@ ResonanceGenerator::ResonanceGenerator() {} void ResonanceGenerator::start() { gcode.home_all_axes(); // Always home axes first - - // Always move to the center of the bed do_blocking_move_to_xy(X_CENTER, Y_CENTER, Z_CLEARANCE_FOR_HOMING); - + rt_params.start_pos = current_position; active = true; done = false; - #if ENABLED(FT_MOTION) - if (ftMotion.cfg.active) - rt_time = FTM_TS; - #if HAS_STANDARD_MOTION - else - rt_time = 0.001f; - #endif - #else - rt_time = 0.001f; - #endif - - // Safe Acceleration per Hz for Z axis + // Clamp Z-axis acceleration for safety if (rt_params.axis == Z_AXIS) NOMORE(rt_params.accel_per_hz, 15.0f); + // Calculate time constant for sine sweep + const float rt_time = rt_params.octave_duration * (logf(RATIO) / logf(2.0f)); + #if HAS_STANDARD_MOTION - if (TERN1(FT_MOTION, !ftMotion.cfg.active)){ - // Constant speed to be adjusted if necessary + if (TERN1(FT_MOTION, !ftMotion.cfg.active)) { block.reset(); block.initial_rate = (rt_params.axis == Z_AXIS) ? 2000 : 8000; } #endif - // Precompute sine sweep const - amplitude_precalc = (rt_params.amplitude_correction * rt_params.accel_per_hz * 0.25f) / sq(M_PI); - current_freq = rt_params.min_freq; - freq_mul = exp2f(rt_time / rt_params.octave_duration); + // Precompute fixed-point sine sweep parameters + amplitude_precalc_fp = F2FP((rt_params.amplitude_correction * rt_params.accel_per_hz * 0.25f) / sq(M_PI)); + current_freq_fp = F2FP(rt_params.min_freq); + freq_to_phase_fp = F2FP(2.0f * M_PI * rt_time); + max_freq_fp = F2FP(rt_params.max_freq); + phase_fp = 0; } void ResonanceGenerator::abort() { @@ -109,36 +97,41 @@ void ResonanceGenerator::reset() { done = false; } -// Returns the next position offset for the current frequency float ResonanceGenerator::calc_next_pos() { - // Amplitude based on a sinusoidal wave : A = accel / (4 * PI^2 * f^2) - const float amplitude = amplitude_precalc / current_freq; - - // Phase accumulation in radians - phase += current_freq * M_TAU * rt_time; - if (phase >= M_TAU) phase -= M_TAU; - - const float r = (phase > M_PI) ? (phase - M_TAU) : phase; - const float r2 = r * r; + // Phase accumulation and wrapping within [0, 2π) + phase_fp += (int32_t)(((int64_t)current_freq_fp * freq_to_phase_fp) >> FP_BITS); + if (phase_fp >= M_TAU_FP) phase_fp -= M_TAU_FP; + else if (phase_fp < 0) phase_fp += M_TAU_FP; - // New postion - return (amplitude * r * (1.0f - 0.101321184f * r2)); + // -π <= r_fp <= π + int32_t r_fp = (phase_fp > M_PI_FP) ? phase_fp - M_TAU_FP : phase_fp; + + // Calculate windowing polynomial: 1.0 - 0.101321184 * r² + int32_t poly_fp = FP_ONE - ((C0101321184_FP * ((r_fp * r_fp) >> FP_BITS)) >> FP_BITS); + + // Combine amplitude, phase, and polynomial and return new position + int32_t amplitude_fp = (int32_t)(((int64_t)amplitude_precalc_fp * FP_ONE) / current_freq_fp); + int32_t pos_fp = (int32_t)((((int64_t)amplitude_fp * r_fp) >> FP_BITS) * poly_fp) >> FP_BITS; + + return FP2F(pos_fp); } #if ENABLED(FT_MOTION) void ResonanceGenerator::fill_stepper_plan_buffer() { - static xyze_pos_t traj_coords = rt_params.start_pos; + xyze_pos_t traj_coords = rt_params.start_pos; + // Save starting position, avoid cumulative errors + const float start_pos = rt_params.start_pos[rt_params.axis]; while (!ftMotion.stepping.is_full()) { - // Calculate current frequency - current_freq *= freq_mul; - if (current_freq > rt_params.max_freq) { + // Calculate current frequency with exponential sweep + current_freq_fp += current_freq_fp >> 16; + if (current_freq_fp > max_freq_fp) { done = true; return; } // Resonate the axis being tested - traj_coords[rt_params.axis] += calc_next_pos(); + traj_coords[rt_params.axis] = start_pos + calc_next_pos(); // Store in buffer ftMotion.stepping_enqueue(traj_coords); @@ -148,31 +141,37 @@ float ResonanceGenerator::calc_next_pos() { #if HAS_STANDARD_MOTION block_t *ResonanceGenerator::generate_resonance_block() { + // Static variables to retain state between calls and avoid cumulative errors + static float prev_pos = 0.0f, step_accumulator = 0.0f; + const float step_mm = planner.settings.axis_steps_per_mm[rt_params.axis]; + const uint8_t axis_bit = 1 << rt_params.axis; - // Calculate current frequency - current_freq *= freq_mul; - if (current_freq > rt_params.max_freq) { + // Calculate current frequency with exponential sweep + current_freq_fp += current_freq_fp >> 16; + if (current_freq_fp > max_freq_fp) { done = true; return nullptr; } - // mm → steps - const float delta_mm = calc_next_pos(); - const float prod = delta_mm * step_mm; - const int32_t delta_steps = (int32_t)(prod > 0.0f ? prod + 0.5f : prod - 0.5f); + // Calculate position and accumulate steps + float current_pos = calc_next_pos(); + step_accumulator += (current_pos - prev_pos) * step_mm; + prev_pos = current_pos; + + // Extract steps + int32_t delta_steps = (int32_t)floor(step_accumulator); + step_accumulator -= delta_steps; + uint32_t abs_steps = abs(delta_steps); - // Steps for target axis - block.steps[rt_params.axis] = abs(delta_steps); - // Total event count - block.step_event_count = abs(delta_steps); - - // Direction - if (delta_steps > 0) - block.direction_bits &= ~(1 << rt_params.axis); + // Update block + block.steps[rt_params.axis] = abs_steps; + block.step_event_count = abs_steps; + if (delta_steps < 0) + block.direction_bits |= axis_bit; else - block.direction_bits |= (1 << rt_params.axis); - + block.direction_bits &= ~axis_bit; + return █ } #endif diff --git a/Marlin/src/feature/resonance/resonance_generator.h b/Marlin/src/feature/resonance/resonance_generator.h index 693ae7a511..02023eafd9 100644 --- a/Marlin/src/feature/resonance/resonance_generator.h +++ b/Marlin/src/feature/resonance/resonance_generator.h @@ -29,9 +29,23 @@ #include "../../module/planner.h" #endif -#ifndef M_TAU - #define M_TAU (2.0f * M_PI) -#endif +// Fixed-point configuration +#define FP_BITS 16 // 16 bits for fractional part +#define FP_ONE (1UL << FP_BITS) // Fixed-point 1.0 + +// Convert float to fixed-point +#define F2FP(x) ((int32_t)((x) * FP_ONE + 0.5f)) + +// Convert fixed-point to float +#define FP2F(x) ((float)(x) / FP_ONE) + +// Fixed-point constants +#define M_TAU_FP F2FP(2.0f * M_PI) +#define M_PI_FP F2FP(M_PI) +#define C0101321184_FP F2FP(0.101321184f) + +// For rt_time calculation, perfect match between octave duration and frequency sweep +#define RATIO (1.0f + 1.0f / 65536.0f) // 1 + 1/2^16 typedef struct ResonanceTestParams { AxisEnum axis = NO_AXIS_ENUM; // Axis to test @@ -46,7 +60,7 @@ typedef struct ResonanceTestParams { class ResonanceGenerator { public: static resonance_test_params_t rt_params; // Resonance test parameters - float timeline; // Timeline Value to calculate resonance frequency + float timeline; // Timeline Value to calculate resonance frequency ResonanceGenerator(); @@ -78,11 +92,16 @@ class ResonanceGenerator { private: float calc_next_pos(); // Calculate next position point based on current frequency - static float rt_time; // Test timer - static float freq_mul; // Frequency multiplier for sine sweeping - static float amplitude_precalc; // Precalculated part of amplitude formula - float current_freq; // Current frequency being generated in sinusoidal motion - static float phase; // Current phase in radians + + // Fixed-point variables + int32_t amplitude_precalc_fp; // Fixed-point amplitude precalculation + int32_t current_freq_fp; // Fixed-point current frequency + + // Phase variables (in radians, stored as fixed-point) + int32_t phase_fp; // Fixed-point phase accumulator + static int32_t freq_to_phase_fp; // Fixed-point frequency to phase conversion + + int32_t max_freq_fp; // Fixed-point maximum frequency #if HAS_STANDARD_MOTION static block_t block; #endif diff --git a/Marlin/src/module/stepper.cpp b/Marlin/src/module/stepper.cpp index 4f94fbd107..9e2cc3c791 100644 --- a/Marlin/src/module/stepper.cpp +++ b/Marlin/src/module/stepper.cpp @@ -660,8 +660,8 @@ bool Stepper::disable_axis(const AxisEnum axis) { } else rtg.abort(); - - return interval;; + + return interval; } void Stepper::resonance_pulse_phase_isr() {