From de98df1651cd3768d94892edb9d548a84e8120e5 Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Mon, 6 Oct 2025 23:27:28 -0500 Subject: [PATCH] =?UTF-8?q?=F0=9F=8E=A8=20Apply=20sq,=20cu,=20const?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Marlin/src/module/ft_motion.cpp | 13 +++++++------ .../ft_motion/poly5_trajectory_generator.h | 18 +++++++++--------- .../ft_motion/poly6_trajectory_generator.cpp | 8 ++++---- .../ft_motion/poly6_trajectory_generator.h | 6 +++--- .../module/ft_motion/trajectory_generator.h | 6 +++--- .../trapezoidal_trajectory_generator.h | 14 +++++++------- Marlin/src/module/motion.cpp | 5 +++-- Marlin/src/module/probe.cpp | 4 ++-- Marlin/src/module/temperature.cpp | 2 +- 9 files changed, 39 insertions(+), 37 deletions(-) diff --git a/Marlin/src/module/ft_motion.cpp b/Marlin/src/module/ft_motion.cpp index d8bbbc50ca..219fb1a3f6 100644 --- a/Marlin/src/module/ft_motion.cpp +++ b/Marlin/src/module/ft_motion.cpp @@ -223,7 +223,8 @@ void FTMotion::loop() { // Interpolation (generation of step commands from fixed time trajectory). while (batchRdyForInterp - && (stepperCmdBuffItems() < (FTM_STEPPERCMD_BUFF_SIZE) - (FTM_STEPS_PER_UNIT_TIME))) { + && (stepperCmdBuffItems() < (FTM_STEPPERCMD_BUFF_SIZE) - (FTM_STEPS_PER_UNIT_TIME)) + ) { generateStepsFromTrajectory(interpIdx); if (++interpIdx == FTM_BATCH_SIZE) { batchRdyForInterp = false; @@ -231,7 +232,7 @@ void FTMotion::loop() { } } - // Report busy status to planner. + // Set busy status for use by planner.busy() busy = (stepperCmdBuffHasData || blockProcRdy || batchRdy || batchRdyForInterp); } @@ -491,7 +492,7 @@ void FTMotion::runoutBlock() { ratio.reset(); uint32_t max_intervals = PROP_BATCHES * (FTM_BATCH_SIZE) + n_to_settle_shaper + n_to_fill_batch_after_settling; - const float reminder_from_last_block = - tau; + const float reminder_from_last_block = -tau; const float total_duration = max_intervals * FTM_TS + reminder_from_last_block; // Plan a zero-motion trajectory for runout @@ -527,6 +528,7 @@ void FTMotion::setTrajectoryType(const TrajectoryType type) { } // Load / convert block data from planner to fixed-time control variables. +// Called from FTMotion::loop() at the fetch of the next planner block. void FTMotion::loadBlockData(block_t * const current_block) { // Cache the extruder index for this block TERN_(DISTINCT_E_FACTORS, block_extruder_axis = E_AXIS_N(current_block->extruder)); @@ -541,11 +543,9 @@ void FTMotion::loadBlockData(block_t * const current_block) { const float mmps = totalLength / current_block->step_event_count; // (mm/step) Distance for each step const float initial_speed = mmps * current_block->initial_rate; // (mm/s) Start feedrate const float final_speed = mmps * current_block->final_rate; // (mm/s) End feedrate - const float accel = current_block->acceleration; - const float nominal_speed = current_block->nominal_speed; // Plan the trajectory using the trajectory generator - currentGenerator.plan(initial_speed, final_speed, accel, nominal_speed, totalLength); + currentGenerator.plan(initial_speed, final_speed, current_block->acceleration, current_block->nominal_speed, totalLength); // Accel + Coasting + Decel + datapoints const float reminder_from_last_block = - tau; @@ -570,6 +570,7 @@ void FTMotion::loadBlockData(block_t * const current_block) { } // Generate data points of the trajectory. +// Called from FTMotion::loop() at the fetch of a new planner block, after loadBlockData. void FTMotion::generateTrajectoryPointsFromBlock() { const float total_duration = currentGenerator.getTotalDuration(); if (tau + FTM_TS > total_duration) { diff --git a/Marlin/src/module/ft_motion/poly5_trajectory_generator.h b/Marlin/src/module/ft_motion/poly5_trajectory_generator.h index 5a924b00d9..91ee23b094 100644 --- a/Marlin/src/module/ft_motion/poly5_trajectory_generator.h +++ b/Marlin/src/module/ft_motion/poly5_trajectory_generator.h @@ -34,25 +34,25 @@ class Poly5TrajectoryGenerator : public TrajectoryGenerator { public: Poly5TrajectoryGenerator() = default; - void plan(float initial_speed, float final_speed, float acceration, float nominal_speed, float distance) override { + void plan(const float initial_speed, const float final_speed, const float acceleration, const float nominal_speed, const float distance) override { this->initial_speed = initial_speed; this->nominal_speed = nominal_speed; // Calculate timing phases using the same logic as trapezoidal generator - const float one_over_acc = 1.0f / acceration; + const float one_over_acc = 1.0f / acceleration; const float ldiff = distance + 0.5f * one_over_acc * (sq(this->initial_speed) + sq(final_speed)); T2 = ldiff / this->nominal_speed - one_over_acc * this->nominal_speed; if (T2 < 0.0f) { T2 = 0.0f; - this->nominal_speed = sqrtf(ldiff * acceration); + this->nominal_speed = sqrtf(ldiff * acceleration); } T1 = (this->nominal_speed - this->initial_speed) * one_over_acc; T3 = (this->nominal_speed - final_speed) * one_over_acc; const float d1 = (this->initial_speed + this->nominal_speed) * T1 * 0.5f; - const float T1_2 = T1 * T1; + const float T1_2 = sq(T1); const float T1_3 = T1_2 * T1; const float T1_4 = T1_3 * T1; const float T1_5 = T1_4 * T1; @@ -69,7 +69,7 @@ public: // Deceration phase const float d3 = (this->nominal_speed + final_speed) * T3 * 0.5f; - const float T3_2 = T3 * T3; + const float T3_2 = sq(T3); const float T3_3 = T3_2 * T3; const float T3_4 = T3_3 * T3; const float T3_5 = T3_4 * T3; @@ -81,22 +81,22 @@ public: dec_c5 = (6.0f * d3 - 3.0f * (this->nominal_speed + final_speed) * T3) / T3_5; } - void planRunout(float duration) override { + void planRunout(const float duration) override { reset(); T2 = duration; } - float getDistanceAtTime(float t) const override { + float getDistanceAtTime(const float t) const override { if (t < T1) { // Acceration phase - return t * (acc_c1 + t * t * (acc_c3 + t * (acc_c4 + t * acc_c5))); + return t * (acc_c1 + sq(t) * (acc_c3 + t * (acc_c4 + t * acc_c5))); } else if (t <= (T1 + T2)) { // Coasting phase return pos_before_coast + this->nominal_speed * (t - T1); } // Deceration phase const float tau = t - (T1 + T2); - return pos_after_coast + tau * (dec_c1 + tau * tau * (dec_c3 + tau * (dec_c4 + tau * dec_c5))); + return pos_after_coast + tau * (dec_c1 + sq(tau) * (dec_c3 + tau * (dec_c4 + tau * dec_c5))); } float getTotalDuration() const override { return T1 + T2 + T3; } diff --git a/Marlin/src/module/ft_motion/poly6_trajectory_generator.cpp b/Marlin/src/module/ft_motion/poly6_trajectory_generator.cpp index 9f20d0f793..6d01eb96bb 100644 --- a/Marlin/src/module/ft_motion/poly6_trajectory_generator.cpp +++ b/Marlin/src/module/ft_motion/poly6_trajectory_generator.cpp @@ -97,9 +97,9 @@ void Poly6TrajectoryGenerator::plan(const float initial_speed, const float final dec_c4 = -15.0f * delta_p + 7.0f * delta_v; dec_c5 = 6.0f * delta_p - 3.0f * delta_v; - const float a5_mid = s5pp_u(dec_c3, dec_c4, dec_c5, 0.5f) / (Ts * Ts); + const float a5_mid = s5pp_u(dec_c3, dec_c4, dec_c5, 0.5f) / sq(Ts); const float a_mid_target = -ftMotion.cfg.poly6_acceleration_overshoot * acceleration; - dec_c6 = (Ts * Ts) * (a_mid_target - a5_mid) / Kpp_mid; + dec_c6 = sq(Ts) * (a_mid_target - a5_mid) / Kpp_mid; } } @@ -121,8 +121,8 @@ float Poly6TrajectoryGenerator::getDistanceAtTime(const float t) const { return pos_before_coast + this->nominal_speed * (t - T1); } // Decel phase - const float tau = t - (T1 + T2); - const float u = tau / T3; + const float tau = t - (T1 + T2), + u = tau / T3; return s5_u(pos_after_coast, this->nominal_speed, T3, dec_c3, dec_c4, dec_c5, u) + dec_c6 * K_u(pos_after_coast, this->nominal_speed, T3, u); } diff --git a/Marlin/src/module/ft_motion/poly6_trajectory_generator.h b/Marlin/src/module/ft_motion/poly6_trajectory_generator.h index 77e2954555..6ecc6f064c 100644 --- a/Marlin/src/module/ft_motion/poly6_trajectory_generator.h +++ b/Marlin/src/module/ft_motion/poly6_trajectory_generator.h @@ -50,18 +50,18 @@ private: // Base quintic in position (end accel = 0): s5(u) = s0 + v0*Ts*u + c3 u^3 + c4 u^4 + c5 u^5 static inline float s5_u(const float s0, const float v0, const float Ts, const float c3, const float c4, const float c5, const float u) { - const float u2=u * u, u3=u2 * u, u4=u3 * u, u5=u4 * u; + const float u2 = sq(u), u3 = u2 * u, u4 = u3 * u, u5 = u4 * u; return s0 + v0 * Ts * u + c3 * u3 + c4 * u4 + c5 * u5; } static inline float s5pp_u(const float c3, const float c4, const float c5, const float u) { // d^2/du^2 (c3 u^3 + c4 u^4 + c5 u^5) = 6a u + 12 c4 u^2 + 20 c5 u^3 - return 6.0f * c3 * u + 12.0f * c4 * u * u + 20.0f * c5 * u * u * u; + return 6.0f * c3 * u + 12.0f * c4 * sq(u) + 20.0f * c5 * cu(u); } // Shape term K(u) = u^3(1-u)^3 added in position with scalar c static inline float K_u(const float /*s0*/, const float /*v0*/, const float /*Ts*/, const float u) { const float um1 = 1.0f - u; - return (u * u * u) * (um1 * um1 * um1); + return cu(u) * cu(um1); } // Timings and kinematics diff --git a/Marlin/src/module/ft_motion/trajectory_generator.h b/Marlin/src/module/ft_motion/trajectory_generator.h index ab1b5d3814..991ea6075c 100644 --- a/Marlin/src/module/ft_motion/trajectory_generator.h +++ b/Marlin/src/module/ft_motion/trajectory_generator.h @@ -39,20 +39,20 @@ public: * @param nominal_speed Peak feedrate [mm/s] * @param distance Total distance to travel [mm] */ - virtual void plan(float initial_speed, float final_speed, float acceleration, float nominal_speed, float distance) = 0; + virtual void plan(const float initial_speed, const float final_speed, const float acceleration, const float nominal_speed, const float distance) = 0; /** * Plan a zero-motion trajectory for a specific duration. * @param duration The total time for the runout phase [s] */ - virtual void planRunout(float duration) = 0; + virtual void planRunout(const float duration) = 0; /** * Get the distance traveled at time t. * @param t Time since start of trajectory [s] * @return Distance traveled [mm] */ - virtual float getDistanceAtTime(float t) const = 0; + virtual float getDistanceAtTime(const float t) const = 0; /** * Get the total duration of the trajectory. diff --git a/Marlin/src/module/ft_motion/trapezoidal_trajectory_generator.h b/Marlin/src/module/ft_motion/trapezoidal_trajectory_generator.h index 1ca0636b1e..1c96618f50 100644 --- a/Marlin/src/module/ft_motion/trapezoidal_trajectory_generator.h +++ b/Marlin/src/module/ft_motion/trapezoidal_trajectory_generator.h @@ -33,13 +33,13 @@ class TrapezoidalTrajectoryGenerator : public TrajectoryGenerator { public: TrapezoidalTrajectoryGenerator() = default; - void plan(float initial_speed, float final_speed, float acceleration, float nominal_speed, float distance) override { + void plan(const float initial_speed, const float final_speed, const float acceleration, const float nominal_speed, const float distance) override { this->initial_speed = initial_speed; this->nominal_speed = nominal_speed; this->acceleration = acceleration; const float one_over_accel = 1.0f / acceleration; - const float ldiff = distance + 0.5f * one_over_accel * (initial_speed * initial_speed + final_speed * final_speed); + const float ldiff = distance + 0.5f * one_over_accel * (sq(initial_speed) + sq(final_speed)); T2 = ldiff / nominal_speed - one_over_accel * nominal_speed; if (T2 < 0.0f) { @@ -51,30 +51,30 @@ public: T3 = (this->nominal_speed - final_speed) * one_over_accel; // Calculate the distance traveled during the accel phase - pos_before_coast = initial_speed * T1 + 0.5f * acceleration * T1 * T1; + pos_before_coast = initial_speed * T1 + 0.5f * acceleration * sq(T1); // Calculate the distance traveled during the coast phase pos_after_coast = pos_before_coast + this->nominal_speed * T2; } - float getDistanceAtTime(float t) const override { + float getDistanceAtTime(const float t) const override { if (t < T1) { // Acceleration phase - return (this->initial_speed * t) + (0.5f * this->acceleration * t * t); + return (this->initial_speed * t) + (0.5f * this->acceleration * sq(t)); } else if (t <= (T1 + T2)) { // Coasting phase return pos_before_coast + this->nominal_speed * (t - T1); } // Deceleration phase const float tau_decel = t - (T1 + T2); - return pos_after_coast + this->nominal_speed * tau_decel - 0.5f * this->acceleration * tau_decel * tau_decel; + return pos_after_coast + this->nominal_speed * tau_decel - 0.5f * this->acceleration * sq(tau_decel); } float getTotalDuration() const override { return T1 + T2 + T3; } - void planRunout(float duration) override { + void planRunout(const float duration) override { reset(); T2 = duration; // Coast at zero speed for the entire duration } diff --git a/Marlin/src/module/motion.cpp b/Marlin/src/module/motion.cpp index 1a66fc7425..d7967f6aaf 100644 --- a/Marlin/src/module/motion.cpp +++ b/Marlin/src/module/motion.cpp @@ -272,10 +272,11 @@ void report_current_position_projected() { #define HOMING_CURRENT(A) TERN(EDITABLE_HOMING_CURRENT, homing_current_mA.A, A##_CURRENT_HOME) // Saves the running current of the motor at the moment the function is called and sets current to CURRENT_HOME - #define _SAVE_SET_CURRENT(A) \ + #define _SAVE_SET_CURRENT(A) do{ \ saved_current_mA.A = stepper##A.getMilliamps(); \ stepper##A.rms_current(HOMING_CURRENT(A)); \ - debug_current(F(STR_##A), saved_current_mA.A, HOMING_CURRENT(A)) + debug_current(F(STR_##A), saved_current_mA.A, HOMING_CURRENT(A)); \ + }while(0) #define _MAP_SAVE_SET(A) OPTCODE(A##_HAS_HOME_CURRENT, _SAVE_SET_CURRENT(A)) diff --git a/Marlin/src/module/probe.cpp b/Marlin/src/module/probe.cpp index 758d2300bc..91a3268739 100644 --- a/Marlin/src/module/probe.cpp +++ b/Marlin/src/module/probe.cpp @@ -52,7 +52,7 @@ #include "delta.h" #endif -#if ENABLED(SENSORLESS_PROBING) +#if HAS_DELTA_SENSORLESS_PROBING abc_float_t offset_sensorless_adj{0}; float largest_sensorless_adj = 0; #endif @@ -1128,6 +1128,6 @@ float Probe::probe_at_point( } } -#endif +#endif // HAS_DELTA_SENSORLESS_PROBING #endif // HAS_BED_PROBE diff --git a/Marlin/src/module/temperature.cpp b/Marlin/src/module/temperature.cpp index 172d044e42..acffddfa81 100644 --- a/Marlin/src/module/temperature.cpp +++ b/Marlin/src/module/temperature.cpp @@ -1362,7 +1362,7 @@ void Temperature::factory_reset() { const float t1 = tuner.get_sample_1_temp(), t2 = tuner.get_sample_2_temp(), t3 = tuner.get_sample_3_temp(); - float asymp_temp = (t2 * t2 - t1 * t3) / (2 * t2 - t1 - t3), + float asymp_temp = (sq(t2) - t1 * t3) / (2 * t2 - t1 - t3), block_responsiveness = -log((t2 - asymp_temp) / (t1 - asymp_temp)) / tuner.get_sample_interval(); #if ENABLED(MPC_AUTOTUNE_DEBUG)