🐛 FT Motion trajectory fixes (#28101)

Author: dbuezas

Marlin/src/module/ft_motion.cpp

@@ -75,7 +75,7 @@ float FTMotion::tau = 0.0f;                         // (s) Time since start of b
 TrapezoidalTrajectoryGenerator FTMotion::trapezoidalGenerator;
 Poly5TrajectoryGenerator FTMotion::poly5Generator;
 Poly6TrajectoryGenerator FTMotion::poly6Generator;
-TrajectoryGenerator& FTMotion::currentGenerator = FTMotion::trapezoidalGenerator;
+TrajectoryGenerator* FTMotion::currentGenerator = &FTMotion::trapezoidalGenerator;
 TrajectoryType FTMotion::trajectoryType = TrajectoryType::FTM_TRAJECTORY_TYPE;
 
 // Make vector variables.
@@ -496,7 +496,7 @@ void FTMotion::runoutBlock() {
   const float total_duration = max_intervals * FTM_TS + reminder_from_last_block;
 
   // Plan a zero-motion trajectory for runout
-  currentGenerator.planRunout(total_duration);
+  currentGenerator->planRunout(total_duration);
 
   blockProcRdy = true; // since ratio is 0, the trajectory positions won't advance in any axis
 }
@@ -511,7 +511,7 @@ int32_t FTMotion::stepperCmdBuffItems() {
 void FTMotion::init() {
   update_shaping_params();
   TERN_(FTM_SMOOTHING, update_smoothing_params());
-  setTrajectoryType(TrajectoryType::FTM_TRAJECTORY_TYPE);
+  setTrajectoryType(cfg.trajectory_type);
   reset(); // Precautionary.
 }
 
@@ -520,11 +520,11 @@ void FTMotion::setTrajectoryType(const TrajectoryType type) {
   cfg.trajectory_type = trajectoryType = type;
   switch (type) {
     default: cfg.trajectory_type = trajectoryType = TrajectoryType::FTM_TRAJECTORY_TYPE;
-    case TrajectoryType::TRAPEZOIDAL: currentGenerator = trapezoidalGenerator; break;
-    case TrajectoryType::POLY5:       currentGenerator = poly5Generator; break;
-    case TrajectoryType::POLY6:       currentGenerator = poly6Generator; break;
+    case TrajectoryType::TRAPEZOIDAL: currentGenerator = &trapezoidalGenerator; break;
+    case TrajectoryType::POLY5:       currentGenerator = &poly5Generator; break;
+    case TrajectoryType::POLY6:       currentGenerator = &poly6Generator; break;
   }
-  currentGenerator.reset(); // Reset the selected generator
+  currentGenerator->reset(); // Reset the selected generator
 }
 
 // Load / convert block data from planner to fixed-time control variables.
@@ -545,7 +545,7 @@ void FTMotion::loadBlockData(block_t * const current_block) {
   const float final_speed = mmps * current_block->final_rate;       // (mm/s) End feedrate
 
   // Plan the trajectory using the trajectory generator
-  currentGenerator.plan(initial_speed, final_speed, current_block->acceleration, current_block->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;
@@ -555,7 +555,7 @@ void FTMotion::loadBlockData(block_t * const current_block) {
   TERN_(FTM_HAS_LIN_ADVANCE, use_advance_lead = current_block->use_advance_lead);
 
   // Watch endstops until the move ends
-  const float total_duration = currentGenerator.getTotalDuration();
+  const float total_duration = currentGenerator->getTotalDuration();
   uint32_t max_intervals = ceil((total_duration + reminder_from_last_block) * FTM_FS);
   const millis_t move_end_ti = millis() + SEC_TO_MS((FTM_TS) * float(max_intervals + num_samples_shaper_settle() + ((PROP_BATCHES) + 1) * (FTM_BATCH_SIZE)) + (float(FTM_STEPPERCMD_BUFF_SIZE) / float(FTM_STEPPER_FS)));
 
@@ -572,7 +572,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();
+  const float total_duration = currentGenerator->getTotalDuration();
   if (tau + FTM_TS > total_duration) {
     // TODO: refactor code so this thing is not twice.
     // the reason of it being in the beginning, is that a block can be so short that it has
@@ -589,7 +589,7 @@ void FTMotion::generateTrajectoryPointsFromBlock() {
                                   // tau can start negative, but it always holds that `tau > -FTM_TS`
 
     // Get distance from trajectory generator
-    const float dist = currentGenerator.getDistanceAtTime(tau);
+    const float dist = currentGenerator->getDistanceAtTime(tau);
 
     #define _SET_TRAJ(q) traj.q[traj_idx_set] = startPos.q + ratio.q * dist;
     LOGICAL_AXIS_MAP_LC(_SET_TRAJ);

Marlin/src/module/ft_motion.h

@@ -73,7 +73,7 @@ typedef struct FTConfig {
     float linearAdvK = FTM_LINEAR_ADV_DEFAULT_K;          // Linear advance gain.
   #endif
 
-  TrajectoryType trajectory_type = TrajectoryType::TRAPEZOIDAL; // Trajectory generator type
+  TrajectoryType trajectory_type = TrajectoryType::FTM_TRAJECTORY_TYPE; // Trajectory generator type
   float poly6_acceleration_overshoot; // Overshoot factor for Poly6 (1.25 to 2.0)
 } ft_config_t;
 
@@ -186,7 +186,7 @@ class FTMotion {
     static TrapezoidalTrajectoryGenerator trapezoidalGenerator;
     static Poly5TrajectoryGenerator poly5Generator;
     static Poly6TrajectoryGenerator poly6Generator;
-    static TrajectoryGenerator& currentGenerator;
+    static TrajectoryGenerator* currentGenerator;
     static TrajectoryType trajectoryType;
 
     // Number of batches needed to propagate the current trajectory to the stepper.