Removed the non-working pressure advance feature.

Improved accuracy of diagonal moves by oversampling the path discretization.
Accelerated the planner by rewriting time critical routines from floating
point to fixed point arithmetics.

Firmware/Marlin_main.cpp

@@ -1038,22 +1038,15 @@ void setup()
   // is being written into the EEPROM, so the update procedure will be triggered only once.
     lang_selected = eeprom_read_byte((uint8_t*)EEPROM_LANG);
     if (lang_selected >= LANG_NUM){
-    lcd_mylang();
+      lcd_mylang();
     }
     
   if (eeprom_read_byte((uint8_t*)EEPROM_BABYSTEP_Z_SET) == 0x0ff) {
       // Reset the babystepping values, so the printer will not move the Z axis up when the babystepping is enabled.
-      // eeprom_update_byte((uint8_t*)EEPROM_BABYSTEP_X, 0x0ff);
-      // eeprom_update_byte((uint8_t*)EEPROM_BABYSTEP_Y, 0x0ff);
-      eeprom_update_byte((uint8_t*)EEPROM_BABYSTEP_Z, 0x0ff);
-      // Get the selected laugnage index before display update.
-      lang_selected = eeprom_read_byte((uint8_t*)EEPROM_LANG);
-      if (lang_selected >= LANG_NUM)
-          lang_selected = LANG_ID_DEFAULT; // Czech language
+      eeprom_update_word((uint16_t*)EEPROM_BABYSTEP_Z, 0);
       // Show the message.
       lcd_show_fullscreen_message_and_wait_P(MSG_BABYSTEP_Z_NOT_SET);
       lcd_update_enable(true);
-      // lcd_implementation_clear();
   }
 
   // Store the currently running firmware into an eeprom,
