Merge pull request #6634 from thinkyhead/bf_fresh_gcode_parser

New GCode Parser

Marlin/Conditionals_post.h

@@ -838,4 +838,7 @@
   // Shorthand
   #define GRID_MAX_POINTS ((GRID_MAX_POINTS_X) * (GRID_MAX_POINTS_Y))
 
+  // Add commands that need sub-codes to this list
+  #define USE_GCODE_SUBCODES ENABLED(G38_PROBE_TARGET)
+
 #endif // CONDITIONALS_POST_H

Marlin/Configuration_adv.h

@@ -1226,4 +1226,9 @@
  */
 #define PROPORTIONAL_FONT_RATIO 1.0
 
+/**
+ * Spend 28 bytes of SRAM to optimize the GCode parser
+ */
+#define FASTER_GCODE_PARSER
+
 #endif // CONFIGURATION_ADV_H

Marlin/G26_Mesh_Validation_Tool.cpp

@@ -34,6 +34,7 @@
   #include "stepper.h"
   #include "temperature.h"
   #include "ultralcd.h"
+  #include "gcode.h"
 
   #define EXTRUSION_MULTIPLIER 1.0
   #define RETRACTION_MULTIPLIER 1.0
@@ -130,11 +131,7 @@
   void set_destination_to_current();
   void set_current_to_destination();
   void prepare_move_to_destination();
-  float code_value_float();
+  void lcd_setstatuspgm(const char* const message, const uint8_t level);
-  float code_value_linear_units();
-  float code_value_axis_units(const AxisEnum axis);
-  bool code_value_bool();
-  bool code_has_value();
   void sync_plan_position_e();
   void chirp_at_user();
 
@@ -625,29 +622,29 @@
     g26_hotend_temp           = HOTEND_TEMP;
     g26_prime_flag            = 0;
 
-    g26_ooze_amount           = code_seen('O') && code_has_value() ? code_value_linear_units() : OOZE_AMOUNT;
+    g26_ooze_amount           = parser.seen('O') && parser.has_value() ? parser.value_linear_units() : OOZE_AMOUNT;
-    g26_keep_heaters_on       = code_seen('K') && code_value_bool();
+    g26_keep_heaters_on       = parser.seen('K') && parser.value_bool();
-    g26_continue_with_closest = code_seen('C') && code_value_bool();
+    g26_continue_with_closest = parser.seen('C') && parser.value_bool();
 
