From 92ba726f7f6b7552891f10ce1a2eeb21a1352736 Mon Sep 17 00:00:00 2001
From: Mitch Bradley <wmb@firmworks.com>
Date: Sat, 24 Jul 2021 15:44:57 -1000
Subject: [PATCH] Adjust homing mask according to extant motors

---
 Grbl_Esp32/src/Limits.cpp       | 7 ++-----
 Grbl_Esp32/src/Machine/Axes.cpp | 1 +
 Grbl_Esp32/src/Machine/Axes.h   | 1 +
 Grbl_Esp32/src/Machine/Gang.cpp | 3 +++
 4 files changed, 7 insertions(+), 5 deletions(-)

diff --git a/Grbl_Esp32/src/Limits.cpp b/Grbl_Esp32/src/Limits.cpp
index 27f28633..0b63a0db 100644
--- a/Grbl_Esp32/src/Limits.cpp
+++ b/Grbl_Esp32/src/Limits.cpp
@@ -243,8 +243,7 @@ static void limits_go_home(uint8_t cycle_mask, uint32_t n_locate_cycles) {
 
         // XXX we need to include gang1 in the remaining mask
         // The following might fail if only one gang has limit switches. Anaylze me.
-        uint32_t remainingMotors = cycle_mask | (cycle_mask << 16);
-        //uint32_t remainingMotors = cycle_mask;  // this works!!! but not for squaring
+        uint32_t remainingMotors = (cycle_mask | (cycle_mask << 16)) & Machine::Axes::motorMask;
 
         uint32_t old = 0;
 
@@ -252,7 +251,6 @@ static void limits_go_home(uint8_t cycle_mask, uint32_t n_locate_cycles) {
             if (approach) {
                 // Check limit state. Lock out cycle axes when they change.
                 // XXX do we check only the switch in the direction of motion?
-                //uint32_t limitedAxes = limits_check(-1);
                 uint32_t limitedAxes = Machine::Axes::posLimitMask | Machine::Axes::negLimitMask;
 
                 if (old != remainingMotors) {
@@ -453,9 +451,8 @@ void limits_init() {
 // Return a mask of the switches that are engaged.
 AxisMask limits_check(AxisMask check_mask) {
     // Expand the bitmask to include both gangs
-
     bit_true(check_mask, check_mask << 16);
-    return (Machine::Axes::posLimitMask & check_mask) | (Machine::Axes::negLimitMask & check_mask);
+    return (Machine::Axes::posLimitMask | Machine::Axes::negLimitMask) & check_mask;
 }
 
 // Returns limit state as a bit-wise uint8 variable. Each bit indicates an axis limit, where
diff --git a/Grbl_Esp32/src/Machine/Axes.cpp b/Grbl_Esp32/src/Machine/Axes.cpp
index 3853a35f..d8fad4cb 100644
--- a/Grbl_Esp32/src/Machine/Axes.cpp
+++ b/Grbl_Esp32/src/Machine/Axes.cpp
@@ -12,6 +12,7 @@ namespace Machine {
     uint32_t Axes::negLimitMask = 0;
     uint32_t Axes::homingMask   = 0;
     uint32_t Axes::limitMask    = 0;
+    uint32_t Axes::motorMask    = 0;
 
     Axes::Axes() : _axis() {
         for (int i = 0; i < MAX_N_AXIS; ++i) {
diff --git a/Grbl_Esp32/src/Machine/Axes.h b/Grbl_Esp32/src/Machine/Axes.h
index d38b6d3c..86834dd6 100644
--- a/Grbl_Esp32/src/Machine/Axes.h
+++ b/Grbl_Esp32/src/Machine/Axes.h
@@ -43,6 +43,7 @@ namespace Machine {
         static uint32_t negLimitMask;
         static uint32_t homingMask;
         static uint32_t limitMask;
+        static uint32_t motorMask;
 
         inline char axisName(int index) { return index < MAX_N_AXIS ? _names[index] : '?'; }
 
diff --git a/Grbl_Esp32/src/Machine/Gang.cpp b/Grbl_Esp32/src/Machine/Gang.cpp
index b99b621a..8d23f7f2 100644
--- a/Grbl_Esp32/src/Machine/Gang.cpp
+++ b/Grbl_Esp32/src/Machine/Gang.cpp
@@ -35,6 +35,9 @@ namespace Machine {
     }
 
     void Gang::init() {
+        if (strcmp(_motor->name, "null_motor") != 0) {
+            bitnum_istrue(Axes::motorMask, _axis + 16 * i);
+        }
         _motor->init();
         if (_endstops) {
             _endstops->init();