Remove cumulative error in segments without cruising (take 3)

Remove another division by precomputing the division directly in
adv_comp.

Firmware/planner.cpp

@@ -225,15 +225,15 @@ void calculate_trapezoid_for_block(block_t *block, float entry_speed, float exit
   uint32_t accel_decel_steps = accelerate_steps + decelerate_steps;
   // Size of Plateau of Nominal Rate.
   uint32_t plateau_steps     = 0;
-  // Maximum effective speed reached in the trapezoid (mm/s)
-  float max_speed;
+  // Maximum effective speed reached in the trapezoid (step/min)
+  float max_rate;
 
   // Is the Plateau of Nominal Rate smaller than nothing? That means no cruising, and we will
   // have to use intersection_distance() to calculate when to abort acceleration and start braking
   // in order to reach the final_rate exactly at the end of this block.
   if (accel_decel_steps < block->step_event_count.wide) {
     plateau_steps = block->step_event_count.wide - accel_decel_steps;
-    max_speed = block->nominal_speed;
+    max_rate = block->nominal_rate;
   } else {
     uint32_t acceleration_x4  = acceleration << 2;
     // Avoid negative numbers
@@ -267,15 +267,15 @@ void calculate_trapezoid_for_block(block_t *block, float entry_speed, float exit
         accelerate_steps = block->step_event_count.wide - decelerate_steps;
     }
 
-    max_speed = sqrt(acceleration_x2 * accelerate_steps + initial_rate_sqr) / block->speed_factor;
+    max_rate = sqrt(acceleration_x2 * accelerate_steps + initial_rate_sqr);
   }
 
 #ifdef LIN_ADVANCE
   uint16_t final_adv_steps = 0;
   uint16_t max_adv_steps = 0;
   if (block->use_advance_lead) {
-      final_adv_steps = exit_speed * block->adv_comp;
-      max_adv_steps = max_speed * block->adv_comp;
+      final_adv_steps = final_rate * block->adv_comp;
+      max_adv_steps = max_rate * block->adv_comp;
   }
 #endif
 
@@ -1142,51 +1142,6 @@ Having the real displacement of the head, we can calculate the total movement le
 
   block->acceleration_rate = (long)((float)block->acceleration_st * (16777216.0 / (F_CPU / 8.0)));
 
-#ifdef LIN_ADVANCE
-  if (block->use_advance_lead) {
-      // the nominal speed doesn't change past this point: calculate the compression ratio for the
-      // segment (the required advance steps are computed during trapezoid planning)
-      block->adv_comp = extruder_advance_K * e_D_ratio * cs.axis_steps_per_unit[E_AXIS];
-
-      float advance_speed;
-      if (e_D_ratio > 0)
-          advance_speed = (extruder_advance_K * e_D_ratio * block->acceleration * cs.axis_steps_per_unit[E_AXIS]);
-      else
-          advance_speed = cs.max_jerk[E_AXIS] * cs.axis_steps_per_unit[E_AXIS];
-
-      // to save more space we avoid another copy of calc_timer and go through slow division, but we
-      // still need to replicate the *exact* same step grouping policy (see below)
-      if (advance_speed > MAX_STEP_FREQUENCY) advance_speed = MAX_STEP_FREQUENCY;
-      float advance_rate = (F_CPU / 8.0) / advance_speed;
-      if (advance_speed > 20000) {
-          block->advance_rate = advance_rate * 4;
-          block->advance_step_loops = 4;
-      }
-      else if (advance_speed > 10000) {
-          block->advance_rate = advance_rate * 2;
-          block->advance_step_loops = 2;
-      }
-      else
-      {
-          // never overflow the internal accumulator with very low rates
-          if (advance_rate < UINT16_MAX)
-              block->advance_rate = advance_rate;
-          else
-              block->advance_rate = UINT16_MAX;
-          block->advance_step_loops = 1;
-      }
-
-      #ifdef LA_DEBUG
-      if (block->advance_step_loops > 2)
-          // @wavexx: we should really check for the difference between step_loops and
-          //          advance_step_loops instead. A difference of more than 1 will lead
-          //          to uneven speed and *should* be adjusted here by furthermore
-          //          reducing the speed.
-          SERIAL_ECHOLNPGM("LA: More than 2 steps per eISR loop executed.");
-      #endif
-  }
-#endif
-
   // Start with a safe speed.
   // Safe speed is the speed, from which the machine may halt to stop immediately.
   float safe_speed = block->nominal_speed;
@@ -1312,6 +1267,53 @@ Having the real displacement of the head, we can calculate the total movement le
 
   // Precalculate the division, so when all the trapezoids in the planner queue get recalculated, the division is not repeated.
   block->speed_factor = block->nominal_rate / block->nominal_speed;
+
+#ifdef LIN_ADVANCE
+  if (block->use_advance_lead) {
+      // calculate the compression ratio for the segment (the required advance steps are computed
+      // during trapezoid planning)
+      float adv_comp = extruder_advance_K * e_D_ratio * cs.axis_steps_per_unit[E_AXIS]; // (step/(mm/s))
+      block->adv_comp = adv_comp / block->speed_factor; // step/(step/min)
+
+      float advance_speed;
+      if (e_D_ratio > 0)
+          advance_speed = (extruder_advance_K * e_D_ratio * block->acceleration * cs.axis_steps_per_unit[E_AXIS]);
+      else
+          advance_speed = cs.max_jerk[E_AXIS] * cs.axis_steps_per_unit[E_AXIS];
+
+      // to save more space we avoid another copy of calc_timer and go through slow division, but we
+      // still need to replicate the *exact* same step grouping policy (see below)
+      if (advance_speed > MAX_STEP_FREQUENCY) advance_speed = MAX_STEP_FREQUENCY;
+      float advance_rate = (F_CPU / 8.0) / advance_speed;
+      if (advance_speed > 20000) {
+          block->advance_rate = advance_rate * 4;
+          block->advance_step_loops = 4;
+      }
+      else if (advance_speed > 10000) {
+          block->advance_rate = advance_rate * 2;
+          block->advance_step_loops = 2;
+      }
+      else
+      {
+          // never overflow the internal accumulator with very low rates
+          if (advance_rate < UINT16_MAX)
+              block->advance_rate = advance_rate;
+          else
+              block->advance_rate = UINT16_MAX;
+          block->advance_step_loops = 1;
+      }
+
+      #ifdef LA_DEBUG
+      if (block->advance_step_loops > 2)
+          // @wavexx: we should really check for the difference between step_loops and
+          //          advance_step_loops instead. A difference of more than 1 will lead
+          //          to uneven speed and *should* be adjusted here by furthermore
+          //          reducing the speed.
+          SERIAL_ECHOLNPGM("LA: More than 2 steps per eISR loop executed.");
+      #endif
+  }
+#endif
+
   calculate_trapezoid_for_block(block, block->entry_speed, safe_speed);
 
   if (block->step_event_count.wide <= 32767)