-    if (code_seen('B')) {
+    if (parser.seen('B')) {
-      g26_bed_temp = code_value_temp_abs();
+      g26_bed_temp = parser.value_celsius();
       if (!WITHIN(g26_bed_temp, 15, 140)) {
         SERIAL_PROTOCOLLNPGM("?Specified bed temperature not plausible.");
         return UBL_ERR;
       }
     }
 
-    if (code_seen('L')) {
+    if (parser.seen('L')) {
-      g26_layer_height = code_value_linear_units();
+      g26_layer_height = parser.value_linear_units();
       if (!WITHIN(g26_layer_height, 0.0, 2.0)) {
         SERIAL_PROTOCOLLNPGM("?Specified layer height not plausible.");
         return UBL_ERR;
       }
     }
 
-    if (code_seen('Q')) {
+    if (parser.seen('Q')) {
-      if (code_has_value()) {
+      if (parser.has_value()) {
-        g26_retraction_multiplier = code_value_float();
+        g26_retraction_multiplier = parser.value_float();
         if (!WITHIN(g26_retraction_multiplier, 0.05, 15.0)) {
           SERIAL_PROTOCOLLNPGM("?Specified Retraction Multiplier not plausible.");
           return UBL_ERR;
@@ -659,20 +656,20 @@
       }
     }
 
-    if (code_seen('S')) {
+    if (parser.seen('S')) {
-      g26_nozzle = code_value_float();
+      g26_nozzle = parser.value_float();
       if (!WITHIN(g26_nozzle, 0.1, 1.0)) {
         SERIAL_PROTOCOLLNPGM("?Specified nozzle size not plausible.");
         return UBL_ERR;
       }
     }
 
-    if (code_seen('P')) {
+    if (parser.seen('P')) {
-      if (!code_has_value())
+      if (!parser.has_value())
         g26_prime_flag = -1;
       else {
         g26_prime_flag++;
-        g26_prime_length = code_value_linear_units();
+        g26_prime_length = parser.value_linear_units();
         if (!WITHIN(g26_prime_length, 0.0, 25.0)) {
           SERIAL_PROTOCOLLNPGM("?Specified prime length not plausible.");
           return UBL_ERR;
@@ -680,8 +677,8 @@
       }
     }
 
-    if (code_seen('F')) {
+    if (parser.seen('F')) {
-      g26_filament_diameter = code_value_linear_units();
+      g26_filament_diameter = parser.value_linear_units();
       if (!WITHIN(g26_filament_diameter, 1.0, 4.0)) {
         SERIAL_PROTOCOLLNPGM("?Specified filament size not plausible.");
         return UBL_ERR;
@@ -693,27 +690,28 @@
 
     g26_extrusion_multiplier *= g26_filament_diameter * sq(g26_nozzle) / sq(0.3); // Scale up by nozzle size
 
-    if (code_seen('H')) {
+    if (parser.seen('H')) {
-      g26_hotend_temp = code_value_temp_abs();
+      g26_hotend_temp = parser.value_celsius();
       if (!WITHIN(g26_hotend_temp, 165, 280)) {
         SERIAL_PROTOCOLLNPGM("?Specified nozzle temperature not plausible.");
         return UBL_ERR;
       }
     }
 
-    if (code_seen('U')) {
+    if (parser.seen('U')) {
       randomSeed(millis());
-      random_deviation = code_has_value() ? code_value_float() : 50.0;
+      // This setting will persist for the next G26
+      random_deviation = parser.has_value() ? parser.value_float() : 50.0;
     }
 
-    g26_repeats = code_seen('R') ? (code_has_value() ? code_value_int() : GRID_MAX_POINTS+1) : GRID_MAX_POINTS+1;
+    g26_repeats = parser.seen('R') ? (parser.has_value() ? parser.value_int() : GRID_MAX_POINTS + 1) : GRID_MAX_POINTS + 1;
     if (g26_repeats < 1) {
       SERIAL_PROTOCOLLNPGM("?(R)epeat value not plausible; must be at least 1.");
       return UBL_ERR;
     }
 
-    g26_x_pos = code_seen('X') ? code_value_linear_units() : current_position[X_AXIS];
+    g26_x_pos = parser.seen('X') ? parser.value_linear_units() : current_position[X_AXIS];
-    g26_y_pos = code_seen('Y') ? code_value_linear_units() : current_position[Y_AXIS];
+    g26_y_pos = parser.seen('Y') ? parser.value_linear_units() : current_position[Y_AXIS];
     if (!position_is_reachable_xy(g26_x_pos, g26_y_pos)) {
       SERIAL_PROTOCOLLNPGM("?Specified X,Y coordinate out of bounds.");
       return UBL_ERR;
@@ -722,7 +720,7 @@
     /**
      * Wait until all parameters are verified before altering the state!
      */
-    state.active = !code_seen('D');
+    state.active = !parser.seen('D');
 
     return UBL_OK;
   }

Marlin/M100_Free_Mem_Chk.cpp

@@ -61,6 +61,7 @@ extern size_t  __heap_start, __heap_end, __flp;
 extern char __bss_end;
 
 #include "Marlin.h"
+#include "gcode.h"
 #include "hex_print_routines.h"
 
 //
@@ -188,7 +189,7 @@ void free_memory_pool_report(char * const ptr, const int16_t size) {
    *  This is useful to check the correctness of the M100 D and the M100 F commands.
    */
   void corrupt_free_memory(char *ptr, const uint16_t size) {
-    if (code_seen('C')) {
+    if (parser.seen('C')) {
       ptr += 8;
       const uint16_t near_top = top_of_stack() - ptr - 250, // -250 to avoid interrupt activity that's altered the stack.
                      j = near_top / (size + 1);
@@ -247,23 +248,23 @@ void gcode_M100() {
 
   // Always init on the first invocation of M100
   static bool m100_not_initialized = true;
-  if (m100_not_initialized || code_seen('I')) {
+  if (m100_not_initialized || parser.seen('I')) {
     m100_not_initialized = false;
     init_free_memory(ptr, sp - ptr);
   }
 
   #if ENABLED(M100_FREE_MEMORY_DUMPER)
-    if (code_seen('D'))
+    if (parser.seen('D'))
       return dump_free_memory(ptr, sp);
   #endif
 
-  if (code_seen('F'))
+  if (parser.seen('F'))
     return free_memory_pool_report(ptr, sp - ptr);
 
   #if ENABLED(M100_FREE_MEMORY_CORRUPTOR)
 
-    if (code_seen('C'))
+    if (parser.seen('C'))
-      return corrupt_free_memory(ptr, code_value_int());
+      return corrupt_free_memory(ptr, parser.value_int());
 
   #endif
 }

Marlin/Marlin.h

@@ -287,22 +287,6 @@ extern float soft_endstop_min[XYZ], soft_endstop_max[XYZ];
   void update_software_endstops(const AxisEnum axis);
 #endif
 
-// GCode support for external objects
-bool code_seen(char);
-int code_value_int();
-int16_t code_value_temp_abs();
-int16_t code_value_temp_diff();
-
-#if ENABLED(INCH_MODE_SUPPORT)
-  float code_value_linear_units();
-  float code_value_axis_units(const AxisEnum axis);
-  float code_value_per_axis_unit(const AxisEnum axis);
-#else
-  #define code_value_linear_units() code_value_float()
-  #define code_value_axis_units(A) code_value_float()
-  #define code_value_per_axis_unit(A) code_value_float()
-#endif
-
 #if IS_KINEMATIC
   extern float delta[ABC];
   void inverse_kinematics(const float logical[XYZ]);

Marlin/configuration_store.cpp

@@ -178,6 +178,10 @@ MarlinSettings settings;
 #include "temperature.h"
 #include "ultralcd.h"
 
+#if ENABLED(INCH_MODE_SUPPORT) || (ENABLED(ULTIPANEL) && ENABLED(TEMPERATURE_UNITS_SUPPORT))
+  #include "gcode.h"
+#endif
+
 #if ENABLED(MESH_BED_LEVELING)
   #include "mesh_bed_leveling.h"
 #endif
@@ -1331,13 +1335,12 @@ void MarlinSettings::reset() {
      */
     CONFIG_ECHO_START;
     #if ENABLED(INCH_MODE_SUPPORT)
-      extern float linear_unit_factor, volumetric_unit_factor;
+      #define LINEAR_UNIT(N) ((N) / parser.linear_unit_factor)
-      #define LINEAR_UNIT(N) ((N) / linear_unit_factor)
+      #define VOLUMETRIC_UNIT(N) ((N) / (volumetric_enabled ? parser.volumetric_unit_factor : parser.linear_unit_factor))
-      #define VOLUMETRIC_UNIT(N) ((N) / (volumetric_enabled ? volumetric_unit_factor : linear_unit_factor))
       SERIAL_ECHOPGM("  G2");
-      SERIAL_CHAR(linear_unit_factor == 1.0 ? '1' : '0');
+      SERIAL_CHAR(parser.linear_unit_factor == 1.0 ? '1' : '0');
       SERIAL_ECHOPGM(" ; Units in ");
-      serialprintPGM(linear_unit_factor == 1.0 ? PSTR("mm\n") : PSTR("inches\n"));
+      serialprintPGM(parser.linear_unit_factor == 1.0 ? PSTR("mm\n") : PSTR("inches\n"));
     #else
       #define LINEAR_UNIT(N) N
       #define VOLUMETRIC_UNIT(N) N
@@ -1351,13 +1354,11 @@ void MarlinSettings::reset() {
 
       CONFIG_ECHO_START;
       #if ENABLED(TEMPERATURE_UNITS_SUPPORT)
-        extern TempUnit input_temp_units;
+        #define TEMP_UNIT(N) parser.to_temp_units(N)
-        extern float to_temp_units(const float &f);
-        #define TEMP_UNIT(N) to_temp_units(N)
         SERIAL_ECHOPGM("  M149 ");
-        SERIAL_CHAR(input_temp_units == TEMPUNIT_K ? 'K' : input_temp_units == TEMPUNIT_F ? 'F' : 'C');
+        SERIAL_CHAR(parser.temp_units_code());
         SERIAL_ECHOPGM(" ; Units in ");
-        serialprintPGM(input_temp_units == TEMPUNIT_K ? PSTR("Kelvin\n") : input_temp_units == TEMPUNIT_F ? PSTR("Fahrenheit\n") : PSTR("Celsius\n"));
+        serialprintPGM(parser.temp_units_name());
       #else
         #define TEMP_UNIT(N) N
         SERIAL_ECHOLNPGM("  M149 C ; Units in Celsius\n");

Marlin/example_configurations/Cartesio/Configuration_adv.h

@@ -1219,4 +1219,9 @@
  */
 #define PROPORTIONAL_FONT_RATIO 1.0
 
+/**
+ * Spend 28 bytes of SRAM to optimize the GCode parser
+ */
+#define FASTER_GCODE_PARSER
+
 #endif // CONFIGURATION_ADV_H

Marlin/example_configurations/Felix/Configuration_adv.h

@@ -1219,4 +1219,9 @@
  */
 #define PROPORTIONAL_FONT_RATIO 1.0
 
+/**
+ * Spend 28 bytes of SRAM to optimize the GCode parser
+ */
+#define FASTER_GCODE_PARSER
+
 #endif // CONFIGURATION_ADV_H

Marlin/example_configurations/FolgerTech-i3-2020/Configuration_adv.h

@@ -1230,4 +1230,9 @@
  */
 #define PROPORTIONAL_FONT_RATIO 1.5
 
+/**
+ * Spend 28 bytes of SRAM to optimize the GCode parser
+ */
+#define FASTER_GCODE_PARSER
+
 #endif // CONFIGURATION_ADV_H

Marlin/example_configurations/Hephestos/Configuration_adv.h

@@ -1219,4 +1219,9 @@
  */
 #define PROPORTIONAL_FONT_RATIO 1.0
 
+/**
+ * Spend 28 bytes of SRAM to optimize the GCode parser
+ */
+#define FASTER_GCODE_PARSER
+
 #endif // CONFIGURATION_ADV_H

Marlin/example_configurations/Hephestos_2/Configuration_adv.h

@@ -1203,4 +1203,9 @@
  */
 #define PROPORTIONAL_FONT_RATIO 1.0
 
+/**
+ * Spend 28 bytes of SRAM to optimize the GCode parser
+ */
+#define FASTER_GCODE_PARSER
+
 #endif // CONFIGURATION_ADV_H

Marlin/example_configurations/K8200/Configuration_adv.h

@@ -1232,4 +1232,9 @@
  */
 #define PROPORTIONAL_FONT_RATIO 1.0
 
+/**
+ * Spend 28 bytes of SRAM to optimize the GCode parser
+ */
+#define FASTER_GCODE_PARSER
+
 #endif // CONFIGURATION_ADV_H

Marlin/example_configurations/K8400/Configuration_adv.h

@@ -1219,4 +1219,9 @@
  */
 #define PROPORTIONAL_FONT_RATIO 1.0
 
+/**
+ * Spend 28 bytes of SRAM to optimize the GCode parser
+ */
+#define FASTER_GCODE_PARSER
+
 #endif // CONFIGURATION_ADV_H

Marlin/example_configurations/RigidBot/Configuration_adv.h

@@ -1219,4 +1219,9 @@
  */
 #define PROPORTIONAL_FONT_RATIO 1.0
 
+/**
+ * Spend 28 bytes of SRAM to optimize the GCode parser
+ */
+#define FASTER_GCODE_PARSER
+
 #endif // CONFIGURATION_ADV_H

Marlin/example_configurations/SCARA/Configuration_adv.h

@@ -1219,4 +1219,9 @@
  */
 #define PROPORTIONAL_FONT_RATIO 1.0
 
+/**
+ * Spend 28 bytes of SRAM to optimize the GCode parser
+ */
+#define FASTER_GCODE_PARSER
+
 #endif // CONFIGURATION_ADV_H

Marlin/example_configurations/TAZ4/Configuration_adv.h

@@ -1219,4 +1219,9 @@
  */
 #define PROPORTIONAL_FONT_RATIO 1.0
 
+/**
+ * Spend 28 bytes of SRAM to optimize the GCode parser
+ */
+#define FASTER_GCODE_PARSER
+
 #endif // CONFIGURATION_ADV_H

Marlin/example_configurations/TinyBoy2/Configuration_adv.h

@@ -1222,4 +1222,9 @@
  */
 #define PROPORTIONAL_FONT_RATIO 1.0
 
+/**
+ * Spend 28 bytes of SRAM to optimize the GCode parser
+ */
+#define FASTER_GCODE_PARSER
+
 #endif // CONFIGURATION_ADV_H

Marlin/example_configurations/WITBOX/Configuration_adv.h

@@ -1219,4 +1219,9 @@
  */
 #define PROPORTIONAL_FONT_RATIO 1.0
 
+/**
+ * Spend 28 bytes of SRAM to optimize the GCode parser
+ */
+#define FASTER_GCODE_PARSER
+
 #endif // CONFIGURATION_ADV_H

Marlin/example_configurations/delta/FLSUN/auto_calibrate/Configuration_adv.h

@@ -1224,4 +1224,9 @@
  */
 #define PROPORTIONAL_FONT_RATIO 1.0
 
+/**
+ * Spend 28 bytes of SRAM to optimize the GCode parser
+ */
+#define FASTER_GCODE_PARSER
+
 #endif // CONFIGURATION_ADV_H

Marlin/example_configurations/delta/FLSUN/kossel_mini/Configuration_adv.h

@@ -1223,4 +1223,9 @@
  */
 #define PROPORTIONAL_FONT_RATIO 1.0
 
+/**
+ * Spend 28 bytes of SRAM to optimize the GCode parser
+ */
+#define FASTER_GCODE_PARSER
+
 #endif // CONFIGURATION_ADV_H

Marlin/example_configurations/delta/generic/Configuration_adv.h

@@ -1221,4 +1221,9 @@
  */
 #define PROPORTIONAL_FONT_RATIO 1.0
 
+/**
+ * Spend 28 bytes of SRAM to optimize the GCode parser
+ */
+#define FASTER_GCODE_PARSER
+
 #endif // CONFIGURATION_ADV_H

Marlin/example_configurations/delta/kossel_mini/Configuration_adv.h

@@ -1221,4 +1221,9 @@
  */
 #define PROPORTIONAL_FONT_RATIO 1.0
 
+/**
+ * Spend 28 bytes of SRAM to optimize the GCode parser
+ */
+#define FASTER_GCODE_PARSER
+
 #endif // CONFIGURATION_ADV_H

Marlin/example_configurations/delta/kossel_pro/Configuration_adv.h

@@ -1226,4 +1226,9 @@
  */
 #define PROPORTIONAL_FONT_RATIO 1.0
 
+/**
+ * Spend 28 bytes of SRAM to optimize the GCode parser
+ */
+#define FASTER_GCODE_PARSER
+
 #endif // CONFIGURATION_ADV_H

Marlin/example_configurations/delta/kossel_xl/Configuration_adv.h

@@ -1221,4 +1221,9 @@
  */
 #define PROPORTIONAL_FONT_RATIO 1.0
 
+/**
+ * Spend 28 bytes of SRAM to optimize the GCode parser
+ */
+#define FASTER_GCODE_PARSER
+
 #endif // CONFIGURATION_ADV_H

Marlin/example_configurations/gCreate_gMax1.5+/Configuration_adv.h

@@ -1226,4 +1226,9 @@
  */
 #define PROPORTIONAL_FONT_RATIO 1.5
 
+/**
+ * Spend 28 bytes of SRAM to optimize the GCode parser
+ */
+#define FASTER_GCODE_PARSER
+
 #endif // CONFIGURATION_ADV_H

Marlin/example_configurations/makibox/Configuration_adv.h

@@ -1219,4 +1219,9 @@
  */
 #define PROPORTIONAL_FONT_RATIO 1.0
 
+/**
+ * Spend 28 bytes of SRAM to optimize the GCode parser
+ */
+#define FASTER_GCODE_PARSER
+
 #endif // CONFIGURATION_ADV_H

Marlin/example_configurations/tvrrug/Round2/Configuration_adv.h

@@ -1219,4 +1219,9 @@
  */
 #define PROPORTIONAL_FONT_RATIO 1.0
 
+/**
+ * Spend 28 bytes of SRAM to optimize the GCode parser
+ */
+#define FASTER_GCODE_PARSER
+
 #endif // CONFIGURATION_ADV_H

Marlin/example_configurations/wt150/Configuration_adv.h

@@ -1222,4 +1222,9 @@
  */
 #define PROPORTIONAL_FONT_RATIO 1.0
 
+/**
+ * Spend 28 bytes of SRAM to optimize the GCode parser
+ */
+#define FASTER_GCODE_PARSER
+
 #endif // CONFIGURATION_ADV_H

Marlin/gcode.cpp

@@ -0,0 +1,279 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+/**
+ * gcode.cpp - Parser for a GCode line, providing a parameter interface.
+ */
+
+#include "gcode.h"
+
+#include "Marlin.h"
+#include "language.h"
+
+// Must be declared for allocation and to satisfy the linker
+// Zero values need no initialization.
+
+#if ENABLED(INCH_MODE_SUPPORT)
+  float GCodeParser::linear_unit_factor, GCodeParser::volumetric_unit_factor;
+#endif
+
+#if ENABLED(TEMPERATURE_UNITS_SUPPORT)
+  TempUnit GCodeParser::input_temp_units;
+#endif
+
+char *GCodeParser::command_ptr,
+     *GCodeParser::string_arg,
+     *GCodeParser::value_ptr;
+char GCodeParser::command_letter;
+int GCodeParser::codenum;
+#if USE_GCODE_SUBCODES
+  int GCodeParser::subcode;
+#endif
+
+#if ENABLED(FASTER_GCODE_PARSER)
+  // Optimized Parameters
+  byte GCodeParser::codebits[4];   // found bits
+  uint8_t GCodeParser::param[26];  // parameter offsets from command_ptr
+#else
+  char *GCodeParser::command_args; // start of parameters
+#endif
+
+// Create a global instance of the GCode parser singleton
+GCodeParser parser;
+
+/**
+ * Clear all code-seen (and value pointers)
+ *
+ * Since each param is set/cleared on seen codes,
+ * this may be optimized by commenting out ZERO(param)
+ */
+void GCodeParser::reset() {
+  string_arg = NULL;                    // No whole line argument
+  command_letter = '?';                 // No command letter
+  codenum = 0;                          // No command code
+  #if USE_GCODE_SUBCODES
+    subcode = 0;                        // No command sub-code
+  #endif
+  #if ENABLED(FASTER_GCODE_PARSER)
+    ZERO(codebits);                     // No codes yet
+    //ZERO(param);                      // No parameters (should be safe to comment out this line)
+  #endif
+}
+
+// Populate all fields by parsing a single line of GCode
+// 58 bytes of SRAM are used to speed up seen/value
+void GCodeParser::parse(char *p) {
+
+  reset(); // No codes to report
+
+  // Skip spaces
+  while (*p == ' ') ++p;
+
+  // Skip N[-0-9] if included in the command line
+  if (*p == 'N' && NUMERIC_SIGNED(p[1])) {
+    #if ENABLED(FASTER_GCODE_PARSER)
+      //set('N', p + 1);     // (optional) Set the 'N' parameter value
+    #endif
+    p += 2;                  // skip N[-0-9]
+    while (NUMERIC(*p)) ++p; // skip [0-9]*
+    while (*p == ' ')   ++p; // skip [ ]*
+  }
+
+  // *p now points to the current command, which should be G, M, or T
+  command_ptr = p;
+
+  // Get the command letter, which must be G, M, or T
+  const char letter = *p++;
+
+  // Nullify asterisk and trailing whitespace
+  char *starpos = strchr(p, '*');
+  if (starpos) {
+    --starpos;                          // *
+    while (*starpos == ' ') --starpos;  // spaces...
+    starpos[1] = '\0';
+  }
+
+  // Bail if the letter is not G, M, or T
+  switch (letter) { case 'G': case 'M': case 'T': break; default: return; }
+
+  // Skip spaces to get the numeric part
+  while (*p == ' ') p++;
+
+  // Bail if there's no command code number
+  if (!NUMERIC(*p)) return;
+
+  // Save the command letter at this point
+  // A '?' signifies an unknown command
+  command_letter = letter;
+
+  // Get the code number - integer digits only
+  codenum = 0;
+  do {
+    codenum *= 10, codenum += *p++ - '0';
+  } while (NUMERIC(*p));
+
+  // Allow for decimal point in command
+  #if USE_GCODE_SUBCODES
+    if (*p == '.') {
+      p++;
+      while (NUMERIC(*p))
+        subcode *= 10, subcode += *p++ - '0';
+    }
+  #endif
+
+  // Skip all spaces to get to the first argument, or nul
+  while (*p == ' ') p++;
+
+  // The command parameters (if any) start here, for sure!
+
+  #if DISABLED(FASTER_GCODE_PARSER)
+    command_args = p; // Scan for parameters in seen()
+  #endif
+
+  // Only use string_arg for these M codes
+  if (letter == 'M') switch (codenum) { case 23: case 28: case 30: case 117: case 928: string_arg = p; return; default: break; }
+
+  #if ENABLED(DEBUG_GCODE_PARSER)
+    const bool debug = codenum == 800;
+  #endif
+
+  /**
+   * Find all parameters, set flags and pointers for fast parsing
+   *
+   * Most codes ignore 'string_arg', but those that want a string will get the right pointer.
+   * The following loop assigns the first "parameter" having no numeric value to 'string_arg'.
+   * This allows M0/M1 with expire time to work: "M0 S5 You Win!"
+   */
+  string_arg = NULL;
+  while (char code = *p++) {                    // Get the next parameter. A NUL ends the loop
+
+    // Special handling for M32 [P] !/path/to/file.g#
+    // The path must be the last parameter
+    if (code == '!' && letter == 'M' && codenum == 32) {
+      string_arg = p;                           // Name starts after '!'
+      char * const lb = strchr(p, '#');         // Already seen '#' as SD char (to pause buffering)
+      if (lb) *lb = '\0';                       // Safe to mark the end of the filename
+      return;
+    }
+
+    // Arguments MUST be uppercase for fast GCode parsing
+    #if ENABLED(FASTER_GCODE_PARSER)
+      #define PARAM_TEST WITHIN(code, 'A', 'Z')
+    #else
+      #define PARAM_TEST true
+    #endif
+
+    if (PARAM_TEST) {
+
+      const bool has_num = DECIMAL_SIGNED(*p);  // The parameter has a number [-+0-9.]
+
+      #if ENABLED(DEBUG_GCODE_PARSER)
+        if (debug) {
+          SERIAL_ECHOPAIR("Got letter ", code); // DEBUG
+          SERIAL_ECHOPAIR(" at index ", (int)(p - command_ptr - 1)); // DEBUG
+          if (has_num) SERIAL_ECHOPGM(" (has_num)");
+        }
+      #endif
+
+      if (!has_num && !string_arg) {            // No value? First time, keep as string_arg
+        string_arg = p - 1;
+        #if ENABLED(DEBUG_GCODE_PARSER)
+          if (debug) SERIAL_ECHOPAIR(" string_arg: ", hex_address((void*)string_arg)); // DEBUG
+        #endif
+      }
+
+      #if ENABLED(DEBUG_GCODE_PARSER)
+        if (debug) SERIAL_EOL;
+      #endif
+
+      #if ENABLED(FASTER_GCODE_PARSER)
+        set(code, has_num ? p : NULL            // Set parameter exists and pointer (NULL for no number)
+          #if ENABLED(DEBUG_GCODE_PARSER)
+            , debug
+          #endif
+        );
+      #endif
+    }
+    else if (!string_arg) {                     // Not A-Z? First time, keep as the string_arg
+      string_arg = p - 1;
+      #if ENABLED(DEBUG_GCODE_PARSER)
+        if (debug) SERIAL_ECHOPAIR(" string_arg: ", hex_address((void*)string_arg)); // DEBUG
+      #endif
+    }
+
+    while (*p && *p != ' ') p++;                // Skip over the parameter
+    while (*p == ' ') p++;                      // Skip over all spaces
+  }
+}
+
+void GCodeParser::unknown_command_error() {
+  SERIAL_ECHO_START;
+  SERIAL_ECHOPAIR(MSG_UNKNOWN_COMMAND, command_ptr);
+  SERIAL_CHAR('"');
+  SERIAL_EOL;
+}
+
+#if ENABLED(DEBUG_GCODE_PARSER)
+
+  void GCodeParser::debug() {
+    SERIAL_ECHOPAIR("Command: ", command_ptr);
+    SERIAL_ECHOPAIR(" (", command_letter);
+    SERIAL_ECHO(codenum);
+    SERIAL_ECHOLNPGM(")");
+    #if ENABLED(FASTER_GCODE_PARSER)
+      SERIAL_ECHO(" args: \"");
+      for (char c = 'A'; c <= 'Z'; ++c)
+        if (seen(c)) { SERIAL_CHAR(c); SERIAL_CHAR(' '); }
+    #else
+      SERIAL_ECHOPAIR(" args: \"", command_args);
+    #endif
+    SERIAL_ECHOPGM("\"");
+    if (string_arg) {
+      SERIAL_ECHOPGM(" string: \"");
+      SERIAL_ECHO(string_arg);
+      SERIAL_CHAR('"');
+    }
+    SERIAL_ECHOPGM("\n\n");
+    for (char c = 'A'; c <= 'Z'; ++c) {
+      if (seen(c)) {
+        SERIAL_ECHOPAIR("Code '", c); SERIAL_ECHOPGM("':");
+        if (has_value()) {
+          SERIAL_ECHOPAIR("\n    float: ", value_float());
+          SERIAL_ECHOPAIR("\n     long: ", value_long());
+          SERIAL_ECHOPAIR("\n    ulong: ", value_ulong());
+          SERIAL_ECHOPAIR("\n   millis: ", value_millis());
+          SERIAL_ECHOPAIR("\n   sec-ms: ", value_millis_from_seconds());
+          SERIAL_ECHOPAIR("\n      int: ", value_int());
+          SERIAL_ECHOPAIR("\n   ushort: ", value_ushort());
+          SERIAL_ECHOPAIR("\n     byte: ", (int)value_byte());
+          SERIAL_ECHOPAIR("\n     bool: ", (int)value_bool());
+          SERIAL_ECHOPAIR("\n   linear: ", value_linear_units());
+          SERIAL_ECHOPAIR("\n  celsius: ", value_celsius());
+        }
+        else
+          SERIAL_ECHOPGM(" (no value)");
+        SERIAL_ECHOPGM("\n\n");
+      }
+    }
+  }
+
+#endif // DEBUG_GCODE_PARSER

Marlin/gcode.h

@@ -0,0 +1,285 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+/**
+ * gcode.h - Parser for a GCode line, providing a parameter interface.
+ *           Codes like M149 control the way the GCode parser behaves,
+ *           so settings for these codes are located in this class.
+ */
+
+#ifndef GCODE_H
+#define GCODE_H
+
+#include "enum.h"
+#include "types.h"
+#include "MarlinConfig.h"
+
+//#define DEBUG_GCODE_PARSER
+
+#if ENABLED(DEBUG_GCODE_PARSER)
+  #include "hex_print_routines.h"
+  #include "serial.h"
+#endif
+
+#if ENABLED(INCH_MODE_SUPPORT)
+  extern bool volumetric_enabled;
+#endif
+
+/**
+ * GCode parser
+ *
+ *  - Parse a single gcode line for its letter, code, subcode, and parameters
+ *  - FASTER_GCODE_PARSER:
+ *    - Flags existing params (1 bit each)
+ *    - Stores value offsets (1 byte each)
+ *  - Provide accessors for parameters:
+ *    - Parameter exists
+ *    - Parameter has value
+ *    - Parameter value in different units and types
+ */
+class GCodeParser {
+
+private:
+  static char *value_ptr;           // Set by seen, used to fetch the value
+
+  #if ENABLED(FASTER_GCODE_PARSER)
+    static byte codebits[4];        // Parameters pre-scanned
+    static uint8_t param[26];       // For A-Z, offsets into command args
+  #else
+    static char *command_args;      // Args start here, for slow scan
+  #endif
+
+public:
+
+  // Global states for GCode-level units features
+
+  #if ENABLED(INCH_MODE_SUPPORT)
+    static float linear_unit_factor, volumetric_unit_factor;
+  #endif
+
+  #if ENABLED(TEMPERATURE_UNITS_SUPPORT)
+    static TempUnit input_temp_units;
+  #endif
+
+  // Command line state
+  static char *command_ptr,               // The command, so it can be echoed
+              *string_arg;                // string of command line
+
+  static char command_letter;             // G, M, or T
+  static int codenum;                     // 123
+  #if USE_GCODE_SUBCODES
+    static int subcode;                   // .1
+  #endif
+
+  #if ENABLED(DEBUG_GCODE_PARSER)
+    void debug();
+  #endif
+
+  // Reset is done before parsing
+  static void reset();
+
+  #if ENABLED(FASTER_GCODE_PARSER)
+
+    // Set the flag and pointer for a parameter
+    static void set(const char c, char * const ptr
+      #if ENABLED(DEBUG_GCODE_PARSER)
+        , const bool debug=false
+      #endif
+    ) {
+      const uint8_t ind = c - 'A';
+      if (ind >= COUNT(param)) return;           // Only A-Z
+      SBI(codebits[ind >> 3], ind & 0x7);        // parameter exists
+      param[ind] = ptr ? ptr - command_ptr : 0;  // parameter offset or 0
+      #if ENABLED(DEBUG_GCODE_PARSER)
+        if (debug) {
+          SERIAL_ECHOPAIR("Set bit ", (int)(ind & 0x7));
+          SERIAL_ECHOPAIR(" of index ", (int)(ind >> 3));
+          SERIAL_ECHOLNPAIR(" | param = ", hex_address((void*)param[ind]));
+        }
+      #endif
+    }
+
+    // Code seen bit was set. If not found, value_ptr is unchanged.
+    // This allows "if (seen('A')||seen('B'))" to use the last-found value.
+    static bool seen(const char c) {
+      const uint8_t ind = c - 'A';
+      if (ind >= COUNT(param)) return false; // Only A-Z
+      const bool b = TEST(codebits[ind >> 3], ind & 0x7);
+      if (b) value_ptr = command_ptr + param[ind];
+      return b;
+    }
+
+  #else
+
+    // Code is found in the string. If not found, value_ptr is unchanged.
+    // This allows "if (seen('A')||seen('B'))" to use the last-found value.
+    static bool seen(const char c) {
+      char *p = strchr(command_args, c);
+      const bool b = !!p;
+      if (b) value_ptr = DECIMAL_SIGNED(*p) ? p + 1 : NULL;
+      return b;
+    }
+
+  #endif // FASTER_GCODE_PARSER
+
+  // Populate all fields by parsing a single line of GCode
+  // This uses 54 bytes of SRAM to speed up seen/value
+  static void parse(char * p);
+
+  // Code value pointer was set
+  FORCE_INLINE static bool has_value() { return value_ptr != NULL; }
+
+  // Float removes 'E' to prevent scientific notation interpretation
+  inline static float value_float() {
+    if (value_ptr) {
+      char *e = value_ptr;
+      for (;;) {
+        const char c = *e;
+        if (c == '\0' || c == ' ') break;
+        if (c == 'E' || c == 'e') {
+          *e = '\0';
+          const float ret = strtod(value_ptr, NULL);
+          *e = c;
+          return ret;
+        }
+        ++e;
+      }
+      return strtod(value_ptr, NULL);
+    }
+    return 0.0;
+  }
+
+  // Code value as a long or ulong
+  inline          static long value_long()  { return value_ptr ? strtol(value_ptr, NULL, 10) : 0L; }
+  inline unsigned static long value_ulong() { return value_ptr ? strtoul(value_ptr, NULL, 10) : 0UL; }
+
+  // Code value for use as time
+  FORCE_INLINE static millis_t value_millis() { return value_ulong(); }
+  FORCE_INLINE static millis_t value_millis_from_seconds() { return value_float() * 1000UL; }
+
+  // Reduce to fewer bits
+  FORCE_INLINE static int value_int()    { return (int)value_long(); }
+  FORCE_INLINE uint16_t value_ushort()   { return (uint16_t)value_long(); }
+  inline static uint8_t value_byte()     { return (uint8_t)(constrain(value_long(), 0, 255)); }
+
+  // Bool is true with no value or non-zero
+  inline static bool value_bool()        { return !has_value() || value_byte(); }
+
+  // Units modes: Inches, Fahrenheit, Kelvin
+
+  #if ENABLED(INCH_MODE_SUPPORT)
+
+    inline static void set_input_linear_units(LinearUnit units) {
+      switch (units) {
+        case LINEARUNIT_INCH:
+          linear_unit_factor = 25.4;
+          break;
+        case LINEARUNIT_MM:
+        default:
+          linear_unit_factor = 1.0;
+          break;
+      }
+      volumetric_unit_factor = pow(linear_unit_factor, 3.0);
+    }
+
+    inline static float axis_unit_factor(const AxisEnum axis) {
+      return (axis >= E_AXIS && volumetric_enabled ? volumetric_unit_factor : linear_unit_factor);
+    }
+
+    inline static float value_linear_units()                     { return value_float() * linear_unit_factor; }
+    inline static float value_axis_units(const AxisEnum axis)    { return value_float() * axis_unit_factor(axis); }
+    inline static float value_per_axis_unit(const AxisEnum axis) { return value_float() / axis_unit_factor(axis); }
+
+  #else
+
+    FORCE_INLINE static float value_linear_units()                  {            return value_float(); }
+    FORCE_INLINE static float value_axis_units(const AxisEnum a)    { UNUSED(a); return value_float(); }
+    FORCE_INLINE static float value_per_axis_unit(const AxisEnum a) { UNUSED(a); return value_float(); }
+
+  #endif
+
+  #if ENABLED(TEMPERATURE_UNITS_SUPPORT)
+
+    inline static void set_input_temp_units(TempUnit units) { input_temp_units = units; }
+
+    #if ENABLED(ULTIPANEL) && DISABLED(DISABLE_M503)
+
+      FORCE_INLINE static char temp_units_code() {
+        return input_temp_units == TEMPUNIT_K ? 'K' : input_temp_units == TEMPUNIT_F ? 'F' : 'C';
+      }
+      FORCE_INLINE static char* temp_units_name() {
+        return input_temp_units == TEMPUNIT_K ? PSTR("Kelvin") : input_temp_units == TEMPUNIT_F ? PSTR("Fahrenheit") : PSTR("Celsius")
+      }
+      inline static float to_temp_units(const float &f) {
+        switch (input_temp_units) {
+          case TEMPUNIT_F:
+            return f * 0.5555555556 + 32.0;
+          case TEMPUNIT_K:
+            return f + 273.15;
+          case TEMPUNIT_C:
+          default:
+            return f;
+        }
+      }
+
+    #endif // ULTIPANEL && !DISABLE_M503
+
+    inline static float value_celsius() {
+      const float f = value_float();
+      switch (input_temp_units) {
+        case TEMPUNIT_F:
+          return (f - 32.0) * 0.5555555556;
+        case TEMPUNIT_K:
+          return f - 273.15;
+        case TEMPUNIT_C:
+        default:
+          return f;
+      }
+    }
+
+    inline static float value_celsius_diff() {
+      switch (input_temp_units) {
+        case TEMPUNIT_F:
+          return value_float() * 0.5555555556;
+        case TEMPUNIT_C:
+        case TEMPUNIT_K:
+        default:
+          return value_float();
+      }
+    }
+
+  #else
+
+    FORCE_INLINE static float value_celsius()      { return value_float(); }
+    FORCE_INLINE static float value_celsius_diff() { return value_float(); }
+
+  #endif
+
+  FORCE_INLINE static float value_feedrate() { return value_linear_units(); }
+
+  void unknown_command_error();
+
+};
+
+extern GCodeParser parser;
+
+#endif // GCODE_H

Marlin/hex_print_routines.cpp

@@ -20,7 +20,9 @@
  *
  */
 #include "Marlin.h"
-#if ENABLED(AUTO_BED_LEVELING_UBL) || ENABLED(M100_FREE_MEMORY_WATCHER)
+#include "gcode.h"
+
+#if ENABLED(AUTO_BED_LEVELING_UBL) || ENABLED(M100_FREE_MEMORY_WATCHER) || ENABLED(DEBUG_GCODE_PARSER)
 
 #include "hex_print_routines.h"
 
@@ -50,4 +52,4 @@ void print_hex_byte(const uint8_t b)         { SERIAL_ECHO(hex_byte(b));    }
 void print_hex_word(const uint16_t w)        { SERIAL_ECHO(hex_word(w));    }
 void print_hex_address(const void * const w) { SERIAL_ECHO(hex_address(w)); }
 
-#endif // AUTO_BED_LEVELING_UBL || M100_FREE_MEMORY_WATCHER
+#endif // AUTO_BED_LEVELING_UBL || M100_FREE_MEMORY_WATCHER || DEBUG_GCODE_PARSER

Marlin/hex_print_routines.h

@@ -24,8 +24,9 @@
 #define HEX_PRINT_ROUTINES_H
 
 #include "MarlinConfig.h"
+#include "gcode.h"
 
-#if ENABLED(AUTO_BED_LEVELING_UBL) || ENABLED(M100_FREE_MEMORY_WATCHER)
+#if ENABLED(AUTO_BED_LEVELING_UBL) || ENABLED(M100_FREE_MEMORY_WATCHER) || ENABLED(DEBUG_GCODE_PARSER)
 
 //
 // Utility functions to create and print hex strings as nybble, byte, and word.
@@ -43,5 +44,5 @@ void print_hex_byte(const uint8_t b);
 void print_hex_word(const uint16_t w);
 void print_hex_address(const void * const w);
 
-#endif // AUTO_BED_LEVELING_UBL || M100_FREE_MEMORY_WATCHER
+#endif // AUTO_BED_LEVELING_UBL || M100_FREE_MEMORY_WATCHER || DEBUG_GCODE_PARSER
 #endif // HEX_PRINT_ROUTINES_H

Marlin/macros.h

@@ -124,7 +124,9 @@
 
 #define WITHIN(V,L,H) ((V) >= (L) && (V) <= (H))
 #define NUMERIC(a) WITHIN(a, '0', '9')
-#define NUMERIC_SIGNED(a) (NUMERIC(a) || (a) == '-')
+#define DECIMAL(a) (NUMERIC(a) || a == '.')
+#define NUMERIC_SIGNED(a) (NUMERIC(a) || (a) == '-' || (a) == '+')
+#define DECIMAL_SIGNED(a) (DECIMAL(a) || (a) == '-' || (a) == '+')
 #define COUNT(a) (sizeof(a)/sizeof(*a))
 #define ZERO(a) memset(a,0,sizeof(a))
 #define COPY(a,b) memcpy(a,b,min(sizeof(a),sizeof(b)))

Marlin/planner.cpp

@@ -64,6 +64,7 @@
 #include "ultralcd.h"
 #include "language.h"
 #include "ubl.h"
+#include "gcode.h"
 
 #include "Marlin.h"
 
@@ -1549,10 +1550,10 @@ void Planner::refresh_positioning() {
 #if ENABLED(AUTOTEMP)
 
   void Planner::autotemp_M104_M109() {
-    autotemp_enabled = code_seen('F');
+    autotemp_enabled = parser.seen('F');
-    if (autotemp_enabled) autotemp_factor = code_value_temp_diff();
+    if (autotemp_enabled) autotemp_factor = parser.value_celsius_diff();
-    if (code_seen('S')) autotemp_min = code_value_temp_abs();
+    if (parser.seen('S')) autotemp_min = parser.value_celsius();
-    if (code_seen('B')) autotemp_max = code_value_temp_abs();
+    if (parser.seen('B')) autotemp_max = parser.value_celsius();
   }
 
 #endif

Marlin/ubl_G29.cpp

@@ -30,6 +30,7 @@
   #include "configuration_store.h"
   #include "ultralcd.h"
   #include "stepper.h"
+  #include "gcode.h"
 
   #include <math.h>
   #include "least_squares_fit.h"
@@ -47,10 +48,6 @@
   float lcd_z_offset_edit();
   extern float meshedit_done;
   extern long babysteps_done;
-  extern float code_value_float();
-  extern uint8_t code_value_byte();
-  extern bool code_value_bool();
-  extern bool code_has_value();
   extern float probe_pt(const float &x, const float &y, bool, int);
   extern bool set_probe_deployed(bool);
 
@@ -322,26 +319,20 @@
       return;
     }
 
-    // Check for commands that require the printer to be homed.
+    // Check for commands that require the printer to be homed
     if (axis_unhomed_error()) {
-      if (code_seen('J'))
+      const int8_t p_val = parser.seen('P') && parser.has_value() ? parser.value_int() : -1;
+      if (p_val == 1 || p_val == 2 || p_val == 4 || parser.seen('J'))
         home_all_axes();
-      else if (code_seen('P')) {
-        if (code_has_value()) {
-          const int p_val = code_value_int();
-          if (p_val == 1 || p_val == 2 || p_val == 4)
-            home_all_axes();
-        }
-      }
     }
 
     if (g29_parameter_parsing()) return; // abort if parsing the simple parameters causes a problem,
 
-    // Invalidate Mesh Points. This command is a little bit asymetrical because
+    // Invalidate Mesh Points. This command is a little bit asymmetrical because
     // it directly specifies the repetition count and does not use the 'R' parameter.
-    if (code_seen('I')) {
+    if (parser.seen('I')) {
       uint8_t cnt = 0;
-      g29_repetition_cnt = code_has_value() ? code_value_int() : 1;
+      g29_repetition_cnt = parser.has_value() ? parser.value_int() : 1;
       while (g29_repetition_cnt--) {
         if (cnt > 20) { cnt = 0; idle(); }
         const mesh_index_pair location = find_closest_mesh_point_of_type(REAL, g29_x_pos, g29_y_pos, USE_NOZZLE_AS_REFERENCE, NULL, false);
@@ -355,10 +346,10 @@
       SERIAL_PROTOCOLLNPGM("Locations invalidated.\n");
     }
 
-    if (code_seen('Q')) {
+    if (parser.seen('Q')) {
-      const int test_pattern = code_has_value() ? code_value_int() : -99;
+      const int test_pattern = parser.has_value() ? parser.value_int() : -99;
       if (!WITHIN(test_pattern, -1, 2)) {
-        SERIAL_PROTOCOLLNPGM("Invalid test_pattern value. (0-2)\n");
+        SERIAL_PROTOCOLLNPGM("Invalid test_pattern value. (-1 to 2)\n");
         return;
       }
       SERIAL_PROTOCOLLNPGM("Loading test_pattern values.\n");
@@ -385,15 +376,15 @@
           // Allow the user to specify the height because 10mm is a little extreme in some cases.
           for (uint8_t x = (GRID_MAX_POINTS_X) / 3; x < 2 * (GRID_MAX_POINTS_X) / 3; x++)   // Create a rectangular raised area in
             for (uint8_t y = (GRID_MAX_POINTS_Y) / 3; y < 2 * (GRID_MAX_POINTS_Y) / 3; y++) // the center of the bed
-              z_values[x][y] += code_seen('C') ? g29_constant : 9.99;
+              z_values[x][y] += parser.seen('C') ? g29_constant : 9.99;
           break;
       }
     }
 
-    if (code_seen('J')) {
+    if (parser.seen('J')) {
       if (g29_grid_size) {  // if not 0 it is a normal n x n grid being probed
         save_ubl_active_state_and_disable();
-        tilt_mesh_based_on_probed_grid(code_seen('T'));
+        tilt_mesh_based_on_probed_grid(parser.seen('T'));
         restore_ubl_active_state_and_leave();
       }
       else { // grid_size == 0 : A 3-Point leveling has been requested
@@ -425,7 +416,7 @@
       }
     }
 
-    if (code_seen('P')) {
+    if (parser.seen('P')) {
       if (WITHIN(g29_phase_value, 0, 1) && state.storage_slot == -1) {
         state.storage_slot = 0;
         SERIAL_PROTOCOLLNPGM("Default storage slot 0 selected.");
@@ -444,7 +435,7 @@
           //
           // Invalidate Entire Mesh and Automatically Probe Mesh in areas that can be reached by the probe
           //
-          if (!code_seen('C')) {
+          if (!parser.seen('C')) {
             invalidate();
             SERIAL_PROTOCOLLNPGM("Mesh invalidated. Probing mesh.");
           }
@@ -455,7 +446,7 @@
             SERIAL_PROTOCOLLNPGM(").\n");
           }
           probe_entire_mesh(g29_x_pos + X_PROBE_OFFSET_FROM_EXTRUDER, g29_y_pos + Y_PROBE_OFFSET_FROM_EXTRUDER,
-                            code_seen('T'), code_seen('E'), code_seen('U'));
+                            parser.seen('T'), parser.seen('E'), parser.seen('U'));
           break;
 
         case 2: {
@@ -481,30 +472,29 @@
             #endif
           }
 
-          if (code_seen('C')) {
+          if (parser.seen('C')) {
             g29_x_pos = current_position[X_AXIS];
             g29_y_pos = current_position[Y_AXIS];
           }
 
           float height = Z_CLEARANCE_BETWEEN_PROBES;
 
-          if (code_seen('B')) {
+          if (parser.seen('B')) {
-            g29_card_thickness = code_has_value() ? code_value_float() : measure_business_card_thickness(height);
+            g29_card_thickness = parser.has_value() ? parser.value_float() : measure_business_card_thickness(height);
-
             if (fabs(g29_card_thickness) > 1.5) {
               SERIAL_PROTOCOLLNPGM("?Error in Business Card measurement.");
               return;
             }
           }
 
-          if (code_seen('H') && code_has_value()) height = code_value_float();
+          if (parser.seen('H') && parser.has_value()) height = parser.value_float();
 
           if (!position_is_reachable_xy(g29_x_pos, g29_y_pos)) {
-            SERIAL_PROTOCOLLNPGM("(X,Y) outside printable radius.");
+            SERIAL_PROTOCOLLNPGM("XY outside printable radius.");
             return;
           }
 
-          manually_probe_remaining_mesh(g29_x_pos, g29_y_pos, height, g29_card_thickness, code_seen('T'));
+          manually_probe_remaining_mesh(g29_x_pos, g29_y_pos, height, g29_card_thickness, parser.seen('T'));
           SERIAL_PROTOCOLLNPGM("G29 P2 finished.");
         } break;
 
@@ -531,7 +521,7 @@
               }
             }
           } else {
-            const float cvf = code_value_float();
+            const float cvf = parser.value_float();
             switch((int)truncf(cvf * 10.0) - 30) {   // 3.1 -> 1
               #if ENABLED(UBL_G29_P31)
                 case 1: {
@@ -561,9 +551,7 @@
           //
           // Fine Tune (i.e., Edit) the Mesh
           //
-
+          fine_tune_mesh(g29_x_pos, g29_y_pos, parser.seen('T'));
-          fine_tune_mesh(g29_x_pos, g29_y_pos, code_seen('T'));
-
           break;
 
         case 5: find_mean_mesh_height(); break;
@@ -576,22 +564,22 @@
     // Much of the 'What?' command can be eliminated. But until we are fully debugged, it is
     // good to have the extra information. Soon... we prune this to just a few items
     //
-    if (code_seen('W')) g29_what_command();
+    if (parser.seen('W')) g29_what_command();
 
     //
     // When we are fully debugged, this may go away. But there are some valid
     // use cases for the users. So we can wait and see what to do with it.
     //
 
-    if (code_seen('K')) // Kompare Current Mesh Data to Specified Stored Mesh
+    if (parser.seen('K')) // Kompare Current Mesh Data to Specified Stored Mesh
       g29_compare_current_mesh_to_stored_mesh();
 
     //
     // Load a Mesh from the EEPROM
     //
 
-    if (code_seen('L')) {     // Load Current Mesh Data
+    if (parser.seen('L')) {     // Load Current Mesh Data
-      g29_storage_slot = code_has_value() ? code_value_int() : state.storage_slot;
+      g29_storage_slot = parser.has_value() ? parser.value_int() : state.storage_slot;
 
       int16_t a = settings.calc_num_meshes();
 
@@ -616,8 +604,8 @@
     // Store a Mesh in the EEPROM
     //
 
-    if (code_seen('S')) {     // Store (or Save) Current Mesh Data
+    if (parser.seen('S')) {     // Store (or Save) Current Mesh Data
-      g29_storage_slot = code_has_value() ? code_value_int() : state.storage_slot;
+      g29_storage_slot = parser.has_value() ? parser.value_int() : state.storage_slot;
 
       if (g29_storage_slot == -1) {                     // Special case, we are going to 'Export' the mesh to the
         SERIAL_ECHOLNPGM("G29 I 999");              // host in a form it can be reconstructed on a different machine
@@ -654,15 +642,17 @@
       SERIAL_PROTOCOLLNPGM("Done.");
     }
 
-    if (code_seen('T'))
+    if (parser.seen('T'))
-      display_map(code_has_value() ? code_value_int() : 0);
+      display_map(parser.has_value() ? parser.value_int() : 0);
 
-    /*
+    /**
      * This code may not be needed...  Prepare for its removal...
      *
-    if (code_seen('Z')) {
+     */
-      if (code_has_value())
+    #if 0
-        state.z_offset = code_value_float();   // do the simple case. Just lock in the specified value
+    if (parser.seen('Z')) {
+      if (parser.has_value())
+        state.z_offset = parser.value_float();   // do the simple case. Just lock in the specified value
       else {
         save_ubl_active_state_and_disable();
         //float measured_z = probe_pt(g29_x_pos + X_PROBE_OFFSET_FROM_EXTRUDER, g29_y_pos + Y_PROBE_OFFSET_FROM_EXTRUDER, ProbeDeployAndStow, g29_verbose_level);
@@ -712,7 +702,7 @@
         restore_ubl_active_state_and_leave();
       }
     }
-    */
+    #endif
 
     LEAVE:
 
@@ -1015,10 +1005,7 @@
 
       if (do_ubl_mesh_map) display_map(g29_map_type);  // show user where we're probing
 
-      if (code_seen('B'))
+      serialprintPGM(parser.seen('B') ? PSTR("Place shim & measure") : PSTR("Measure")); // TODO: Make translatable strings
-        LCD_MESSAGEPGM("Place shim & measure"); // TODO: Make translatable string
-      else
-        LCD_MESSAGEPGM("Measure"); // TODO: Make translatable string
 
       while (ubl_lcd_clicked()) delay(50);             // wait for user to release encoder wheel
       delay(50);                                       // debounce
@@ -1073,13 +1060,13 @@
     g29_constant = 0.0;
     g29_repetition_cnt = 0;
 
-    g29_x_flag = code_seen('X') && code_has_value();
+    g29_x_flag = parser.seen('X') && parser.has_value();
-    g29_x_pos = g29_x_flag ? code_value_float() : current_position[X_AXIS];
+    g29_x_pos = g29_x_flag ? parser.value_float() : current_position[X_AXIS];
-    g29_y_flag = code_seen('Y') && code_has_value();
+    g29_y_flag = parser.seen('Y') && parser.has_value();
-    g29_y_pos = g29_y_flag ? code_value_float() : current_position[Y_AXIS];
+    g29_y_pos = g29_y_flag ? parser.value_float() : current_position[Y_AXIS];
 
-    if (code_seen('R')) {
+    if (parser.seen('R')) {
-      g29_repetition_cnt = code_has_value() ? code_value_int() : GRID_MAX_POINTS;
+      g29_repetition_cnt = parser.has_value() ? parser.value_int() : GRID_MAX_POINTS;
       NOMORE(g29_repetition_cnt, GRID_MAX_POINTS);
       if (g29_repetition_cnt < 1) {
         SERIAL_PROTOCOLLNPGM("?(R)epetition count invalid (1+).\n");
@@ -1087,22 +1074,22 @@
       }
     }
 
-    g29_verbose_level = code_seen('V') ? code_value_int() : 0;
+    g29_verbose_level = parser.seen('V') ? parser.value_int() : 0;
     if (!WITHIN(g29_verbose_level, 0, 4)) {
       SERIAL_PROTOCOLLNPGM("?(V)erbose level is implausible (0-4).\n");
       err_flag = true;
     }
 
-    if (code_seen('P')) {
+    if (parser.seen('P')) {
-      g29_phase_value = code_value_int();
+      g29_phase_value = parser.value_int();
       if (!WITHIN(g29_phase_value, 0, 6)) {
         SERIAL_PROTOCOLLNPGM("?(P)hase value invalid (0-6).\n");
         err_flag = true;
       }
     }
 
-    if (code_seen('J')) {
+    if (parser.seen('J')) {
-      g29_grid_size = code_has_value() ? code_value_int() : 0;
+      g29_grid_size = parser.has_value() ? parser.value_int() : 0;
       if (g29_grid_size && !WITHIN(g29_grid_size, 2, 9)) {
         SERIAL_PROTOCOLLNPGM("?Invalid grid size (J) specified (2-9).\n");
         err_flag = true;
@@ -1125,27 +1112,32 @@
 
     if (err_flag) return UBL_ERR;
 
-    // Activate or deactivate UBL
+    /**
-    if (code_seen('A')) {
+     * Activate or deactivate UBL
-      if (code_seen('D')) {
+     * Note: UBL's G29 restores the state set here when done.
+     *       Leveling is being enabled here with old data, possibly
+     *       none. Error handling should disable for safety...
+     */
+    if (parser.seen('A')) {
+      if (parser.seen('D')) {
         SERIAL_PROTOCOLLNPGM("?Can't activate and deactivate at the same time.\n");
         return UBL_ERR;
       }
       state.active = true;
       report_state();
     }
-    else if (code_seen('D')) {
+    else if (parser.seen('D')) {
       state.active = false;
       report_state();
     }
 
     // Set global 'C' flag and its value
-    if ((g29_c_flag = code_seen('C')))
+    if ((g29_c_flag = parser.seen('C')))
-      g29_constant = code_value_float();
+      g29_constant = parser.value_float();
 
     #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
-      if (code_seen('F') && code_has_value()) {
+      if (parser.seen('F') && parser.has_value()) {
-        const float fh = code_value_float();
+        const float fh = parser.value_float();
         if (!WITHIN(fh, 0.0, 100.0)) {
           SERIAL_PROTOCOLLNPGM("?(F)ade height for Bed Level Correction not plausible.\n");
           return UBL_ERR;
@@ -1154,7 +1146,7 @@
       }
     #endif
 
-    g29_map_type = code_seen('T') && code_has_value() ? code_value_int() : 0;
+    g29_map_type = parser.seen('T') && parser.has_value() ? parser.value_int() : 0;
     if (!WITHIN(g29_map_type, 0, 1)) {
       SERIAL_PROTOCOLLNPGM("Invalid map type.\n");
       return UBL_ERR;
@@ -1319,13 +1311,13 @@
       return;
     }
 
-    if (!code_has_value()) {
+    if (!parser.has_value()) {
       SERIAL_PROTOCOLLNPGM("?Storage slot # required.");
       SERIAL_PROTOCOLLNPAIR("?Use 0 to ", a - 1);
       return;
     }
 
-    g29_storage_slot = code_value_int();
+    g29_storage_slot = parser.value_int();
 
     if (!WITHIN(g29_storage_slot, 0, a - 1)) {
       SERIAL_PROTOCOLLNPGM("?Invalid storage slot.");
@@ -1416,7 +1408,7 @@
   }
 
   void unified_bed_leveling::fine_tune_mesh(const float &lx, const float &ly, const bool do_ubl_mesh_map) {
-    if (!code_seen('R'))    // fine_tune_mesh() is special. If no repetition count flag is specified
+    if (!parser.seen('R'))    // fine_tune_mesh() is special. If no repetition count flag is specified
       g29_repetition_cnt = 1;   // do exactly one mesh location. Otherwise use what the parser decided.
 
     mesh_index_pair location;
@@ -1587,7 +1579,7 @@
       const float x = float(x_min) + ix * dx;
       for (int8_t iy = 0; iy < g29_grid_size; iy++) {
         const float y = float(y_min) + dy * (zig_zag ? g29_grid_size - 1 - iy : iy);
-        float measured_z = probe_pt(LOGICAL_X_POSITION(x), LOGICAL_Y_POSITION(y), code_seen('E'), g29_verbose_level); // TODO: Needs error handling
+        float measured_z = probe_pt(LOGICAL_X_POSITION(x), LOGICAL_Y_POSITION(y), parser.seen('E'), g29_verbose_level); // TODO: Needs error handling
         #if ENABLED(DEBUG_LEVELING_FEATURE)
           if (DEBUGGING(LEVELING)) {
             SERIAL_CHAR('(');