Lock movement before executing M569.1; implement remote M569.2

8 files changed, 78 insertions, 14 deletions

diff --git a/src/CAN/CanInterface.cpp b/src/CAN/CanInterface.cpp
index 502fad4f..9b42ab44 100644
--- a/src/CAN/CanInterface.cpp
+++ b/src/CAN/CanInterface.cpp
@@ -892,6 +892,13 @@ pre(driver.IsRemote())
 	{
 	case -1:
 	case 0:
+		if (gb.SeenAny("RS"))
+		{
+			if (!reprap.GetGCodes().LockMovementAndWaitForStandstill(gb))
+			{
+				return GCodeResult::notFinished;
+			}
+		}
 		{
 			CanMessageGenericConstructor cons(M569Params);
 			cons.PopulateFromCommand(gb);
@@ -899,12 +906,26 @@ pre(driver.IsRemote())
 		}
 
 	case 1:
+		if (gb.SeenAny("STERID"))
+		{
+			if (!reprap.GetGCodes().LockMovementAndWaitForStandstill(gb))
+			{
+				return GCodeResult::notFinished;
+			}
+		}
 		{
 			CanMessageGenericConstructor cons(M569Point1Params);
 			cons.PopulateFromCommand(gb);
 			return cons.SendAndGetResponse(CanMessageType::m569p1, driver.boardAddress, reply);
 		}
 
+	case 2:
+		{
+			CanMessageGenericConstructor cons(M569Point2Params);
+			cons.PopulateFromCommand(gb);
+			return cons.SendAndGetResponse(CanMessageType::m569p2, driver.boardAddress, reply);
+		}
+
 	default:
 		return GCodeResult::errorNotSupported;
 	}
diff --git a/src/GCodes/GCodeBuffer/BinaryParser.cpp b/src/GCodes/GCodeBuffer/BinaryParser.cpp
index 78145ef1..bd69b162 100644
--- a/src/GCodes/GCodeBuffer/BinaryParser.cpp
+++ b/src/GCodes/GCodeBuffer/BinaryParser.cpp
@@ -88,6 +88,25 @@ bool BinaryParser::Seen(char c) noexcept
 	return false;
 }
 
