G43.1/G49 tool length offset installed. Minor bug fix.

- Minor bug fix that caused G92.1 not to work. The mantissa of G92.1
was not computed correctly due to floating point round-off errors and
the use of trunc(). Fixed it by changing the computation with round().

- Installed tool length offsets with G43.1 and G49! True tool length
offsets via G43 are not supported, because these require us to store
tool data that we don’t have space for. But we’ve come up with a good
solution for users that need this. Instead we are strictly using the
dynamic version G43.1 via linuxcnc.org. Visit their website for more
details on the command.

- G43.1 operates by requiring an axis word and value to offset the
configured tool length axis, which can be configured for any axis
(default Z-axis) in config.h. For example, ```G43.1 Z0.5``` will offset
the work coordinates from Z0.0 to Z-0.5.

- G49 will cancel the last tool length offset value and reset it to
zero.

- Tweaked the ‘$#’ parameters report. `Probe` is now `PRB` and a new
value `TLO` states the tool length offset value.

config.h

@@ -85,9 +85,9 @@
 #define N_STARTUP_LINE 2 // Integer (1-3)
 
 // Number of floating decimal points printed by Grbl for certain value types. These settings are 
-// determined by realistic and commonly values observed in CNC machines. For example, position
+// determined by realistic and commonly observed values in CNC machines. For example, position
-// values cannot be less than 0.001mm or 0.0001in, because machines are never more precise than 
+// values cannot be less than 0.001mm or 0.0001in, because machines can not be physically more
-// this. So, there is likely no need to change these, but you can if you need to here.
+// precise this. So, there is likely no need to change these, but you can if you need to here.
 // NOTE: Must be an integer value from 0 to ~4. More than 4 may exhibit round-off errors.
 #define N_DECIMAL_COORDVALUE_INCH 4 // Coordinate or position value in inches
 #define N_DECIMAL_COORDVALUE_MM   3 // Coordinate or position value in mm
@@ -128,6 +128,11 @@
 // step smoothing. See stepper.c for more details on the AMASS system works.
 #define ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING  // Default enabled. Comment to disable.
 
+// Sets which axis the tool length offset is applied. Assumes the spindle is always parallel with 
+// the selected axis with the tool oriented toward the negative direction. In other words, a positive
+// tool length offset value is subtracted from the current location.
+#define TOOL_LENGTH_OFFSET_AXIS Z_AXIS // Default z-axis. Valid values are X_AXIS, Y_AXIS, or Z_AXIS.
+
 // Enables variable spindle output voltage for different RPM values. On the Arduino Uno, the spindle
 // enable pin will output 5V for maximum RPM with 256 intermediate levels and 0V when disabled.
 // NOTE: IMPORTANT for Arduino Unos! When enabled, the Z-limit pin D11 and spindle enable pin D12 switch!

gcode.c

@@ -119,6 +119,7 @@ uint8_t gc_execute_line(char *line)
   uint8_t int_value = 0;
   uint8_t mantissa = 0; // NOTE: For mantissa values > 255, variable type must be changed to uint16_t.
 
