Fixes for remote endstops

1 files changed, 54 insertions, 53 deletions

diff --git a/src/Endstops/EndstopsManager.cpp b/src/Endstops/EndstopsManager.cpp
index 99d03aad..f0f43b79 100644
--- a/src/Endstops/EndstopsManager.cpp
+++ b/src/Endstops/EndstopsManager.cpp
@@ -196,6 +196,8 @@ GCodeResult EndstopsManager::HandleM574(GCodeBuffer& gb, const StringRef& reply)
 	{
 		reply.copy("Endstop configuration");
 		char sep = ':';
+		ReadLocker lock(endstopsLock);
+
 		for (size_t axis = 0; axis < reprap.GetGCodes().GetTotalAxes(); ++axis)
 		{
 			reply.catf("%c %c: ", sep, reprap.GetGCodes().GetAxisLetters()[axis]);
@@ -237,71 +239,70 @@ GCodeResult EndstopsManager::HandleM574(GCodeBuffer& gb, const StringRef& reply)
 			return GCodeResult::error;
 		}
 
+		WriteLocker lock(endstopsLock);
+
 		delete axisEndstops[lastAxisSeen];
 		axisEndstops[lastAxisSeen] = nullptr;
 		LocalSwitchEndstop * const sw = new LocalSwitchEndstop(lastAxisSeen, lastPosSeen);
-		GCodeResult rslt = sw->Configure(gb, reply, inputType);
+		const GCodeResult rslt = sw->Configure(gb, reply, inputType);
 		axisEndstops[lastAxisSeen] = sw;
-		if (rslt != GCodeResult::ok && rslt != GCodeResult::warning)
-		{
-			return rslt;
-		}
+		return rslt;
 	}
-	else
+
+	// No P parameter, so there may be multiple axes
+	for (size_t axis = 0; axis < reprap.GetGCodes().GetTotalAxes(); ++axis)
 	{
-		// No P parameter, so there may be multiple axes
-		for (size_t axis = 0; axis < reprap.GetGCodes().GetTotalAxes(); ++axis)
+		const char c = reprap.GetGCodes().GetAxisLetters()[axis];
+		if (gb.Seen(c))
 		{
-			const char c = reprap.GetGCodes().GetAxisLetters()[axis];
-			if (gb.Seen(c))
+			const EndStopPosition pos = (EndStopPosition)gb.GetUIValue();		// we range-checked this earlier
+			WriteLocker lock(endstopsLock);
+
+			if (pos == EndStopPosition::noEndStop)
+			{
+				delete axisEndstops[axis];
+				axisEndstops[axis] = nullptr;
+			}
+			else
 			{
-				const EndStopPosition pos = (EndStopPosition)gb.GetUIValue();		// we range-checked this earlier
-				if (pos == EndStopPosition::noEndStop)
+				switch (inputType)
 				{
+				case EndStopInputType::motorStallAny:
+					// Asking for stall detection endstop, so we can delete any existing endstop(s) and create new ones
 					delete axisEndstops[axis];
-					axisEndstops[axis] = nullptr;
-				}
-				else
-				{
-					switch (inputType)
+					axisEndstops[axis] = new StallDetectionEndstop(axis, pos, false);
+					break;
+
+				case EndStopInputType::motorStallIndividual:
+					// Asking for stall detection endstop, so we can delete any existing endstop(s) and create new ones
+					delete axisEndstops[axis];
+					axisEndstops[axis] = new StallDetectionEndstop(axis, pos, true);
+					break;
+
+				case EndStopInputType::zProbeAsEndstop:
+					// Asking for a ZProbe or stall detection endstop, so we can delete any existing endstop(s) and create new ones
+					delete axisEndstops[axis];
+					axisEndstops[axis] = new ZProbeEndstop(axis, pos);
+					break;
+
+				case EndStopInputType::activeHigh:
+				case EndStopInputType::activeLow:
+					if (   axisEndstops[axis] == nullptr
+						|| (axisEndstops[axis]->GetEndstopType() != EndStopInputType::activeHigh && axisEndstops[axis]->GetEndstopType() != EndStopInputType::activeLow)
+					   )
+					{
+						// Asking for a switch endstop but we don't already have one, so we don't know what pin number(s) it should use
+						reply.printf("Pin name needed for switch-type endstop on %c axis", c);
+						return GCodeResult::error;
+					}
+					else
 					{
-					case EndStopInputType::motorStallAny:
-						// Asking for stall detection endstop, so we can delete any existing endstop(s) and create new ones
-						delete axisEndstops[axis];
-						axisEndstops[axis] = new StallDetectionEndstop(axis, pos, false);
-						break;
-
-					case EndStopInputType::motorStallIndividual:
-						// Asking for stall detection endstop, so we can delete any existing endstop(s) and create new ones
-						delete axisEndstops[axis];
-						axisEndstops[axis] = new StallDetectionEndstop(axis, pos, true);
-						break;
-
-					case EndStopInputType::zProbeAsEndstop:
-						// Asking for a ZProbe or stall detection endstop, so we can delete any existing endstop(s) and create new ones
-						delete axisEndstops[axis];
-						axisEndstops[axis] = new ZProbeEndstop(axis, pos);
-						break;
-
-					case EndStopInputType::activeHigh:
-					case EndStopInputType::activeLow:
-						if (   axisEndstops[axis] == nullptr
-							|| (axisEndstops[axis]->GetEndstopType() != EndStopInputType::activeHigh && axisEndstops[axis]->GetEndstopType() != EndStopInputType::activeLow)
-						   )
-						{
-							// Asking for a switch endstop but we don't already have one, so we don't know what pin number(s) it should use
-							reply.printf("Pin name needed for switch-type endstop on %c axis", c);
-							return GCodeResult::error;
-						}
-						else
-						{
-							((LocalSwitchEndstop *)axisEndstops[axis])->Reconfigure(pos, inputType);
-						}
-						break;
-
-					default:
-						break;
+						((LocalSwitchEndstop *)axisEndstops[axis])->Reconfigure(pos, inputType);
 					}
+					break;
+
+				default:
+					break;
 				}
 			}
 		}