+// Return true if any of the parameter letters in the bitmap were seen
+bool BinaryParser::SeenAny(Bitmap<uint32_t> bm) const noexcept
+{
+	if (bufferLength != 0 && header->numParameters != 0)
+	{
+		const char *parameterStart = reinterpret_cast<const char*>(gb.buffer) + sizeof(CodeHeader);
+		for (size_t i = 0; i < header->numParameters; i++)
+		{
+			const CodeParameter *param = reinterpret_cast<const CodeParameter*>(parameterStart + i * sizeof(CodeParameter));
+			const char paramLetter = param->letter;
+			if (paramLetter >= 'A' && paramLetter <= 'Z' && bm.IsBitSet(paramLetter - 'A'))
+			{
+				return true;
+			}
+		}
+	}
+	return false;
+}
+
 char BinaryParser::GetCommandLetter() const noexcept
 {
 	return (bufferLength != 0) ? header->letter : 'Q';
diff --git a/src/GCodes/GCodeBuffer/BinaryParser.h b/src/GCodes/GCodeBuffer/BinaryParser.h
index 5a4bcc38..560ab67e 100644
--- a/src/GCodes/GCodeBuffer/BinaryParser.h
+++ b/src/GCodes/GCodeBuffer/BinaryParser.h
@@ -24,10 +24,11 @@ class BinaryParser
 {
 public:
 	BinaryParser(GCodeBuffer& gcodeBuffer) noexcept;
-	void Init() noexcept; 											// Set it up to parse another G-code
-	void Put(const uint32_t *data, size_t len) noexcept;			// Add an entire binary code, overwriting any existing content
-	void DecodeCommand() noexcept;									// Print the buffer content in debug mode and prepare for execution
-	bool Seen(char c) noexcept SPEED_CRITICAL;						// Is a character present?
+	void Init() noexcept; 														// Set it up to parse another G-code
+	void Put(const uint32_t *data, size_t len) noexcept;						// Add an entire binary code, overwriting any existing content
+	void DecodeCommand() noexcept;												// Print the buffer content in debug mode and prepare for execution
+	bool Seen(char c) noexcept SPEED_CRITICAL;									// Is a character present?
+	bool SeenAny(Bitmap<uint32_t> bm) const noexcept;							// Return true if any of the parameter letters in the bitmap were seen
 
 	char GetCommandLetter() const noexcept;
 	bool HasCommandNumber() const noexcept;
diff --git a/src/GCodes/GCodeBuffer/GCodeBuffer.cpp b/src/GCodes/GCodeBuffer/GCodeBuffer.cpp
index d4ee40fd..a23ec6e6 100644
--- a/src/GCodes/GCodeBuffer/GCodeBuffer.cpp
+++ b/src/GCodes/GCodeBuffer/GCodeBuffer.cpp
@@ -345,6 +345,12 @@ bool GCodeBuffer::Seen(char c) noexcept
 	return PARSER_OPERATION(Seen(c));
 }
 
+// Return true if any of the parameter letters in the bitmap were seen
+bool GCodeBuffer::SeenAny(Bitmap<uint32_t> bm) const noexcept
+{
+	return PARSER_OPERATION(SeenAny(bm));
+}
+
 // Test for character present, throw error if not
 void GCodeBuffer::MustSee(char c) THROWS(GCodeException)
 {
diff --git a/src/GCodes/GCodeBuffer/GCodeBuffer.h b/src/GCodes/GCodeBuffer/GCodeBuffer.h
index 59e86b72..b6cec720 100644
--- a/src/GCodes/GCodeBuffer/GCodeBuffer.h
+++ b/src/GCodes/GCodeBuffer/GCodeBuffer.h
@@ -72,6 +72,7 @@ public:
 	bool Seen(char c) noexcept SPEED_CRITICAL;										// Is a character present?
 	void MustSee(char c) THROWS(GCodeException);									// Test for character present, throw error if not
 	char MustSee(char c1, char c2) THROWS(GCodeException);							// Test for one of two characters present, throw error if not
+	inline bool SeenAny(const char *s) const noexcept { return SeenAny(Bitmap<uint32_t>(ParametersToBitmap(s))); }
 
 	float GetFValue() THROWS(GCodeException) SPEED_CRITICAL;						// Get a float after a key letter
 	float GetDistance() THROWS(GCodeException);										// Get a distance or coordinate and convert it from inches to mm if necessary
@@ -243,6 +244,15 @@ protected:
 	DECLARE_OBJECT_MODEL
 
 private:
+	bool SeenAny(Bitmap<uint32_t> bm) const noexcept;								// Return true if any of the parameter letters in the bitmap were seen
+
+	// Convert a string of uppercase parameter letters to a bit map
+	static inline constexpr uint32_t ParametersToBitmap(const char *s) noexcept
+	{
+		return (*s == 0) ? 0
+			: (*s >= 'A' && *s <= 'Z') ? ((uint32_t)1 << (*s - 'A')) | ParametersToBitmap(s + 1)
+				: ParametersToBitmap(s + 1);
+	}
 
 #if SUPPORT_OBJECT_MODEL
 	const char *GetStateText() const noexcept;
diff --git a/src/GCodes/GCodeBuffer/StringParser.cpp b/src/GCodes/GCodeBuffer/StringParser.cpp
index 691be0ac..d1e4d984 100644
--- a/src/GCodes/GCodeBuffer/StringParser.cpp
+++ b/src/GCodes/GCodeBuffer/StringParser.cpp
@@ -1059,6 +1059,12 @@ bool StringParser::Seen(char c) noexcept
 	return false;
 }
 
+// Return true if any of the parameter letters in the bitmap were seen
+bool StringParser::SeenAny(Bitmap<uint32_t> bm) const noexcept
+{
+	return parametersPresent.Intersects(bm);
+}
+
 // Get a float after a G Code letter found by a call to Seen()
 float StringParser::GetFValue() THROWS(GCodeException)
 {
diff --git a/src/GCodes/GCodeBuffer/StringParser.h b/src/GCodes/GCodeBuffer/StringParser.h
index 3e7c515b..44662b08 100644
--- a/src/GCodes/GCodeBuffer/StringParser.h
+++ b/src/GCodes/GCodeBuffer/StringParser.h
@@ -43,10 +43,11 @@ public:
 	bool IsLastCommand() const noexcept;
 	bool ContainsExpression() const noexcept { return seenExpression; }
 
-	bool Seen(char c) noexcept SPEED_CRITICAL;					// Is a character present?
-	float GetFValue() THROWS(GCodeException) SPEED_CRITICAL;	// Get a float after a key letter
-	float GetDistance() THROWS(GCodeException) SPEED_CRITICAL;	// Get a distance or coordinate and convert it from inches to mm if necessary
-	int32_t GetIValue() THROWS(GCodeException) SPEED_CRITICAL;	// Get an integer after a key letter
+	bool Seen(char c) noexcept SPEED_CRITICAL;									// Is a character present?
+	bool SeenAny(Bitmap<uint32_t> bm) const noexcept;							// Return true if any of the parameter letters in the bitmap were seen
+	float GetFValue() THROWS(GCodeException) SPEED_CRITICAL;					// Get a float after a key letter
+	float GetDistance() THROWS(GCodeException) SPEED_CRITICAL;					// Get a distance or coordinate and convert it from inches to mm if necessary
+	int32_t GetIValue() THROWS(GCodeException) SPEED_CRITICAL;					// Get an integer after a key letter
 	uint32_t GetUIValue() THROWS(GCodeException);								// Get an unsigned integer value
 	DriverId GetDriverId() THROWS(GCodeException);								// Get a driver ID
 	void GetIPAddress(IPAddress& returnedIp) THROWS(GCodeException);			// Get an IP address quad after a key letter
diff --git a/src/GCodes/GCodes3.cpp b/src/GCodes/GCodes3.cpp
index 645ca9c0..74ba76a9 100644
--- a/src/GCodes/GCodes3.cpp
+++ b/src/GCodes/GCodes3.cpp
@@ -1384,22 +1384,22 @@ GCodeResult GCodes::ConfigureLocalDriver(GCodeBuffer& gb, const StringRef& reply
 {
 	if (drive < platform.GetNumActualDirectDrivers())
 	{
-		bool seen = false;
-		if (gb.Seen('S'))
+		if (gb.SeenAny("RS"))
 		{
 			if (!LockMovementAndWaitForStandstill(gb))
 			{
 				return GCodeResult::notFinished;
 			}
+		}
+
+		bool seen = false;
+		if (gb.Seen('S'))
+		{
 			seen = true;
 			platform.SetDirectionValue(drive, gb.GetIValue() != 0);
 		}
 		if (gb.Seen('R'))
 		{
-			if (!LockMovementAndWaitForStandstill(gb))
-			{
-				return GCodeResult::notFinished;
-			}
 			seen = true;
 			platform.SetEnableValue(drive, (int8_t)gb.GetIValue());
 		}