@@ -2961,8 +2954,9 @@ void process_commands()
                 bool result = sample_mesh_and_store_reference();
                 // if (result) babystep_apply();
             } else {
-                // Reset the baby step value.
+                // Reset the baby step value and the baby step applied flag.
                 eeprom_write_byte((unsigned char*)EEPROM_BABYSTEP_Z_SET, 0xFF);
+                eeprom_update_word((uint16_t*)EEPROM_BABYSTEP_Z, 0);
                 // Complete XYZ calibration.
                 BedSkewOffsetDetectionResultType result = find_bed_offset_and_skew(verbosity_level);
                 uint8_t point_too_far_mask = 0;
@@ -4820,11 +4814,7 @@ void prepare_move()
 
   // Do not use feedmultiply for E or Z only moves
   if( (current_position[X_AXIS] == destination [X_AXIS]) && (current_position[Y_AXIS] == destination [Y_AXIS])) {
-#ifdef MESH_BED_LEVELING
-      mesh_plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
-#else
       plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
-#endif
   }
   else {
 #ifdef MESH_BED_LEVELING

Firmware/planner.cpp

@@ -110,6 +110,11 @@ block_t block_buffer[BLOCK_BUFFER_SIZE];            // A ring buffer for motion
 volatile unsigned char block_buffer_head;           // Index of the next block to be pushed
 volatile unsigned char block_buffer_tail;           // Index of the block to process now
 
+#ifdef PLANNER_DIAGNOSTICS
+// Diagnostic function: Minimum number of planned moves since the last 
+static uint8_t g_cntr_planner_queue_min = 0;
+#endif /* PLANNER_DIAGNOSTICS */
+
 //===========================================================================
 //=============================private variables ============================
 //===========================================================================
@@ -171,59 +176,86 @@ FORCE_INLINE float intersection_distance(float initial_rate, float final_rate, f
   }
 }
 
-// Calculates trapezoid parameters so that the entry- and exit-speed is compensated by the provided factors.
+#define MINIMAL_STEP_RATE 120
 
-void calculate_trapezoid_for_block(block_t *block, float entry_factor, float exit_factor) {
-  unsigned long initial_rate = ceil(block->nominal_rate*entry_factor); // (step/min)
-  unsigned long final_rate = ceil(block->nominal_rate*exit_factor); // (step/min)
+// Calculates trapezoid parameters so that the entry- and exit-speed is compensated by the provided factors.
+void calculate_trapezoid_for_block(block_t *block, float entry_speed, float exit_speed) 
+{
+  // These two lines are the only floating point calculations performed in this routine.
+  uint32_t initial_rate = ceil(entry_speed * block->speed_factor); // (step/min)
+  uint32_t final_rate   = ceil(exit_speed  * block->speed_factor); // (step/min)
 
   // Limit minimal step rate (Otherwise the timer will overflow.)
-  if(initial_rate <120) {
-    initial_rate=120; 
-  }
-  if(final_rate < 120) {
-    final_rate=120;  
-  }
-
-  long acceleration = block->acceleration_st;
-  int32_t accelerate_steps =
-    ceil(estimate_acceleration_distance(initial_rate, block->nominal_rate, acceleration));
-  int32_t decelerate_steps =
-    floor(estimate_acceleration_distance(block->nominal_rate, final_rate, -acceleration));
-
-  // Calculate the size of Plateau of Nominal Rate.
-  int32_t plateau_steps = block->step_event_count-accelerate_steps-decelerate_steps;
+  if (initial_rate < MINIMAL_STEP_RATE)
+      initial_rate = MINIMAL_STEP_RATE;
+  if (initial_rate > block->nominal_rate)
+      initial_rate = block->nominal_rate;
+  if (final_rate < MINIMAL_STEP_RATE)
+      final_rate = MINIMAL_STEP_RATE;
+  if (final_rate > block->nominal_rate)
+      final_rate = block->nominal_rate;
+
+  uint32_t acceleration      = block->acceleration_st;
+  if (acceleration == 0)
+      // Don't allow zero acceleration.
+      acceleration = 1;
+  // estimate_acceleration_distance(float initial_rate, float target_rate, float acceleration)
+  // (target_rate*target_rate-initial_rate*initial_rate)/(2.0*acceleration));
+  uint32_t initial_rate_sqr  = initial_rate*initial_rate;
+  //FIXME assert that this result fits a 64bit unsigned int.
+  uint32_t nominal_rate_sqr  = block->nominal_rate*block->nominal_rate;
+  uint32_t final_rate_sqr    = final_rate*final_rate;
+  uint32_t acceleration_x2   = acceleration << 1;
+  // ceil(estimate_acceleration_distance(initial_rate, block->nominal_rate, acceleration));
+  uint32_t accelerate_steps  = (nominal_rate_sqr - initial_rate_sqr + acceleration_x2 - 1) / acceleration_x2;
+  // floor(estimate_acceleration_distance(block->nominal_rate, final_rate, -acceleration));
+  uint32_t decelerate_steps  = (nominal_rate_sqr - final_rate_sqr) / acceleration_x2;
+  uint32_t accel_decel_steps = accelerate_steps + decelerate_steps;
+  // Size of Plateau of Nominal Rate.
+  uint32_t plateau_steps     = 0;
 
   // 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 (plateau_steps < 0) {
-    accelerate_steps = ceil(intersection_distance(initial_rate, final_rate, acceleration, block->step_event_count));
-    accelerate_steps = max(accelerate_steps,0); // Check limits due to numerical round-off
-    accelerate_steps = min((uint32_t)accelerate_steps,block->step_event_count);//(We can cast here to unsigned, because the above line ensures that we are above zero)
-    plateau_steps = 0;
+  if (accel_decel_steps < block->step_event_count) {
+    plateau_steps = block->step_event_count - accel_decel_steps;
+  } else {
+    uint32_t acceleration_x4  = acceleration << 2;
+    // Avoid negative numbers
+    if (final_rate_sqr >= initial_rate_sqr) {
+        // accelerate_steps = ceil(intersection_distance(initial_rate, final_rate, acceleration, block->step_event_count));
+        // intersection_distance(float initial_rate, float final_rate, float acceleration, float distance) 
+        // (2.0*acceleration*distance-initial_rate*initial_rate+final_rate*final_rate)/(4.0*acceleration);
+//        accelerate_steps = (block->step_event_count >> 1) + (final_rate_sqr - initial_rate_sqr + acceleration_x4 - 1 + (block->step_event_count & 1) * acceleration_x2) / acceleration_x4;
+        accelerate_steps = final_rate_sqr - initial_rate_sqr + acceleration_x4 - 1;
+        if (block->step_event_count & 1)
+            accelerate_steps += acceleration_x2;
+        accelerate_steps += block->step_event_count >> 1;
+        accelerate_steps /= acceleration_x4;
+        if (accelerate_steps > block->step_event_count)
+            accelerate_steps = block->step_event_count;
+    } else {
+//        decelerate_steps = (block->step_event_count >> 1) + (initial_rate_sqr - final_rate_sqr + (block->step_event_count & 1) * acceleration_x2) / acceleration_x4;
+        decelerate_steps = initial_rate_sqr - final_rate_sqr;
+        if (block->step_event_count & 1)
+            decelerate_steps += acceleration_x2;
+        decelerate_steps += block->step_event_count >> 1;
+        decelerate_steps /= acceleration_x4;
+        if (decelerate_steps > block->step_event_count)
+            decelerate_steps = block->step_event_count;
+        accelerate_steps = block->step_event_count - decelerate_steps;
+    }
   }
 
-#ifdef ADVANCE
-  volatile long initial_advance = block->advance*entry_factor*entry_factor; 
-  volatile long final_advance = block->advance*exit_factor*exit_factor;
-#endif // ADVANCE
-
-  // block->accelerate_until = accelerate_steps;
-  // block->decelerate_after = accelerate_steps+plateau_steps;
   CRITICAL_SECTION_START;  // Fill variables used by the stepper in a critical section
   if (! block->busy) { // Don't update variables if block is busy.
     block->accelerate_until = accelerate_steps;
     block->decelerate_after = accelerate_steps+plateau_steps;
     block->initial_rate = initial_rate;
     block->final_rate = final_rate;
-#ifdef ADVANCE
-    block->initial_advance = initial_advance;
-    block->final_advance = final_advance;
-#endif //ADVANCE
   }
   CRITICAL_SECTION_END;
-}                    
+}
 
 // Calculates the maximum allowable entry speed, when you must be able to reach target_velocity using the 
 // decceleration within the allotted distance.
@@ -249,6 +281,27 @@ FORCE_INLINE float max_allowable_entry_speed(float decceleration, float target_v
 // the set limit. Finally it will:
 //
 //   3. Recalculate trapezoids for all blocks.
+//
+//FIXME This routine is called 15x every time a new line is added to the planner,
+// therefore it is a bottle neck and it shall be rewritten into a Fixed Point arithmetics,
+// if the CPU is found lacking computational power.
+//
+// Following sources may be used to optimize the 8-bit AVR code:
+// http://www.mikrocontroller.net/articles/AVR_Arithmetik
+// http://darcy.rsgc.on.ca/ACES/ICE4M/FixedPoint/avrfix.pdf
+// 
+// https://github.com/gcc-mirror/gcc/blob/master/libgcc/config/avr/lib1funcs-fixed.S
+// https://gcc.gnu.org/onlinedocs/gcc/Fixed-Point.html
+// https://gcc.gnu.org/onlinedocs/gccint/Fixed-point-fractional-library-routines.html
+// 
+// https://ucexperiment.wordpress.com/2015/04/04/arduino-s15-16-fixed-point-math-routines/
+// https://mekonik.wordpress.com/2009/03/18/arduino-avr-gcc-multiplication/
+// https://github.com/rekka/avrmultiplication
+// 
+// https://people.ece.cornell.edu/land/courses/ece4760/Math/Floating_point/
+// https://courses.cit.cornell.edu/ee476/Math/
+// https://courses.cit.cornell.edu/ee476/Math/GCC644/fixedPt/multASM.S
+//
 void planner_recalculate(const float &safe_final_speed) 
 {
     // Reverse pass
@@ -291,8 +344,12 @@ void planner_recalculate(const float &safe_final_speed)
                 // segment and the maximum acceleration allowed for this segment.
                 // If nominal length true, max junction speed is guaranteed to be reached even if decelerating to a jerk-from-zero velocity.
                 // Only compute for max allowable speed if block is decelerating and nominal length is false.
+                // entry_speed is uint16_t, 24 bits would be sufficient for block->acceleration and block->millimiteres, if scaled to um.
+                // therefore an optimized assembly 24bit x 24bit -> 32bit multiply would be more than sufficient
+                // together with an assembly 32bit->16bit sqrt function.
                 current->entry_speed = ((current->flag & BLOCK_FLAG_NOMINAL_LENGTH) || current->max_entry_speed <= next->entry_speed) ?
                     current->max_entry_speed :
+                    // min(current->max_entry_speed, sqrt(next->entry_speed*next->entry_speed+2*current->acceleration*current->millimeters));
                     min(current->max_entry_speed, max_allowable_entry_speed(-current->acceleration,next->entry_speed,current->millimeters));
                 current->flag |= BLOCK_FLAG_RECALCULATE;
             }
@@ -325,7 +382,7 @@ void planner_recalculate(const float &safe_final_speed)
             // Recalculate if current block entry or exit junction speed has changed.
             if ((prev->flag | current->flag) & BLOCK_FLAG_RECALCULATE) {
                 // NOTE: Entry and exit factors always > 0 by all previous logic operations.
-                calculate_trapezoid_for_block(prev, prev->entry_speed/prev->nominal_speed, current->entry_speed/prev->nominal_speed);
+                calculate_trapezoid_for_block(prev, prev->entry_speed, current->entry_speed);
                 // Reset current only to ensure next trapezoid is computed.
                 prev->flag &= ~BLOCK_FLAG_RECALCULATE;
             }
@@ -338,7 +395,7 @@ void planner_recalculate(const float &safe_final_speed)
 
     // Last/newest block in buffer. Exit speed is set with safe_final_speed. Always recalculated.
     current = block_buffer + prev_block_index(block_buffer_head);
-    calculate_trapezoid_for_block(current, current->entry_speed/current->nominal_speed, safe_final_speed/current->nominal_speed);
+    calculate_trapezoid_for_block(current, current->entry_speed, safe_final_speed);
     current->flag &= ~BLOCK_FLAG_RECALCULATE;
 
 //    SERIAL_ECHOLNPGM("planner_recalculate - 4");
@@ -471,6 +528,15 @@ void planner_abort_soft()
 }
 */
 
+#ifdef PLANNER_DIAGNOSTICS
+static inline void planner_update_queue_min_counter()
+{
+  uint8_t new_counter = moves_planned();
+  if (new_counter < g_cntr_planner_queue_min)
+    g_cntr_planner_queue_min = new_counter;
+}
+#endif /* PLANNER_DIAGNOSTICS */
+
 void planner_abort_hard()
 {
     // Abort the stepper routine and flush the planner queue.
@@ -527,11 +593,18 @@ void plan_buffer_line(float x, float y, float z, const float &e, float feed_rate
           manage_inactivity(false); 
           lcd_update();
       } while (block_buffer_tail == next_buffer_head);
-      if (waiting_inside_plan_buffer_line_print_aborted)
+      if (waiting_inside_plan_buffer_line_print_aborted) {
           // Inside the lcd_update() routine the print has been aborted.
           // Cancel the print, do not plan the current line this routine is waiting on.
+#ifdef PLANNER_DIAGNOSTICS
+          planner_update_queue_min_counter();
+#endif /* PLANNER_DIAGNOSTICS */
           return;
+      }
   }
+#ifdef PLANNER_DIAGNOSTICS
+  planner_update_queue_min_counter();
+#endif /* PLANNER_DIAGNOSTICS */
 
 #ifdef ENABLE_AUTO_BED_LEVELING
   apply_rotation_xyz(plan_bed_level_matrix, x, y, z);
@@ -637,14 +710,20 @@ block->steps_y = labs((target[X_AXIS]-position[X_AXIS]) - (target[Y_AXIS]-positi
 #endif
   block->steps_z = labs(target[Z_AXIS]-position[Z_AXIS]);
   block->steps_e = labs(target[E_AXIS]-position[E_AXIS]);
-  block->steps_e *= volumetric_multiplier[active_extruder];
-  block->steps_e *= extrudemultiply;
-  block->steps_e /= 100;
+  if (volumetric_multiplier[active_extruder] != 1.f)
+    block->steps_e *= volumetric_multiplier[active_extruder];
+  if (extrudemultiply != 100) {
+    block->steps_e *= extrudemultiply;
+    block->steps_e /= 100;
+  }
   block->step_event_count = max(block->steps_x, max(block->steps_y, max(block->steps_z, block->steps_e)));
 
   // Bail if this is a zero-length block
   if (block->step_event_count <= dropsegments)
   { 
+#ifdef PLANNER_DIAGNOSTICS
+    planner_update_queue_min_counter();
+#endif /* PLANNER_DIAGNOSTICS */
     return; 
   }
 
@@ -869,6 +948,8 @@ Having the real displacement of the head, we can calculate the total movement le
   }
 
   // Compute and limit the acceleration rate for the trapezoid generator.  
+  // block->step_event_count ... event count of the fastest axis
+  // block->millimeters ... Euclidian length of the XYZ movement or the E length, if no XYZ movement.
   float steps_per_mm = block->step_event_count/block->millimeters;
   if(block->steps_x == 0 && block->steps_y == 0 && block->steps_z == 0)
   {
@@ -888,49 +969,27 @@ Having the real displacement of the head, we can calculate the total movement le
     if(((float)block->acceleration_st * (float)block->steps_z / (float)block->step_event_count ) > axis_steps_per_sqr_second[Z_AXIS])
       block->acceleration_st = axis_steps_per_sqr_second[Z_AXIS];
   }
+  // Acceleration of the segment, in mm/sec^2
   block->acceleration = block->acceleration_st / steps_per_mm;
-  block->acceleration_rate = (long)((float)block->acceleration_st * (16777216.0 / (F_CPU / 8.0)));
 
-#if 0  // Use old jerk for now
-  // Compute path unit vector
-  double unit_vec[3];
-
-  unit_vec[X_AXIS] = delta_mm[X_AXIS]*inverse_millimeters;
-  unit_vec[Y_AXIS] = delta_mm[Y_AXIS]*inverse_millimeters;
-  unit_vec[Z_AXIS] = delta_mm[Z_AXIS]*inverse_millimeters;
-
-  // Compute maximum allowable entry speed at junction by centripetal acceleration approximation.
-  // Let a circle be tangent to both previous and current path line segments, where the junction
-  // deviation is defined as the distance from the junction to the closest edge of the circle,
-  // colinear with the circle center. The circular segment joining the two paths represents the
-  // path of centripetal acceleration. Solve for max velocity based on max acceleration about the
-  // radius of the circle, defined indirectly by junction deviation. This may be also viewed as
-  // path width or max_jerk in the previous grbl version. This approach does not actually deviate
-  // from path, but used as a robust way to compute cornering speeds, as it takes into account the
-  // nonlinearities of both the junction angle and junction velocity.
-  double vmax_junction = MINIMUM_PLANNER_SPEED; // Set default max junction speed
-
-  // Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles.
-  if ((block_buffer_head != block_buffer_tail) && (previous_nominal_speed > 0.0)) {
-    // Compute cosine of angle between previous and current path. (prev_unit_vec is negative)
-    // NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity.
-    double cos_theta = - previous_unit_vec[X_AXIS] * unit_vec[X_AXIS]
-      - previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS]
-      - previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] ;
-
-    // Skip and use default max junction speed for 0 degree acute junction.
-    if (cos_theta < 0.95) {
-      vmax_junction = min(previous_nominal_speed,block->nominal_speed);
-      // Skip and avoid divide by zero for straight junctions at 180 degrees. Limit to min() of nominal speeds.
-      if (cos_theta > -0.95) {
-        // Compute maximum junction velocity based on maximum acceleration and junction deviation
-        double sin_theta_d2 = sqrt(0.5*(1.0-cos_theta)); // Trig half angle identity. Always positive.
-        vmax_junction = min(vmax_junction,
-        sqrt(block->acceleration * junction_deviation * sin_theta_d2/(1.0-sin_theta_d2)) );
-      }
-    }
+#if 1
+  // Oversample diagonal movements by a power of 2 up to 8x
+  // to achieve more accurate diagonal movements.
+  uint8_t bresenham_oversample = 1;
+  for (uint8_t i = 0; i < 3; ++ i) {
+    if (block->nominal_rate >= 5000) // 5kHz
+      break;
+    block->nominal_rate << 1;
+    bresenham_oversample << 1;
+    block->step_event_count << 1;
   }
+  if (bresenham_oversample > 1)
+    // Lower the acceleration steps/sec^2 to account for the oversampling.
+    block->acceleration_st = (block->acceleration_st + (bresenham_oversample >> 1)) / bresenham_oversample;
 #endif
+
+  block->acceleration_rate = (long)((float)block->acceleration_st * (16777216.0 / (F_CPU / 8.0)));
+
   // 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;
@@ -1047,34 +1106,9 @@ Having the real displacement of the head, we can calculate the total movement le
   previous_nominal_speed = block->nominal_speed;
   previous_safe_speed = safe_speed;
 
-#ifdef ADVANCE
-  // Calculate advance rate
-  if((block->steps_e == 0) || (block->steps_x == 0 && block->steps_y == 0 && block->steps_z == 0)) {
-    block->advance_rate = 0;
-    block->advance = 0;
-  }
-  else {
-    long acc_dist = estimate_acceleration_distance(0, block->nominal_rate, block->acceleration_st);
-    float advance = (STEPS_PER_CUBIC_MM_E * EXTRUDER_ADVANCE_K) * 
-      (current_speed[E_AXIS] * current_speed[E_AXIS] * EXTRUSION_AREA * EXTRUSION_AREA)*256;
-    block->advance = advance;
-    if(acc_dist == 0) {
-      block->advance_rate = 0;
-    } 
-    else {
-      block->advance_rate = advance / (float)acc_dist;
-    }
-  }
-  /*
-    SERIAL_ECHO_START;
-   SERIAL_ECHOPGM("advance :");
-   SERIAL_ECHO(block->advance/256.0);
-   SERIAL_ECHOPGM("advance rate :");
-   SERIAL_ECHOLN(block->advance_rate/256.0);
-   */
-#endif // ADVANCE
-
-  calculate_trapezoid_for_block(block, block->entry_speed/block->nominal_speed, safe_speed/block->nominal_speed);
+  // 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;
+  calculate_trapezoid_for_block(block, block->entry_speed, safe_speed);
 
   // Move the buffer head. From now the block may be picked up by the stepper interrupt controller.
   block_buffer_head = next_buffer_head;
@@ -1092,6 +1126,9 @@ Having the real displacement of the head, we can calculate the total movement le
 //  SERIAL_ECHO(int(moves_planned()));
 //  SERIAL_ECHOLNPGM("");
 
+#ifdef PLANNER_DIAGNOSTICS
+  planner_update_queue_min_counter();
+#endif /* PLANNER_DIAGNOSTIC */
   st_wake_up();
 }
 
