Added G38.6-9 to force probe units in mm

bdring · Aug 13, 2023 · 2e7b101 · 2e7b101
1 parent a5bfcb0
commit 2e7b101
Showing 1 changed file with 10 additions and 5 deletions.
diff --git a/FluidNC/src/GCode.cpp b/FluidNC/src/GCode.cpp
@@ -172,6 +172,7 @@ Error gc_execute_line(char* line) {
     bool syncLaser     = false;
     bool disableLaser  = false;
     bool laserIsMotion = false;
+    bool forceG21      = false;  // Used for G38.6-9
 
     auto    n_axis = config->_axes->_numberAxis;
     float   coord_data[MAX_N_AXIS];  // Used by WCO-related commands
@@ -320,6 +321,10 @@ Error gc_execute_line(char* line) {
                         probeExplicit = true;
 
                         axis_command = AxisCommand::MotionMode;
+                        if (mantissa >= 60) {
+                            forceG21 = true;
+                            mantissa -= 60;
+                        }
                         switch (mantissa) {
                             case 20:
                                 gc_block.modal.motion = Motion::ProbeToward;
@@ -794,7 +799,7 @@ Error gc_execute_line(char* line) {
         if (bitnum_is_false(value_words, GCodeWord::F)) {
             FAIL(Error::GcodeUndefinedFeedRate);
         }
-        if (gc_block.modal.units == Units::Inches) {
+        if (!forceG21 && gc_block.modal.units == Units::Inches) {
             gc_block.values.f *= MM_PER_INCH;
         }
     } else {
@@ -822,7 +827,7 @@ Error gc_execute_line(char* line) {
             // - In units per mm mode: If F word passed, ensure value is in mm/min, otherwise push last state value.
             if (gc_state.modal.feed_rate == FeedRate::UnitsPerMin) {  // Last state is also G94
                 if (bitnum_is_true(value_words, GCodeWord::F)) {
-                    if (gc_block.modal.units == Units::Inches) {
+                    if (!forceG21 && gc_block.modal.units == Units::Inches) {
                         gc_block.values.f *= MM_PER_INCH;
                     }
                 } else {
@@ -895,7 +900,7 @@ Error gc_execute_line(char* line) {
 
     // [12. Set length units ]: N/A
     // Pre-convert XYZ coordinate values to millimeters, if applicable.
-    if (gc_block.modal.units == Units::Inches) {
+    if (!forceG21 && gc_block.modal.units == Units::Inches) {
         for (size_t idx = 0; idx < n_axis; idx++) {  // Axes indices are consistent, so loop may be used.
             if ((idx < A_AXIS || idx > C_AXIS) && bitnum_is_true(axis_words, idx)) {
                 gc_block.values.xyz[idx] *= MM_PER_INCH;
@@ -1152,7 +1157,7 @@ Error gc_execute_line(char* line) {
                             FAIL(Error::GcodeInvalidTarget);  // [Invalid target]
                         }
                         // Convert radius value to proper units.
-                        if (gc_block.modal.units == Units::Inches) {
+                        if (!forceG21 && gc_block.modal.units == Units::Inches) {
                             gc_block.values.r *= MM_PER_INCH;
                         }
                         /*  We need to calculate the center of the circle that has the designated radius and passes
@@ -1246,7 +1251,7 @@ Error gc_execute_line(char* line) {
                         }
                         clear_bits(value_words, (bitnum_to_mask(GCodeWord::I) | bitnum_to_mask(GCodeWord::J) | bitnum_to_mask(GCodeWord::K)));
                         // Convert IJK values to proper units.
-                        if (gc_block.modal.units == Units::Inches) {
+                        if (!forceG21 && gc_block.modal.units == Units::Inches) {
                             for (size_t idx = 0; idx < n_axis; idx++) {  // Axes indices are consistent, so loop may be used to save flash space.
                                 if (ijk_words & bitnum_to_mask(idx)) {
                                     gc_block.values.ijk[idx] *= MM_PER_INCH;