diff --git a/Marlin/Marlin.h b/Marlin/Marlin.h
index ad4f82bd14..0158a1d714 100644
--- a/Marlin/Marlin.h
+++ b/Marlin/Marlin.h
@@ -247,7 +247,7 @@ inline void refresh_cmd_timeout() { previous_cmd_ms = millis(); }
 
 extern float homing_feedrate[];
 extern bool axis_relative_modes[];
-extern int feedmultiply;
+extern int feedrate_multiplier;
 extern bool volumetric_enabled;
 extern int extruder_multiply[EXTRUDERS]; // sets extrude multiply factor (in percent) for each extruder individually
 extern float filament_size[EXTRUDERS]; // cross-sectional area of filament (in millimeters), typically around 1.75 or 2.85, 0 disables the volumetric calculations for the extruder.
@@ -309,8 +309,8 @@ extern int fanSpeed;
   extern float retract_recover_length, retract_recover_length_swap, retract_recover_feedrate;
 #endif
 
-extern millis_t starttime;
-extern millis_t stoptime;
+extern millis_t print_job_start_ms;
+extern millis_t print_job_stop_ms;
 
 // Handling multiple extruders pins
 extern uint8_t active_extruder;
diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index 1f02695284..236b2be32b 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -67,136 +67,149 @@
   #include <SPI.h>
 #endif
 
-// look here for descriptions of G-codes: http://linuxcnc.org/handbook/gcode/g-code.html
-// http://objects.reprap.org/wiki/Mendel_User_Manual:_RepRapGCodes
+/**
+ * Look here for descriptions of G-codes:
+ *  - http://linuxcnc.org/handbook/gcode/g-code.html
+ *  - http://objects.reprap.org/wiki/Mendel_User_Manual:_RepRapGCodes
+ *
+ * Help us document these G-codes online:
+ *  - http://reprap.org/wiki/G-code
+ *  - https://github.com/MarlinFirmware/Marlin/wiki/Marlin-G-Code
+ */
 