+
   while (line[char_counter] != 0) { // Loop until no more g-code words in line.
     
     // Import the next g-code word, expecting a letter followed by a value. Otherwise, error out.
@@ -135,7 +136,8 @@ uint8_t gc_execute_line(char *line)
     // we would simply need to change the mantissa to int16, but this add compiled flash space.
     // Maybe update this later. 
     int_value = trunc(value);
-    mantissa = trunc(100*(value - int_value)); // Compute mantissa for Gxx.x commands
+    mantissa =  round(100*(value - int_value)); // Compute mantissa for Gxx.x commands.
+        // NOTE: Rounding must be used to catch small floating point errors. 
 
     // Check if the g-code word is supported or errors due to modal group violations or has
     // been repeated in the g-code block. If ok, update the command or record its value.
@@ -239,16 +241,18 @@ uint8_t gc_execute_line(char *line)
             break;
           case 43: case 49:
             word_bit = MODAL_GROUP_G8;
-            if (int_value == 49) { 
+            // NOTE: The NIST g-code standard vaguely states that when a tool length offset is changed,
+            // there cannot be any axis motion or coordinate offsets updated. Meaning G43, G43.1, and G49
+            // all are explicit axis commands, regardless if they require axis words or not. 
+            if (axis_command) { FAIL(STATUS_GCODE_AXIS_COMMAND_CONFLICT); } // [Axis word/command conflict] }
+            axis_command = AXIS_COMMAND_TOOL_LENGTH_OFFSET;
+            if (int_value == 49) { // G49
               gc_block.modal.tool_length = TOOL_LENGTH_OFFSET_CANCEL; 
-            // Else command is G43. Only G43.1 is supported. G43 is NOT.
+            } else if (mantissa == 10) { // G43.1
-            } else if (mantissa == 1) { 
-              // * G43.1 is requires an axis word to operate and cannot exist with other axis
-              // commands in the same line. However, it's not explicitly defined this way.
-              if (axis_command) { FAIL(STATUS_GCODE_AXIS_COMMAND_CONFLICT); } // [Axis word/command conflict] }
-              axis_command = AXIS_COMMAND_TOOL_LENGTH_OFFSET;
               gc_block.modal.tool_length = TOOL_LENGTH_OFFSET_ENABLE_DYNAMIC;
             } else { FAIL(STATUS_GCODE_UNSUPPORTED_COMMAND); } // [Unsupported G43.x command]
+            mantissa = 0; // Set to zero to indicate valid non-integer G command.
+            break;
           case 54: case 55: case 56: case 57: case 58: case 59: 
             // NOTE: G59.x are not supported. (But their int_values would be 60, 61, and 62.)
             word_bit = MODAL_GROUP_G12;
@@ -483,7 +487,15 @@ uint8_t gc_execute_line(char *line)
   // [13. Cutter radius compensation ]: NOT SUPPORTED. Error, if G53 is active.
   
   // [14. Cutter length compensation ]: G43 NOT SUPPORTED, but G43.1 and G49 are. 
-  // G43.1 Error with motion command in same line. (Already done by axis command checks in parser.)
+  // [G43.1 Errors]: Motion command in same line. 
+  //   NOTE: Although not explicitly stated so, G43.1 should be applied to only one valid 
+  //   axis that is configured (in config.h). There should be an error if the configured axis
+  //   is absent or if any of the other axis words are present.
+  if (axis_command == AXIS_COMMAND_TOOL_LENGTH_OFFSET ) { // Indicates called in block.
+    if (gc_block.modal.tool_length == TOOL_LENGTH_OFFSET_ENABLE_DYNAMIC) {
+      if (axis_words ^ (1<<TOOL_LENGTH_OFFSET_AXIS)) { FAIL(STATUS_GCODE_G43_DYNAMIC_AXIS_ERROR); }
+    }
+  }
   
   // [15. Coordinate system selection ]: *N/A. Error, if cutter radius comp is active.
   // TODO: An EEPROM read of the coordinate data may require a buffer sync when the cycle
@@ -531,10 +543,12 @@ uint8_t gc_execute_line(char *line)
         // Update axes defined only in block. Always in machine coordinates. Can change non-active system.
         if (bit_istrue(axis_words,bit(idx)) ) {
           if (gc_block.values.l == 20) {
-            // NOTE: Undefined if G92 offsets apply to this calculation. Omitted.
+            // L20: Update coordinate system axis at current position (with modifiers) with programmed value
-            parameter_data[idx] = gc_state.position[idx]-gc_block.values.xyz[idx]; // L20: Update axis current position to target
+            parameter_data[idx] = gc_state.position[idx]-gc_state.coord_offset[idx]-gc_block.values.xyz[idx];
+            if (idx == TOOL_LENGTH_OFFSET_AXIS) { parameter_data[idx] -= gc_state.tool_length_offset; }
           } else {
-            parameter_data[idx] = gc_block.values.xyz[idx]; // L2: Update coordinate system axis
+            // L2: Update coordinate system axis to programmed value.
+            parameter_data[idx] = gc_block.values.xyz[idx]; 
           }
         }
       }
@@ -548,6 +562,7 @@ uint8_t gc_execute_line(char *line)
       for (idx=0; idx<N_AXIS; idx++) { // Axes indices are consistent, so loop may be used.
         if (bit_istrue(axis_words,bit(idx)) ) {
           gc_block.values.xyz[idx] = gc_state.position[idx]-coordinate_data[idx]-gc_block.values.xyz[idx];
+          if (idx == TOOL_LENGTH_OFFSET_AXIS) { gc_block.values.xyz[idx] -= gc_state.tool_length_offset; }
         } else {
           gc_block.values.xyz[idx] = gc_state.coord_offset[idx];
         }
@@ -555,30 +570,33 @@ uint8_t gc_execute_line(char *line)
       break;
       
     default:
-    
+
       // At this point, the rest of the explicit axis commands treat the axis values as the traditional
       // target position with the coordinate system offsets, G92 offsets, absolute override, and distance
       // modes applied. This includes the motion mode commands. We can now pre-compute the target position.
       // NOTE: Tool offsets may be appended to these conversions when/if this feature is added.
-      if (axis_words) {
+      if (axis_command != AXIS_COMMAND_TOOL_LENGTH_OFFSET ) // TLO block any axis command.
-        for (idx=0; idx<N_AXIS; idx++) { // Axes indices are consistent, so loop may be used to save flash space.
+        if (axis_words) {
-          if ( bit_isfalse(axis_words,bit(idx)) ) {
+          for (idx=0; idx<N_AXIS; idx++) { // Axes indices are consistent, so loop may be used to save flash space.
-            gc_block.values.xyz[idx] = gc_state.position[idx]; // No axis word in block. Keep same axis position.
+            if ( bit_isfalse(axis_words,bit(idx)) ) {
-          } else {
+              gc_block.values.xyz[idx] = gc_state.position[idx]; // No axis word in block. Keep same axis position.
-            // Update specified value according to distance mode or ignore if absolute override is active.
+            } else {
-            // NOTE: G53 is never active with G28/30 since they are in the same modal group.
+              // Update specified value according to distance mode or ignore if absolute override is active.
-            if (gc_block.non_modal_command != NON_MODAL_ABSOLUTE_OVERRIDE) {
+              // NOTE: G53 is never active with G28/30 since they are in the same modal group.
-              // Apply coordinate offsets based on distance mode.
+              if (gc_block.non_modal_command != NON_MODAL_ABSOLUTE_OVERRIDE) {
-              if (gc_block.modal.distance == DISTANCE_MODE_ABSOLUTE) {
+                // Apply coordinate offsets based on distance mode.
-                gc_block.values.xyz[idx] += coordinate_data[idx] + gc_state.coord_offset[idx];
+                if (gc_block.modal.distance == DISTANCE_MODE_ABSOLUTE) {
-              } else {  // Incremental mode
+                  gc_block.values.xyz[idx] += coordinate_data[idx] + gc_state.coord_offset[idx];
-                gc_block.values.xyz[idx] += gc_state.position[idx];
+                  if (idx == TOOL_LENGTH_OFFSET_AXIS) { gc_block.values.xyz[idx] += gc_state.tool_length_offset; }
+                } else {  // Incremental mode
+                  gc_block.values.xyz[idx] += gc_state.position[idx];
+                }
               }
             }
           }
         }
       }
-    
+          
       // Check remaining non-modal commands for errors.
       switch (gc_block.non_modal_command) {        
         case NON_MODAL_GO_HOME_0: 
@@ -843,7 +861,18 @@ uint8_t gc_execute_line(char *line)
 
   // [13. Cutter radius compensation ]: NOT SUPPORTED
 
-  // [14. Cutter length compensation ]: NOT SUPPORTED
+  // [14. Cutter length compensation ]: G43.1 and G49 supported. G43 NOT SUPPORTED.
+  // NOTE: If G43 were supported, its operation wouldn't be any different from G43.1 in terms
+  // of execution. The error-checking step would simply load the offset value into the correct
+  // axis of the block XYZ value array. 
+  if (axis_command == AXIS_COMMAND_TOOL_LENGTH_OFFSET ) { // Indicates a change.
+    gc_state.modal.tool_length = gc_block.modal.tool_length;
+    if (gc_state.modal.tool_length == TOOL_LENGTH_OFFSET_ENABLE_DYNAMIC) { // G43.1
+      gc_state.tool_length_offset = gc_block.values.xyz[TOOL_LENGTH_OFFSET_AXIS];
+    } else { // G49
+      gc_state.tool_length_offset = 0.0;
+    }
+  }
   
   // [15. Coordinate system selection ]:
   if (gc_state.modal.coord_select != gc_block.modal.coord_select) {

report.c

@@ -204,7 +204,7 @@ void report_probe_parameters()
   float print_position[N_AXIS];
  
   // Report in terms of machine position.
-  printPgmString(PSTR("[Probe:")); 
+  printPgmString(PSTR("[PRB:")); 
   for (i=0; i< N_AXIS; i++) {
     print_position[i] = sys.probe_position[i]/settings.steps_per_mm[i];
     printFloat_CoordValue(print_position[i]);
@@ -243,6 +243,9 @@ void report_ngc_parameters()
     if (i < (N_AXIS-1)) { printPgmString(PSTR(",")); }
     else { printPgmString(PSTR("]\r\n")); }
   } 
+  printPgmString(PSTR("[TLO:")); // Print tool length offset value
+  printFloat_CoordValue(gc_state.tool_length_offset);
+  printPgmString(PSTR("]\r\n"));
   report_probe_parameters(); // Print probe parameters. Not persistent in memory.
 }
 
@@ -362,6 +365,7 @@ void report_realtime_status()
   printPgmString(PSTR("WPos:")); 
   for (i=0; i< N_AXIS; i++) {
     print_position[i] -= gc_state.coord_system[i]+gc_state.coord_offset[i];
+    if (i == TOOL_LENGTH_OFFSET_AXIS) { print_position[i] -= gc_state.tool_length_offset; }    
     printFloat_CoordValue(print_position[i]);
     if (i < (N_AXIS-1)) { printPgmString(PSTR(",")); }
   }

report.h

@@ -52,6 +52,7 @@
 #define STATUS_GCODE_NO_OFFSETS_IN_PLANE 35
 #define STATUS_GCODE_PROBE_TRIGGERED 36
 #define STATUS_GCODE_UNUSED_WORDS 37
+#define STATUS_GCODE_G43_DYNAMIC_AXIS_ERROR 38
 
 // Define Grbl alarm codes. Less than zero to distinguish alarm error from status error.
 #define ALARM_LIMIT_ERROR -1