diff --git a/Marlin/src/module/ft_motion.cpp b/Marlin/src/module/ft_motion.cpp index d8cc2e5f45..c7a558d7d2 100644 --- a/Marlin/src/module/ft_motion.cpp +++ b/Marlin/src/module/ft_motion.cpp @@ -349,6 +349,7 @@ bool FTMotion::plan_next_block() { if (current_block->is_sync_pos()) stepper._set_position(current_block->position); continue; } + ensure_float_precision(); #if ENABLED(POWER_LOSS_RECOVERY) recovery.info.sdpos = current_block->sdpos; @@ -400,6 +401,39 @@ bool FTMotion::plan_next_block() { } } +/** + * Ensure extruder position stays within floating point precision bounds. + * Float32 numbers have 23 bits of precision, so the minimum increment ("resolution") around a value x is: + * resolution = 2^(floor(log2(|x|)) - 23) + * By resetting at ±1'000mm (1 meter), we get a minimum resolution of ~ 0.00006mm, enough for smoothing to work well. + */ +void FTMotion::ensure_float_precision() { + constexpr float FTM_POSITION_WRAP_THRESHOLD = 1'000.0f; // (mm) Reset when position exceeds this to prevent floating point precision loss + #if HAS_EXTRUDERS + if (fabs(endPos_prevBlock.E) >= FTM_POSITION_WRAP_THRESHOLD) { + const float offset = -endPos_prevBlock.E; + endPos_prevBlock.E += offset; + + // Offset extruder shaping buffer + #if ALL(HAS_FTM_SHAPING, FTM_SHAPER_E) + for (uint32_t i = 0; i < FTM_ZMAX; ++i) shaping.E.d_zi[i] += offset; + #endif + + // Offset extruder smoothing buffer + #if ENABLED(FTM_SMOOTHING) + for (uint8_t i = 0; i < FTM_SMOOTHING_ORDER; ++i) smoothing.E.smoothing_pass[i] += offset; + #endif + + // Offset linear advance previous position + prev_traj_e += offset; + + // Offset stepper current position + const int64_t delta_steps_q48_16 = offset * planner.settings.axis_steps_per_mm[block_extruder_axis] * (1ULL << 16); + stepping.curr_steps_q48_16.E += delta_steps_q48_16; + }; + #endif +} + xyze_float_t FTMotion::calc_traj_point(const float dist) { xyze_float_t traj_coords; #define _SET_TRAJ(q) traj_coords.q = startPos.q + ratio.q * dist; diff --git a/Marlin/src/module/ft_motion.h b/Marlin/src/module/ft_motion.h index 991d3efcef..7af440b163 100644 --- a/Marlin/src/module/ft_motion.h +++ b/Marlin/src/module/ft_motion.h @@ -280,6 +280,7 @@ class FTMotion { static void fill_stepper_plan_buffer(); static xyze_float_t calc_traj_point(const float dist); static bool plan_next_block(); + static void ensure_float_precision(); }; // class FTMotion diff --git a/Marlin/src/module/ft_motion/stepping.h b/Marlin/src/module/ft_motion/stepping.h index 9274cbf369..a845e78e3d 100644 --- a/Marlin/src/module/ft_motion/stepping.h +++ b/Marlin/src/module/ft_motion/stepping.h @@ -233,9 +233,11 @@ typedef struct Stepping { FORCE_INLINE bool is_busy() { return !(is_empty() && ticks_left_in_frame_fp == 0); } + FORCE_INLINE bool is_empty() { return stepper_plan_head == stepper_plan_tail; } + FORCE_INLINE bool is_full() { return ((stepper_plan_head + 1) & FTM_BUFFER_MASK) == stepper_plan_tail; }