15 files changed, 475 insertions, 339 deletions

diff --git a/src/GCodes/GCodeBuffer/BinaryParser.cpp b/src/GCodes/GCodeBuffer/BinaryParser.cpp
index de4b7b02..1b6d3398 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';
@@ -455,19 +474,58 @@ void BinaryParser::GetPossiblyQuotedString(const StringRef& str, bool allowEmpty
 	}
 }
 
-void BinaryParser::GetFloatArray(float arr[], size_t& length, bool doPad) THROWS(GCodeException)
+void BinaryParser::GetFloatArray(float arr[], size_t& length) THROWS(GCodeException)
 {
-	GetArray(arr, length, doPad, DataType::Float);
+	if (seenParameter == nullptr)
+	{
+		THROW_INTERNAL_ERROR;
+	}
+
+	if (seenParameter->type == DataType::Expression)
+	{
+		ExpressionParser parser(gb, seenParameterValue, seenParameterValue + seenParameter->intValue, -1);
+		parser.ParseFloatArray(arr, length);
+	}
+	else
+	{
+		GetArray(arr, length);
+	}
 }
 
-void BinaryParser::GetIntArray(int32_t arr[], size_t& length, bool doPad) THROWS(GCodeException)
+void BinaryParser::GetIntArray(int32_t arr[], size_t& length) THROWS(GCodeException)
 {
-	GetArray(arr, length, doPad, DataType::Int);
+	if (seenParameter == nullptr)
+	{
+		THROW_INTERNAL_ERROR;
+	}
+
+	if (seenParameter->type == DataType::Expression)
+	{
+		ExpressionParser parser(gb, seenParameterValue, seenParameterValue + seenParameter->intValue, -1);
+		parser.ParseIntArray(arr, length);
+	}
+	else
+	{
+		GetArray(arr, length);
+	}
 }
 
-void BinaryParser::GetUnsignedArray(uint32_t arr[], size_t& length, bool doPad) THROWS(GCodeException)
+void BinaryParser::GetUnsignedArray(uint32_t arr[], size_t& length) THROWS(GCodeException)
 {
-	GetArray(arr, length, doPad, DataType::UInt);
+	if (seenParameter == nullptr)
+	{
+		THROW_INTERNAL_ERROR;
+	}
+
+	if (seenParameter->type == DataType::Expression)
+	{
+		ExpressionParser parser(gb, seenParameterValue, seenParameterValue + seenParameter->intValue, -1);
+		parser.ParseUnsignedArray(arr, length);
+	}
+	else
+	{
+		GetArray(arr, length);
+	}
 }
 
 // Get a :-separated list of drivers after a key letter
@@ -499,15 +557,12 @@ void BinaryParser::GetDriverIdArray(DriverId arr[], size_t& length) THROWS(GCode
 		break;
 
 	case DataType::Expression:
-	{
-		float temp[seenParameter->intValue];
-		GetArray(temp, length, false, DataType::Float);
-		for (int i = 0; i < seenParameter->intValue; i++)
 		{
-			SetDriverIdFromBinary(arr[i], temp[i]);
+			ExpressionParser parser(gb, seenParameterValue, seenParameterValue + seenParameter->intValue, -1);
+			parser.ParseDriverIdArray(arr, length);
+			parser.CheckForExtraCharacters();
 		}
 		break;
-	}
 
 	default:
 		length = 0;
@@ -573,13 +628,8 @@ void BinaryParser::AppendFullCommand(const StringRef &s) const noexcept
 	}
 }
 
-template<typename T> void BinaryParser::GetArray(T arr[], size_t& length, bool doPad, DataType type) THROWS(GCodeException)
+template<typename T> void BinaryParser::GetArray(T arr[], size_t& length) THROWS(GCodeException)
 {
-	if (seenParameter == nullptr)
-	{
-		THROW_INTERNAL_ERROR;
-	}
-
 	int lastIndex = -1;
 	switch (seenParameter->type)
 	{
@@ -627,122 +677,12 @@ template<typename T> void BinaryParser::GetArray(T arr[], size_t& length, bool d
 		lastIndex = seenParameter->intValue - 1;
 		break;
 
-	case DataType::Expression:
-	{
-		const char *pos = seenParameterValue, *endPos = seenParameterValue + seenParameter->intValue;
-
-		// Check if the whole expression is encapsulated in curly braces and remove them if necessary
-		if (*pos == '{' && pos != endPos)
-		{
-			bool isEncapsulated = true, inQuotes = false;
-			size_t numBraces = 1;
-			for (const char *str = pos + 1; str < endPos; str++)
-			{
-				if (inQuotes)
-				{
-					inQuotes = (*str != '"');
-				}
-				else if (*str == '"')
-				{
-					inQuotes = true;
-				}
-				else if (*str == '{')
-				{
-					numBraces++;
-				}
-				else if (*str == '}')
-				{
-					numBraces--;
-					if (numBraces == 0)
-					{
-						const char *curPos = str;
-						while (str != endPos && strchr("\t ", *++str) != nullptr) { }
-						if (str == endPos)
-						{
-							endPos = curPos + 1;
-						}
-						else
-						{
-							isEncapsulated = false;
-						}
-						break;
-					}
-				}
-			}
-
-			if (isEncapsulated)
-			{
-				pos++;
-				endPos--;
-			}
-		}
-
-		// Read array expression
-		for (;;)
-		{
-			if (lastIndex >= (int)length)
-			{
-				throw ConstructParseException("array too long, max length = %u", (uint32_t)length);
-			}
-
-			// Read the next expression value
-			ExpressionParser parser(gb, pos, endPos, -1);
-			switch (type)
-			{
-			case DataType::Int:
-				arr[++lastIndex] = (T)parser.ParseInteger();
-				break;
-
-			case DataType::UInt:
-				arr[++lastIndex] = (T)parser.ParseUnsigned();
-				break;
-
-			case DataType::Float:
-				arr[++lastIndex] = (T)parser.ParseFloat();
-				break;
-
-			default:
-				throw ConstructParseException("Unsupported array data type");
-			}
-			parser.CheckForExtraCharacters(true);
-			pos = parser.GetEndptr();
-
-			if (pos++ >= endPos)
-			{
-				break;
-			}
-		}
-
-		if (doPad && lastIndex == 0)
-		{
-			for (size_t i = 1; i < length; i++)
-			{
-				arr[i] = arr[0];
-			}
-		}
-		else
-		{
-			length = lastIndex + 1;
-		}
-		break;
-	}
-
 	default:
 		length = 0;
 		return;
 	}
 
-	if (doPad && lastIndex == 0)
-	{
-		for (size_t i = 1; i < length; i++)
-		{
-			arr[i] = arr[0];
-		}
-	}
-	else
-	{
-		length = lastIndex + 1;
-	}
+	length = lastIndex + 1;
 }
 
 void BinaryParser::CheckArrayLength(size_t maxLength) THROWS(GCodeException)
diff --git a/src/GCodes/GCodeBuffer/BinaryParser.h b/src/GCodes/GCodeBuffer/BinaryParser.h
index 3495d9c8..adb2286b 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;
@@ -45,9 +46,9 @@ public:
 	void GetCompleteParameters(const StringRef& str) THROWS(GCodeException);					// Get the complete parameter string
 	void GetQuotedString(const StringRef& str, bool allowEmpty) THROWS(GCodeException);			// Get and copy a quoted string
 	void GetPossiblyQuotedString(const StringRef& str, bool allowEmpty) THROWS(GCodeException);	// Get and copy a string which may or may not be quoted
-	void GetFloatArray(float arr[], size_t& length, bool doPad) THROWS(GCodeException) SPEED_CRITICAL; // Get a colon-separated list of floats after a key letter
-	void GetIntArray(int32_t arr[], size_t& length, bool doPad) THROWS(GCodeException);			// Get a :-separated list of ints after a key letter
-	void GetUnsignedArray(uint32_t arr[], size_t& length, bool doPad) THROWS(GCodeException);	// Get a :-separated list of unsigned ints after a key letter
+	void GetFloatArray(float arr[], size_t& length) THROWS(GCodeException) SPEED_CRITICAL;		// Get a colon-separated list of floats after a key letter
+	void GetIntArray(int32_t arr[], size_t& length) THROWS(GCodeException);						// Get a :-separated list of ints after a key letter
+	void GetUnsignedArray(uint32_t arr[], size_t& length) THROWS(GCodeException);				// Get a :-separated list of unsigned ints after a key letter
 	void GetDriverIdArray(DriverId arr[], size_t& length) THROWS(GCodeException);				// Get a :-separated list of drivers after a key letter
 
 	void SetFinished() noexcept;									// Set the G Code finished
@@ -72,7 +73,7 @@ private:
 	GCodeException ConstructParseException(const char *str, uint32_t param) const noexcept;
 
 	size_t AddPadding(size_t bytesRead) const noexcept { return (bytesRead + 3u) & (~3u); }
-	template<typename T> void GetArray(T arr[], size_t& length, bool doPad, DataType type) THROWS(GCodeException) SPEED_CRITICAL;
+	template<typename T> void GetArray(T arr[], size_t& length) THROWS(GCodeException) SPEED_CRITICAL;
 	void WriteParameters(const StringRef& s, bool quoteStrings) const noexcept;
 
 	size_t bufferLength;
diff --git a/src/GCodes/GCodeBuffer/ExpressionParser.cpp b/src/GCodes/GCodeBuffer/ExpressionParser.cpp
index 0e3ac432..0c65a39c 100644
--- a/src/GCodes/GCodeBuffer/ExpressionParser.cpp
+++ b/src/GCodes/GCodeBuffer/ExpressionParser.cpp
@@ -560,6 +560,60 @@ uint32_t ExpressionParser::ParseUnsigned() THROWS(GCodeException)
 	}
 }
 
