From e6faee455b37d1148b0723224051661d201b3872 Mon Sep 17 00:00:00 2001
From: bdring <barton.dring@gmail.com>
Date: Sat, 19 Dec 2020 07:11:28 -0600
Subject: [PATCH] MPos now has forward kinematics

---
 Grbl_Esp32/Custom/CoreXY.cpp | 18 ++++++++++++------
 Grbl_Esp32/src/Report.cpp    |  3 ++-
 2 files changed, 14 insertions(+), 7 deletions(-)

diff --git a/Grbl_Esp32/Custom/CoreXY.cpp b/Grbl_Esp32/Custom/CoreXY.cpp
index b1c6fb87..7b195898 100644
--- a/Grbl_Esp32/Custom/CoreXY.cpp
+++ b/Grbl_Esp32/Custom/CoreXY.cpp
@@ -292,7 +292,7 @@ void inverse_kinematics(float* target, plan_line_data_t* pl_data, float* positio
 }
 
 // motors -> cartesian
-void forward_kinematics(float* position) {
+void forward_kinematics(float* position) {  // Note: the Position value is not used. sys_position is used
     float   calc_fwd[MAX_N_AXIS];
     float   wco[MAX_N_AXIS];
     float   print_position[N_AXIS];
@@ -301,13 +301,19 @@ void forward_kinematics(float* position) {
     memcpy(current_position, sys_position, sizeof(sys_position));
     system_convert_array_steps_to_mpos(print_position, current_position);
 
-    // determine the Work Coordinate Offsets for each axis
+    // If we want to see WPos, apply the offsets
+
     auto n_axis = number_axis->get();
     for (int axis = 0; axis < n_axis; axis++) {
-        // Apply work coordinate offsets and tool length offset to current position.
-        wco[axis] = gc_state.coord_system[axis] + gc_state.coord_offset[axis];
-        if (axis == TOOL_LENGTH_OFFSET_AXIS) {
-            wco[axis] += gc_state.tool_length_offset;
+        // determine the Work Coordinate Offsets for each axis
+        if (bit_isfalse(status_mask->get(), RtStatus::Position)) {
+            // Apply work coordinate offsets and tool length offset to current position.
+            wco[axis] = gc_state.coord_system[axis] + gc_state.coord_offset[axis];
+            if (axis == TOOL_LENGTH_OFFSET_AXIS) {
+                wco[axis] += gc_state.tool_length_offset;
+            }
+        } else {  // MPos
+            wco[axis] = 0.0;
         }
     }
 
diff --git a/Grbl_Esp32/src/Report.cpp b/Grbl_Esp32/src/Report.cpp
index 5ddca2ad..af6e9107 100644
--- a/Grbl_Esp32/src/Report.cpp
+++ b/Grbl_Esp32/src/Report.cpp
@@ -655,11 +655,12 @@ void report_realtime_status(uint8_t client) {
             }
         }
     }
+    // allow a custom machine to apply forward kinematics
+    forward_kinematics(print_position);  // a weak definition does nothing. Users can provide strong version
     // Report machine position
     if (bit_istrue(status_mask->get(), RtStatus::Position)) {
         strcat(status, "|MPos:");
     } else {
-        forward_kinematics(print_position); // a weak definition does nothing. Users can provide strong version
         strcat(status, "|WPos:");
     }
     report_util_axis_values(print_position, temp);