@@ -1172,3 +1209,15 @@ void reset_acceleration_rates()
         axis_steps_per_sqr_second[i] = max_acceleration_units_per_sq_second[i] * axis_steps_per_unit[i];
         }
 }
+
+#ifdef PLANNER_DIAGNOSTICS
+uint8_t planner_queue_min()
+{
+  return g_cntr_planner_queue_min;
+}
+
+void planner_queue_min_reset()
+{
+  g_cntr_planner_queue_min = moves_planned();
+}
+#endif /* PLANNER_DIAGNOSTICS */

Firmware/planner.h

@@ -55,12 +55,6 @@ typedef struct {
   // accelerate_until and decelerate_after are set by calculate_trapezoid_for_block() and they need to be synchronized with the stepper interrupt controller.
   long accelerate_until;                    // The index of the step event on which to stop acceleration
   long decelerate_after;                    // The index of the step event on which to start decelerating
-  #ifdef ADVANCE
-    long advance_rate;
-    volatile long initial_advance;
-    volatile long final_advance;
-    float advance;
-  #endif
 
   // Fields used by the motion planner to manage acceleration
 //  float speed_x, speed_y, speed_z, speed_e;        // Nominal mm/sec for each axis
@@ -82,12 +76,18 @@ typedef struct {
 
   // Settings for the trapezoid generator (runs inside an interrupt handler).
   // Changing the following values in the planner needs to be synchronized with the interrupt handler by disabling the interrupts.
+  //FIXME nominal_rate, initial_rate and final_rate are limited to uint16_t by MultiU24X24toH16 in the stepper interrupt anyway!
   unsigned long nominal_rate;                        // The nominal step rate for this block in step_events/sec 
   unsigned long initial_rate;                        // The jerk-adjusted step rate at start of block  
   unsigned long final_rate;                          // The minimal rate at exit
   unsigned long acceleration_st;                     // acceleration steps/sec^2
+  //FIXME does it have to be unsigned long? Probably uint8_t would be just fine.
   unsigned long fan_speed;
   volatile char busy;
+
+
+  // Pre-calculated division for the calculate_trapezoid_for_block() routine to run faster.
+  float speed_factor;
 } block_t;
 
 #ifdef ENABLE_AUTO_BED_LEVELING
@@ -196,3 +196,11 @@ void set_extrude_min_temp(float temp);
 
 void reset_acceleration_rates();
 #endif
+
+// #define PLANNER_DIAGNOSTICS
+#ifdef PLANNER_DIAGNOSTICS
+// Diagnostic functions to display planner buffer underflow on the display.
+extern uint8_t planner_queue_min();
+// Diagnostic function: Reset the minimum planner segments.
+extern void planner_queue_min_reset();
+#endif /* PLANNER_DIAGNOSTICS */

Firmware/stepper.cpp

@@ -47,22 +47,17 @@ block_t *current_block;  // A pointer to the block currently being traced
 
 // Variables used by The Stepper Driver Interrupt
 static unsigned char out_bits;        // The next stepping-bits to be output
-static long counter_x,       // Counter variables for the bresenham line tracer
-            counter_y,
-            counter_z,
-            counter_e;
-volatile static unsigned long step_events_completed; // The number of step events executed in the current block
-#ifdef ADVANCE
-  static long advance_rate, advance, final_advance = 0;
-  static long old_advance = 0;
-  static long e_steps[3];
-#endif
-static long acceleration_time, deceleration_time;
+static int32_t counter_x,       // Counter variables for the bresenham line tracer
+               counter_y,
+               counter_z,
+               counter_e;
+volatile static uint32_t step_events_completed; // The number of step events executed in the current block
+static int32_t  acceleration_time, deceleration_time;
 //static unsigned long accelerate_until, decelerate_after, acceleration_rate, initial_rate, final_rate, nominal_rate;
-static unsigned short acc_step_rate; // needed for deccelaration start point
-static char step_loops;
-static unsigned short OCR1A_nominal;
-static unsigned short step_loops_nominal;
+static uint16_t acc_step_rate; // needed for deccelaration start point
+static uint8_t  step_loops;
+static uint16_t OCR1A_nominal;
+static uint8_t  step_loops_nominal;
 
 volatile long endstops_trigsteps[3]={0,0,0};
 volatile long endstops_stepsTotal,endstops_stepsDone;
@@ -306,13 +301,6 @@ FORCE_INLINE unsigned short calc_timer(unsigned short step_rate) {
 // Initializes the trapezoid generator from the current block. Called whenever a new
 // block begins.
 FORCE_INLINE void trapezoid_generator_reset() {
-  #ifdef ADVANCE
-    advance = current_block->initial_advance;
-    final_advance = current_block->final_advance;
-    // Do E steps + advance steps
-    e_steps[current_block->active_extruder] += ((advance >>8) - old_advance);
-    old_advance = advance >>8;
-  #endif
   deceleration_time = 0;
   // step_rate to timer interval
   OCR1A_nominal = calc_timer(current_block->nominal_rate);
@@ -359,10 +347,6 @@ ISR(TIMER1_COMPA_vect)
           return;
         }
       #endif
-
-//      #ifdef ADVANCE
-//      e_steps[current_block->active_extruder] = 0;
-//      #endif
     }
     else {
         OCR1A=2000; // 1kHz.
@@ -531,37 +515,20 @@ ISR(TIMER1_COMPA_vect)
     }
     #endif
 
-    #ifndef ADVANCE
-      if ((out_bits & (1<<E_AXIS)) != 0) {  // -direction
-        REV_E_DIR();
-        count_direction[E_AXIS]=-1;
-      }
-      else { // +direction
-        NORM_E_DIR();
-        count_direction[E_AXIS]=1;
-      }
-    #endif //!ADVANCE
-
-
+    if ((out_bits & (1<<E_AXIS)) != 0) {  // -direction
+      REV_E_DIR();
+      count_direction[E_AXIS]=-1;
+    }
+    else { // +direction
+      NORM_E_DIR();
+      count_direction[E_AXIS]=1;
+    }
 
-    for(int8_t i=0; i < step_loops; i++) { // Take multiple steps per interrupt (For high speed moves)
+    for(uint8_t i=0; i < step_loops; i++) { // Take multiple steps per interrupt (For high speed moves)
       #ifndef AT90USB
       MSerial.checkRx(); // Check for serial chars.
       #endif
 
-      #ifdef ADVANCE
-      counter_e += current_block->steps_e;
-      if (counter_e > 0) {
-        counter_e -= current_block->step_event_count;
-        if ((out_bits & (1<<E_AXIS)) != 0) { // - direction
-          e_steps[current_block->active_extruder]--;
-        }
-        else {
-          e_steps[current_block->active_extruder]++;
-        }
-      }
-      #endif //ADVANCE
-
         counter_x += current_block->steps_x;
         if (counter_x > 0) {
           WRITE(X_STEP_PIN, !INVERT_X_STEP_PIN);
@@ -604,7 +571,6 @@ ISR(TIMER1_COMPA_vect)
         #endif
       }
 
-      #ifndef ADVANCE
         counter_e += current_block->steps_e;
         if (counter_e > 0) {
           WRITE_E_STEP(!INVERT_E_STEP_PIN);
@@ -612,7 +578,6 @@ ISR(TIMER1_COMPA_vect)
           count_position[E_AXIS]+=count_direction[E_AXIS];
           WRITE_E_STEP(INVERT_E_STEP_PIN);
         }
-      #endif //!ADVANCE
       step_events_completed += 1;
       if(step_events_completed >= current_block->step_event_count) break;
     }
@@ -620,7 +585,7 @@ ISR(TIMER1_COMPA_vect)
     unsigned short timer;
     unsigned short step_rate;
     if (step_events_completed <= (unsigned long int)current_block->accelerate_until) {
-
+      // v = t * a   ->   acc_step_rate = acceleration_time * current_block->acceleration_rate
       MultiU24X24toH16(acc_step_rate, acceleration_time, current_block->acceleration_rate);
       acc_step_rate += current_block->initial_rate;
 
@@ -632,16 +597,6 @@ ISR(TIMER1_COMPA_vect)
       timer = calc_timer(acc_step_rate);
       OCR1A = timer;
       acceleration_time += timer;
-      #ifdef ADVANCE
-        for(int8_t i=0; i < step_loops; i++) {
-          advance += advance_rate;
-        }
-        //if(advance > current_block->advance) advance = current_block->advance;
-        // Do E steps + advance steps
-        e_steps[current_block->active_extruder] += ((advance >>8) - old_advance);
-        old_advance = advance >>8;
-
-      #endif // ADVANCE
     }
     else if (step_events_completed > (unsigned long int)current_block->decelerate_after) {
       MultiU24X24toH16(step_rate, deceleration_time, current_block->acceleration_rate);
@@ -661,15 +616,6 @@ ISR(TIMER1_COMPA_vect)
       timer = calc_timer(step_rate);
       OCR1A = timer;
       deceleration_time += timer;
-      #ifdef ADVANCE
-        for(int8_t i=0; i < step_loops; i++) {
-          advance -= advance_rate;
-        }
-        if(advance < final_advance) advance = final_advance;
-        // Do E steps + advance steps
-        e_steps[current_block->active_extruder] += ((advance >>8) - old_advance);
-        old_advance = advance >>8;
-      #endif //ADVANCE
     }
     else {
       OCR1A = OCR1A_nominal;
@@ -685,63 +631,6 @@ ISR(TIMER1_COMPA_vect)
   }
 }
 
-#ifdef ADVANCE
-  unsigned char old_OCR0A;
-  // Timer interrupt for E. e_steps is set in the main routine;
-  // Timer 0 is shared with millies
-  ISR(TIMER0_COMPA_vect)
-  {
-    old_OCR0A += 52; // ~10kHz interrupt (250000 / 26 = 9615kHz)
-    OCR0A = old_OCR0A;
-    // Set E direction (Depends on E direction + advance)
-    for(unsigned char i=0; i<4;i++) {
-      if (e_steps[0] != 0) {
-        WRITE(E0_STEP_PIN, INVERT_E_STEP_PIN);
-        if (e_steps[0] < 0) {
-          WRITE(E0_DIR_PIN, INVERT_E0_DIR);
-          e_steps[0]++;
-          WRITE(E0_STEP_PIN, !INVERT_E_STEP_PIN);
-        }
-        else if (e_steps[0] > 0) {
-          WRITE(E0_DIR_PIN, !INVERT_E0_DIR);
-          e_steps[0]--;
-          WRITE(E0_STEP_PIN, !INVERT_E_STEP_PIN);
-        }
-      }
- #if EXTRUDERS > 1
-      if (e_steps[1] != 0) {
-        WRITE(E1_STEP_PIN, INVERT_E_STEP_PIN);
-        if (e_steps[1] < 0) {
-          WRITE(E1_DIR_PIN, INVERT_E1_DIR);
-          e_steps[1]++;
-          WRITE(E1_STEP_PIN, !INVERT_E_STEP_PIN);
-        }
-        else if (e_steps[1] > 0) {
-          WRITE(E1_DIR_PIN, !INVERT_E1_DIR);
-          e_steps[1]--;
-          WRITE(E1_STEP_PIN, !INVERT_E_STEP_PIN);
-        }
-      }
- #endif
- #if EXTRUDERS > 2
-      if (e_steps[2] != 0) {
-        WRITE(E2_STEP_PIN, INVERT_E_STEP_PIN);
-        if (e_steps[2] < 0) {
-          WRITE(E2_DIR_PIN, INVERT_E2_DIR);
-          e_steps[2]++;
-          WRITE(E2_STEP_PIN, !INVERT_E_STEP_PIN);
-        }
-        else if (e_steps[2] > 0) {
-          WRITE(E2_DIR_PIN, !INVERT_E2_DIR);
-          e_steps[2]--;
-          WRITE(E2_STEP_PIN, !INVERT_E_STEP_PIN);
-        }
-      }
- #endif
-    }
-  }
-#endif // ADVANCE
-
 void st_init()
 {
   digipot_init(); //Initialize Digipot Motor Current
@@ -930,17 +819,6 @@ void st_init()
   TCNT1 = 0;
   ENABLE_STEPPER_DRIVER_INTERRUPT();
 
-  #ifdef ADVANCE
-  #if defined(TCCR0A) && defined(WGM01)
-    TCCR0A &= ~(1<<WGM01);
-    TCCR0A &= ~(1<<WGM00);
-  #endif
-    e_steps[0] = 0;
-    e_steps[1] = 0;
-    e_steps[2] = 0;
-    TIMSK0 |= (1<<OCIE0A);
-  #endif //ADVANCE
-
   enable_endstops(true); // Start with endstops active. After homing they can be disabled
   sei();
 }

Firmware/temperature.cpp

@@ -1211,6 +1211,7 @@ void max_temp_error(uint8_t e) {
     WRITE(FAN_PIN, 1);
     WRITE(EXTRUDER_0_AUTO_FAN_PIN, 1);
     WRITE(BEEPER, 1);
+    // fanSpeed will consumed by the check_axes_activity() routine.
     fanSpeed=255;
 }
 

Firmware/ultralcd.cpp

@@ -327,7 +327,6 @@ void set_language_from_EEPROM() {
   }
 }
 
