From 0af52e0ba2b1c6e93b6b33cf735ca3cf28db8d9c Mon Sep 17 00:00:00 2001
From: Paciente8159 <joaoextreia@gmail.com>
Date: Fri, 23 Aug 2019 17:25:41 +0100
Subject: [PATCH] Implemented axis skew compensation

Implemented coordinate unskew operations to report the transformed machine position.
---
 grbl/system.c | 11 +++++++++++
 1 file changed, 11 insertions(+)

diff --git a/grbl/system.c b/grbl/system.c
index b9ece56..442ae14 100644
--- a/grbl/system.c
+++ b/grbl/system.c
@@ -302,6 +302,7 @@ float system_convert_axis_steps_to_mpos(int32_t *steps, uint8_t idx)
   #else
     pos = steps[idx]/settings.steps_per_mm[idx];
   #endif
+
   return(pos);
 }
 
@@ -312,6 +313,16 @@ void system_convert_array_steps_to_mpos(float *position, int32_t *steps)
   for (idx=0; idx<N_AXIS; idx++) {
     position[idx] = system_convert_axis_steps_to_mpos(steps, idx);
   }
+
+  //perform unskew of the coordinates
+  #ifdef ENABLE_SKEW_COMPENSATION
+    position[X_AXIS] += position[Y_AXIS] * settings.xy_skew_factor;
+    #ifdef ALLAXIS_SKEW_COMPENSATION
+      position[X_AXIS] += position[Z_AXIS] * settings.xz_skew_factor;
+      position[Y_AXIS] += position[Z_AXIS] * settings.yz_skew_factor;
+    #endif
+  #endif
+
   return;
 }