+DriverId ExpressionParser::ParseDriverId() THROWS(GCodeException)
+{
+	ExpressionValue val = Parse();
+	ConvertToDriverId(val, true);
+	return val.GetDriverIdValue();
+}
+
+void ExpressionParser::ParseArray(size_t& length, function_ref<void(size_t index)> processElement) THROWS(GCodeException)
+{
+	size_t numElements = 0;
+	AdvancePointer();					// skip the '{'
+	while (numElements < length)
+	{
+		processElement(numElements);
+		++numElements;
+		if (CurrentCharacter() != ',')
+		{
+			break;
+		}
+		if (numElements == length)
+		{
+			ThrowParseException("Array too long");
+		}
+		AdvancePointer();				// skip the '{'
+	}
+	if (CurrentCharacter() != '}')
+	{
+		ThrowParseException("Expected '}'");
+	}
+	AdvancePointer();					// skip the '{'
+	length = numElements;
+}
+
+// This is called when we expect a non-empty float array parameter and we have encountered (but not skipped) '{'
+void ExpressionParser::ParseFloatArray(float arr[], size_t& length) THROWS(GCodeException)
+{
+	ParseArray(length, [this, &arr](size_t index) { arr[index] = ParseFloat(); });
+}
+
+void ExpressionParser::ParseIntArray(int32_t arr[], size_t& length) THROWS(GCodeException)
+{
+	ParseArray(length, [this, &arr](size_t index) { arr[index] = ParseInteger(); });
+}
+
+void ExpressionParser::ParseUnsignedArray(uint32_t arr[], size_t& length) THROWS(GCodeException)
+{
+	ParseArray(length, [this, &arr](size_t index) { arr[index] = ParseUnsigned(); });
+}
+
+void ExpressionParser::ParseDriverIdArray(DriverId arr[], size_t& length) THROWS(GCodeException)
+{
+	ParseArray(length, [this, &arr](size_t index) { arr[index] = ParseDriverId(); });
+}
+
 void ExpressionParser::BalanceNumericTypes(ExpressionValue& val1, ExpressionValue& val2, bool evaluate) const THROWS(GCodeException)
 {
 	// First convert any Uint64 or Uint32 operands to float
@@ -704,6 +758,51 @@ void ExpressionParser::ConvertToString(ExpressionValue& val, bool evaluate) noex
 	}
 }
 
+void ExpressionParser::ConvertToDriverId(ExpressionValue& val, bool evaluate) const THROWS(GCodeException)
+{
+	switch (val.GetType())
+	{
+	case TypeCode::DriverId:
+		break;
+
+	case TypeCode::Int32:
+#if SUPPORT_CAN_EXPANSION
+		val.Set(DriverId(0, val.uVal));
+#else
+		val.Set(DriverId(val.uVal));
+#endif
+		break;
+
+	case TypeCode::Float:
+		{
+			const float f10val = 10.0 * val.fVal;
+			const int32_t ival = lrintf(f10val);
+#if SUPPORT_CAN_EXPANSION
+			if (ival >= 0 && fabsf(f10val - (float)ival) <= 0.002)
+			{
+				val.Set(ival/10, ival % 10);
+			}
+#else
+			if (ival >= 0 && ival < 10 && fabsf(f10val - (float)ival) <= 0.002)
+			{
+				val.Set(0, ival % 10);
+			}
+#endif
+			else
+			{
+				ThrowParseException("invalid driver ID");
+			}
+		}
+		break;
+
+	default:
+		if (evaluate)
+		{
+			ThrowParseException("expected driver ID");
+		}
+	}
+}
+
 void ExpressionParser::SkipWhiteSpace() noexcept
 {
 	char c;
diff --git a/src/GCodes/GCodeBuffer/ExpressionParser.h b/src/GCodes/GCodeBuffer/ExpressionParser.h
index bfd4de4e..adfff7c7 100644
--- a/src/GCodes/GCodeBuffer/ExpressionParser.h
+++ b/src/GCodes/GCodeBuffer/ExpressionParser.h
@@ -24,6 +24,13 @@ public:
 	float ParseFloat() THROWS(GCodeException);
 	int32_t ParseInteger() THROWS(GCodeException);
 	uint32_t ParseUnsigned() THROWS(GCodeException);
+	DriverId ParseDriverId() THROWS(GCodeException);
+
+	// These 4 functions may be removed when we support array-valued expressions more generally
+	void ParseFloatArray(float arr[], size_t& length) THROWS(GCodeException);
+	void ParseIntArray(int32_t arr[], size_t& length) THROWS(GCodeException);
+	void ParseUnsignedArray(uint32_t arr[], size_t& length) THROWS(GCodeException);
+	void ParseDriverIdArray(DriverId arr[], size_t& length) THROWS(GCodeException);
 
 	void SkipWhiteSpace() noexcept;
 	void CheckForExtraCharacters(bool isArrayExpression = false) THROWS(GCodeException);
@@ -41,11 +48,15 @@ private:
 	void __attribute__((noinline)) ParseIdentifierExpression(ExpressionValue& rslt, bool evaluate, bool applyLengthOperator, bool applyExists) THROWS(GCodeException)
 		pre(readPointer >= 0; isalpha(gb.buffer[readPointer]));
 	void __attribute__((noinline)) ParseQuotedString(ExpressionValue& rslt) THROWS(GCodeException);
+
+	void ParseArray(size_t& length, function_ref<void(size_t index)> processElement) THROWS(GCodeException);
+
 	void GetVariableValue(ExpressionValue& rslt, const VariableSet *vars, const char *name, bool parameter, bool wantExists) THROWS(GCodeException);
 
 	void ConvertToFloat(ExpressionValue& val, bool evaluate) const THROWS(GCodeException);
 	void ConvertToBool(ExpressionValue& val, bool evaluate) const THROWS(GCodeException);
 	void ConvertToString(ExpressionValue& val, bool evaluate) noexcept;
+	void ConvertToDriverId(ExpressionValue& val, bool evaluate) const THROWS(GCodeException);
 
 	void CheckStack(uint32_t calledFunctionStackUsage) const THROWS(GCodeException);
 
diff --git a/src/GCodes/GCodeBuffer/GCodeBuffer.cpp b/src/GCodes/GCodeBuffer/GCodeBuffer.cpp
index 37e616c2..bc607448 100644
--- a/src/GCodes/GCodeBuffer/GCodeBuffer.cpp
+++ b/src/GCodes/GCodeBuffer/GCodeBuffer.cpp
@@ -19,6 +19,7 @@
 #include <GCodes/GCodeException.h>
 #include <Platform/RepRap.h>
 #include <Platform/Platform.h>
+#include <Movement/StepTimer.h>
 
 // Macros to reduce the amount of explicit conditional compilation in this file
 #if HAS_LINUX_INTERFACE
@@ -49,17 +50,17 @@ constexpr ObjectModelTableEntry GCodeBuffer::objectModelTable[] =
 {
 	// Within each group, these entries must be in alphabetical order
 	// 0. inputs[] root
-	{ "axesRelative",		OBJECT_MODEL_FUNC((bool)self->machineState->axesRelative),					ObjectModelEntryFlags::none },
-	{ "compatibility",		OBJECT_MODEL_FUNC(self->machineState->compatibility.ToString()),			ObjectModelEntryFlags::none },
-	{ "distanceUnit",		OBJECT_MODEL_FUNC((self->machineState->usingInches) ? "inch" : "mm"),		ObjectModelEntryFlags::none },
-	{ "drivesRelative",		OBJECT_MODEL_FUNC((bool)self->machineState->drivesRelative),				ObjectModelEntryFlags::none },
-	{ "feedRate",			OBJECT_MODEL_FUNC(self->machineState->feedRate, 1),							ObjectModelEntryFlags::live },
-	{ "inMacro",			OBJECT_MODEL_FUNC((bool)self->machineState->doingFileMacro),				ObjectModelEntryFlags::none },
-	{ "lineNumber",			OBJECT_MODEL_FUNC((int32_t)self->GetLineNumber()),							ObjectModelEntryFlags::live },
-	{ "name",				OBJECT_MODEL_FUNC(self->codeChannel.ToString()),							ObjectModelEntryFlags::none },
-	{ "stackDepth",			OBJECT_MODEL_FUNC((int32_t)self->GetStackDepth()),							ObjectModelEntryFlags::none },
-	{ "state",				OBJECT_MODEL_FUNC(self->GetStateText()),									ObjectModelEntryFlags::live },
-	{ "volumetric",			OBJECT_MODEL_FUNC((bool)self->machineState->volumetricExtrusion),			ObjectModelEntryFlags::none },
+	{ "axesRelative",		OBJECT_MODEL_FUNC((bool)self->machineState->axesRelative),							ObjectModelEntryFlags::none },
+	{ "compatibility",		OBJECT_MODEL_FUNC(self->machineState->compatibility.ToString()),					ObjectModelEntryFlags::none },
+	{ "distanceUnit",		OBJECT_MODEL_FUNC(self->GetDistanceUnits()),										ObjectModelEntryFlags::none },
+	{ "drivesRelative",		OBJECT_MODEL_FUNC((bool)self->machineState->drivesRelative),						ObjectModelEntryFlags::none },
+	{ "feedRate",			OBJECT_MODEL_FUNC(InverseConvertSpeedToMmPerSec(self->machineState->feedRate), 1),	ObjectModelEntryFlags::live },
+	{ "inMacro",			OBJECT_MODEL_FUNC((bool)self->machineState->doingFileMacro),						ObjectModelEntryFlags::none },
+	{ "lineNumber",			OBJECT_MODEL_FUNC((int32_t)self->GetLineNumber()),									ObjectModelEntryFlags::live },
+	{ "name",				OBJECT_MODEL_FUNC(self->codeChannel.ToString()),									ObjectModelEntryFlags::none },
+	{ "stackDepth",			OBJECT_MODEL_FUNC((int32_t)self->GetStackDepth()),									ObjectModelEntryFlags::none },
+	{ "state",				OBJECT_MODEL_FUNC(self->GetStateText()),											ObjectModelEntryFlags::live },
+	{ "volumetric",			OBJECT_MODEL_FUNC((bool)self->machineState->volumetricExtrusion),					ObjectModelEntryFlags::none },
 };
 
 constexpr uint8_t GCodeBuffer::objectModelTableDescriptor[] = { 1, 11 };
@@ -344,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)
 {
@@ -388,6 +395,24 @@ float GCodeBuffer::GetDistance() THROWS(GCodeException)
 	return ConvertDistance(GetFValue());
 }
 
