|
@@ -793,19 +793,21 @@ void Planner::calculate_trapezoid_for_block(block_t * const block, const_float_t
|
793
|
793
|
uint32_t cruise_rate = block->nominal_rate;
|
794
|
794
|
#endif
|
795
|
795
|
|
796
|
|
- const int32_t accel = block->acceleration_steps_per_s2;
|
797
|
|
-
|
798
|
796
|
// Steps for acceleration, plateau and deceleration
|
799
|
797
|
int32_t plateau_steps = block->step_event_count;
|
800
|
798
|
uint32_t accelerate_steps = 0,
|
801
|
799
|
decelerate_steps = 0;
|
802
|
800
|
|
|
801
|
+ const int32_t accel = block->acceleration_steps_per_s2;
|
|
802
|
+ float inverse_accel = 0.0f;
|
803
|
803
|
if (accel != 0) {
|
804
|
|
- // Steps required for acceleration, deceleration to/from nominal rate
|
805
|
|
- const float nominal_rate_sq = sq(float(block->nominal_rate));
|
806
|
|
- float accelerate_steps_float = (nominal_rate_sq - sq(float(initial_rate))) * (0.5f / accel);
|
|
804
|
+ const float inverse_accel = 1.0f / accel,
|
|
805
|
+ half_inverse_accel = 0.5f * inverse_accel,
|
|
806
|
+ nominal_rate_sq = sq(float(block->nominal_rate)),
|
|
807
|
+ // Steps required for acceleration, deceleration to/from nominal rate
|
|
808
|
+ decelerate_steps_float = half_inverse_accel * (nominal_rate_sq - sq(float(final_rate)));
|
|
809
|
+ float accelerate_steps_float = half_inverse_accel * (nominal_rate_sq - sq(float(initial_rate)));
|
807
|
810
|
accelerate_steps = CEIL(accelerate_steps_float);
|
808
|
|
- const float decelerate_steps_float = (nominal_rate_sq - sq(float(final_rate))) * (0.5f / accel);
|
809
|
811
|
decelerate_steps = FLOOR(decelerate_steps_float);
|
810
|
812
|
|
811
|
813
|
// Steps between acceleration and deceleration, if any
|
|
@@ -828,9 +830,10 @@ void Planner::calculate_trapezoid_for_block(block_t * const block, const_float_t
|
828
|
830
|
}
|
829
|
831
|
|
830
|
832
|
#if ENABLED(S_CURVE_ACCELERATION)
|
|
833
|
+ const float rate_factor = inverse_accel * (STEPPER_TIMER_RATE);
|
831
|
834
|
// Jerk controlled speed requires to express speed versus time, NOT steps
|
832
|
|
- uint32_t acceleration_time = (float(cruise_rate - initial_rate) / accel) * (STEPPER_TIMER_RATE),
|
833
|
|
- deceleration_time = (float(cruise_rate - final_rate) / accel) * (STEPPER_TIMER_RATE),
|
|
835
|
+ uint32_t acceleration_time = rate_factor * float(cruise_rate - initial_rate),
|
|
836
|
+ deceleration_time = rate_factor * float(cruise_rate - final_rate),
|
834
|
837
|
// And to offload calculations from the ISR, we also calculate the inverse of those times here
|
835
|
838
|
acceleration_time_inverse = get_period_inverse(acceleration_time),
|
836
|
839
|
deceleration_time_inverse = get_period_inverse(deceleration_time);
|