-//Implemented Codes
-//-------------------
-// G0  -> G1
-// G1  - Coordinated Movement X Y Z E
-// G2  - CW ARC
-// G3  - CCW ARC
-// G4  - Dwell S<seconds> or P<milliseconds>
-// G10 - retract filament according to settings of M207
-// G11 - retract recover filament according to settings of M208
-// G28 - Home one or more axes
-// G29 - Detailed Z-Probe, probes the bed at 3 or more points.  Will fail if you haven't homed yet.
-// G30 - Single Z Probe, probes bed at current XY location.
-// G31 - Dock sled (Z_PROBE_SLED only)
-// G32 - Undock sled (Z_PROBE_SLED only)
-// G90 - Use Absolute Coordinates
-// G91 - Use Relative Coordinates
-// G92 - Set current position to coordinates given
-
-// M Codes
-// M0   - Unconditional stop - Wait for user to press a button on the LCD (Only if ULTRA_LCD is enabled)
-// M1   - Same as M0
-// M17  - Enable/Power all stepper motors
-// M18  - Disable all stepper motors; same as M84
-// M20  - List SD card
-// M21  - Init SD card
-// M22  - Release SD card
-// M23  - Select SD file (M23 filename.g)
-// M24  - Start/resume SD print
-// M25  - Pause SD print
-// M26  - Set SD position in bytes (M26 S12345)
-// M27  - Report SD print status
-// M28  - Start SD write (M28 filename.g)
-// M29  - Stop SD write
-// M30  - Delete file from SD (M30 filename.g)
-// M31  - Output time since last M109 or SD card start to serial
-// M32  - Select file and start SD print (Can be used _while_ printing from SD card files):
-//        syntax "M32 /path/filename#", or "M32 S<startpos bytes> !filename#"
-//        Call gcode file : "M32 P !filename#" and return to caller file after finishing (similar to #include).
-//        The '#' is necessary when calling from within sd files, as it stops buffer prereading
-// M42  - Change pin status via gcode Use M42 Px Sy to set pin x to value y, when omitting Px the onboard led will be used.
-// M48  - Measure Z_Probe repeatability. M48 [n # of points] [X position] [Y position] [V_erboseness #] [E_ngage Probe] [L # of legs of travel]
-// M80  - Turn on Power Supply
-// M81  - Turn off Power Supply
-// M82  - Set E codes absolute (default)
-// M83  - Set E codes relative while in Absolute Coordinates (G90) mode
-// M84  - Disable steppers until next move,
-//        or use S<seconds> to specify an inactivity timeout, after which the steppers will be disabled.  S0 to disable the timeout.
-// M85  - Set inactivity shutdown timer with parameter S<seconds>. To disable set zero (default)
-// M92  - Set axis_steps_per_unit - same syntax as G92
-// M104 - Set extruder target temp
-// M105 - Read current temp
-// M106 - Fan on
-// M107 - Fan off
-// M109 - Sxxx Wait for extruder current temp to reach target temp. Waits only when heating
-//        Rxxx Wait for extruder current temp to reach target temp. Waits when heating and cooling
-//        IF AUTOTEMP is enabled, S<mintemp> B<maxtemp> F<factor>. Exit autotemp by any M109 without F
-// M112 - Emergency stop
-// M114 - Output current position to serial port
-// M115 - Capabilities string
-// M117 - display message
-// M119 - Output Endstop status to serial port
-// M120 - Enable endstop detection
-// M121 - Disable endstop detection
-// M126 - Solenoid Air Valve Open (BariCUDA support by jmil)
-// M127 - Solenoid Air Valve Closed (BariCUDA vent to atmospheric pressure by jmil)
-// M128 - EtoP Open (BariCUDA EtoP = electricity to air pressure transducer by jmil)
-// M129 - EtoP Closed (BariCUDA EtoP = electricity to air pressure transducer by jmil)
-// M140 - Set bed target temp
-// M150 - Set BlinkM Color Output R: Red<0-255> U(!): Green<0-255> B: Blue<0-255> over i2c, G for green does not work.
-// M190 - Sxxx Wait for bed current temp to reach target temp. Waits only when heating
-//        Rxxx Wait for bed current temp to reach target temp. Waits when heating and cooling
-// M200 - set filament diameter and set E axis units to cubic millimeters (use S0 to set back to millimeters).:D<millimeters>- 
-// M201 - Set max acceleration in units/s^2 for print moves (M201 X1000 Y1000)
-// M202 - Set max acceleration in units/s^2 for travel moves (M202 X1000 Y1000) Unused in Marlin!!
-// M203 - Set maximum feedrate that your machine can sustain (M203 X200 Y200 Z300 E10000) in mm/sec
-// M204 - Set default acceleration: P for Printing moves, R for Retract only (no X, Y, Z) moves and T for Travel (non printing) moves (ex. M204 P800 T3000 R9000) in mm/sec^2
-// M205 -  advanced settings:  minimum travel speed S=while printing T=travel only,  B=minimum segment time X= maximum xy jerk, Z=maximum Z jerk, E=maximum E jerk
-// M206 - Set additional homing offset
-// M207 - Set retract length S[positive mm] F[feedrate mm/min] Z[additional zlift/hop], stays in mm regardless of M200 setting
-// M208 - Set recover=unretract length S[positive mm surplus to the M207 S*] F[feedrate mm/sec]
-// M209 - S<1=true/0=false> enable automatic retract detect if the slicer did not support G10/11: every normal extrude-only move will be classified as retract depending on the direction.
-// M218 - Set hotend offset (in mm): T<extruder_number> X<offset_on_X> Y<offset_on_Y>
-// M220 - Set speed factor override percentage: S<factor in percent>
-// M221 - Set extrude factor override percentage: S<factor in percent>
-// M226 - Wait until the specified pin reaches the state required: P<pin number> S<pin state>
-// M240 - Trigger a camera to take a photograph
-// M250 - Set LCD contrast C<contrast value> (value 0..63)
-// M280 - Set servo position absolute. P: servo index, S: angle or microseconds
-// M300 - Play beep sound S<frequency Hz> P<duration ms>
-// M301 - Set PID parameters P I and D
-// M302 - Allow cold extrudes, or set the minimum extrude S<temperature>.
-// M303 - PID relay autotune S<temperature> sets the target temperature. (default target temperature = 150C)
-// M304 - Set bed PID parameters P I and D
-// M380 - Activate solenoid on active extruder
-// M381 - Disable all solenoids
-// M400 - Finish all moves
-// M401 - Lower z-probe if present
-// M402 - Raise z-probe if present
-// M404 - N<dia in mm> Enter the nominal filament width (3mm, 1.75mm ) or will display nominal filament width without parameters
-// M405 - Turn on Filament Sensor extrusion control.  Optional D<delay in cm> to set delay in centimeters between sensor and extruder
-// M406 - Turn off Filament Sensor extrusion control
-// M407 - Display measured filament diameter
-// M500 - Store parameters in EEPROM
-// M501 - Read parameters from EEPROM (if you need reset them after you changed them temporarily).
-// M502 - Revert to the default "factory settings". You still need to store them in EEPROM afterwards if you want to.
-// M503 - Print the current settings (from memory not from EEPROM). Use S0 to leave off headings.
-// M540 - Use S[0|1] to enable or disable the stop SD card print on endstop hit (requires ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED)
-// M600 - Pause for filament change X[pos] Y[pos] Z[relative lift] E[initial retract] L[later retract distance for removal]
-// M665 - Set delta configurations: L<diagonal rod> R<delta radius> S<segments/s>
-// M666 - Set delta endstop adjustment
-// M605 - Set dual x-carriage movement mode: S<mode> [ X<duplication x-offset> R<duplication temp offset> ]
-// M907 - Set digital trimpot motor current using axis codes.
-// M908 - Control digital trimpot directly.
-// M350 - Set microstepping mode.
-// M351 - Toggle MS1 MS2 pins directly.
-
-// ************ SCARA Specific - This can change to suit future G-code regulations
-// M360 - SCARA calibration: Move to cal-position ThetaA (0 deg calibration)
-// M361 - SCARA calibration: Move to cal-position ThetaB (90 deg calibration - steps per degree)
-// M362 - SCARA calibration: Move to cal-position PsiA (0 deg calibration)
-// M363 - SCARA calibration: Move to cal-position PsiB (90 deg calibration - steps per degree)
-// M364 - SCARA calibration: Move to cal-position PSIC (90 deg to Theta calibration position)
-// M365 - SCARA calibration: Scaling factor, X, Y, Z axis
-//************* SCARA End ***************
-
-// M928 - Start SD logging (M928 filename.g) - ended by M29
-// M999 - Restart after being stopped by error
+/**
+ * Implemented Codes
+ * -------------------
+ *
+ * "G" Codes
+ *
+ * G0  -> G1
+ * G1  - Coordinated Movement X Y Z E
+ * G2  - CW ARC
+ * G3  - CCW ARC
+ * G4  - Dwell S<seconds> or P<milliseconds>
+ * G10 - retract filament according to settings of M207
+ * G11 - retract recover filament according to settings of M208
+ * G28 - Home one or more axes
+ * G29 - Detailed Z-Probe, probes the bed at 3 or more points.  Will fail if you haven't homed yet.
+ * G30 - Single Z Probe, probes bed at current XY location.
+ * G31 - Dock sled (Z_PROBE_SLED only)
+ * G32 - Undock sled (Z_PROBE_SLED only)
+ * G90 - Use Absolute Coordinates
+ * G91 - Use Relative Coordinates
+ * G92 - Set current position to coordinates given
+ *
+ * "M" Codes
+ *
+ * M0   - Unconditional stop - Wait for user to press a button on the LCD (Only if ULTRA_LCD is enabled)
+ * M1   - Same as M0
+ * M17  - Enable/Power all stepper motors
+ * M18  - Disable all stepper motors; same as M84
+ * M20  - List SD card
+ * M21  - Init SD card
+ * M22  - Release SD card
+ * M23  - Select SD file (M23 filename.g)
+ * M24  - Start/resume SD print
+ * M25  - Pause SD print
+ * M26  - Set SD position in bytes (M26 S12345)
+ * M27  - Report SD print status
+ * M28  - Start SD write (M28 filename.g)
+ * M29  - Stop SD write
+ * M30  - Delete file from SD (M30 filename.g)
+ * M31  - Output time since last M109 or SD card start to serial
+ * M32  - Select file and start SD print (Can be used _while_ printing from SD card files):
+ *        syntax "M32 /path/filename#", or "M32 S<startpos bytes> !filename#"
+ *        Call gcode file : "M32 P !filename#" and return to caller file after finishing (similar to #include).
+ *        The '#' is necessary when calling from within sd files, as it stops buffer prereading
+ * M42  - Change pin status via gcode Use M42 Px Sy to set pin x to value y, when omitting Px the onboard led will be used.
+ * M48  - Measure Z_Probe repeatability. M48 [n # of points] [X position] [Y position] [V_erboseness #] [E_ngage Probe] [L # of legs of travel]
+ * M80  - Turn on Power Supply
+ * M81  - Turn off Power Supply
+ * M82  - Set E codes absolute (default)
+ * M83  - Set E codes relative while in Absolute Coordinates (G90) mode
+ * M84  - Disable steppers until next move,
+ *        or use S<seconds> to specify an inactivity timeout, after which the steppers will be disabled.  S0 to disable the timeout.
+ * M85  - Set inactivity shutdown timer with parameter S<seconds>. To disable set zero (default)
+ * M92  - Set axis_steps_per_unit - same syntax as G92
+ * M104 - Set extruder target temp
+ * M105 - Read current temp
+ * M106 - Fan on
+ * M107 - Fan off
+ * M109 - Sxxx Wait for extruder current temp to reach target temp. Waits only when heating
+ *        Rxxx Wait for extruder current temp to reach target temp. Waits when heating and cooling
+ *        IF AUTOTEMP is enabled, S<mintemp> B<maxtemp> F<factor>. Exit autotemp by any M109 without F
+ * M112 - Emergency stop
+ * M114 - Output current position to serial port
+ * M115 - Capabilities string
+ * M117 - display message
+ * M119 - Output Endstop status to serial port
+ * M120 - Enable endstop detection
+ * M121 - Disable endstop detection
+ * M126 - Solenoid Air Valve Open (BariCUDA support by jmil)
+ * M127 - Solenoid Air Valve Closed (BariCUDA vent to atmospheric pressure by jmil)
+ * M128 - EtoP Open (BariCUDA EtoP = electricity to air pressure transducer by jmil)
+ * M129 - EtoP Closed (BariCUDA EtoP = electricity to air pressure transducer by jmil)
+ * M140 - Set bed target temp
+ * M150 - Set BlinkM Color Output R: Red<0-255> U(!): Green<0-255> B: Blue<0-255> over i2c, G for green does not work.
+ * M190 - Sxxx Wait for bed current temp to reach target temp. Waits only when heating
+ *        Rxxx Wait for bed current temp to reach target temp. Waits when heating and cooling
+ * M200 - set filament diameter and set E axis units to cubic millimeters (use S0 to set back to millimeters).:D<millimeters>- 
+ * M201 - Set max acceleration in units/s^2 for print moves (M201 X1000 Y1000)
+ * M202 - Set max acceleration in units/s^2 for travel moves (M202 X1000 Y1000) Unused in Marlin!!
+ * M203 - Set maximum feedrate that your machine can sustain (M203 X200 Y200 Z300 E10000) in mm/sec
+ * M204 - Set default acceleration: P for Printing moves, R for Retract only (no X, Y, Z) moves and T for Travel (non printing) moves (ex. M204 P800 T3000 R9000) in mm/sec^2
+ * M205 -  advanced settings:  minimum travel speed S=while printing T=travel only,  B=minimum segment time X= maximum xy jerk, Z=maximum Z jerk, E=maximum E jerk
+ * M206 - Set additional homing offset
+ * M207 - Set retract length S[positive mm] F[feedrate mm/min] Z[additional zlift/hop], stays in mm regardless of M200 setting
+ * M208 - Set recover=unretract length S[positive mm surplus to the M207 S*] F[feedrate mm/sec]
+ * M209 - S<1=true/0=false> enable automatic retract detect if the slicer did not support G10/11: every normal extrude-only move will be classified as retract depending on the direction.
+ * M218 - Set hotend offset (in mm): T<extruder_number> X<offset_on_X> Y<offset_on_Y>
+ * M220 - Set speed factor override percentage: S<factor in percent>
+ * M221 - Set extrude factor override percentage: S<factor in percent>
+ * M226 - Wait until the specified pin reaches the state required: P<pin number> S<pin state>
+ * M240 - Trigger a camera to take a photograph
+ * M250 - Set LCD contrast C<contrast value> (value 0..63)
+ * M280 - Set servo position absolute. P: servo index, S: angle or microseconds
+ * M300 - Play beep sound S<frequency Hz> P<duration ms>
+ * M301 - Set PID parameters P I and D
+ * M302 - Allow cold extrudes, or set the minimum extrude S<temperature>.
+ * M303 - PID relay autotune S<temperature> sets the target temperature. (default target temperature = 150C)
+ * M304 - Set bed PID parameters P I and D
+ * M380 - Activate solenoid on active extruder
+ * M381 - Disable all solenoids
+ * M400 - Finish all moves
+ * M401 - Lower z-probe if present
+ * M402 - Raise z-probe if present
+ * M404 - N<dia in mm> Enter the nominal filament width (3mm, 1.75mm ) or will display nominal filament width without parameters
+ * M405 - Turn on Filament Sensor extrusion control.  Optional D<delay in cm> to set delay in centimeters between sensor and extruder
+ * M406 - Turn off Filament Sensor extrusion control
+ * M407 - Display measured filament diameter
+ * M500 - Store parameters in EEPROM
+ * M501 - Read parameters from EEPROM (if you need reset them after you changed them temporarily).
+ * M502 - Revert to the default "factory settings". You still need to store them in EEPROM afterwards if you want to.
+ * M503 - Print the current settings (from memory not from EEPROM). Use S0 to leave off headings.
+ * M540 - Use S[0|1] to enable or disable the stop SD card print on endstop hit (requires ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED)
+ * M600 - Pause for filament change X[pos] Y[pos] Z[relative lift] E[initial retract] L[later retract distance for removal]
+ * M665 - Set delta configurations: L<diagonal rod> R<delta radius> S<segments/s>
+ * M666 - Set delta endstop adjustment
+ * M605 - Set dual x-carriage movement mode: S<mode> [ X<duplication x-offset> R<duplication temp offset> ]
+ * M907 - Set digital trimpot motor current using axis codes.
+ * M908 - Control digital trimpot directly.
+ * M350 - Set microstepping mode.
+ * M351 - Toggle MS1 MS2 pins directly.
+ *
+ * ************ SCARA Specific - This can change to suit future G-code regulations
+ * M360 - SCARA calibration: Move to cal-position ThetaA (0 deg calibration)
+ * M361 - SCARA calibration: Move to cal-position ThetaB (90 deg calibration - steps per degree)
+ * M362 - SCARA calibration: Move to cal-position PsiA (0 deg calibration)
+ * M363 - SCARA calibration: Move to cal-position PsiB (90 deg calibration - steps per degree)
+ * M364 - SCARA calibration: Move to cal-position PSIC (90 deg to Theta calibration position)
+ * M365 - SCARA calibration: Scaling factor, X, Y, Z axis
+ * ************* SCARA End ***************
+ *
+ * M928 - Start SD logging (M928 filename.g) - ended by M29
+ * M999 - Restart after being stopped by error
+ */
 
 #ifdef SDSUPPORT
   CardReader card;
