Allow an expression to be used as a driver ID

3 files changed, 86 insertions, 22 deletions

diff --git a/src/GCodes/GCodeBuffer/BinaryParser.cpp b/src/GCodes/GCodeBuffer/BinaryParser.cpp
index f929f462..78145ef1 100644
--- a/src/GCodes/GCodeBuffer/BinaryParser.cpp
+++ b/src/GCodes/GCodeBuffer/BinaryParser.cpp
@@ -242,6 +242,28 @@ void BinaryParser::SetDriverIdFromBinary(DriverId& did, uint32_t val) THROWS(GCo
 #endif
 }
 
+void BinaryParser::SetDriverIdFromFloat(DriverId& did, float fval) THROWS(GCodeException)
+{
+	fval *= 10.0;
+	const int32_t ival = lrintf(fval);
+#if SUPPORT_CAN_EXPANSION
+	if (ival >= 0 && fabsf(fval - (float)ival) <= 0.002)
+	{
+		did.boardAddress = ival/10;
+		did.localDriver = ival % 10;
+	}
+#else
+	if (ival >= 0 && ival < 10 && fabsf(fval - (float)ival) <= 0.002)
+	{
+		did.localDriver = ival % 10;
+	}
+#endif
+	else
+	{
+		throw ConstructParseException("Invalid driver ID expression");
+	}
+}
+
 // Get a driver ID
 DriverId BinaryParser::GetDriverId() THROWS(GCodeException)
 {
@@ -259,6 +281,19 @@ DriverId BinaryParser::GetDriverId() THROWS(GCodeException)
 		SetDriverIdFromBinary(value, seenParameter->uintValue);
 		break;
 
+	case DataType::Float:
+		SetDriverIdFromFloat(value, seenParameter->floatValue);
+		break;
+
+	case DataType::Expression:
+		{
+			ExpressionParser parser(gb, seenParameterValue, seenParameterValue + seenParameter->intValue, -1);
+			const float fval = parser.ParseFloat();
+			parser.CheckForExtraCharacters();
+			SetDriverIdFromFloat(value, fval);
+		}
+		break;
+
 	default:
 		break;
 	}
diff --git a/src/GCodes/GCodeBuffer/BinaryParser.h b/src/GCodes/GCodeBuffer/BinaryParser.h
index 18f0c675..5a4bcc38 100644
--- a/src/GCodes/GCodeBuffer/BinaryParser.h
+++ b/src/GCodes/GCodeBuffer/BinaryParser.h
@@ -66,6 +66,7 @@ private:
 
 	void CheckArrayLength(size_t maxLength) THROWS(GCodeException);
 	void SetDriverIdFromBinary(DriverId& did, uint32_t val) THROWS(GCodeException);
+	void SetDriverIdFromFloat(DriverId& did, float fval) THROWS(GCodeException);
 	GCodeException ConstructParseException(const char *str) const noexcept;
 	GCodeException ConstructParseException(const char *str, const char *param) const noexcept;
 	GCodeException ConstructParseException(const char *str, uint32_t param) const noexcept;
diff --git a/src/GCodes/GCodeBuffer/StringParser.cpp b/src/GCodes/GCodeBuffer/StringParser.cpp
index 4c89a853..691be0ac 100644
--- a/src/GCodes/GCodeBuffer/StringParser.cpp
+++ b/src/GCodes/GCodeBuffer/StringParser.cpp
@@ -1728,36 +1728,64 @@ int32_t StringParser::ReadIValue() THROWS(GCodeException)
 DriverId StringParser::ReadDriverIdValue() THROWS(GCodeException)
 {
 	DriverId result;
-	const uint32_t v1 = ReadUIValue();
-#if SUPPORT_CAN_EXPANSION
-	if (gb.buffer[readPointer] == '.')
-	{
-		++readPointer;
-		const uint32_t v2 = ReadUIValue();
-		result.localDriver = v2;
-		result.boardAddress = v1;
-	}
-	else
+	if (gb.buffer[readPointer] == '{')
 	{
-		result.localDriver = v1;
-		result.boardAddress = CanInterface::GetCanAddress();
-	}
+		// Allow a floating point expression to be converted to a driver ID
+		// We assume that a driver ID only ever has a single fractional digit. This means that e.g. 3.10 will be treated the same as 3.1.
+		ExpressionParser parser(gb, gb.buffer + readPointer, gb.buffer + ARRAY_SIZE(gb.buffer), commandIndent + readPointer);
+		const float val = 10.0 * parser.ParseFloat();
+		readPointer = parser.GetEndptr() - gb.buffer;
+		const int32_t ival = lrintf(val);
+#if SUPPORT_CAN_EXPANSION
+		if (ival >= 0 && fabsf(val - (float)ival) <= 0.002)
+		{
+			result.boardAddress = ival/10;
+			result.localDriver = ival % 10;
+		}
 #else
-	// We now allow driver names of the form "0.x" on boards without CAN expansion
-	if (gb.buffer[readPointer] == '.')
-	{
-		if (v1 != 0)
+		if (ival >= 0 && ival < 10 && fabsf(val - (float)ival) <= 0.002)
 		{
-			throw ConstructParseException("Board address of driver must be 0");
+			result.localDriver = ival % 10;
+		}
+#endif
+		else
+		{
+			throw ConstructParseException("Invalid driver ID expression");
 		}
-		++readPointer;
-		result.localDriver = ReadUIValue();
 	}
 	else
 	{
-		result.localDriver = v1;
-	}
+		const uint32_t v1 = ReadUIValue();
+#if SUPPORT_CAN_EXPANSION
+		if (gb.buffer[readPointer] == '.')
+		{
+			++readPointer;
+			const uint32_t v2 = ReadUIValue();
+			result.localDriver = v2;
+			result.boardAddress = v1;
+		}
+		else
+		{
+			result.localDriver = v1;
+			result.boardAddress = CanInterface::GetCanAddress();
+		}
+#else
+		// We now allow driver names of the form "0.x" on boards without CAN expansion
+		if (gb.buffer[readPointer] == '.')
+		{
+			if (v1 != 0)
+			{
+				throw ConstructParseException("Board address of driver must be 0");
+			}
+			++readPointer;
+			result.localDriver = ReadUIValue();
+		}
+		else
+		{
+			result.localDriver = v1;
+		}
 #endif
+	}
 	return result;
 }