+// Get a speed in mm/min or inches/min and convert it to mm/step_clock
+float GCodeBuffer::GetSpeed() THROWS(GCodeException)
+{
+	return ConvertSpeed(GetFValue());
+}
+
+// Get a speed in mm/min mm/sec and convert it to mm/step_clock
+float GCodeBuffer::GetSpeedFromMm(bool useSeconds) THROWS(GCodeException)
+{
+	return ConvertSpeedFromMm(GetFValue(), useSeconds);
+}
+
+// Get an acceleration in mm/sec^2 and convert it to mm/step_clock^2
+float GCodeBuffer::GetAcceleration() THROWS(GCodeException)
+{
+	return ConvertAcceleration(GetFValue());
+}
+
 // Get an integer after a key letter
 int32_t GCodeBuffer::GetIValue() THROWS(GCodeException)
 {
@@ -482,19 +507,46 @@ void GCodeBuffer::GetReducedString(const StringRef& str) THROWS(GCodeException)
 // Get a colon-separated list of floats after a key letter
 void GCodeBuffer::GetFloatArray(float arr[], size_t& length, bool doPad) THROWS(GCodeException)
 {
-	PARSER_OPERATION(GetFloatArray(arr, length, doPad));
+	const size_t maxLength = length;
+	PARSER_OPERATION(GetFloatArray(arr, length));
+	// If there is one entry and doPad is true, fill the rest of the array with the first entry.
+	if (doPad && length == 1)
+	{
+		while (length < maxLength)
+		{
+			arr[length++] = arr[0];
+		}
+	}
 }
 
 // Get a :-separated list of ints after a key letter
 void GCodeBuffer::GetIntArray(int32_t arr[], size_t& length, bool doPad) THROWS(GCodeException)
 {
-	PARSER_OPERATION(GetIntArray(arr, length, doPad));
+	const size_t maxLength = length;
+	PARSER_OPERATION(GetIntArray(arr, length));
+	// If there is one entry and doPad is true, fill the rest of the array with the first entry.
+	if (doPad && length == 1)
+	{
+		while (length < maxLength)
+		{
+			arr[length++] = arr[0];
+		}
+	}
 }
 
 // Get a :-separated list of unsigned ints after a key letter
 void GCodeBuffer::GetUnsignedArray(uint32_t arr[], size_t& length, bool doPad) THROWS(GCodeException)
 {
-	PARSER_OPERATION(GetUnsignedArray(arr, length, doPad));
+	const size_t maxLength = length;
+	PARSER_OPERATION(GetUnsignedArray(arr, length));
+	// If there is one entry and doPad is true, fill the rest of the array with the first entry.
+	if (doPad && length == 1)
+	{
+		while (length < maxLength)
+		{
+			arr[length++] = arr[0];
+		}
+	}
 }
 
 // Get a :-separated list of drivers after a key letter
@@ -727,13 +779,30 @@ GCodeMachineState& GCodeBuffer::CurrentFileMachineState() const noexcept
 // Convert from inches to mm if necessary
 float GCodeBuffer::ConvertDistance(float distance) const noexcept
 {
-	return (machineState->usingInches) ? distance * InchToMm : distance;
+	return (UsingInches()) ? distance * InchToMm : distance;
 }
 
 // Convert from mm to inches if necessary
 float GCodeBuffer::InverseConvertDistance(float distance) const noexcept
 {
-	return (machineState->usingInches) ? distance/InchToMm : distance;
+	return (UsingInches()) ? distance/InchToMm : distance;
+}
+
+// Convert speed from mm/min or inches/min to mm per step clock
+float GCodeBuffer::ConvertSpeed(float speed) const noexcept
+{
+	return speed * ((UsingInches()) ? InchToMm/(StepClockRate * iMinutesToSeconds) : 1.0/(StepClockRate * iMinutesToSeconds));
+}
+
+// Convert speed to mm/min or inches/min
+float GCodeBuffer::InverseConvertSpeed(float speed) const noexcept
+{
+	return speed * ((UsingInches()) ? (StepClockRate * iMinutesToSeconds)/InchToMm : (float)(StepClockRate * iMinutesToSeconds));
+}
+
+const char *GCodeBuffer::GetDistanceUnits() const noexcept
+{
+	return (UsingInches()) ? "in" : "mm";
 }
 
 // Return the  current stack depth
diff --git a/src/GCodes/GCodeBuffer/GCodeBuffer.h b/src/GCodes/GCodeBuffer/GCodeBuffer.h
index fb1f7a3f..b6cec720 100644
--- a/src/GCodes/GCodeBuffer/GCodeBuffer.h
+++ b/src/GCodes/GCodeBuffer/GCodeBuffer.h
@@ -72,9 +72,13 @@ 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
+	float GetSpeed() THROWS(GCodeException);										// Get a speed in mm/min or inches/min and convert it to mm/step_clock
+	float GetSpeedFromMm(bool useSeconds) THROWS(GCodeException);					// Get a speed in mm/min or optionally /sec and convert it to mm/step_clock
+	float GetAcceleration() THROWS(GCodeException);									// Get an acceleration in mm/sec^2 or inches/sec^2 and convert it to mm/step_clock^2
 	int32_t GetIValue() THROWS(GCodeException) SPEED_CRITICAL;						// Get an integer after a key letter
 	int32_t GetLimitedIValue(char c, int32_t minValue, int32_t maxValue) THROWS(GCodeException)
 		pre(minvalue <= maxValue)
@@ -127,8 +131,13 @@ public:
 	GCodeMachineState::BlockState& GetBlockState() const noexcept { return CurrentFileMachineState().CurrentBlockState(); }
 	uint16_t GetBlockIndent() const noexcept { return GetBlockState().GetIndent(); }
 
+	void UseInches(bool inchesNotMm) noexcept { machineState->usingInches = inchesNotMm; }
+	bool UsingInches() const noexcept { return machineState->usingInches; }
 	float ConvertDistance(float distance) const noexcept;
 	float InverseConvertDistance(float distance) const noexcept;
+	float ConvertSpeed(float speed) const noexcept;
+	float InverseConvertSpeed(float speed) const noexcept;
+	const char *GetDistanceUnits() const noexcept;
 	unsigned int GetStackDepth() const noexcept;
 	bool PushState(bool withinSameFile) noexcept;				// Push state returning true if successful (i.e. stack not overflowed)
 	bool PopState(bool withinSameFile) noexcept;				// Pop state returning true if successful (i.e. no stack underrun)
@@ -235,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 9247c577..4876f3d6 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)
 {
@@ -1074,35 +1080,32 @@ float StringParser::GetFValue() THROWS(GCodeException)
 
 // Get a colon-separated list of floats after a key letter
 // If doPad is true then we allow just one element to be given, in which case we fill all elements with that value
-void StringParser::GetFloatArray(float arr[], size_t& returnedLength, bool doPad) THROWS(GCodeException)
+void StringParser::GetFloatArray(float arr[], size_t& returnedLength) THROWS(GCodeException)
 {
 	if (readPointer <= 0)
 	{
 		THROW_INTERNAL_ERROR;
 	}
 
-	size_t length = 0;
-	for (;;)
+	if (gb.buffer[readPointer] == '{')
 	{
-		CheckArrayLength(length, returnedLength);
-		arr[length++] = ReadFloatValue();
-		if (gb.buffer[readPointer] != EXPRESSION_LIST_SEPARATOR && gb.buffer[readPointer] != LIST_SEPARATOR)
-		{
-			break;
-		}
-		++readPointer;
+		ExpressionParser parser(gb, gb.buffer + readPointer, gb.buffer + ARRAY_SIZE(gb.buffer), commandIndent + readPointer);
+		parser.ParseFloatArray(arr, returnedLength);
 	}
-
-	// Special case if there is one entry and returnedLength requests several. Fill the array with the first entry.
-	if (doPad && length == 1 && returnedLength > 1)
+	else
 	{
-		for (size_t i = 1; i < returnedLength; i++)
+		size_t length = 0;
+		for (;;)
 		{
-			arr[i] = arr[0];
+			CheckArrayLength(length, returnedLength);
+			arr[length++] = ReadFloatValue();
+			if (gb.buffer[readPointer] != LIST_SEPARATOR)
+			{
+				break;
+			}
+			++readPointer;
 		}
-	}
-	else
-	{
+
 		returnedLength = length;
 	}
 
@@ -1110,72 +1113,67 @@ void StringParser::GetFloatArray(float arr[], size_t& returnedLength, bool doPad
 }
 
 // Get a :-separated list of ints after a key letter
-void StringParser::GetIntArray(int32_t arr[], size_t& returnedLength, bool doPad) THROWS(GCodeException)
+void StringParser::GetIntArray(int32_t arr[], size_t& returnedLength) THROWS(GCodeException)
 {
 	if (readPointer <= 0)
 	{
 		THROW_INTERNAL_ERROR;
 	}
 
-	size_t length = 0;
-	for (;;)
+	if (gb.buffer[readPointer] == '{')
 	{
-		CheckArrayLength(length, returnedLength);
-		arr[length] = ReadIValue();
-		length++;
-		if (gb.buffer[readPointer] != EXPRESSION_LIST_SEPARATOR && gb.buffer[readPointer] != LIST_SEPARATOR)
-		{
-			break;
-		}
-		++readPointer;
+		ExpressionParser parser(gb, gb.buffer + readPointer, gb.buffer + ARRAY_SIZE(gb.buffer), commandIndent + readPointer);
+		parser.ParseIntArray(arr, returnedLength);
 	}
-
-	// Special case if there is one entry and returnedLength requests several. Fill the array with the first entry.
-	if (doPad && length == 1 && returnedLength > 1)
+	else
 	{
-		for (size_t i = 1; i < returnedLength; i++)
+		size_t length = 0;
+		for (;;)
 		{
-			arr[i] = arr[0];
+			CheckArrayLength(length, returnedLength);
+			arr[length] = ReadIValue();
+			length++;
+			if (gb.buffer[readPointer] != LIST_SEPARATOR)
+			{
+				break;
+			}
+			++readPointer;
 		}
-	}
-	else
-	{
+
 		returnedLength = length;
 	}
+
 	readPointer = -1;
 }
 
 // Get a :-separated list of unsigned ints after a key letter
-void StringParser::GetUnsignedArray(uint32_t arr[], size_t& returnedLength, bool doPad) THROWS(GCodeException)
+void StringParser::GetUnsignedArray(uint32_t arr[], size_t& returnedLength) THROWS(GCodeException)
 {
 	if (readPointer <= 0)
 	{
 		THROW_INTERNAL_ERROR;
 	}
 
-	size_t length = 0;
-	for (;;)
+	if (gb.buffer[readPointer] == '{')
 	{
-		CheckArrayLength(length, returnedLength);
-		arr[length] = ReadUIValue();
-		length++;
-		if (gb.buffer[readPointer] != EXPRESSION_LIST_SEPARATOR && gb.buffer[readPointer] != LIST_SEPARATOR)
-		{
-			break;
-		}
-		++readPointer;
+		ExpressionParser parser(gb, gb.buffer + readPointer, gb.buffer + ARRAY_SIZE(gb.buffer), commandIndent + readPointer);
+		parser.ParseUnsignedArray(arr, returnedLength);
 	}
-
-	// Special case if there is one entry and returnedLength requests several. Fill the array with the first entry.
-	if (doPad && length == 1 && returnedLength > 1)
+	else
 	{
-		for (size_t i = 1; i < returnedLength; i++)
+		size_t length = 0;
+		for (;;)
 		{
-			arr[i] = arr[0];
+			CheckArrayLength(length, returnedLength);
+			arr[length] = ReadUIValue();
+			length++;
+			if (gb.buffer[readPointer] != LIST_SEPARATOR)
+			{
+				break;
+			}
+			++readPointer;
 		}
-	}
-	else
-	{
+
 		returnedLength = length;
 	}
 
@@ -1190,20 +1188,29 @@ void StringParser::GetDriverIdArray(DriverId arr[], size_t& returnedLength) THRO
 		THROW_INTERNAL_ERROR;
 	}
 
-	size_t length = 0;
-	for (;;)
+	if (gb.buffer[readPointer] == '{')
+	{
+		ExpressionParser parser(gb, gb.buffer + readPointer, gb.buffer + ARRAY_SIZE(gb.buffer), commandIndent + readPointer);
+		parser.ParseDriverIdArray(arr, returnedLength);
+	}
+	else
 	{
-		CheckArrayLength(length, returnedLength);
-		arr[length] = ReadDriverIdValue();
-		length++;
-		if (gb.buffer[readPointer] != EXPRESSION_LIST_SEPARATOR && gb.buffer[readPointer] != LIST_SEPARATOR)
+		size_t length = 0;
+		for (;;)
 		{
-			break;
+			CheckArrayLength(length, returnedLength);
+			arr[length] = ReadDriverIdValue();
+			length++;
+			if (gb.buffer[readPointer] != LIST_SEPARATOR)
+			{
+				break;
+			}
+			++readPointer;
 		}
-		++readPointer;
+
+		returnedLength = length;
 	}
 
-	returnedLength = length;
 	readPointer = -1;
 }
 
diff --git a/src/GCodes/GCodeBuffer/StringParser.h b/src/GCodes/GCodeBuffer/StringParser.h
index 3e7c515b..b56aa9f9 100644
--- a/src/GCodes/GCodeBuffer/StringParser.h
+++ b/src/GCodes/GCodeBuffer/StringParser.h
@@ -43,22 +43,23 @@ 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
 	void GetMacAddress(MacAddress& mac) THROWS(GCodeException);					// Get a MAC address sextet after a key letter
 	void GetUnprecedentedString(const StringRef& str, bool allowEmpty) THROWS(GCodeException);	// Get a string with no preceding key letter
-	void GetCompleteParameters(const StringRef& str) const noexcept;			// Get the complete parameter string
-	void GetQuotedString(const StringRef& str, bool allowEmpty) THROWS(GCodeException);	// Get and copy a quoted string
+	void GetCompleteParameters(const StringRef& str) const noexcept;							// Get the complete parameter string
+	void GetQuotedString(const StringRef& str, bool allowEmpty) THROWS(GCodeException);			// Get and copy a quoted string
 	void GetPossiblyQuotedString(const StringRef& str, bool allowEmpty) THROWS(GCodeException);	// Get and copy a string which may or may not be quoted
-	void GetFloatArray(float arr[], size_t& length, bool doPad) THROWS(GCodeException) SPEED_CRITICAL; // Get a colon-separated list of floats after a key letter
-	void GetIntArray(int32_t arr[], size_t& length, bool doPad) THROWS(GCodeException);		// Get a :-separated list of ints after a key letter
-	void GetUnsignedArray(uint32_t arr[], size_t& length, bool doPad) THROWS(GCodeException);	// Get a :-separated list of unsigned ints after a key letter
-	void GetDriverIdArray(DriverId arr[], size_t& length) THROWS(GCodeException);	// Get a :-separated list of drivers after a key letter
+	void GetFloatArray(float arr[], size_t& length) THROWS(GCodeException) SPEED_CRITICAL; 		// Get a colon-separated list of floats after a key letter
+	void GetIntArray(int32_t arr[], size_t& length) THROWS(GCodeException);						// Get a :-separated list of ints after a key letter
+	void GetUnsignedArray(uint32_t arr[], size_t& length) THROWS(GCodeException);				// Get a :-separated list of unsigned ints after a key letter
+	void GetDriverIdArray(DriverId arr[], size_t& length) THROWS(GCodeException);				// Get a :-separated list of drivers after a key letter
 
 	void SetFinished() noexcept;											// Set the G Code finished
 	void SetCommsProperties(uint32_t arg) noexcept { checksumRequired = (arg & 1); }
diff --git a/src/GCodes/GCodeMachineState.cpp b/src/GCodes/GCodeMachineState.cpp
index 1af502d1..1e6e6dda 100644
--- a/src/GCodes/GCodeMachineState.cpp
+++ b/src/GCodes/GCodeMachineState.cpp
@@ -12,7 +12,7 @@
 
 // Create a default initialised GCodeMachineState
 GCodeMachineState::GCodeMachineState() noexcept
-	: feedRate(DefaultFeedRate * SecondsToMinutes),
+	: feedRate(ConvertSpeedFromMmPerMin(DefaultFeedRate)),
 #if HAS_LINUX_INTERFACE
 	  fileId(NoFileId),
 #endif
diff --git a/src/GCodes/GCodeQueue.cpp b/src/GCodes/GCodeQueue.cpp
index 9412e735..a87a1f9b 100644
--- a/src/GCodes/GCodeQueue.cpp
+++ b/src/GCodes/GCodeQueue.cpp
@@ -220,14 +220,15 @@ void GCodeQueue::Clear() noexcept
 
 void GCodeQueue::Diagnostics(MessageType mtype) noexcept
 {
-	reprap.GetPlatform().MessageF(mtype, "Code queue is %s\n", (queuedItems == nullptr) ? "empty." : "not empty:");
-	if (queuedItems != nullptr)
+	if (queuedItems == nullptr)
+	{
+		reprap.GetPlatform().Message(mtype, "Code queue is empty\n");
+	}
+	else
 	{
 		const QueuedCode *item = queuedItems;
-		size_t queueLength = 0;
 		do
 		{
-			queueLength++;
 #if HAS_LINUX_INTERFACE
 			// The following may output binary gibberish if this code is stored in binary.
 			// We could restore this message by using GCodeBuffer::AppendFullCommand but there is probably no need to
@@ -237,7 +238,6 @@ void GCodeQueue::Diagnostics(MessageType mtype) noexcept
 				reprap.GetPlatform().MessageF(mtype, "Queued '%.*s' for move %" PRIu32 "\n", item->dataLength, item->data, item->executeAtMove);
 			}
 		} while ((item = item->Next()) != nullptr);
-		reprap.GetPlatform().MessageF(mtype, "%d of %d codes have been queued.\n", queueLength, maxQueuedCodes);
 	}
 }
 