@@ -210,12 +223,16 @@ static float destination[NUM_AXIS] = { 0.0 };
 bool axis_known_position[3] = { false };
 
 static long gcode_N, gcode_LastN, Stopped_gcode_LastN = 0;
-static char cmdbuffer[BUFSIZE][MAX_CMD_SIZE];
+
+static int cmd_queue_index_r = 0;
+static int cmd_queue_index_w = 0;
+static int commands_in_queue = 0;
+static char command_queue[BUFSIZE][MAX_CMD_SIZE];
 
 float homing_feedrate[] = HOMING_FEEDRATE;
 bool axis_relative_modes[] = AXIS_RELATIVE_MODES;
-int feedmultiply = 100; //100->1 200->2
-int saved_feedmultiply;
+int feedrate_multiplier = 100; //100->1 200->2
+int saved_feedrate_multiplier;
 int extruder_multiply[EXTRUDERS] = ARRAY_BY_EXTRUDERS(100, 100, 100, 100);
 bool volumetric_enabled = false;
 float filament_size[EXTRUDERS] = ARRAY_BY_EXTRUDERS(DEFAULT_NOMINAL_FILAMENT_DIA, DEFAULT_NOMINAL_FILAMENT_DIA, DEFAULT_NOMINAL_FILAMENT_DIA, DEFAULT_NOMINAL_FILAMENT_DIA);
@@ -234,9 +251,6 @@ const char axis_codes[NUM_AXIS] = {'X', 'Y', 'Z', 'E'};
 
 static float offset[3] = { 0 };
 static bool relative_mode = false;  //Determines Absolute or Relative Coordinates
-static int bufindr = 0;
-static int bufindw = 0;
-static int buflen = 0;
 static char serial_char;
 static int serial_count = 0;
 static boolean comment_mode = false;
@@ -247,10 +261,10 @@ const int sensitive_pins[] = SENSITIVE_PINS; ///< Sensitive pin list for M42
 millis_t previous_cmd_ms = 0;
 static millis_t max_inactive_time = 0;
 static millis_t stepper_inactive_time = DEFAULT_STEPPER_DEACTIVE_TIME * 1000L;
-millis_t starttime = 0; ///< Print job start time
-millis_t stoptime = 0;  ///< Print job stop time
+millis_t print_job_start_ms = 0; ///< Print job start time
+millis_t print_job_stop_ms = 0;  ///< Print job stop time
 static uint8_t target_extruder;
-bool CooldownNoWait = true;
+bool no_wait_for_cooling = true;
 bool target_direction;
 
 #ifdef ENABLE_AUTO_BED_LEVELING
@@ -357,7 +371,7 @@ bool target_direction;
 #endif
 
 #ifdef FILAMENT_RUNOUT_SENSOR
-   static bool filrunoutEnqued = false;
+   static bool filrunoutEnqueued = false;
 #endif
 
 #ifdef SDSUPPORT
@@ -410,8 +424,10 @@ void serial_echopair_P(const char *s_P, unsigned long v) { serialprintPGM(s_P);
   }
 #endif //!SDSUPPORT
 
