@@ -134,7 +134,7 @@ void FTMotion::loop() {
134134 }
135135
136136 while (!blockProcRdy && (stepper.current_block = planner.get_current_block ())) {
137- if (stepper.current_block ->is_sync ()) { // Sync block?
137+ if (stepper.current_block ->is_sync ()) { // Sync block?
138138 if (stepper.current_block ->is_sync_pos ()) // Position sync? Set the position.
139139 stepper._set_position (stepper.current_block ->position );
140140 discard_planner_block_protected ();
@@ -166,7 +166,7 @@ void FTMotion::loop() {
166166 discard_planner_block_protected ();
167167
168168 // Check if the block needs to be runout:
169- if (!batchRdy && !planner.movesplanned ()){
169+ if (!batchRdy && !planner.movesplanned ()) {
170170 runoutBlock ();
171171 makeVector (); // Do an additional makeVector call to guarantee batchRdy set this loop.
172172 }
@@ -196,7 +196,7 @@ void FTMotion::loop() {
196196 batchRdy = false ; // Clear so makeVector() can resume generating points.
197197 }
198198
199- // Interpolation.
199+ // Interpolation (generation of step commands from fixed time trajectory) .
200200 while (batchRdyForInterp
201201 && (stepperCmdBuffItems () < (FTM_STEPPERCMD_BUFF_SIZE) - (FTM_STEPS_PER_UNIT_TIME))) {
202202 convertToSteps (interpIdx);
@@ -350,14 +350,14 @@ void FTMotion::loop() {
350350 void FTMotion::update_shaping_params () {
351351 #if HAS_X_AXIS
352352 if ((shaping.x .ena = AXIS_HAS_SHAPER (X))) {
353- shaping.x .set_axis_shaping_A (cfg.shaper [X_AXIS] , cfg.zeta [X_AXIS] , cfg.vtol [X_AXIS] );
354- shaping.x .set_axis_shaping_N (cfg.shaper [X_AXIS] , cfg.baseFreq [X_AXIS] , cfg.zeta [X_AXIS] );
353+ shaping.x .set_axis_shaping_A (cfg.shaper . x , cfg.zeta . x , cfg.vtol . x );
354+ shaping.x .set_axis_shaping_N (cfg.shaper . x , cfg.baseFreq . x , cfg.zeta . x );
355355 }
356356 #endif
357357 #if HAS_Y_AXIS
358358 if ((shaping.y .ena = AXIS_HAS_SHAPER (Y))) {
359- shaping.y .set_axis_shaping_A (cfg.shaper [Y_AXIS] , cfg.zeta [Y_AXIS] , cfg.vtol [Y_AXIS] );
360- shaping.y .set_axis_shaping_N (cfg.shaper [Y_AXIS] , cfg.baseFreq [Y_AXIS] , cfg.zeta [Y_AXIS] );
359+ shaping.y .set_axis_shaping_A (cfg.shaper . y , cfg.zeta . y , cfg.vtol . y );
360+ shaping.y .set_axis_shaping_N (cfg.shaper . y , cfg.baseFreq . y , cfg.zeta . y );
361361 }
362362 #endif
363363 }
@@ -407,27 +407,29 @@ void FTMotion::discard_planner_block_protected() {
407407 }
408408}
409409
410- // Sets up a pseudo block to allow motion to settle buffers to empty. This is
411- // called when the planner has only one block left. The buffers will be filled
412- // with the last commanded position by setting the startPosn block variable to
413- // the last position of the previous block and all ratios to zero such that no
414- // axes' positions are incremented.
410+ /* *
411+ * Set up a pseudo block to allow motion to settle and buffers to empty.
412+ * Called when the planner has one block left. The buffers will be filled
413+ * with the last commanded position by setting the startPosn block variable to
414+ * the last position of the previous block and all ratios to zero such that no
415+ * axes' positions are incremented.
416+ */
415417void FTMotion::runoutBlock () {
416418
417419 startPosn = endPosn_prevBlock;
418420 ratio.reset ();
419421
420- int32_t n_to_fill_batch = FTM_WINDOW_SIZE - makeVector_batchIdx;
422+ const int32_t n_to_fill_batch = ( FTM_WINDOW_SIZE) - makeVector_batchIdx;
421423
422- // This line is to be modified for FBS use; do not optimize out.
423- int32_t n_to_settle_cmpnstr = ( TERN_ (HAS_X_AXIS, shaping. x . ena ) || TERN_ (HAS_Y_AXIS, shaping. y . ena )) ? FTM_ZMAX : 0 ;
424+ // This line or function is to be modified for FBS use; do not optimize out.
425+ const int32_t n_to_settle_shaper = num_samples_shaper_settle () ;
424426
425- int32_t n_to_fill_batch_after_settling = (n_to_settle_cmpnstr > n_to_fill_batch) ?
426- FTM_BATCH_SIZE - ((n_to_settle_cmpnstr - n_to_fill_batch) % FTM_BATCH_SIZE) : n_to_fill_batch - n_to_settle_cmpnstr ;
427+ const int32_t n_diff = n_to_settle_shaper - n_to_fill_batch,
428+ n_to_fill_batch_after_settling = n_diff > 0 ? (FTM_BATCH_SIZE) - (n_diff % ( FTM_BATCH_SIZE)) : -n_diff ;
427429
428- int32_t n_to_settle_and_fill_batch = n_to_settle_cmpnstr + n_to_fill_batch_after_settling;
430+ const int32_t n_to_settle_and_fill_batch = n_to_settle_shaper + n_to_fill_batch_after_settling;
429431
430- max_intervals = PROP_BATCHES * FTM_BATCH_SIZE + n_to_settle_and_fill_batch;
432+ max_intervals = ( PROP_BATCHES) * ( FTM_BATCH_SIZE) + n_to_settle_and_fill_batch;
431433
432434 blockProcRdy = true ;
433435}
@@ -571,13 +573,13 @@ void FTMotion::makeVector() {
571573 accel_k = decel_P; // (mm/s^2) Acceleration K factor from Decel phase
572574 }
573575
574- #define _FTM_TRAJ ( A ) traj.A [makeVector_batchIdx] = startPosn.A + ratio.A * dist;
575- LOGICAL_AXIS_MAP_LC (_FTM_TRAJ );
576+ #define _SET_TRAJ ( q ) traj.q [makeVector_batchIdx] = startPosn.q + ratio.q * dist;
577+ LOGICAL_AXIS_MAP_LC (_SET_TRAJ );
576578
577579 #if HAS_EXTRUDERS
578580 if (cfg.linearAdvEna ) {
579581 float dedt_adj = (traj.e [makeVector_batchIdx] - e_raw_z1) * (FTM_FS);
580- if (ratio.e > 0 .0f ) dedt_adj += accel_k * cfg.linearAdvK * 0 . 0001f ;
582+ if (ratio.e > 0 .0f ) dedt_adj += accel_k * cfg.linearAdvK ;
581583
582584 e_raw_z1 = traj.e [makeVector_batchIdx];
583585 e_advanced_z1 += dedt_adj * (FTM_TS);
@@ -590,29 +592,32 @@ void FTMotion::makeVector() {
590592 switch (cfg.dynFreqMode ) {
591593
592594 #if HAS_DYNAMIC_FREQ_MM
593- case dynFreqMode_Z_BASED:
594- if (traj.z [makeVector_batchIdx] != 0 .0f ) { // Only update if Z changed.
595+ case dynFreqMode_Z_BASED: {
596+ static float oldz = 0 .0f ;
597+ const float z = traj.z [makeVector_batchIdx];
598+ if (z != oldz) { // Only update if Z changed.
599+ oldz = z;
595600 #if HAS_X_AXIS
596- const float xf = cfg.baseFreq [X_AXIS] + cfg.dynFreqK [X_AXIS] * traj. z [makeVector_batchIdx] ;
597- shaping.x .set_axis_shaping_N (cfg.shaper [X_AXIS] , _MAX (xf, FTM_MIN_SHAPE_FREQ), cfg.zeta [X_AXIS] );
601+ const float xf = cfg.baseFreq . x + cfg.dynFreqK . x * z ;
602+ shaping.x .set_axis_shaping_N (cfg.shaper . x , _MAX (xf, FTM_MIN_SHAPE_FREQ), cfg.zeta . x );
598603 #endif
599604 #if HAS_Y_AXIS
600- const float yf = cfg.baseFreq [Y_AXIS] + cfg.dynFreqK [Y_AXIS] * traj. z [makeVector_batchIdx] ;
601- shaping.y .set_axis_shaping_N (cfg.shaper [Y_AXIS] , _MAX (yf, FTM_MIN_SHAPE_FREQ), cfg.zeta [Y_AXIS] );
605+ const float yf = cfg.baseFreq . y + cfg.dynFreqK . y * z ;
606+ shaping.y .set_axis_shaping_N (cfg.shaper . y , _MAX (yf, FTM_MIN_SHAPE_FREQ), cfg.zeta . y );
602607 #endif
603608 }
604- break ;
609+ } break ;
605610 #endif
606611
607612 #if HAS_DYNAMIC_FREQ_G
608613 case dynFreqMode_MASS_BASED:
609614 // Update constantly. The optimization done for Z value makes
610615 // less sense for E, as E is expected to constantly change.
611616 #if HAS_X_AXIS
612- shaping.x .set_axis_shaping_N (cfg.shaper [X_AXIS], cfg.baseFreq [X_AXIS] + cfg.dynFreqK [X_AXIS] * traj.e [makeVector_batchIdx], cfg.zeta [X_AXIS] );
617+ shaping.x .set_axis_shaping_N (cfg.shaper . x , cfg.baseFreq . x + cfg.dynFreqK . x * traj.e [makeVector_batchIdx], cfg.zeta . x );
613618 #endif
614619 #if HAS_Y_AXIS
615- shaping.y .set_axis_shaping_N (cfg.shaper [Y_AXIS] , cfg.baseFreq [Y_AXIS] + cfg.dynFreqK [Y_AXIS] * traj.e [makeVector_batchIdx], cfg.zeta [Y_AXIS] );
620+ shaping.y .set_axis_shaping_N (cfg.shaper . y , cfg.baseFreq . y + cfg.dynFreqK . y * traj.e [makeVector_batchIdx], cfg.zeta . y );
616621 #endif
617622 break ;
618623 #endif
@@ -722,7 +727,7 @@ void FTMotion::convertToSteps(const uint32_t idx) {
722727 err_P += delta;
723728
724729 // Set up step/dir bits for all axes
725- #define _COMMAND_RUN (AXIS ) command_set[_AXIS(AXIS )](err_P[_AXIS(AXIS)] , steps[_AXIS(AXIS)] , cmd, _BV(FT_BIT_DIR_##AXIS ), _BV(FT_BIT_STEP_##AXIS ));
730+ #define _COMMAND_RUN (A ) command_set[_AXIS(A )](err_P.A , steps.A , cmd, _BV(FT_BIT_DIR_##A ), _BV(FT_BIT_STEP_##A ));
726731 LOGICAL_AXIS_MAP (_COMMAND_RUN);
727732
728733 // Next circular buffer index
0 commit comments