diff --git a/src/GCodes/GCodes.cpp b/src/GCodes/GCodes.cpp
index 9c716f3b..85e88c33 100644
--- a/src/GCodes/GCodes.cpp
+++ b/src/GCodes/GCodes.cpp
@@ -451,6 +451,7 @@ void GCodes::Spin() noexcept
 	   )	// if autoPause is active
 	{
 		(void)SpinGCodeBuffer(*autoPauseGCode);
+		(void)SpinGCodeBuffer(*queuedGCode);						// autopause sometimes to wait for queued GCodes to complete, so spin queuedGCodes too to avoid lockup
 	}
 	else
 	{
@@ -646,9 +647,7 @@ bool GCodes::DoFilePrint(GCodeBuffer& gb, const StringRef& reply) noexcept
 				// We never get here if the file ends in M0 because CancelPrint gets called directly in that case.
 				// Don't close the file until all moves have been completed, in case the print gets paused.
 				// Also, this keeps the state as 'Printing' until the print really has finished.
-				if (   LockMovementAndWaitForStandstill(gb)					// wait until movement has finished
-					&& IsCodeQueueIdle()									// must also wait until deferred command queue has caught up
-				   )
+				if (LockMovementAndWaitForStandstill(gb))				// wait until movement has finished and deferred command queue has caught up
 				{
 					StopPrint(StopPrintReason::normalCompletion);
 				}
@@ -784,9 +783,7 @@ bool GCodes::DoFilePrint(GCodeBuffer& gb, const StringRef& reply) noexcept
 				// We never get here if the file ends in M0 because CancelPrint gets called directly in that case.
 				// Don't close the file until all moves have been completed, in case the print gets paused.
 				// Also, this keeps the state as 'Printing' until the print really has finished.
-				if (   LockMovementAndWaitForStandstill(gb)					// wait until movement has finished
-					&& IsCodeQueueIdle()									// must also wait until deferred command queue has caught up
-				   )
+				if (LockMovementAndWaitForStandstill(gb))					// wait until movement has finished and deferred command queue has caught up
 				{
 					StopPrint(StopPrintReason::normalCompletion);
 				}
@@ -1495,7 +1492,7 @@ void GCodes::SaveResumeInfo(bool wasPowerFailure) noexcept
 				buf.catf("\nG0 F6000 Z%.3f\n", (double)pauseRestorePoint.moveCoords[Z_AXIS]);
 
 				// Set the feed rate
-				buf.catf("G1 F%.1f", (double)(pauseRestorePoint.feedRate * MinutesToSeconds));
+				buf.catf("G1 F%.1f", (double)InverseConvertSpeedToMmPerMin(pauseRestorePoint.feedRate));
 #if SUPPORT_LASER
 				if (machineType == MachineType::laser)
 				{
@@ -1577,7 +1574,12 @@ bool GCodes::LockMovementAndWaitForStandstill(GCodeBuffer& gb) noexcept
 		return false;
 	}
 
-	gb.MotionStopped();								// must do this after we have finished waiting, so that we don't stop waiting when executing G4
+	if (&gb != queuedGCode && !IsCodeQueueIdle())		// wait for deferred command queue to catch up
+	{
+		return false;
+	}
+
+	gb.MotionStopped();									// must do this after we have finished waiting, so that we don't stop waiting when executing G4
 
 	if (RTOSIface::GetCurrentTask() == Tasks::GetMainTask())
 	{
@@ -1628,7 +1630,7 @@ const char * GCodes::LoadExtrusionAndFeedrateFromGCode(GCodeBuffer& gb, bool isP
 		moveBuffer.applyM220M221 = (moveBuffer.moveType == 0 && isPrintingMove && !gb.IsDoingFileMacro());
 		if (gb.Seen(feedrateLetter))
 		{
-			gb.LatestMachineState().feedRate = gb.GetDistance() * SecondsToMinutes;	// update requested speed, not allowing for speed factor
+			gb.LatestMachineState().feedRate = gb.GetSpeed();				// update requested speed, not allowing for speed factor
 		}
 		moveBuffer.feedRate = (moveBuffer.applyM220M221)
 								? speedFactor * gb.LatestMachineState().feedRate
@@ -1638,7 +1640,7 @@ const char * GCodes::LoadExtrusionAndFeedrateFromGCode(GCodeBuffer& gb, bool isP
 	else
 	{
 		moveBuffer.applyM220M221 = false;
-		moveBuffer.feedRate = DefaultG0FeedRate;					// use maximum feed rate, the M203 parameters will limit it
+		moveBuffer.feedRate = ConvertSpeedFromMmPerMin(DefaultG0FeedRate);	// use maximum feed rate, the M203 parameters will limit it
 		moveBuffer.usingStandardFeedrate = false;
 	}
 
@@ -1648,11 +1650,11 @@ const char * GCodes::LoadExtrusionAndFeedrateFromGCode(GCodeBuffer& gb, bool isP
 		moveBuffer.coords[drive] = 0.0;
 	}
 	moveBuffer.hasPositiveExtrusion = false;
-	moveBuffer.virtualExtruderPosition = virtualExtruderPosition;	// save this before we update it
+	moveBuffer.virtualExtruderPosition = virtualExtruderPosition;			// save this before we update it
 	ExtrudersBitmap extrudersMoving;
 
 	// Check if we are extruding
-	if (gb.Seen(extrudeLetter))							// DC 2018-08-07: at E3D's request, extrusion is now recognised even on uncoordinated moves
+	if (gb.Seen(extrudeLetter))												// DC 2018-08-07: at E3D's request, extrusion is now recognised even on uncoordinated moves
 	{
 		// Check that we have a tool to extrude with
 		Tool* const tool = reprap.GetCurrentTool();
@@ -2116,7 +2118,7 @@ bool GCodes::DoStraightMove(GCodeBuffer& gb, bool isCoordinated, const char *& e
 				moveLengthSquared += fsquare(currentUserPosition[Z_AXIS] - initialUserPosition[Z_AXIS]);
 			}
 			const float moveLength = fastSqrtf(moveLengthSquared);
-			const float moveTime = moveLength/moveBuffer.feedRate;			// this is a best-case time, often the move will take longer
+			const float moveTime = moveLength/(moveBuffer.feedRate * StepClockRate);		// this is a best-case time, often the move will take longer
 			moveBuffer.totalSegments = (unsigned int)max<long>(1, lrintf(min<float>(moveLength * kin.GetReciprocalMinSegmentLength(), moveTime * kin.GetSegmentsPerSecond())));
 		}
 		else
@@ -2466,7 +2468,7 @@ bool GCodes::DoArcMove(GCodeBuffer& gb, bool clockwise, const char *& err)
 	// We leave out the square term because it is very small
 	// In CNC applications even very small deviations can be visible, so we use a smaller segment length at low speeds
 	const float arcSegmentLength = constrain<float>
-									(	min<float>(fastSqrtf(8 * moveBuffer.arcRadius * MaxArcDeviation), moveBuffer.feedRate * (1.0/MinArcSegmentsPerSec)),
+									(	min<float>(fastSqrtf(8 * moveBuffer.arcRadius * MaxArcDeviation), moveBuffer.feedRate * StepClockRate * (1.0/MinArcSegmentsPerSec)),
 										MinArcSegmentLength,
 										MaxArcSegmentLength
 									);
@@ -2814,10 +2816,32 @@ bool GCodes::DoFileMacro(GCodeBuffer& gb, const char* fileName, bool reportMissi
 
 #if HAS_LINUX_INTERFACE || HAS_MASS_STORAGE
 	gb.LatestMachineState().doingFileMacro = true;
-	gb.LatestMachineState().runningM501 = (codeRunning == 501);
-	gb.LatestMachineState().runningM502 = (codeRunning == 502);
-	gb.LatestMachineState().runningSystemMacro = (codeRunning == SystemHelperMacroCode || codeRunning == AsyncSystemMacroCode || codeRunning == 29 || codeRunning == 32);
-																		// running a system macro e.g. homing, so don't use workplace coordinates
+
+	// The following three flags need to be inherited in the case that a system macro calls another macro, e.g.homeall.g calls homez.g. The Push call copied them over already.
+	switch (codeRunning)
+	{
+	case 501:
+		gb.LatestMachineState().runningM501 = true;
+		gb.LatestMachineState().runningSystemMacro = true;			// running a system macro e.g. homing, so don't use workplace coordinates
+		break;
+
+	case 502:
+		gb.LatestMachineState().runningM502 = true;
+		gb.LatestMachineState().runningSystemMacro = true;			// running a system macro e.g. homing, so don't use workplace coordinates
+		break;
+
+	case SystemHelperMacroCode:
+	case AsyncSystemMacroCode:
+	case ToolChangeMacroCode:
+	case 29:
+	case 32:
+		gb.LatestMachineState().runningSystemMacro = true;			// running a system macro e.g. homing, so don't use workplace coordinates
+		break;
+
+	default:
+		break;
+	}
+
 	gb.SetState(GCodeState::normal);
 	gb.Init();
 
diff --git a/src/GCodes/GCodes2.cpp b/src/GCodes/GCodes2.cpp
index d5dc4af4..ee50c201 100644
--- a/src/GCodes/GCodes2.cpp
+++ b/src/GCodes/GCodes2.cpp
@@ -267,12 +267,12 @@ bool GCodes::HandleGcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx
 			break;
 
 		case 20: // Inches (which century are we living in, here?)
-			gb.LatestMachineState().usingInches = true;
+			gb.UseInches(true);
 			reprap.InputsUpdated();
 			break;
 
 		case 21: // mm
-			gb.LatestMachineState().usingInches = false;
+			gb.UseInches(false);
 			reprap.InputsUpdated();
 			break;
 
@@ -515,9 +515,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx
 					reply.copy("Pause the print before attempting to cancel it");
 					result = GCodeResult::error;
 				}
-				else if (   !LockMovementAndWaitForStandstill(gb)	// wait until everything has stopped
-						 || !IsCodeQueueIdle()						// must also wait until deferred command queue has caught up
-						)
+				else if (!LockMovementAndWaitForStandstill(gb))	// wait until everything has stopped and deferred command queue has caught up
 				{
 					return false;
 				}
@@ -1464,9 +1462,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx
 			case 109: // Deprecated in RRF, but widely generated by slicers
 				{
 					const bool movementWasLocked = gb.LatestMachineState().lockedResources.IsBitSet(MoveResource);
-					if (   !LockMovementAndWaitForStandstill(gb)		// wait until movement has finished
-						|| !IsCodeQueueIdle()							// also wait until deferred command queue has caught up to avoid out-of-order execution
-					   )
+					if (!LockMovementAndWaitForStandstill(gb))		// wait until movement has finished and deferred command queue has caught up to avoid out-of-order execution
 					{
 						return false;
 					}
@@ -1677,9 +1673,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx
 				break;
 
 			case 116: // Wait for set temperatures
-				if (   !LockMovementAndWaitForStandstill(gb)		// wait until movement has finished
-					|| !IsCodeQueueIdle()							// also wait until deferred command queue has caught up to avoid out-of-order execution
-				   )
+				if (!LockMovementAndWaitForStandstill(gb))		// wait until movement has finished and deferred command queue has caught up to avoid out-of-order execution
 				{
 					return false;
 				}
@@ -2021,9 +2015,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx
 
 			case 190: // Set bed temperature and wait
 			case 191: // Set chamber temperature and wait
-				if (   !LockMovementAndWaitForStandstill(gb)		// wait until movement has finished
-					|| !IsCodeQueueIdle()							// also wait until deferred command queue has caught up to avoid out-of-order execution
-				   )
+				if (!LockMovementAndWaitForStandstill(gb))			// wait until movement has finished and deferred command queue has caught up to avoid out-of-order execution
 				{
 					return false;
 				}
@@ -2109,7 +2101,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx
 					{
 						if (gb.Seen(axisLetters[axis]))
 						{
-							platform.SetAcceleration(axis, gb.GetDistance());
+							platform.SetAcceleration(axis, gb.GetAcceleration());
 							seen = true;
 						}
 					}
@@ -2122,7 +2114,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx
 						gb.GetFloatArray(eVals, eCount, true);
 						for (size_t e = 0; e < eCount; e++)
 						{
-							platform.SetAcceleration(ExtruderToLogicalDrive(e), gb.ConvertDistance(eVals[e]));
+							platform.SetAcceleration(ExtruderToLogicalDrive(e), ConvertAcceleration(eVals[e]));
 						}
 					}
 
@@ -2132,16 +2124,16 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx
 					}
 					else
 					{
-						reply.printf("Accelerations (mm/sec^2): ");
+						reply.copy("Accelerations (mm/sec^2): ");
 						for (size_t axis = 0; axis < numTotalAxes; ++axis)
 						{
-							reply.catf("%c: %.1f, ", axisLetters[axis], (double)platform.Acceleration(axis));
+							reply.catf("%c: %.1f, ", axisLetters[axis], (double)InverseConvertAcceleration(platform.Acceleration(axis)));
 						}
 						reply.cat("E:");
 						char sep = ' ';
 						for (size_t extruder = 0; extruder < numExtruders; extruder++)
 						{
-							reply.catf("%c%.1f", sep, (double)platform.Acceleration(ExtruderToLogicalDrive(extruder)));
+							reply.catf("%c%.1f", sep, (double)InverseConvertAcceleration(platform.Acceleration(ExtruderToLogicalDrive(extruder))));
 							sep = ':';
 						}
 					}
@@ -2152,14 +2144,13 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx
 				{
 					// Units are mm/sec if S1 is given, else mm/min
 					const bool usingMmPerSec = (gb.Seen('S') && gb.GetIValue() == 1);
-					const float settingMultiplier = (usingMmPerSec) ? 1.0 : SecondsToMinutes;
 					bool seen = false;
 
 					// Do the minimum first, because we constrain the maximum rates to be no lower than it
 					if (gb.Seen('I'))
 					{
 						seen = true;
-						platform.SetMinMovementSpeed(gb.GetDistance() * settingMultiplier);
+						platform.SetMinMovementSpeed(gb.GetSpeedFromMm(usingMmPerSec));
 					}
 
 					for (size_t axis = 0; axis < numTotalAxes; ++axis)
@@ -2167,7 +2158,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx
 						if (gb.Seen(axisLetters[axis]))
 						{
 							seen = true;
-							platform.SetMaxFeedrate(axis, gb.GetDistance() * settingMultiplier);
+							platform.SetMaxFeedrate(axis, gb.GetSpeedFromMm(usingMmPerSec));
 						}
 					}
 
@@ -2179,7 +2170,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx
 						gb.GetFloatArray(eVals, eCount, true);
 						for (size_t e = 0; e < eCount; e++)
 						{
-							platform.SetMaxFeedrate(ExtruderToLogicalDrive(e), gb.ConvertDistance(eVals[e]) * settingMultiplier);
+							platform.SetMaxFeedrate(ExtruderToLogicalDrive(e), ConvertSpeedFromMm(eVals[e], usingMmPerSec));
 						}
 					}
 
@@ -2189,20 +2180,19 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx
 					}
 					else
 					{
-						const float reportingMultiplier = (usingMmPerSec) ? 1.0 : MinutesToSeconds;
-						reply.printf("Max speeds (mm/%s): ", (usingMmPerSec) ? "sec" : "min");
+						reply.printf("Max speeds (%s)): ", (usingMmPerSec) ? "mm/sec" : "mm/min");
 						for (size_t axis = 0; axis < numTotalAxes; ++axis)
 						{
-							reply.catf("%c: %.1f, ", axisLetters[axis], (double)(platform.MaxFeedrate(axis) * reportingMultiplier));
+							reply.catf("%c: %.1f, ", axisLetters[axis], (double)InverseConvertSpeedToMm(platform.MaxFeedrate(axis), usingMmPerSec));
 						}
 						reply.cat("E:");
 						char sep = ' ';
 						for (size_t extruder = 0; extruder < numExtruders; extruder++)
 						{
-							reply.catf("%c%.1f", sep, (double)(platform.MaxFeedrate(ExtruderToLogicalDrive(extruder)) * reportingMultiplier));
+							reply.catf("%c%.1f", sep, (double)InverseConvertSpeedToMm(platform.MaxFeedrate(ExtruderToLogicalDrive(extruder)), usingMmPerSec));
 							sep = ':';
 						}
-						reply.catf(", min. speed %.2f", (double)(platform.MinMovementSpeed() * reportingMultiplier));
+						reply.catf(", min. speed %.2f", (double)InverseConvertSpeedToMm(platform.MinMovementSpeed(), usingMmPerSec));
 					}
 				}
 				break;
@@ -2287,7 +2277,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx
 
 					if (!seen)
 					{
-						reply.copy("Axis limit");
+						reply.copy("Axis limits (mm)");
 						char sep = 's';
 						for (size_t axis = 0; axis < numTotalAxes; axis++)
 						{
@@ -2456,7 +2446,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx
 						{
 							// The pipeline is empty, so execute the babystepping move immediately
 							SetMoveBufferDefaults();
-							moveBuffer.feedRate = DefaultFeedRate;
+							moveBuffer.feedRate = ConvertSpeedFromMmPerMin(DefaultFeedRate);
 							moveBuffer.tool = reprap.GetCurrentTool();
 							NewMoveAvailable(1);
 						}
@@ -3320,13 +3310,13 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx
 			case 205: // Set/print maximum jerk speeds in mm/sec
 			case 566: // Set/print maximum jerk speeds in mm/min
 				{
-					const float multiplier1 = (code == 566) ? SecondsToMinutes : 1.0;
+					const bool useMmPerSec = (code == 205);
 					bool seenAxis = false, seenExtruder = false;
 					for (size_t axis = 0; axis < numTotalAxes; axis++)
 					{
 						if (gb.Seen(axisLetters[axis]))
 						{
-							platform.SetInstantDv(axis, gb.GetDistance() * multiplier1);
+							platform.SetInstantDv(axis, gb.GetSpeedFromMm(useMmPerSec));
 							seenAxis = true;
 						}
 					}
@@ -3339,7 +3329,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx
 						gb.GetFloatArray(eVals, eCount, true);
 						for (size_t e = 0; e < eCount; e++)
 						{
-							platform.SetInstantDv(ExtruderToLogicalDrive(e), eVals[e] * multiplier1);
+							platform.SetInstantDv(ExtruderToLogicalDrive(e), ConvertSpeedFromMm(eVals[e], useMmPerSec));
 						}
 					}
 
@@ -3355,17 +3345,16 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx
 					}
 					else if (!seenExtruder)
 					{
-						const float multiplier2 = (code == 566) ? MinutesToSeconds : 1.0;
-						reply.printf("Maximum jerk rates (mm/%s): ", (code == 566) ? "min" : "sec");
+						reply.printf("Maximum jerk rates (%s): ", (useMmPerSec) ? "mm/sec" : "mm/min");
 						for (size_t axis = 0; axis < numTotalAxes; ++axis)
 						{
-							reply.catf("%c: %.1f, ", axisLetters[axis], (double)(platform.GetInstantDv(axis) * multiplier2));
+							reply.catf("%c: %.1f, ", axisLetters[axis], (double)InverseConvertSpeedToMm(platform.GetInstantDv(axis), useMmPerSec));
 						}
 						reply.cat("E:");
 						char sep = ' ';
 						for (size_t extruder = 0; extruder < numExtruders; extruder++)
 						{
-							reply.catf("%c%.1f", sep, (double)(platform.GetInstantDv(ExtruderToLogicalDrive(extruder)) * multiplier2));
+							reply.catf("%c%.1f", sep, (double)InverseConvertSpeedToMm(platform.GetInstantDv(ExtruderToLogicalDrive(extruder)), useMmPerSec));
 							sep = ':';
 						}
 						if (code == 566)
@@ -3459,25 +3448,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx
 				break;
 
 			case 572: // Set/report pressure advance
-				if (gb.Seen('S'))
-				{
-					const float advance = gb.GetFValue();
-					if (!LockMovementAndWaitForStandstill(gb))
-					{
-						return false;
-					}
-					result = platform.SetPressureAdvance(advance, gb, reply);
-				}
-				else
-				{
-					reply.copy("Extruder pressure advance");
-					char c = ':';
-					for (size_t i = 0; i < numExtruders; ++i)
-					{
-						reply.catf("%c %.3f", c, (double)platform.GetPressureAdvance(i));
-						c = ',';
-					}
-				}
+				result = reprap.GetMove().ConfigurePressureAdvance(gb, reply);
 				break;
 
 			case 573: // Report heater average PWM
@@ -3735,11 +3706,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx
 #endif
 
 			case 593: // Configure dynamic ringing cancellation
-				if (!LockMovementAndWaitForStandstill(gb))
-				{
-					return false;
-				}
-				result = reprap.GetMove().GetShaper().Configure(gb, reply);
+				result = reprap.GetMove().GetAxisShaper().Configure(gb, reply);
 				break;
 
 #if SUPPORT_ASYNC_MOVES
@@ -4022,8 +3989,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx
 						// Get the feedrate (if any) and kick off a new move
 						if (gb.Seen(feedrateLetter))
 						{
-							const float rate = gb.ConvertDistance(gb.GetFValue());
-							gb.LatestMachineState().feedRate = rate * SecondsToMinutes;		// don't apply the speed factor
+							gb.LatestMachineState().feedRate = gb.GetSpeed();		// don't apply the speed factor
 						}
 						moveBuffer.feedRate = gb.LatestMachineState().feedRate;
 						moveBuffer.usingStandardFeedrate = true;
@@ -4660,7 +4626,7 @@ bool GCodes::HandleTcode(GCodeBuffer& gb, const StringRef& reply)
 	}
 	else if (gb.Seen('T'))
 	{
-		// We handle "T{expression}" as if it's "T "{expression}, also DSF may pass a T{expression} command in this way
+		// We handle "T T{expression}" as if it's "T "{expression}, also DSF may pass a T{expression} command in this way
 		seen = true;
 		toolNum = gb.GetIValue();
 	}
@@ -4682,7 +4648,7 @@ bool GCodes::HandleTcode(GCodeBuffer& gb, const StringRef& reply)
 
 	if (seen)
 	{
-		if (!LockMovementAndWaitForStandstill(gb) || !IsCodeQueueIdle())
+		if (!LockMovementAndWaitForStandstill(gb))
 		{
 			return false;
 		}
diff --git a/src/GCodes/GCodes3.cpp b/src/GCodes/GCodes3.cpp
index 5bed126e..42388844 100644
--- a/src/GCodes/GCodes3.cpp
+++ b/src/GCodes/GCodes3.cpp
@@ -909,7 +909,7 @@ GCodeResult GCodes::ProbeTool(GCodeBuffer& gb, const StringRef& reply) THROWS(GC
 
 	// Get the feed rate and axis
 	gb.MustSee(feedrateLetter);
-	m585Settings.feedRate = gb.LatestMachineState().feedRate = gb.GetDistance() * SecondsToMinutes;		// don't apply the speed factor to homing and other special moves
+	m585Settings.feedRate = gb.LatestMachineState().feedRate = gb.GetSpeed();		// don't apply the speed factor to homing and other special moves
 	m585Settings.axisNumber = FindAxisLetter(gb);
 	m585Settings.offset = gb.GetDistance();
 
@@ -1000,7 +1000,7 @@ GCodeResult GCodes::FindCenterOfCavity(GCodeBuffer& gb, const StringRef& reply)
 
 	// Get the feed rate, backoff distance, and axis
 	gb.MustSee(feedrateLetter);
-	m675Settings.feedRate = gb.LatestMachineState().feedRate = gb.GetDistance() * SecondsToMinutes;		// don't apply the speed factor to homing and other special moves
+	m675Settings.feedRate = gb.LatestMachineState().feedRate = gb.GetSpeed();		// don't apply the speed factor to homing and other special moves
 	m675Settings.backoffDistance = gb.Seen('R') ? gb.GetDistance() : 5.0;
 	m675Settings.axisNumber = FindAxisLetter(gb);
 
@@ -1378,22 +1378,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());
 		}
diff --git a/src/GCodes/GCodes4.cpp b/src/GCodes/GCodes4.cpp
index 34a28473..ac0961cc 100644
--- a/src/GCodes/GCodes4.cpp
+++ b/src/GCodes/GCodes4.cpp
@@ -384,7 +384,7 @@ void GCodes::RunStateMachine(GCodeBuffer& gb, const StringRef& reply) noexcept
 			if (currentTool != nullptr)
 			{
 				const float newZPos = (moveBuffer.coords[Z_AXIS] - currentTool->GetOffset(Z_AXIS));
-				if(newZPos > platform.AxisMaximum(Z_AXIS) || newZPos < platform.AxisMinimum(Z_AXIS))
+				if (newZPos > platform.AxisMaximum(Z_AXIS) || newZPos < platform.AxisMinimum(Z_AXIS))
 				{
 					gb.LatestMachineState().feedRate = toolChangeRestorePoint.feedRate;
 					doingToolChange = false;
@@ -394,13 +394,13 @@ void GCodes::RunStateMachine(GCodeBuffer& gb, const StringRef& reply) noexcept
 					break;
 				}
 			}
+
 			// Restore the original Z axis user position, so that different tool Z offsets work even if the first move after the tool change doesn't have a Z coordinate
-			// Only do this if we are running as an FDM printer, because it's not appropriate for CNC machines.
 			SetMoveBufferDefaults();
 			currentUserPosition[Z_AXIS] = toolChangeRestorePoint.moveCoords[Z_AXIS];
 			ToolOffsetTransform(currentUserPosition, moveBuffer.coords);
-			moveBuffer.feedRate = DefaultFeedRate * SecondsToMinutes;	// ask for a good feed rate, we may have paused during a slow move
-			moveBuffer.tool = reprap.GetCurrentTool();					// needed so that bed compensation is applied correctly
+			moveBuffer.feedRate = ConvertSpeedFromMmPerMin(DefaultFeedRate);	// ask for a good feed rate, we may have paused during a slow move
+			moveBuffer.tool = reprap.GetCurrentTool();							// needed so that bed compensation is applied correctly
 			NewMoveAvailable(1);
 			gb.AdvanceState();
 		}
@@ -408,7 +408,7 @@ void GCodes::RunStateMachine(GCodeBuffer& gb, const StringRef& reply) noexcept
 
 	case GCodeState::toolChangeComplete:
 	case GCodeState::m109ToolChangeComplete:
-		if (LockMovementAndWaitForStandstill(gb))		// wait for tpost.g to finish executing or the move to height to finish
+		if (LockMovementAndWaitForStandstill(gb))		// wait for the move to height to finish
 		{
 			gb.LatestMachineState().feedRate = toolChangeRestorePoint.feedRate;
 			// We don't restore the default fan speed in case the user wants to use a different one for the new tool
@@ -511,8 +511,8 @@ void GCodes::RunStateMachine(GCodeBuffer& gb, const StringRef& reply) noexcept
 			}
 			SetMoveBufferDefaults();
 			ToolOffsetTransform(currentUserPosition, moveBuffer.coords);
-			moveBuffer.feedRate = DefaultFeedRate * SecondsToMinutes;	// ask for a good feed rate, we may have paused during a slow move
-			moveBuffer.tool = reprap.GetCurrentTool();					// needed so that bed compensation is applied correctly
+			moveBuffer.feedRate = ConvertSpeedFromMmPerMin(DefaultFeedRate);	// ask for a good feed rate, we may have paused during a slow move
+			moveBuffer.tool = reprap.GetCurrentTool();							// needed so that bed compensation is applied correctly
 			if (gb.GetState() == GCodeState::resuming1 && currentZ > pauseRestorePoint.moveCoords[Z_AXIS])
 			{
 				// First move the head to the correct XY point, then move it down in a separate move
@@ -640,14 +640,14 @@ void GCodes::RunStateMachine(GCodeBuffer& gb, const StringRef& reply) noexcept
 				axes.SetBit(axis0Num);
 				axes.SetBit(axis1Num);
 				float axesCoords[MaxAxes];
-				axesCoords[axis0Num] = axis0Coord;
-				axesCoords[axis1Num] = axis1Coord;
+				const auto zp = platform.GetZProbeOrDefault(currentZProbeNumber);
+				axesCoords[axis0Num] = axis0Coord - zp->GetOffset(axis0Num);
+				axesCoords[axis1Num] = axis1Coord - zp->GetOffset(axis1Num);
 				if (move.IsAccessibleProbePoint(axesCoords, axes))
 				{
 					SetMoveBufferDefaults();
-					const auto zp = platform.GetZProbeOrDefault(currentZProbeNumber);
-					moveBuffer.coords[axis0Num] = axis0Coord - zp->GetOffset(axis0Num);
-					moveBuffer.coords[axis1Num] = axis1Coord - zp->GetOffset(axis1Num);
+					moveBuffer.coords[axis0Num] = axesCoords[axis0Num];
+					moveBuffer.coords[axis1Num] = axesCoords[axis1Num];
 					moveBuffer.coords[Z_AXIS] = zp->GetStartingHeight();
 					moveBuffer.feedRate = zp->GetTravelSpeed();
 					NewMoveAvailable(1);
diff --git a/src/GCodes/RestorePoint.cpp b/src/GCodes/RestorePoint.cpp
index edbfbf71..f197b25d 100644
--- a/src/GCodes/RestorePoint.cpp
+++ b/src/GCodes/RestorePoint.cpp
@@ -34,7 +34,7 @@ constexpr ObjectModelTableEntry RestorePoint::objectModelTable[] =
 	{ "coords", 			OBJECT_MODEL_FUNC_NOSELF(&coordinatesArrayDescriptor), 								ObjectModelEntryFlags::none },
 	{ "extruderPos",		OBJECT_MODEL_FUNC(self->virtualExtruderPosition, 1),	 							ObjectModelEntryFlags::none },
 	{ "fanPwm", 			OBJECT_MODEL_FUNC(self->fanSpeed, 2), 												ObjectModelEntryFlags::none },
-	{ "feedRate", 			OBJECT_MODEL_FUNC(self->feedRate, 1), 												ObjectModelEntryFlags::none },
+	{ "feedRate", 			OBJECT_MODEL_FUNC(InverseConvertSpeedToMmPerSec(self->feedRate), 1), 				ObjectModelEntryFlags::none },
 #if SUPPORT_IOBITS
 	{ "ioBits",				OBJECT_MODEL_FUNC_IF(reprap.GetGCodes().GetMachineType() != MachineType::laser,
 													(int32_t)self->laserPwmOrIoBits.ioBits),					ObjectModelEntryFlags::none },
@@ -64,7 +64,7 @@ void RestorePoint::Init() noexcept
 		moveCoords[i] = 0.0;
 	}
 
-	feedRate = DefaultFeedRate * SecondsToMinutes;
+	feedRate = ConvertSpeedFromMmPerMin(DefaultFeedRate);
 	virtualExtruderPosition = 0.0;
 	filePos = noFilePosition;
 	proportionDone = 0.0;