-//Injects the next command from the pending sequence of commands, when possible
-//Return false if and only if no command was pending
+/**
+ * Inject the next command from the command queue, when possible
+ * Return false only if no command was pending
+ */
 static bool drain_queued_commands_P() {
   if (!queued_commands_P) return false;
 
@@ -425,7 +441,7 @@ static bool drain_queued_commands_P() {
   char c;
   while((c = cmd[i]) && c != '\n') i++; // find the end of this gcode command
   cmd[i] = '\0';
-  if (enqueuecommand(cmd)) {        // buffer was not full (else we will retry later)
+  if (enqueuecommand(cmd)) {      // buffer was not full (else we will retry later)
     if (c)
       queued_commands_P += i + 1; // move to next command
     else
@@ -434,45 +450,46 @@ static bool drain_queued_commands_P() {
   return true;
 }
 
-//Record one or many commands to run from program memory.
-//Aborts the current queue, if any.
-//Note: drain_queued_commands_P() must be called repeatedly to drain the commands afterwards
+/**
+ * Record one or many commands to run from program memory.
+ * Aborts the current queue, if any.
+ * Note: drain_queued_commands_P() must be called repeatedly to drain the commands afterwards
+ */
 void enqueuecommands_P(const char* pgcode) {
-    queued_commands_P = pgcode;
-    drain_queued_commands_P(); // first command executed asap (when possible)
+  queued_commands_P = pgcode;
+  drain_queued_commands_P(); // first command executed asap (when possible)
 }
 
-//adds a single command to the main command buffer, from RAM
-//that is really done in a non-safe way.
-//needs overworking someday
-//Returns false if it failed to do so
-bool enqueuecommand(const char *cmd)
-{
-  if(*cmd==';')
-    return false;
-  if(buflen >= BUFSIZE)
-    return false;
-  //this is dangerous if a mixing of serial and this happens
-  strcpy(&(cmdbuffer[bufindw][0]),cmd);
+/**
+ * Copy a command directly into the main command buffer, from RAM.
+ *
+ * This is done in a non-safe way and needs a rework someday.
+ * Returns false if it doesn't add any command
+ */
+bool enqueuecommand(const char *cmd) {
+
+  if (*cmd == ';' || commands_in_queue >= BUFSIZE) return false;
+
+  // This is dangerous if a mixing of serial and this happens
+  char *command = command_queue[cmd_queue_index_w];
+  strcpy(command, cmd);
   SERIAL_ECHO_START;
-  SERIAL_ECHOPGM(MSG_Enqueing);
-  SERIAL_ECHO(cmdbuffer[bufindw]);
+  SERIAL_ECHOPGM(MSG_Enqueueing);
+  SERIAL_ECHO(command);
   SERIAL_ECHOLNPGM("\"");
-  bufindw= (bufindw + 1)%BUFSIZE;
-  buflen += 1;
+  cmd_queue_index_w = (cmd_queue_index_w + 1) % BUFSIZE;
+  commands_in_queue++;
   return true;
 }
 
-void setup_killpin()
-{
+void setup_killpin() {
   #if HAS_KILL
     SET_INPUT(KILL_PIN);
     WRITE(KILL_PIN, HIGH);
   #endif
 }
 
-void setup_filrunoutpin()
-{
+void setup_filrunoutpin() {
   #if HAS_FILRUNOUT
     pinMode(FILRUNOUT_PIN, INPUT);
     #ifdef ENDSTOPPULLUP_FIL_RUNOUT
@@ -482,8 +499,7 @@ void setup_filrunoutpin()
 }
 
 // Set home pin
-void setup_homepin(void)
-{
+void setup_homepin(void) {
   #if HAS_HOME
     SET_INPUT(HOME_PIN);
     WRITE(HOME_PIN, HIGH);
@@ -491,15 +507,13 @@ void setup_homepin(void)
 }
 
 
-void setup_photpin()
-{
+void setup_photpin() {
   #if HAS_PHOTOGRAPH
     OUT_WRITE(PHOTOGRAPH_PIN, LOW);
   #endif
 }
 
-void setup_powerhold()
-{
+void setup_powerhold() {
   #if HAS_SUICIDE
     OUT_WRITE(SUICIDE_PIN, HIGH);
   #endif
@@ -512,15 +526,13 @@ void setup_powerhold()
   #endif
 }
 
-void suicide()
-{
+void suicide() {
   #if HAS_SUICIDE
     OUT_WRITE(SUICIDE_PIN, LOW);
   #endif
 }
 
-void servo_init()
-{
+void servo_init() {
   #if NUM_SERVOS >= 1 && HAS_SERVO_0
     servos[0].attach(SERVO0_PIN);
   #endif
@@ -547,6 +559,24 @@ void servo_init()
   #endif
 }
 
+/**
+ * Marlin entry-point: Set up before the program loop
+ *  - Set up the kill pin, filament runout, power hold
+ *  - Start the serial port
+ *  - Print startup messages and diagnostics
+ *  - Get EEPROM or default settings
+ *  - Initialize managers for:
+ *    • temperature
+ *    • planner
+ *    • watchdog
+ *    • stepper
+ *    • photo pin
+ *    • servos
+ *    • LCD controller
+ *    • Digipot I2C
+ *    • Z probe sled
+ *    • status LEDs
+ */
 void setup() {
   setup_killpin();
   setup_filrunoutpin();
@@ -587,7 +617,7 @@ void setup() {
 
   #ifdef SDSUPPORT
     for (int8_t i = 0; i < BUFSIZE; i++) fromsd[i] = false;
-  #endif // !SDSUPPORT
+  #endif
 
   // loads data from EEPROM if available else uses defaults (and resets step acceleration rate)
   Config_RetrieveSettings();
@@ -628,36 +658,54 @@ void setup() {
   #endif  
 }
 
-
+/**
+ * The main Marlin program loop
+ *
+ *  - Save or log commands to SD
+ *  - Process available commands (if not saving)
+ *  - Call heater manager
+ *  - Call inactivity manager
+ *  - Call endstop manager
+ *  - Call LCD update
+ */
 void loop() {
-  if (buflen < BUFSIZE - 1) get_command();
+  if (commands_in_queue < BUFSIZE - 1) get_command();
 
   #ifdef SDSUPPORT
     card.checkautostart(false);
   #endif
 
-  if (buflen) {
+  if (commands_in_queue) {
+
     #ifdef SDSUPPORT
+
       if (card.saving) {
-        if (strstr_P(cmdbuffer[bufindr], PSTR("M29")) == NULL) {
-          card.write_command(cmdbuffer[bufindr]);
-          if (card.logging)
-            process_commands();
-          else
-            SERIAL_PROTOCOLLNPGM(MSG_OK);
-        }
-        else {
+        char *command = command_queue[cmd_queue_index_r];
+        if (strstr_P(command, PSTR("M29"))) {
+          // M29 closes the file
           card.closefile();
           SERIAL_PROTOCOLLNPGM(MSG_FILE_SAVED);
         }
+        else {
+          // Write the string from the read buffer to SD
+          card.write_command(command);
+          if (card.logging)
+            process_commands(); // The card is saving because it's logging
+          else
+            SERIAL_PROTOCOLLNPGM(MSG_OK);
+        }
       }
       else
         process_commands();
+
     #else
+
       process_commands();
+
     #endif // SDSUPPORT
-    buflen--;
-    bufindr = (bufindr + 1) % BUFSIZE;
+
+    commands_in_queue--;
+    cmd_queue_index_r = (cmd_queue_index_r + 1) % BUFSIZE;
   }
   // Check heater every n milliseconds
   manage_heater();
@@ -666,12 +714,20 @@ void loop() {
   lcd_update();
 }
 
+/**
+ * Add to the circular command queue the next command from:
+ *  - The command-injection queue (queued_commands_P)
+ *  - The active serial input (usually USB)
+ *  - The SD card file being actively printed
+ */
 void get_command() {
 
   if (drain_queued_commands_P()) return; // priority is given to non-serial commands
   
-  while (MYSERIAL.available() > 0 && buflen < BUFSIZE) {
+  while (MYSERIAL.available() > 0 && commands_in_queue < BUFSIZE) {
+
     serial_char = MYSERIAL.read();
+
     if (serial_char == '\n' || serial_char == '\r' ||
        serial_count >= (MAX_CMD_SIZE - 1)
     ) {
@@ -680,16 +736,17 @@ void get_command() {
 
       if (!serial_count) return; // shortcut for empty lines
 
-      cmdbuffer[bufindw][serial_count] = 0; // terminate string
+      char *command = command_queue[cmd_queue_index_w];
+      command[serial_count] = 0; // terminate string
 
       #ifdef SDSUPPORT
-        fromsd[bufindw] = false;
+        fromsd[cmd_queue_index_w] = false;
       #endif
 
-      if (strchr(cmdbuffer[bufindw], 'N') != NULL) {
-        strchr_pointer = strchr(cmdbuffer[bufindw], 'N');
+      if (strchr(command, 'N') != NULL) {
+        strchr_pointer = strchr(command, 'N');
         gcode_N = (strtol(strchr_pointer + 1, NULL, 10));
-        if (gcode_N != gcode_LastN + 1 && strstr_P(cmdbuffer[bufindw], PSTR("M110")) == NULL) {
+        if (gcode_N != gcode_LastN + 1 && strstr_P(command, PSTR("M110")) == NULL) {
           SERIAL_ERROR_START;
           SERIAL_ERRORPGM(MSG_ERR_LINE_NO);
           SERIAL_ERRORLN(gcode_LastN);
@@ -699,11 +756,11 @@ void get_command() {
           return;
         }
 
-        if (strchr(cmdbuffer[bufindw], '*') != NULL) {
+        if (strchr(command, '*') != NULL) {
           byte checksum = 0;
           byte count = 0;
-          while (cmdbuffer[bufindw][count] != '*') checksum ^= cmdbuffer[bufindw][count++];
-          strchr_pointer = strchr(cmdbuffer[bufindw], '*');
+          while (command[count] != '*') checksum ^= command[count++];
+          strchr_pointer = strchr(command, '*');
 
           if (strtol(strchr_pointer + 1, NULL, 10) != checksum) {
             SERIAL_ERROR_START;
@@ -728,7 +785,7 @@ void get_command() {
         //if no errors, continue parsing
       }
       else {  // if we don't receive 'N' but still see '*'
-        if ((strchr(cmdbuffer[bufindw], '*') != NULL)) {
+        if ((strchr(command, '*') != NULL)) {
           SERIAL_ERROR_START;
           SERIAL_ERRORPGM(MSG_ERR_NO_LINENUMBER_WITH_CHECKSUM);
           SERIAL_ERRORLN(gcode_LastN);
@@ -737,8 +794,8 @@ void get_command() {
         }
       }
 
-      if (strchr(cmdbuffer[bufindw], 'G') != NULL) {
-        strchr_pointer = strchr(cmdbuffer[bufindw], 'G');
+      if (strchr(command, 'G') != NULL) {
+        strchr_pointer = strchr(command, 'G');
         switch (strtol(strchr_pointer + 1, NULL, 10)) {
           case 0:
           case 1:
@@ -755,24 +812,24 @@ void get_command() {
       }
 
       // If command was e-stop process now
-      if (strcmp(cmdbuffer[bufindw], "M112") == 0) kill();
+      if (strcmp(command, "M112") == 0) kill();
 
-      bufindw = (bufindw + 1) % BUFSIZE;
-      buflen += 1;
+      cmd_queue_index_w = (cmd_queue_index_w + 1) % BUFSIZE;
+      commands_in_queue += 1;
 
       serial_count = 0; //clear buffer
     }
     else if (serial_char == '\\') {  // Handle escapes
-      if (MYSERIAL.available() > 0  && buflen < BUFSIZE) {
+      if (MYSERIAL.available() > 0  && commands_in_queue < BUFSIZE) {
         // if we have one more character, copy it over
         serial_char = MYSERIAL.read();
-        cmdbuffer[bufindw][serial_count++] = serial_char;
+        command_queue[cmd_queue_index_w][serial_count++] = serial_char;
       }
       // otherwise do nothing
     }
     else { // its not a newline, carriage return or escape char
       if (serial_char == ';') comment_mode = true;
-      if (!comment_mode) cmdbuffer[bufindw][serial_count++] = serial_char;
+      if (!comment_mode) command_queue[cmd_queue_index_w][serial_count++] = serial_char;
     }
   }
 
@@ -785,9 +842,9 @@ void get_command() {
     // this character _can_ occur in serial com, due to checksums. however, no checksums are used in SD printing
 
     static bool stop_buffering = false;
-    if (buflen == 0) stop_buffering = false;
+    if (commands_in_queue == 0) stop_buffering = false;
 
-    while (!card.eof() && buflen < BUFSIZE && !stop_buffering) {
+    while (!card.eof() && commands_in_queue < BUFSIZE && !stop_buffering) {
       int16_t n = card.get();
       serial_char = (char)n;
       if (serial_char == '\n' || serial_char == '\r' ||
@@ -796,9 +853,9 @@ void get_command() {
       ) {
         if (card.eof()) {
           SERIAL_PROTOCOLLNPGM(MSG_FILE_PRINTED);
-          stoptime = millis();
+          print_job_stop_ms = millis();
           char time[30];
-          millis_t t = (stoptime - starttime) / 1000;
+          millis_t t = (print_job_stop_ms - print_job_start_ms) / 1000;
           int hours = t / 60 / 60, minutes = (t / 60) % 60;
           sprintf_P(time, PSTR("%i " MSG_END_HOUR " %i " MSG_END_MINUTE), hours, minutes);
           SERIAL_ECHO_START;
@@ -813,18 +870,18 @@ void get_command() {
           comment_mode = false; //for new command
           return; //if empty line
         }
-        cmdbuffer[bufindw][serial_count] = 0; //terminate string
+        command_queue[cmd_queue_index_w][serial_count] = 0; //terminate string
         // if (!comment_mode) {
-        fromsd[bufindw] = true;
-        buflen += 1;
-        bufindw = (bufindw + 1)%BUFSIZE;
+        fromsd[cmd_queue_index_w] = true;
+        commands_in_queue += 1;
+        cmd_queue_index_w = (cmd_queue_index_w + 1) % BUFSIZE;
         // }
         comment_mode = false; //for new command
         serial_count = 0; //clear buffer
       }
       else {
         if (serial_char == ';') comment_mode = true;
-        if (!comment_mode) cmdbuffer[bufindw][serial_count++] = serial_char;
+        if (!comment_mode) command_queue[cmd_queue_index_w][serial_count++] = serial_char;
       }
     }
 
@@ -854,7 +911,7 @@ long code_value_long() { return strtol(strchr_pointer + 1, NULL, 10); }
 int16_t code_value_short() { return (int16_t)strtol(strchr_pointer + 1, NULL, 10); }
 
 bool code_seen(char code) {
-  strchr_pointer = strchr(cmdbuffer[bufindr], code);
+  strchr_pointer = strchr(command_queue[cmd_queue_index_r], code);
   return (strchr_pointer != NULL);  //Return True if a character was found
 }
 
@@ -1023,7 +1080,7 @@ inline void set_destination_to_current() { memcpy(destination, current_position,
     void prepare_move_raw() {
       refresh_cmd_timeout();
       calculate_delta(destination);
-      plan_buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], destination[E_AXIS], (feedrate/60)*(feedmultiply/100.0), active_extruder);
+      plan_buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], destination[E_AXIS], (feedrate/60)*(feedrate_multiplier/100.0), active_extruder);
       set_current_to_destination();
     }
   #endif
@@ -1176,8 +1233,8 @@ inline void set_destination_to_current() { memcpy(destination, current_position,
 
   static void setup_for_endstop_move() {
     saved_feedrate = feedrate;
-    saved_feedmultiply = feedmultiply;
-    feedmultiply = 100;
+    saved_feedrate_multiplier = feedrate_multiplier;
+    feedrate_multiplier = 100;
     refresh_cmd_timeout();
     enable_endstops(true);
   }
@@ -1187,7 +1244,7 @@ inline void set_destination_to_current() { memcpy(destination, current_position,
       enable_endstops(false);
     #endif
     feedrate = saved_feedrate;
-    feedmultiply = saved_feedmultiply;
+    feedrate_multiplier = saved_feedrate_multiplier;
     refresh_cmd_timeout();
   }
 
@@ -1610,12 +1667,12 @@ static void homeaxis(AxisEnum axis) {
     #define SLED_DOCKING_OFFSET 0
   #endif
 
-  //
-  // Method to dock/undock a sled designed by Charles Bell.
-  //
-  // dock[in]     If true, move to MAX_X and engage the electromagnet
-  // offset[in]   The additional distance to move to adjust docking location
-  //
+  /**
+   * Method to dock/undock a sled designed by Charles Bell.
+   *
+   * dock[in]     If true, move to MAX_X and engage the electromagnet
+   * offset[in]   The additional distance to move to adjust docking location
+   */
   static void dock_sled(bool dock, int offset=0) {
     if (!axis_known_position[X_AXIS] || !axis_known_position[Y_AXIS]) {
       LCD_MESSAGEPGM(MSG_POSITION_UNKNOWN);
@@ -1649,9 +1706,10 @@ static void homeaxis(AxisEnum axis) {
 inline void gcode_G0_G1() {
   if (IsRunning()) {
     get_coordinates(); // For X Y Z E F
+
     #ifdef FWRETRACT
-      if (autoretract_enabled)
-      if (!(code_seen('X') || code_seen('Y') || code_seen('Z')) && code_seen('E')) {
+
+      if (autoretract_enabled && !(code_seen('X') || code_seen('Y') || code_seen('Z')) && code_seen('E')) {
         float echange = destination[E_AXIS] - current_position[E_AXIS];
         // Is this move an attempt to retract or recover?
         if ((echange < -MIN_RETRACT && !retracted[active_extruder]) || (echange > MIN_RETRACT && retracted[active_extruder])) {
@@ -1661,7 +1719,9 @@ inline void gcode_G0_G1() {
           return;
         }
       }
+
     #endif //FWRETRACT
+
     prepare_move();
     //ClearToSend();
   }
@@ -1758,8 +1818,8 @@ inline void gcode_G28() {
   #endif
 
   saved_feedrate = feedrate;
-  saved_feedmultiply = feedmultiply;
-  feedmultiply = 100;
+  saved_feedrate_multiplier = feedrate_multiplier;
+  feedrate_multiplier = 100;
   refresh_cmd_timeout();
 
   enable_endstops(true);
@@ -2013,7 +2073,7 @@ inline void gcode_G28() {
   #endif
 
   feedrate = saved_feedrate;
-  feedmultiply = saved_feedmultiply;
+  feedrate_multiplier = saved_feedrate_multiplier;
   refresh_cmd_timeout();
   endstops_hit_on_purpose(); // clear endstop hit flags
 }
@@ -2659,7 +2719,7 @@ inline void gcode_M17() {
    */
   inline void gcode_M24() {
     card.startFileprint();
-    starttime = millis();
+    print_job_start_ms = millis();
   }
 
   /**
@@ -2691,7 +2751,7 @@ inline void gcode_M17() {
     char* codepos = strchr_pointer + 4;
     char* starpos = strchr(codepos, '*');
     if (starpos) {
-      char* npos = strchr(cmdbuffer[bufindr], 'N');
+      char* npos = strchr(command_queue[cmd_queue_index_r], 'N');
       strchr_pointer = strchr(npos, ' ') + 1;
       *(starpos) = '\0';
     }
@@ -2714,7 +2774,7 @@ inline void gcode_M17() {
       card.closefile();
       char* starpos = strchr(strchr_pointer + 4, '*');
       if (starpos) {
-        char* npos = strchr(cmdbuffer[bufindr], 'N');
+        char* npos = strchr(command_queue[cmd_queue_index_r], 'N');
         strchr_pointer = strchr(npos, ' ') + 1;
         *(starpos) = '\0';
       }
@@ -2728,8 +2788,8 @@ inline void gcode_M17() {
  * M31: Get the time since the start of SD Print (or last M109)
  */
 inline void gcode_M31() {
-  stoptime = millis();
-  millis_t t = (stoptime - starttime) / 1000;
+  print_job_stop_ms = millis();
+  millis_t t = (print_job_stop_ms - print_job_start_ms) / 1000;
   int min = t / 60, sec = t % 60;
   char time[30];
   sprintf_P(time, PSTR("%i min, %i sec"), min, sec);
@@ -2769,7 +2829,7 @@ inline void gcode_M31() {
 
       card.startFileprint();
       if (!call_procedure)
-        starttime = millis(); //procedure calls count as normal print time.
+        print_job_start_ms = millis(); //procedure calls count as normal print time.
     }
   }
 
@@ -2779,7 +2839,7 @@ inline void gcode_M31() {
   inline void gcode_M928() {
     char* starpos = strchr(strchr_pointer + 5, '*');
     if (starpos) {
-      char* npos = strchr(cmdbuffer[bufindr], 'N');
+      char* npos = strchr(command_queue[cmd_queue_index_r], 'N');
       strchr_pointer = strchr(npos, ' ') + 1;
       *(starpos) = '\0';
     }
@@ -3185,8 +3245,8 @@ inline void gcode_M109() {
 
   LCD_MESSAGEPGM(MSG_HEATING);
 
-  CooldownNoWait = code_seen('S');
-  if (CooldownNoWait || code_seen('R')) {
+  no_wait_for_cooling = code_seen('S');
+  if (no_wait_for_cooling || code_seen('R')) {
     float temp = code_value();
     setTargetHotend(temp, target_extruder);
     #ifdef DUAL_X_CARRIAGE
@@ -3218,7 +3278,7 @@ inline void gcode_M109() {
     while((!cancel_heatup)&&((residency_start_ms == -1) ||
           (residency_start_ms >= 0 && (((unsigned int) (millis() - residency_start_ms)) < (TEMP_RESIDENCY_TIME * 1000UL)))) )
   #else
-    while ( target_direction ? (isHeatingHotend(target_extruder)) : (isCoolingHotend(target_extruder)&&(CooldownNoWait==false)) )
+    while ( target_direction ? (isHeatingHotend(target_extruder)) : (isCoolingHotend(target_extruder)&&(no_wait_for_cooling==false)) )
   #endif //TEMP_RESIDENCY_TIME
 
     { // while loop
@@ -3258,7 +3318,7 @@ inline void gcode_M109() {
 
   LCD_MESSAGEPGM(MSG_HEATING_COMPLETE);
   refresh_cmd_timeout();
-  starttime = previous_cmd_ms;
+  print_job_start_ms = previous_cmd_ms;
 }
 
 #if HAS_TEMP_BED
@@ -3269,8 +3329,8 @@ inline void gcode_M109() {
    */
   inline void gcode_M190() {
     LCD_MESSAGEPGM(MSG_BED_HEATING);
-    CooldownNoWait = code_seen('S');
-    if (CooldownNoWait || code_seen('R'))
+    no_wait_for_cooling = code_seen('S');
+    if (no_wait_for_cooling || code_seen('R'))
       setTargetBed(code_value());
 
     millis_t temp_ms = millis();
@@ -3278,7 +3338,7 @@ inline void gcode_M109() {
     cancel_heatup = false;
     target_direction = isHeatingBed(); // true if heating, false if cooling
 
-    while ( (target_direction)&&(!cancel_heatup) ? (isHeatingBed()) : (isCoolingBed()&&(CooldownNoWait==false)) ) {
+    while ((target_direction && !cancel_heatup) ? isHeatingBed() : isCoolingBed() && !no_wait_for_cooling) {
       millis_t ms = millis();
       if (ms > temp_ms + 1000UL) { //Print Temp Reading every 1 second while heating up.
         temp_ms = ms;
@@ -3371,7 +3431,7 @@ inline void gcode_M140() {
  *      This code should ALWAYS be available for EMERGENCY SHUTDOWN!
  */
 inline void gcode_M81() {
-  disable_heater();
+  disable_all_heaters();
   st_synchronize();
   disable_e0();
   disable_e1();
@@ -3803,7 +3863,7 @@ inline void gcode_M206() {
         default:
           SERIAL_ECHO_START;
           SERIAL_ECHOPGM(MSG_UNKNOWN_COMMAND);
-          SERIAL_ECHO(cmdbuffer[bufindr]);
+          SERIAL_ECHO(command_queue[cmd_queue_index_r]);
           SERIAL_ECHOLNPGM("\"");
           return;
       }
@@ -3849,7 +3909,7 @@ inline void gcode_M206() {
  * M220: Set speed percentage factor, aka "Feed Rate" (M220 S95)
  */
 inline void gcode_M220() {
-  if (code_seen('S')) feedmultiply = code_value();
+  if (code_seen('S')) feedrate_multiplier = code_value();
 }
 
 /**
@@ -4485,7 +4545,7 @@ inline void gcode_M503() {
     #endif        
 
     #ifdef FILAMENT_RUNOUT_SENSOR
-      filrunoutEnqued = false;
+      filrunoutEnqueued = false;
     #endif
     
   }
@@ -4619,6 +4679,9 @@ inline void gcode_M999() {
   FlushSerialRequestResend();
 }
 
+/**
+ * T0-T3: Switch tool, usually switching extruders
+ */
 inline void gcode_T() {
   int tmp_extruder = code_value();
   if (tmp_extruder >= EXTRUDERS) {
@@ -5208,7 +5271,7 @@ void process_commands() {
   else {
     SERIAL_ECHO_START;
     SERIAL_ECHOPGM(MSG_UNKNOWN_COMMAND);
-    SERIAL_ECHO(cmdbuffer[bufindr]);
+    SERIAL_ECHO(command_queue[cmd_queue_index_r]);
     SERIAL_ECHOLNPGM("\"");
   }
 
@@ -5216,7 +5279,7 @@ void process_commands() {
 }
 
 void FlushSerialRequestResend() {
-  //char cmdbuffer[bufindr][100]="Resend:";
+  //char command_queue[cmd_queue_index_r][100]="Resend:";
   MYSERIAL.flush();
   SERIAL_PROTOCOLPGM(MSG_RESEND);
   SERIAL_PROTOCOLLN(gcode_LastN + 1);
@@ -5226,7 +5289,7 @@ void FlushSerialRequestResend() {
 void ClearToSend() {
   refresh_cmd_timeout();
   #ifdef SDSUPPORT
-    if (fromsd[bufindr]) return;
+    if (fromsd[cmd_queue_index_r]) return;
   #endif
   SERIAL_PROTOCOLLNPGM(MSG_OK);
 }
@@ -5470,7 +5533,7 @@ void prepare_move() {
     float cartesian_mm = sqrt(sq(difference[X_AXIS]) + sq(difference[Y_AXIS]) + sq(difference[Z_AXIS]));
     if (cartesian_mm < 0.000001) { cartesian_mm = abs(difference[E_AXIS]); }
     if (cartesian_mm < 0.000001) { return; }
-    float seconds = 6000 * cartesian_mm / feedrate / feedmultiply;
+    float seconds = 6000 * cartesian_mm / feedrate / feedrate_multiplier;
     int steps = max(1, int(scara_segments_per_second * seconds));
 
     //SERIAL_ECHOPGM("mm="); SERIAL_ECHO(cartesian_mm);
@@ -5489,7 +5552,7 @@ void prepare_move() {
       //SERIAL_ECHOPGM("delta[Y_AXIS]="); SERIAL_ECHOLN(delta[Y_AXIS]);
       //SERIAL_ECHOPGM("delta[Z_AXIS]="); SERIAL_ECHOLN(delta[Z_AXIS]);
 
-      plan_buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], destination[E_AXIS], feedrate/60*feedmultiply/100.0, active_extruder);
+      plan_buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], destination[E_AXIS], feedrate/60*feedrate_multiplier/100.0, active_extruder);
     }
 
   #endif // SCARA
@@ -5502,7 +5565,7 @@ void prepare_move() {
     float cartesian_mm = sqrt(sq(difference[X_AXIS]) + sq(difference[Y_AXIS]) + sq(difference[Z_AXIS]));
     if (cartesian_mm < 0.000001) cartesian_mm = abs(difference[E_AXIS]);
     if (cartesian_mm < 0.000001) return;
-    float seconds = 6000 * cartesian_mm / feedrate / feedmultiply;
+    float seconds = 6000 * cartesian_mm / feedrate / feedrate_multiplier;
     int steps = max(1, int(delta_segments_per_second * seconds));
 
     // SERIAL_ECHOPGM("mm="); SERIAL_ECHO(cartesian_mm);
@@ -5516,7 +5579,7 @@ void prepare_move() {
       #ifdef ENABLE_AUTO_BED_LEVELING
         adjust_delta(destination);
       #endif
-      plan_buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], destination[E_AXIS], feedrate/60*feedmultiply/100.0, active_extruder);
+      plan_buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], destination[E_AXIS], feedrate/60*feedrate_multiplier/100.0, active_extruder);
     }
 
   #endif // DELTA
@@ -5556,16 +5619,16 @@ void prepare_move() {
   #endif // DUAL_X_CARRIAGE
 
   #if !defined(DELTA) && !defined(SCARA)
-    // Do not use feedmultiply for E or Z only moves
+    // Do not use feedrate_multiplier for E or Z only moves
     if (current_position[X_AXIS] == destination[X_AXIS] && current_position[Y_AXIS] == destination[Y_AXIS]) {
       line_to_destination();
     }
     else {
       #ifdef MESH_BED_LEVELING
-        mesh_plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], (feedrate/60)*(feedmultiply/100.0), active_extruder);
+        mesh_plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], (feedrate/60)*(feedrate_multiplier/100.0), active_extruder);
         return;
       #else
-        line_to_destination(feedrate * feedmultiply / 100.0);
+        line_to_destination(feedrate * feedrate_multiplier / 100.0);
       #endif  // MESH_BED_LEVELING
     }
   #endif // !(DELTA || SCARA)
@@ -5577,7 +5640,7 @@ void prepare_arc_move(char isclockwise) {
   float r = hypot(offset[X_AXIS], offset[Y_AXIS]); // Compute arc radius for mc_arc
 
   // Trace the arc
-  mc_arc(current_position, destination, offset, X_AXIS, Y_AXIS, Z_AXIS, feedrate*feedmultiply/60/100.0, r, isclockwise, active_extruder);
+  mc_arc(current_position, destination, offset, X_AXIS, Y_AXIS, Z_AXIS, feedrate*feedrate_multiplier/60/100.0, r, isclockwise, active_extruder);
 
   // As far as the parser is concerned, the position is now == target. In reality the
   // motion control system might still be processing the action and the real tool position
@@ -5762,7 +5825,7 @@ void manage_inactivity(bool ignore_stepper_queue/*=false*/) {
       filrunout();
   #endif
 
-  if (buflen < BUFSIZE - 1) get_command();
+  if (commands_in_queue < BUFSIZE - 1) get_command();
 
   millis_t ms = millis();
 
@@ -5898,7 +5961,7 @@ void manage_inactivity(bool ignore_stepper_queue/*=false*/) {
 void kill()
 {
   cli(); // Stop interrupts
-  disable_heater();
+  disable_all_heaters();
 
   disable_all_steppers();
 
@@ -5919,18 +5982,18 @@ void kill()
 }
 
 #ifdef FILAMENT_RUNOUT_SENSOR
-   void filrunout()
-   {
-      if (filrunoutEnqued == false) {
-         filrunoutEnqued = true;
-         enqueuecommand("M600");
-      }
-   }
+
+  void filrunout() {
+    if (!filrunoutEnqueued) {
+      filrunoutEnqueued = true;
+      enqueuecommand("M600");
+    }
+  }
+
 #endif
 
-void Stop()
-{
-  disable_heater();
+void Stop() {
+  disable_all_heaters();
   if (IsRunning()) {
     Running = false;
     Stopped_gcode_LastN = gcode_LastN; // Save last g_code for restart
diff --git a/Marlin/configurator/config/language.h b/Marlin/configurator/config/language.h
index 5d1fd6c2fb..eb0ef8c0a1 100644
--- a/Marlin/configurator/config/language.h
+++ b/Marlin/configurator/config/language.h
@@ -110,7 +110,7 @@
 
 // Serial Console Messages (do not translate those!)
 
-#define MSG_Enqueing                        "enqueing \""
+#define MSG_Enqueueing                      "enqueueing \""
 #define MSG_POWERUP                         "PowerUp"
 #define MSG_EXTERNAL_RESET                  " External Reset"
 #define MSG_BROWNOUT_RESET                  " Brown out Reset"
diff --git a/Marlin/dogm_lcd_implementation.h b/Marlin/dogm_lcd_implementation.h
index 546703bfb0..3ab18af75c 100644
--- a/Marlin/dogm_lcd_implementation.h
+++ b/Marlin/dogm_lcd_implementation.h
@@ -269,8 +269,8 @@ static void lcd_implementation_status_screen() {
     }
 
     u8g.setPrintPos(80,48);
-    if (starttime != 0) {
-      uint16_t time = (millis() - starttime) / 60000;
+    if (print_job_start_ms != 0) {
+      uint16_t time = (millis() - print_job_start_ms) / 60000;
       lcd_print(itostr2(time/60));
       lcd_print(':');
       lcd_print(itostr2(time%60));
@@ -337,7 +337,7 @@ static void lcd_implementation_status_screen() {
   lcd_print(LCD_STR_FEEDRATE[0]);
   lcd_setFont(FONT_STATUSMENU);
   u8g.setPrintPos(12,49);
-  lcd_print(itostr3(feedmultiply));
+  lcd_print(itostr3(feedrate_multiplier));
   lcd_print('%');
 
   // Status line
diff --git a/Marlin/language.h b/Marlin/language.h
index 179a1b9563..ba5f3f700e 100644
--- a/Marlin/language.h
+++ b/Marlin/language.h
@@ -110,7 +110,7 @@
 
 // Serial Console Messages (do not translate those!)
 
-#define MSG_Enqueing                        "enqueing \""
+#define MSG_Enqueueing                      "enqueueing \""
 #define MSG_POWERUP                         "PowerUp"
 #define MSG_EXTERNAL_RESET                  " External Reset"
 #define MSG_BROWNOUT_RESET                  " Brown out Reset"
diff --git a/Marlin/temperature.cpp b/Marlin/temperature.cpp
index 9c9d6c5e08..69a2877e70 100644
--- a/Marlin/temperature.cpp
+++ b/Marlin/temperature.cpp
@@ -219,7 +219,7 @@ void PID_autotune(float temp, int extruder, int ncycles)
   
   SERIAL_ECHOLN(MSG_PID_AUTOTUNE_START);
 
-  disable_heater(); // switch off all heaters.
+  disable_all_heaters(); // switch off all heaters.
 
   if (extruder < 0)
     soft_pwm_bed = bias = d = MAX_BED_POWER / 2;
@@ -458,11 +458,11 @@ inline void _temp_error(int e, const char *msg1, const char *msg2) {
 }
 
 void max_temp_error(uint8_t e) {
-  disable_heater();
+  disable_all_heaters();
   _temp_error(e, PSTR(MSG_MAXTEMP_EXTRUDER_OFF), PSTR(MSG_ERR_MAXTEMP));
 }
 void min_temp_error(uint8_t e) {
-  disable_heater();
+  disable_all_heaters();
   _temp_error(e, PSTR(MSG_MINTEMP_EXTRUDER_OFF), PSTR(MSG_ERR_MINTEMP));
 }
 void bed_max_temp_error(void) {
@@ -579,6 +579,14 @@ float get_pid_output(int e) {
   }
 #endif
 
+/**
+ * Manage heating activities for extruder hot-ends and a heated bed
+ *  - Acquire updated temperature readings
+ *  - Invoke thermal runaway protection
+ *  - Manage extruder auto-fan
+ *  - Apply filament width to the extrusion rate (may move)
+ *  - Update the heated bed PID output value
+ */
 void manage_heater() {
 
   if (!temp_meas_ready) return;
@@ -623,7 +631,7 @@ void manage_heater() {
 
     #ifdef TEMP_SENSOR_1_AS_REDUNDANT
       if (fabs(current_temperature[0] - redundant_temperature) > MAX_REDUNDANT_TEMP_SENSOR_DIFF) {
-        disable_heater();
+        disable_all_heaters();
         _temp_error(0, PSTR(MSG_EXTRUDER_SWITCHED_OFF), PSTR(MSG_ERR_REDUNDANT_TEMP));
       }
     #endif // TEMP_SENSOR_1_AS_REDUNDANT
@@ -636,7 +644,22 @@ void manage_heater() {
       next_auto_fan_check_ms = ms + 2500;
     }
   #endif       
-  
+
+  // Control the extruder rate based on the width sensor
+  #ifdef FILAMENT_SENSOR
+    if (filament_sensor) {
+      meas_shift_index = delay_index1 - meas_delay_cm;
+      if (meas_shift_index < 0) meas_shift_index += MAX_MEASUREMENT_DELAY + 1;  //loop around buffer if needed
+      
+      // Get the delayed info and add 100 to reconstitute to a percent of
+      // the nominal filament diameter then square it to get an area
+      meas_shift_index = constrain(meas_shift_index, 0, MAX_MEASUREMENT_DELAY);
+      float vm = pow((measurement_delay[meas_shift_index] + 100.0) / 100.0, 2);
+      if (vm < 0.01) vm = 0.01;
+      volumetric_multiplier[FILAMENT_SENSOR_EXTRUDER_NUM] = vm;
+    }
+  #endif //FILAMENT_SENSOR
+
   #ifndef PIDTEMPBED
     if (ms < next_bed_check_ms) return;
     next_bed_check_ms = ms + BED_CHECK_INTERVAL;
@@ -653,16 +676,7 @@ void manage_heater() {
 
       soft_pwm_bed = current_temperature_bed > BED_MINTEMP && current_temperature_bed < BED_MAXTEMP ? (int)pid_output >> 1 : 0;
 
-    #elif !defined(BED_LIMIT_SWITCHING)
-      // Check if temperature is within the correct range
-      if (current_temperature_bed > BED_MINTEMP && current_temperature_bed < BED_MAXTEMP) {
-        soft_pwm_bed = current_temperature_bed < target_temperature_bed ? MAX_BED_POWER >> 1 : 0;
-      }
-      else {
-        soft_pwm_bed = 0;
-        WRITE_HEATER_BED(LOW);
-      }
-    #else //#ifdef BED_LIMIT_SWITCHING
+    #elif defined(BED_LIMIT_SWITCHING)
       // Check if temperature is within the correct band
       if (current_temperature_bed > BED_MINTEMP && current_temperature_bed < BED_MAXTEMP) {
         if (current_temperature_bed >= target_temperature_bed + BED_HYSTERESIS)
@@ -674,58 +688,47 @@ void manage_heater() {
         soft_pwm_bed = 0;
         WRITE_HEATER_BED(LOW);
       }
+    #else // BED_LIMIT_SWITCHING
+      // Check if temperature is within the correct range
+      if (current_temperature_bed > BED_MINTEMP && current_temperature_bed < BED_MAXTEMP) {
+        soft_pwm_bed = current_temperature_bed < target_temperature_bed ? MAX_BED_POWER >> 1 : 0;
+      }
+      else {
+        soft_pwm_bed = 0;
+        WRITE_HEATER_BED(LOW);
+      }
     #endif
   #endif //TEMP_SENSOR_BED != 0
-  
-  // Control the extruder rate based on the width sensor
-  #ifdef FILAMENT_SENSOR
-    if (filament_sensor) {
-      meas_shift_index = delay_index1 - meas_delay_cm;
-      if (meas_shift_index < 0) meas_shift_index += MAX_MEASUREMENT_DELAY + 1;  //loop around buffer if needed
-      
-      // Get the delayed info and add 100 to reconstitute to a percent of
-      // the nominal filament diameter then square it to get an area
-      meas_shift_index = constrain(meas_shift_index, 0, MAX_MEASUREMENT_DELAY);
-      float vm = pow((measurement_delay[meas_shift_index] + 100.0) / 100.0, 2);
-      if (vm < 0.01) vm = 0.01;
-      volumetric_multiplier[FILAMENT_SENSOR_EXTRUDER_NUM] = vm;
-    }
-  #endif //FILAMENT_SENSOR
 }
 
 #define PGM_RD_W(x)   (short)pgm_read_word(&x)
 // Derived from RepRap FiveD extruder::getTemperature()
 // For hot end temperature measurement.
 static float analog2temp(int raw, uint8_t e) {
-#ifdef TEMP_SENSOR_1_AS_REDUNDANT
-  if (e > EXTRUDERS)
-#else
-  if (e >= EXTRUDERS)
-#endif
-  {
+  #ifdef TEMP_SENSOR_1_AS_REDUNDANT
+    if (e > EXTRUDERS)
+  #else
+    if (e >= EXTRUDERS)
+  #endif
+    {
       SERIAL_ERROR_START;
       SERIAL_ERROR((int)e);
       SERIAL_ERRORLNPGM(MSG_INVALID_EXTRUDER_NUM);
       kill();
       return 0.0;
-  } 
+    } 
+
   #ifdef HEATER_0_USES_MAX6675
-    if (e == 0)
-    {
-      return 0.25 * raw;
-    }
+    if (e == 0) return 0.25 * raw;
   #endif
 
-  if(heater_ttbl_map[e] != NULL)
-  {
+  if (heater_ttbl_map[e] != NULL) {
     float celsius = 0;
     uint8_t i;
     short (*tt)[][2] = (short (*)[][2])(heater_ttbl_map[e]);
 
-    for (i=1; i<heater_ttbllen_map[e]; i++)
-    {
-      if (PGM_RD_W((*tt)[i][0]) > raw)
-      {
+    for (i = 1; i < heater_ttbllen_map[e]; i++) {
+      if (PGM_RD_W((*tt)[i][0]) > raw) {
         celsius = PGM_RD_W((*tt)[i-1][1]) + 
           (raw - PGM_RD_W((*tt)[i-1][0])) * 
           (float)(PGM_RD_W((*tt)[i][1]) - PGM_RD_W((*tt)[i-1][1])) /
@@ -749,10 +752,8 @@ static float analog2tempBed(int raw) {
     float celsius = 0;
     byte i;
 
-    for (i=1; i<BEDTEMPTABLE_LEN; i++)
-    {
-      if (PGM_RD_W(BEDTEMPTABLE[i][0]) > raw)
-      {
+    for (i = 1; i < BEDTEMPTABLE_LEN; i++) {
+      if (PGM_RD_W(BEDTEMPTABLE[i][0]) > raw) {
         celsius  = PGM_RD_W(BEDTEMPTABLE[i-1][1]) + 
           (raw - PGM_RD_W(BEDTEMPTABLE[i-1][0])) * 
           (float)(PGM_RD_W(BEDTEMPTABLE[i][1]) - PGM_RD_W(BEDTEMPTABLE[i-1][1])) /
@@ -816,11 +817,11 @@ static void updateTemperaturesFromRawValues() {
 #endif
 
 
-
-
-
-void tp_init()
-{
+/**
+ * Initialize the temperature manager
+ * The manager is implemented by periodic calls to manage_heater()
+ */
+void tp_init() {
   #if MB(RUMBA) && ((TEMP_SENSOR_0==-1)||(TEMP_SENSOR_1==-1)||(TEMP_SENSOR_2==-1)||(TEMP_SENSOR_BED==-1))
     //disable RUMBA JTAG in case the thermocouple extension is plugged on top of JTAG connector
     MCUCR=BIT(JTD);
@@ -1059,7 +1060,7 @@ void setWatch() {
         SERIAL_ERRORLNPGM(MSG_THERMAL_RUNAWAY_STOP);
         if (heater_id < 0) SERIAL_ERRORLNPGM("bed"); else SERIAL_ERRORLN(heater_id);
         LCD_ALERTMESSAGEPGM(MSG_THERMAL_RUNAWAY);
-        disable_heater();
+        disable_all_heaters();
         disable_all_steppers();
         for (;;) {
           manage_heater();
@@ -1070,7 +1071,7 @@ void setWatch() {
 
 #endif // HAS_HEATER_THERMAL_PROTECTION || HAS_BED_THERMAL_PROTECTION
 
-void disable_heater() {
+void disable_all_heaters() {
   for (int i=0; i<EXTRUDERS; i++) setTargetHotend(0, i);
   setTargetBed(0);
 
@@ -1208,11 +1209,15 @@ static void set_current_temp_raw() {
   temp_meas_ready = true;
 }
 
-//
-// Timer 0 is shared with millies
-//
+/**
+ * Timer 0 is shared with millies
+ *  - Manage PWM to all the heaters and fan
+ *  - Update the raw temperature values
+ *  - Check new temperature values for MIN/MAX errors
+ *  - Step the babysteps value for each axis towards 0
+ */
 ISR(TIMER0_COMPB_vect) {
-  //these variables are only accesible from the ISR, but static, so they don't lose their value
+
   static unsigned char temp_count = 0;
   static TempState temp_state = StartupDelay;
   static unsigned char pwm_count = BIT(SOFT_PWM_SCALE);
@@ -1414,6 +1419,7 @@ ISR(TIMER0_COMPB_vect) {
     #define START_ADC(pin) ADCSRB = 0; SET_ADMUX_ADCSRA(pin)
   #endif
 
+  // Prepare or measure a sensor, each one every 12th frame
   switch(temp_state) {
     case PrepareTemp_0:
       #if HAS_TEMP_0
@@ -1582,16 +1588,16 @@ ISR(TIMER0_COMPB_vect) {
   } // temp_count >= OVERSAMPLENR
 
   #ifdef BABYSTEPPING
-    for (uint8_t axis=X_AXIS; axis<=Z_AXIS; axis++) {
-      int curTodo=babystepsTodo[axis]; //get rid of volatile for performance
+    for (uint8_t axis = X_AXIS; axis <= Z_AXIS; axis++) {
+      int curTodo = babystepsTodo[axis]; //get rid of volatile for performance
      
       if (curTodo > 0) {
         babystep(axis,/*fwd*/true);
-        babystepsTodo[axis]--; //less to do next time
+        babystepsTodo[axis]--; //fewer to do next time
       }
-      else if(curTodo < 0) {
+      else if (curTodo < 0) {
         babystep(axis,/*fwd*/false);
-        babystepsTodo[axis]++; //less to do next time
+        babystepsTodo[axis]++; //fewer to do next time
       }
     }
   #endif //BABYSTEPPING
diff --git a/Marlin/temperature.h b/Marlin/temperature.h
index 847c41b009..f9effaa9ed 100644
--- a/Marlin/temperature.h
+++ b/Marlin/temperature.h
@@ -129,7 +129,7 @@ HOTEND_ROUTINES(0);
 #endif
 
 int getHeaterPower(int heater);
-void disable_heater();
+void disable_all_heaters();
 void setWatch();
 void updatePID();
 
diff --git a/Marlin/ultralcd.cpp b/Marlin/ultralcd.cpp
index 293f819c92..d0fe358ac9 100644
--- a/Marlin/ultralcd.cpp
+++ b/Marlin/ultralcd.cpp
@@ -152,10 +152,10 @@ static void lcd_status_screen();
    *     lcd_implementation_drawmenu_function(sel, row, PSTR(MSG_PAUSE_PRINT), lcd_sdcard_pause)
    *     menu_action_function(lcd_sdcard_pause)
    *
-   *   MENU_ITEM_EDIT(int3, MSG_SPEED, &feedmultiply, 10, 999)
-   *   MENU_ITEM(setting_edit_int3, MSG_SPEED, PSTR(MSG_SPEED), &feedmultiply, 10, 999)
-   *     lcd_implementation_drawmenu_setting_edit_int3(sel, row, PSTR(MSG_SPEED), PSTR(MSG_SPEED), &feedmultiply, 10, 999)
-   *     menu_action_setting_edit_int3(PSTR(MSG_SPEED), &feedmultiply, 10, 999)
+   *   MENU_ITEM_EDIT(int3, MSG_SPEED, &feedrate_multiplier, 10, 999)
+   *   MENU_ITEM(setting_edit_int3, MSG_SPEED, PSTR(MSG_SPEED), &feedrate_multiplier, 10, 999)
+   *     lcd_implementation_drawmenu_setting_edit_int3(sel, row, PSTR(MSG_SPEED), PSTR(MSG_SPEED), &feedrate_multiplier, 10, 999)
+   *     menu_action_setting_edit_int3(PSTR(MSG_SPEED), &feedrate_multiplier, 10, 999)
    *
    */
   #define MENU_ITEM(type, label, args...) do { \
@@ -328,28 +328,28 @@ static void lcd_status_screen() {
 
     #ifdef ULTIPANEL_FEEDMULTIPLY
       // Dead zone at 100% feedrate
-      if ((feedmultiply < 100 && (feedmultiply + int(encoderPosition)) > 100) ||
-              (feedmultiply > 100 && (feedmultiply + int(encoderPosition)) < 100)) {
+      if ((feedrate_multiplier < 100 && (feedrate_multiplier + int(encoderPosition)) > 100) ||
+              (feedrate_multiplier > 100 && (feedrate_multiplier + int(encoderPosition)) < 100)) {
         encoderPosition = 0;
-        feedmultiply = 100;
+        feedrate_multiplier = 100;
       }
-      if (feedmultiply == 100) {
+      if (feedrate_multiplier == 100) {
         if (int(encoderPosition) > ENCODER_FEEDRATE_DEADZONE) {
-          feedmultiply += int(encoderPosition) - ENCODER_FEEDRATE_DEADZONE;
+          feedrate_multiplier += int(encoderPosition) - ENCODER_FEEDRATE_DEADZONE;
           encoderPosition = 0;
         }
         else if (int(encoderPosition) < -ENCODER_FEEDRATE_DEADZONE) {
-          feedmultiply += int(encoderPosition) + ENCODER_FEEDRATE_DEADZONE;
+          feedrate_multiplier += int(encoderPosition) + ENCODER_FEEDRATE_DEADZONE;
           encoderPosition = 0;
         }
       }
       else {
-        feedmultiply += int(encoderPosition);
+        feedrate_multiplier += int(encoderPosition);
         encoderPosition = 0;
       }
     #endif // ULTIPANEL_FEEDMULTIPLY
 
-    feedmultiply = constrain(feedmultiply, 10, 999);
+    feedrate_multiplier = constrain(feedrate_multiplier, 10, 999);
 
   #endif //ULTIPANEL
 }
@@ -456,7 +456,7 @@ void lcd_set_home_offsets() {
 static void lcd_tune_menu() {
   START_MENU();
   MENU_ITEM(back, MSG_MAIN, lcd_main_menu);
-  MENU_ITEM_EDIT(int3, MSG_SPEED, &feedmultiply, 10, 999);
+  MENU_ITEM_EDIT(int3, MSG_SPEED, &feedrate_multiplier, 10, 999);
   #if TEMP_SENSOR_0 != 0
     MENU_MULTIPLIER_ITEM_EDIT(int3, MSG_NOZZLE, &target_temperature[0], 0, HEATER_0_MAXTEMP - 15);
   #endif
diff --git a/Marlin/ultralcd_implementation_hitachi_HD44780.h b/Marlin/ultralcd_implementation_hitachi_HD44780.h
index 2601575e3b..f9ed56a7dc 100644
--- a/Marlin/ultralcd_implementation_hitachi_HD44780.h
+++ b/Marlin/ultralcd_implementation_hitachi_HD44780.h
@@ -550,7 +550,7 @@ static void lcd_implementation_status_screen() {
 
     lcd.setCursor(0, 2);
     lcd.print(LCD_STR_FEEDRATE[0]);
-    lcd.print(itostr3(feedmultiply));
+    lcd.print(itostr3(feedrate_multiplier));
     lcd.print('%');
 
     #if LCD_WIDTH > 19 && defined(SDSUPPORT)
@@ -567,8 +567,8 @@ static void lcd_implementation_status_screen() {
 
     lcd.setCursor(LCD_WIDTH - 6, 2);
     lcd.print(LCD_STR_CLOCK[0]);
-    if (starttime != 0) {
-      uint16_t time = millis()/60000 - starttime/60000;
+    if (print_job_start_ms != 0) {
+      uint16_t time = millis()/60000 - print_job_start_ms/60000;
       lcd.print(itostr2(time/60));
       lcd.print(':');
       lcd.print(itostr2(time%60));