-void lcd_mylang();
 static void lcd_status_screen()
 {
 	

Firmware/ultralcd_implementation_hitachi_HD44780.h

@@ -659,6 +659,7 @@ static void lcd_implementation_status_screen()
 
     //Print the Z coordinates
     lcd.setCursor(LCD_WIDTH - 8-2, 0);
+#if 1
     lcd_printPGM(PSTR("  Z"));
     if (custom_message_type == 1) {
         // In a bed calibration mode.
@@ -667,6 +668,11 @@ static void lcd_implementation_status_screen()
         lcd.print(ftostr32sp(current_position[Z_AXIS] + 0.00001));
         lcd.print(' ');
     }
+#else
+    lcd_printPGM(PSTR(" Queue:"));
+    lcd.print(int(moves_planned()));
+    lcd.print(' ');
+#endif
 
     //Print the Bedtemperature
     lcd.setCursor(0, 1);
@@ -679,15 +685,31 @@ static void lcd_implementation_status_screen()
     lcd_printPGM(PSTR(LCD_STR_DEGREE " "));
     lcd_printPGM(PSTR("  "));
 
+#if 1
     //Print Feedrate
     lcd.setCursor(LCD_WIDTH - 8-2, 1);
     lcd_printPGM(PSTR("  "));
     lcd.print(LCD_STR_FEEDRATE[0]);
     lcd.print(itostr3(feedmultiply));
-    lcd.print('%');
-    lcd_printPGM(PSTR("     "));
-
-
+    lcd_printPGM(PSTR("%%     "));
+#else
+    //Print Feedrate
+    lcd.setCursor(LCD_WIDTH - 8-2, 1);
+    lcd.print(LCD_STR_FEEDRATE[0]);
+    lcd.print(itostr3(feedmultiply));
+    lcd_printPGM(PSTR("%  Q"));
+    {
+      uint8_t queue = planner_queue_min();
+      if (queue < (BLOCK_BUFFER_SIZE >> 1)) {
+        lcd.print('!');
+      } else {
+        lcd.print((char)(queue / 10) + '0');
+        queue %= 10;
+      }
+      lcd.print((char)queue + '0');
+      planner_queue_min_reset();
+    }
+#endif
 	
     //Print SD status
     lcd.setCursor(0, 2);