From 4c6ea5af94e9a26513f031a93f5ac656faab6723 Mon Sep 17 00:00:00 2001
From: bubnikv <bubnikv@gmail.com>
Date: Tue, 2 Aug 2016 15:40:23 +0200
Subject: [PATCH] Fixed crashes when one moved the axes from the menu many
 times up/down, left/right until the planner queue got full.

---
 Firmware/planner.h    |  7 +++++++
 Firmware/ultralcd.cpp | 27 ++++++++++++++++-----------
 2 files changed, 23 insertions(+), 11 deletions(-)

diff --git a/Firmware/planner.h b/Firmware/planner.h
index 268c0c878..9a8f7b1fe 100644
--- a/Firmware/planner.h
+++ b/Firmware/planner.h
@@ -178,6 +178,13 @@ FORCE_INLINE uint8_t moves_planned() {
     return (block_buffer_head + BLOCK_BUFFER_SIZE - block_buffer_tail) & (BLOCK_BUFFER_SIZE - 1);
 }
 
+FORCE_INLINE bool planner_queue_full() {
+    unsigned char next_block_index = block_buffer_head;
+    if (++ next_block_index == BLOCK_BUFFER_SIZE)
+        next_block_index = 0; 
+    return block_buffer_tail == next_block_index;
+}
+
 #ifdef PREVENT_DANGEROUS_EXTRUDE
 void set_extrude_min_temp(float temp);
 #endif
diff --git a/Firmware/ultralcd.cpp b/Firmware/ultralcd.cpp
index 2e8aa07e9..13b6189da 100644
--- a/Firmware/ultralcd.cpp
+++ b/Firmware/ultralcd.cpp
@@ -1150,13 +1150,15 @@ static void lcd_menu_statistics()
 static void _lcd_move(const char *name, int axis, int min, int max) {
   if (encoderPosition != 0) {
     refresh_cmd_timeout();
-    current_position[axis] += float((int)encoderPosition) * move_menu_scale;
-    if (min_software_endstops && current_position[axis] < min) current_position[axis] = min;
-    if (max_software_endstops && current_position[axis] > max) current_position[axis] = max;
-    encoderPosition = 0;
-    world2machine_clamp(current_position[X_AXIS], current_position[Y_AXIS]);
-    plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], manual_feedrate[axis] / 60, active_extruder);
-    lcdDrawUpdate = 1;
+    if (! planner_queue_full()) {
+      current_position[axis] += float((int)encoderPosition) * move_menu_scale;
+      if (min_software_endstops && current_position[axis] < min) current_position[axis] = min;
+      if (max_software_endstops && current_position[axis] > max) current_position[axis] = max;
+      encoderPosition = 0;
+      world2machine_clamp(current_position[X_AXIS], current_position[Y_AXIS]);
+      plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], manual_feedrate[axis] / 60, active_extruder);
+      lcdDrawUpdate = 1;
+    }
   }
   if (lcdDrawUpdate) lcd_implementation_drawedit(name, ftostr31(current_position[axis]));
   if (LCD_CLICKED) lcd_goto_menu(lcd_move_menu_axis);
@@ -1167,10 +1169,13 @@ static void lcd_move_e()
 {
   if (encoderPosition != 0)
   {
-    current_position[E_AXIS] += float((int)encoderPosition) * move_menu_scale;
-    encoderPosition = 0;
-    plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], manual_feedrate[E_AXIS] / 60, active_extruder);
-    lcdDrawUpdate = 1;
+    refresh_cmd_timeout();
+    if (! planner_queue_full()) {
+      current_position[E_AXIS] += float((int)encoderPosition) * move_menu_scale;
+      encoderPosition = 0;
+      plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], manual_feedrate[E_AXIS] / 60, active_extruder);
+      lcdDrawUpdate = 1;
+    }
   }
   if (lcdDrawUpdate)
   {