Merge remote-tracking branch 'origin/3.3-dev' into v3-chrishamm

53 files changed, 752 insertions, 522 deletions

diff --git a/src/Accelerometers/Accelerometers.cpp b/src/Accelerometers/Accelerometers.cpp
index 5ad0ef69..43128ebd 100644
--- a/src/Accelerometers/Accelerometers.cpp
+++ b/src/Accelerometers/Accelerometers.cpp
@@ -423,7 +423,7 @@ GCodeResult Accelerometers::ConfigureAccelerometer(GCodeBuffer& gb, const String
 			return GCodeResult::error;
 		}
 
-		const uint32_t spiFrequency = (gb.Seen('Q')) ? gb.GetUIValue() : DefaultAccelerometerSpiFrequency;
+		const uint32_t spiFrequency = (gb.Seen('Q')) ? gb.GetLimitedUIValue('Q', 500000, 10000001) : DefaultAccelerometerSpiFrequency;
 		temp = new LIS3DH(SharedSpiDevice::GetMainSharedSpiDevice(), spiFrequency, spiCsPort.GetPin(), irqPort.GetPin());
 		if (temp->CheckPresent())
 		{
diff --git a/src/Accelerometers/LIS3DH.cpp b/src/Accelerometers/LIS3DH.cpp
index d4e22502..121ddb1c 100644
--- a/src/Accelerometers/LIS3DH.cpp
+++ b/src/Accelerometers/LIS3DH.cpp
@@ -160,8 +160,10 @@ unsigned int LIS3DH::CollectData(const uint16_t **collectedData, uint16_t &dataR
 
 		*collectedData = reinterpret_cast<const uint16_t*>(dataBuffer);
 		overflowed = (fifoStatus & 0x40) != 0;
-		dataRate = (totalNumRead == 0) ? 0
-					: (totalNumRead * StepTimer::StepClockRate)/(lastInterruptTime - firstInterruptTime);
+		const uint32_t interval = lastInterruptTime - firstInterruptTime;
+		dataRate = (totalNumRead == 0 || interval == 0)
+					? 0
+					: (totalNumRead * StepTimer::StepClockRate)/interval;
 		totalNumRead += numToRead;
 	}
 	return numToRead;
diff --git a/src/Fans/LedStripDriver.cpp b/src/Fans/LedStripDriver.cpp
index 4ec1356c..0774ed96 100644
--- a/src/Fans/LedStripDriver.cpp
+++ b/src/Fans/LedStripDriver.cpp
@@ -477,7 +477,7 @@ void LedStripDriver::Init() noexcept
 // This function handles M150
 // For DotStar LEDs:
 // 	We can handle an unlimited length LED strip, because we can send the data in multiple chunks.
-//	So whenever we receive a m150 command, we send the data immediately, in multiple chunks if our DMA buffer is too small to send it as a single chunk.
+//	So whenever we receive a M150 command, we send the data immediately, in multiple chunks if our DMA buffer is too small to send it as a single chunk.
 //	To send multiple chunks, we process the command once per chunk, using numRemaining to keep track of how many more LEDs need to be written to
 // For NeoPixel LEDs:
 //	If there is a gap or more then about 9us in transmission, the string will reset and the next command will be taken as applying to the start of the strip.
diff --git a/src/FilamentMonitors/LaserFilamentMonitor.cpp b/src/FilamentMonitors/LaserFilamentMonitor.cpp
index 206dcae9..44fc2c1d 100644
--- a/src/FilamentMonitors/LaserFilamentMonitor.cpp
+++ b/src/FilamentMonitors/LaserFilamentMonitor.cpp
@@ -157,6 +157,10 @@ GCodeResult LaserFilamentMonitor::Configure(GCodeBuffer& gb, const StringRef& re
 			else
 			{
 				reply.catf("version %u, ", version);
+				if (switchOpenMask != 0)
+				{
+					reply.cat(((sensorValue & switchOpenMask) != 0) ? "no filament, " : "filament present, ");
+				}
 				if (imageQuality != 0)
 				{
 					reply.catf("quality %u, ", imageQuality);
diff --git a/src/FilamentMonitors/RotatingMagnetFilamentMonitor.cpp b/src/FilamentMonitors/RotatingMagnetFilamentMonitor.cpp
index 2c5b99f5..f20a43e2 100644
--- a/src/FilamentMonitors/RotatingMagnetFilamentMonitor.cpp
+++ b/src/FilamentMonitors/RotatingMagnetFilamentMonitor.cpp
@@ -159,6 +159,10 @@ GCodeResult RotatingMagnetFilamentMonitor::Configure(GCodeBuffer& gb, const Stri
 			else
 			{
 				reply.catf("version %u, ", version);
+				if (switchOpenMask != 0)
+				{
+					reply.cat(((sensorValue & switchOpenMask) != 0) ? "no filament, " : "filament present, ");
+				}
 				if (version >= 3)
 				{
 					reply.catf("mag %u agc %u, ", magnitude, agc);
diff --git a/src/GCodes/GCodeBuffer/BinaryParser.cpp b/src/GCodes/GCodeBuffer/BinaryParser.cpp
index 1e333b9e..f929f462 100644
--- a/src/GCodes/GCodeBuffer/BinaryParser.cpp
+++ b/src/GCodes/GCodeBuffer/BinaryParser.cpp
@@ -754,7 +754,7 @@ void BinaryParser::WriteParameters(const StringRef& s, bool quoteStrings) const
 	}
 }
 
-void BinaryParser::SetParameters(VariableSet& vs, int codeRunning) noexcept
+void BinaryParser::AddParameters(VariableSet& vs, int codeRunning) noexcept
 {
 	if (bufferLength != 0 && header->numParameters != 0)
 	{
@@ -773,7 +773,7 @@ void BinaryParser::SetParameters(VariableSet& vs, int codeRunning) noexcept
 				{
 				case DataType::String:
 					{
-						StringHandle sh(seenParameterValue, seenParameter->intValue);
+						StringHandle sh(seenParameterValue, param->intValue);
 						ev.Set(sh);
 					}
 					break;
@@ -781,22 +781,22 @@ void BinaryParser::SetParameters(VariableSet& vs, int codeRunning) noexcept
 				case DataType::Expression:
 					try
 					{
-						ExpressionParser parser(gb, seenParameterValue, seenParameterValue + seenParameter->intValue, -1);
-						ev.Set(parser.ParseFloat());
+						ExpressionParser parser(gb, seenParameterValue, seenParameterValue + param->intValue, -1);
+						ev = parser.Parse();
 					}
-					catch (const GCodeException&) { }
+					catch (const GCodeException&) { }			// TODO error handling
 					break;
 
 				case DataType::Float:
-					ev.Set(seenParameter->floatValue);
+					ev.Set(param->floatValue);
 					break;
 
 				case DataType::Int:
-					ev.Set(seenParameter->intValue);
+					ev.Set(param->intValue);
 					break;
 
 				case DataType::UInt:
-					ev.Set((int32_t)seenParameter->uintValue);
+					ev.Set((int32_t)param->uintValue);
 					break;
 
 				default:
@@ -824,7 +824,6 @@ void BinaryParser::SetParameters(VariableSet& vs, int codeRunning) noexcept
 			}
 		}
 	}
-	//TODO
 }
 
 GCodeException BinaryParser::ConstructParseException(const char *str) const noexcept
diff --git a/src/GCodes/GCodeBuffer/BinaryParser.h b/src/GCodes/GCodeBuffer/BinaryParser.h
index 19615473..18f0c675 100644
--- a/src/GCodes/GCodeBuffer/BinaryParser.h
+++ b/src/GCodes/GCodeBuffer/BinaryParser.h
@@ -59,7 +59,7 @@ public:
 
 	void PrintCommand(const StringRef& s) const noexcept;
 	void AppendFullCommand(const StringRef &s) const noexcept;
-	void SetParameters(VariableSet& vs, int codeRunning) noexcept;
+	void AddParameters(VariableSet& vs, int codeRunning) noexcept;
 
 private:
 	GCodeBuffer& gb;
diff --git a/src/GCodes/GCodeBuffer/ExpressionParser.cpp b/src/GCodes/GCodeBuffer/ExpressionParser.cpp
index 3575fbb7..2a536da1 100644
--- a/src/GCodes/GCodeBuffer/ExpressionParser.cpp
+++ b/src/GCodes/GCodeBuffer/ExpressionParser.cpp
@@ -12,6 +12,7 @@
 #include <Platform/Platform.h>
 #include <General/NamedEnum.h>
 #include <General/NumericConverter.h>
+#include <Hardware/ExceptionHandlers.h>
 
 #include <limits>
 
@@ -21,6 +22,14 @@
 
 constexpr size_t MaxStringExpressionLength = StringLength100;
 
+namespace StackUsage
+{
+	// The following values are obtained from file ExpressionParser.su generated by the compiler
+	constexpr uint32_t ParseInternal = 72;
+	constexpr uint32_t ParseIdentifierExpression = 256;
+	constexpr uint32_t GetObjectValue_withTable = 48;
+}
+
 // These can't be declared locally inside ParseIdentifierExpression because NamedEnum includes static data
 NamedEnum(NamedConstant, unsigned int, _false, iterations, line, _null, pi, _result, _true);
 NamedEnum(Function, unsigned int, abs, acos, asin, atan, atan2, cos, degrees, exists, floor, isnan, max, min, mod, radians, random, sin, sqrt, tan);
@@ -33,22 +42,23 @@ ExpressionParser::ExpressionParser(const GCodeBuffer& p_gb, const char *text, co
 }
 
 // Evaluate a bracketed expression
-ExpressionValue ExpressionParser::ParseExpectKet(bool evaluate, char closingBracket) THROWS(GCodeException)
+void ExpressionParser::ParseExpectKet(ExpressionValue& rslt, bool evaluate, char closingBracket) THROWS(GCodeException)
 {
-	auto rslt = Parse(evaluate);
+	CheckStack(StackUsage::ParseInternal);
+	ParseInternal(rslt, evaluate, 0);
 	if (CurrentCharacter() != closingBracket)
 	{
-		throw ConstructParseException("expected '%c'", (uint32_t)closingBracket);
+		ThrowParseException("expected '%c'", (uint32_t)closingBracket);
 	}
 	AdvancePointer();
-	return rslt;
 }
 
-// Evaluate an expression
+// Evaluate an expression. Do not call this one recursively!
 ExpressionValue ExpressionParser::Parse(bool evaluate) THROWS(GCodeException)
 {
 	obsoleteField.Clear();
-	ExpressionValue result = ParseInternal(evaluate);
+	ExpressionValue result;
+	ParseInternal(result, evaluate, 0);
 	if (!obsoleteField.IsEmpty())
 	{
 		reprap.GetPlatform().MessageF(WarningMessage, "obsolete object model field %s queried\n", obsoleteField.c_str());
@@ -56,27 +66,9 @@ ExpressionValue ExpressionParser::Parse(bool evaluate) THROWS(GCodeException)
 	return result;
 }
 
-// Evaluate an expression that must either be a numeric or string literal or enclosed in { }
-ExpressionValue ExpressionParser::ParseSimple() THROWS(GCodeException)
-{
-	const char c = CurrentCharacter();
-	if (c == '{')
-	{
-		return ParseExpectKet(true, '}');
-	}
-	if (c == '"')
-	{
-		return ParseQuotedString();
-	}
-	if (isDigit(c))
-	{
-		return ParseNumber();
-	}
-	throw ConstructParseException("expected simple expression");
-}
-
 // Evaluate an expression internally, stopping before any binary operators with priority 'priority' or lower
-ExpressionValue ExpressionParser::ParseInternal(bool evaluate, uint8_t priority) THROWS(GCodeException)
+// This is recursive, so avoid allocating large amounts of data on the stack
+void ExpressionParser::ParseInternal(ExpressionValue& val, bool evaluate, uint8_t priority) THROWS(GCodeException)
 {
 	// Lists of binary operators and their priorities
 	static constexpr const char *operators = "?^&|!=<>+-*/";				// for multi-character operators <= and >= and != this is the first character
@@ -87,16 +79,16 @@ ExpressionValue ExpressionParser::ParseInternal(bool evaluate, uint8_t priority)
 	// Start by looking for a unary operator or opening bracket
 	SkipWhiteSpace();
 	const char c = CurrentCharacter();
-	ExpressionValue val;
 	switch (c)
 	{
 	case '"':
-		val = ParseQuotedString();
+		ParseQuotedString(val);
 		break;
 
 	case '-':
 		AdvancePointer();
-		val = ParseInternal(evaluate, UnaryPriority);
+		CheckStack(StackUsage::ParseInternal);
+		ParseInternal(val, evaluate, UnaryPriority);
 		switch (val.GetType())
 		{
 		case TypeCode::Int32:
@@ -108,13 +100,14 @@ ExpressionValue ExpressionParser::ParseInternal(bool evaluate, uint8_t priority)
 			break;
 
 		default:
-			throw ConstructParseException("expected numeric value after '-'");
+			ThrowParseException("expected numeric value after '-'");
 		}
 		break;
 
 	case '+':
 		AdvancePointer();
-		val = ParseInternal(evaluate, UnaryPriority);
+		CheckStack(StackUsage::ParseInternal);
+		ParseInternal(val, evaluate, UnaryPriority);
 		switch (val.GetType())
 		{
 		case TypeCode::Uint32:
@@ -128,7 +121,7 @@ ExpressionValue ExpressionParser::ParseInternal(bool evaluate, uint8_t priority)
 			break;
 
 		default:
-			throw ConstructParseException("expected numeric or enumeration value after '+'");
+			ThrowParseException("expected numeric or enumeration value after '+'");
 		}
 		break;
 
@@ -138,11 +131,13 @@ ExpressionValue ExpressionParser::ParseInternal(bool evaluate, uint8_t priority)
 		if (isalpha(CurrentCharacter()))
 		{
 			// Probably applying # to an object model array, so optimise by asking the OM for just the length
-			val = ParseIdentifierExpression(evaluate, true, false);
+			CheckStack(StackUsage::ParseIdentifierExpression);
+			ParseIdentifierExpression(val, evaluate, true, false);
 		}
 		else
 		{
-			val = ParseInternal(evaluate, UnaryPriority);
+			CheckStack(StackUsage::ParseInternal);
+			ParseInternal(val, evaluate, UnaryPriority);
 			if (val.GetType() == TypeCode::CString)
 			{
 				val.Set((int32_t)strlen(val.sVal));
@@ -153,24 +148,25 @@ ExpressionValue ExpressionParser::ParseInternal(bool evaluate, uint8_t priority)
 			}
 			else
 			{
-				throw ConstructParseException("expected object model value or string after '#");
+				ThrowParseException("expected object model value or string after '#");
 			}
 		}
 		break;
 
 	case '{':
 		AdvancePointer();
-		val = ParseExpectKet(evaluate, '}');
+		ParseExpectKet(val, evaluate, '}');
 		break;
 
 	case '(':
 		AdvancePointer();
-		val = ParseExpectKet(evaluate, ')');
+		ParseExpectKet(val, evaluate, ')');
 		break;
 
 	case '!':
 		AdvancePointer();
-		val = ParseInternal(evaluate, UnaryPriority);
+		CheckStack(StackUsage::ParseInternal);
+		ParseInternal(val, evaluate, UnaryPriority);
 		ConvertToBool(val, evaluate);
 		val.bVal = !val.bVal;
 		break;
@@ -178,15 +174,16 @@ ExpressionValue ExpressionParser::ParseInternal(bool evaluate, uint8_t priority)
 	default:
 		if (isdigit(c))						// looks like a number
 		{
-			val = ParseNumber();
+			ParseNumber(val);
 		}
 		else if (isalpha(c))				// looks like a variable name
 		{
-			val = ParseIdentifierExpression(evaluate, false, false);
+			CheckStack(StackUsage::ParseIdentifierExpression);
+			ParseIdentifierExpression(val, evaluate, false, false);
 		}
 		else
 		{
-			throw ConstructParseException("expected an expression");
+			ThrowParseException("expected an expression");
 		}
 		break;
 	}
@@ -198,20 +195,19 @@ ExpressionValue ExpressionParser::ParseInternal(bool evaluate, uint8_t priority)
 		char opChar = CurrentCharacter();
 		if (opChar == 0)	// don't pass null to strchr
 		{
-
-			return val;
+			return;
 		}
 
 		const char * const q = strchr(operators, opChar);
 		if (q == nullptr)
 		{
-			return val;
+			return;
 		}
 		const size_t index = q - operators;
 		const uint8_t opPrio = priorities[index];
 		if (opPrio <= priority)
 		{
-			return val;
+			return;
 		}
 
 		AdvancePointer();								// skip the [first] operator character
@@ -222,7 +218,7 @@ ExpressionValue ExpressionParser::ParseInternal(bool evaluate, uint8_t priority)
 		{
 			if (CurrentCharacter() != '=')
 			{
-				throw ConstructParseException("expected '='");
+				ThrowParseException("expected '='");
 			}
 			invert = true;
 			AdvancePointer();
@@ -247,11 +243,13 @@ ExpressionValue ExpressionParser::ParseInternal(bool evaluate, uint8_t priority)
 		case '&':
 			ConvertToBool(val, evaluate);
 			{
-				ExpressionValue val2 = ParseInternal(evaluate && val.bVal, opPrio);		// get the next operand
+				ExpressionValue val2;
+				CheckStack(StackUsage::ParseInternal);
+				ParseInternal(val2, evaluate && val.bVal, opPrio);		// get the next operand
 				if (val.bVal)
 				{
 					ConvertToBool(val2, evaluate);
-					val.bVal = val.bVal && val2.bVal;
+					val.bVal = val2.bVal;
 				}
 			}
 			break;
@@ -259,11 +257,13 @@ ExpressionValue ExpressionParser::ParseInternal(bool evaluate, uint8_t priority)
 		case '|':
 			ConvertToBool(val, evaluate);
 			{
-				ExpressionValue val2 = ParseInternal(evaluate && !val.bVal, opPrio);		// get the next operand
+				ExpressionValue val2;
+				CheckStack(StackUsage::ParseInternal);
+				ParseInternal(val2, evaluate && !val.bVal, opPrio);		// get the next operand
 				if (!val.bVal)
 				{
 					ConvertToBool(val2, evaluate);
-					val.bVal = val.bVal || val2.bVal;
+					val.bVal = val2.bVal;
 				}
 			}
 			break;
@@ -271,20 +271,26 @@ ExpressionValue ExpressionParser::ParseInternal(bool evaluate, uint8_t priority)
 		case '?':
 			ConvertToBool(val, evaluate);
 			{
-				ExpressionValue val2 = ParseInternal(evaluate && val.bVal, opPrio);		// get the second operand
+				const bool b = val.bVal;
+				ExpressionValue val2;
+				CheckStack(StackUsage::ParseInternal);
+				ParseInternal(((b) ? val : val2), evaluate && b, opPrio);		// get the second operand
 				if (CurrentCharacter() != ':')
 				{
-					throw ConstructParseException("expected ':'");
+					ThrowParseException("expected ':'");
 				}
 				AdvancePointer();
-				ExpressionValue val3 = ParseInternal(evaluate && !val.bVal, opPrio - 1);	// get the third operand, which may be a further conditional expression
-				return (val.bVal) ? val2 : val3;
+				// We recently checked the stack for a call to ParseInternal, no need to do it again
+				ParseInternal(((b) ? val2 : val), evaluate && !b, opPrio - 1);	// get the third operand, which may be a further conditional expression
+				return;
 			}
 
 		default:
 			// Handle binary operators that always evaluate both operands
 			{
-				ExpressionValue val2 = ParseInternal(evaluate, opPrio);	// get the next operand
+				ExpressionValue val2;
+				CheckStack(StackUsage::ParseInternal);
+				ParseInternal(val2, evaluate, opPrio);	// get the next operand
 				switch(opChar)
 				{
 				case '+':
@@ -300,7 +306,7 @@ ExpressionValue ExpressionParser::ParseInternal(bool evaluate, uint8_t priority)
 						}
 						else
 						{
-							throw ConstructParseException("invalid operand types");
+							ThrowParseException("invalid operand types");
 						}
 					}
 					else
@@ -337,7 +343,7 @@ ExpressionValue ExpressionParser::ParseInternal(bool evaluate, uint8_t priority)
 						}
 						else
 						{
-							throw ConstructParseException("invalid operand types");
+							ThrowParseException("invalid operand types");
 						}
 					}
 					else
@@ -392,7 +398,7 @@ ExpressionValue ExpressionParser::ParseInternal(bool evaluate, uint8_t priority)
 						break;
 
 					default:
-						throw ConstructParseException("expected numeric or Boolean operands to comparison operator");
+						ThrowParseException("expected numeric or Boolean operands to comparison operator");
 					}
 					val.SetType(TypeCode::Bool);
 					if (invert)
@@ -418,7 +424,7 @@ ExpressionValue ExpressionParser::ParseInternal(bool evaluate, uint8_t priority)
 						break;
 
 					default:
-						throw ConstructParseException("expected numeric or Boolean operands to comparison operator");
+						ThrowParseException("expected numeric or Boolean operands to comparison operator");
 					}
 					val.SetType(TypeCode::Bool);
 					if (invert)
@@ -443,7 +449,7 @@ ExpressionValue ExpressionParser::ParseInternal(bool evaluate, uint8_t priority)
 						switch (val.GetType())
 						{
 						case TypeCode::ObjectModel:
-							throw ConstructParseException("cannot compare objects");
+							ThrowParseException("cannot compare objects");
 
 						case TypeCode::Int32:
 							val.bVal = (val.iVal == val2.iVal);
@@ -470,7 +476,7 @@ ExpressionValue ExpressionParser::ParseInternal(bool evaluate, uint8_t priority)
 							break;
 
 						default:
-							throw ConstructParseException("unexpected operand type to equality operator");
+							ThrowParseException("unexpected operand type to equality operator");
 						}
 					}
 					val.SetType(TypeCode::Bool);
@@ -481,14 +487,7 @@ ExpressionValue ExpressionParser::ParseInternal(bool evaluate, uint8_t priority)
 					break;
 
 				case '^':
-					{
-						String<MaxStringExpressionLength> str;
-						val.AppendAsString(str.GetRef());
-						val2.AppendAsString(str.GetRef());
-						StringHandle sh(str.c_str());
-						val.Set(sh);
-						val2.Release();
-					}
+					StringConcat(val, val2);
 					break;
 				}
 			}
@@ -496,6 +495,17 @@ ExpressionValue ExpressionParser::ParseInternal(bool evaluate, uint8_t priority)
 	} while (true);
 }
 
+// Concatenate val1 and val2 and assign the result to val1
+// This is written as a separate function because it needs a temporary string buffer, and its caller is recursive. Its declaration must be declared 'noinline'.
+/*static*/ void  ExpressionParser::StringConcat(ExpressionValue &val, ExpressionValue &val2) noexcept
+{
+    String<MaxStringExpressionLength> str;
+    val.AppendAsString(str.GetRef());
+    val2.AppendAsString(str.GetRef());
+    StringHandle sh(str.c_str());
+    val.Set(sh);
+}
+
 bool ExpressionParser::ParseBoolean() THROWS(GCodeException)
 {
 	ExpressionValue val = Parse();
@@ -521,12 +531,12 @@ int32_t ExpressionParser::ParseInteger() THROWS(GCodeException)
 	case TypeCode::Uint32:
 		if (val.uVal > (uint32_t)std::numeric_limits<int32_t>::max())
 		{
-			throw ConstructParseException("unsigned integer too large");
+			ThrowParseException("unsigned integer too large");
 		}
 		return (int32_t)val.uVal;
 
 	default:
-		throw ConstructParseException("expected integer value");
+		ThrowParseException("expected integer value");
 	}
 }
 
@@ -543,10 +553,10 @@ uint32_t ExpressionParser::ParseUnsigned() THROWS(GCodeException)
 		{
 			return (uint32_t)val.iVal;
 		}
-		throw ConstructParseException("value must be non-negative");
+		ThrowParseException("value must be non-negative");
 
 	default:
-		throw ConstructParseException("expected non-negative integer value");
+		ThrowParseException("expected non-negative integer value");
 	}
 }
 
@@ -564,7 +574,7 @@ void ExpressionParser::BalanceNumericTypes(ExpressionValue& val1, ExpressionValu
 	{
 		if (evaluate)
 		{
-			throw ConstructParseException("expected numeric operands");
+			ThrowParseException("expected numeric operands");
 		}
 		val1.Set((int32_t)0);
 		val2.Set((int32_t)0);
@@ -603,7 +613,7 @@ void ExpressionParser::BalanceTypes(ExpressionValue& val1, ExpressionValue& val2
 	{
 		if (evaluate)
 		{
-			throw ConstructParseException("cannot convert operands to same type");
+			ThrowParseException("cannot convert operands to same type");
 		}
 		val1.Set((int32_t)0);
 		val2.Set((int32_t)0);
@@ -626,7 +636,7 @@ void ExpressionParser::EnsureNumeric(ExpressionValue& val, bool evaluate) const
 	default:
 		if (evaluate)
 		{
-			throw ConstructParseException("expected numeric operand");
+			ThrowParseException("expected numeric operand");
 		}
 		val.Set((int32_t)0);
 	}
@@ -648,9 +658,9 @@ void ExpressionParser::ConvertToFloat(ExpressionValue& val, bool evaluate) const
 	default:
 		if (evaluate)
 		{
-			throw ConstructParseException("expected numeric operand");
+			ThrowParseException("expected numeric operand");
 		}
-		val.Set(0.0f);
+		val.Set(0.0f, 1);
 	}
 }
 
@@ -660,13 +670,13 @@ void ExpressionParser::ConvertToBool(ExpressionValue& val, bool evaluate) const
 	{
 		if (evaluate)
 		{
-			throw ConstructParseException("expected Boolean operand");
+			ThrowParseException("expected Boolean operand");
 		}
 		val.Set(false);
 	}
 }
 
-void ExpressionParser::ConvertToString(ExpressionValue& val, bool evaluate) THROWS(GCodeException)
+void ExpressionParser::ConvertToString(ExpressionValue& val, bool evaluate) noexcept
 {
 	if (!val.IsStringType())
 	{
@@ -698,28 +708,34 @@ void ExpressionParser::CheckForExtraCharacters() THROWS(GCodeException)
 	SkipWhiteSpace();
 	if (CurrentCharacter() != 0)
 	{
-		throw ConstructParseException("Unexpected characters after expression");
+		ThrowParseException("Unexpected characters after expression");
 	}
 }
 
 // Parse a number. The initial character of the string is a decimal digit.
-ExpressionValue ExpressionParser::ParseNumber() noexcept
+void ExpressionParser::ParseNumber(ExpressionValue& rslt) noexcept
 {
 	NumericConverter conv;
-	conv.Accumulate(CurrentCharacter(), NumericConverter::AcceptSignedFloat | NumericConverter::AcceptHex, [this]()->char { AdvancePointer(); return CurrentCharacter(); });		// must succeed because CurrentCharacter is a decimal digit
-	return (conv.FitsInInt32())
-			? ExpressionValue(conv.GetInt32())
-				: ExpressionValue(conv.GetFloat(), constrain<unsigned int>(conv.GetDigitsAfterPoint(), 1, MaxFloatDigitsDisplayedAfterPoint));
+	conv.Accumulate(CurrentCharacter(), NumericConverter::AcceptSignedFloat | NumericConverter::AcceptHex, [this]()->char { AdvancePointer(); return CurrentCharacter(); });	// must succeed because CurrentCharacter is a decimal digit
+
+	if (conv.FitsInInt32())
+	{
+		rslt.Set(conv.GetInt32());
+	}
+	else
+	{
+		rslt.Set(conv.GetFloat(), constrain<unsigned int>(conv.GetDigitsAfterPoint(), 1, MaxFloatDigitsDisplayedAfterPoint));
+	}
 }
 
 // Parse an identifier expression
 // If 'evaluate' is false then the object model path may not exist, in which case we must ignore error that and parse it all anyway
 // This means we can use expressions such as: if {a.b == null || a.b.c == 1}
-ExpressionValue ExpressionParser::ParseIdentifierExpression(bool evaluate, bool applyLengthOperator, bool applyExists) THROWS(GCodeException)
+void ExpressionParser::ParseIdentifierExpression(ExpressionValue& rslt, bool evaluate, bool applyLengthOperator, bool applyExists) THROWS(GCodeException)
 {
 	if (!isalpha(CurrentCharacter()))
 	{
-		throw ConstructParseException("expected an identifier");
+		ThrowParseException("expected an identifier");
 	}
 
 	String<MaxVariableNameLength> id;
@@ -733,14 +749,16 @@ ExpressionValue ExpressionParser::ParseIdentifierExpression(bool evaluate, bool
 		AdvancePointer();
 		if (c == '[')
 		{
-			const ExpressionValue index = Parse(evaluate);
+			ExpressionValue index;
+			CheckStack(StackUsage::ParseInternal);
+			ParseInternal(index, evaluate, 0);
 			if (CurrentCharacter() != ']')
 			{
-				throw ConstructParseException("expected ']'");
+				ThrowParseException("expected ']'");
 			}
 			if (index.GetType() != TypeCode::Int32)
 			{
-				throw ConstructParseException("expected integer expression");
+				ThrowParseException("expected integer expression");
 			}
 			AdvancePointer();										// skip the ']'
 			context.ProvideIndex(index.iVal);
@@ -748,7 +766,7 @@ ExpressionValue ExpressionParser::ParseIdentifierExpression(bool evaluate, bool
 		}
 		if (id.cat(c))
 		{
-			throw ConstructParseException("variable name too long");;
+			ThrowParseException("variable name too long");;
 		}
 	}
 
@@ -758,56 +776,63 @@ ExpressionValue ExpressionParser::ParseIdentifierExpression(bool evaluate, bool
 	{
 		if (context.WantExists())
 		{
-			throw ConstructParseException(InvalidExistsMessage);
+			ThrowParseException(InvalidExistsMessage);
 		}
 
 		switch (whichConstant.RawValue())
 		{
 		case NamedConstant::_true:
-			return ExpressionValue(true);
+			rslt.Set(true);
+			return;
 
 		case NamedConstant::_false:
-			return ExpressionValue(false);
+			rslt.Set(false);
+			return;
 
 		case NamedConstant::_null:
-			return ExpressionValue(nullptr);
+			rslt.Set(nullptr);
+			return;
 
 		case NamedConstant::pi:
-			return ExpressionValue(Pi);
+			rslt.Set(Pi);
+			return;
 
 		case NamedConstant::iterations:
 			{
 				const int32_t v = gb.CurrentFileMachineState().GetIterations();
 				if (v < 0)
 				{
-					throw ConstructParseException("'iterations' used when not inside a loop");
+					ThrowParseException("'iterations' used when not inside a loop");
 				}
-				return ExpressionValue(v);
+				rslt.Set(v);
 			}
+			return;
 
 		case NamedConstant::_result:
 			{
-				int32_t rslt;
+				int32_t res;
 				switch (gb.GetLastResult())
 				{
 				case GCodeResult::ok:
-					rslt = 0;
+					res = 0;
 					break;
 
 				case GCodeResult::warning:
 				case GCodeResult::warningNotSupported:
-					rslt = 1;
+					res = 1;
 					break;
 
 				default:
-					rslt = 2;
+					res = 2;
 					break;
 				}
-				return ExpressionValue(rslt);
+				rslt.Set(res);
 			}
+			return;
 
 		case NamedConstant::line:
-			return ExpressionValue((int32_t)gb.GetLineNumber());
+			rslt.Set((int32_t)gb.GetLineNumber());
+			return;
 
 		default:
 			THROW_INTERNAL_ERROR;
@@ -821,24 +846,25 @@ ExpressionValue ExpressionParser::ParseIdentifierExpression(bool evaluate, bool
 		// It's a function call
 		if (context.WantExists())
 		{
-			throw ConstructParseException(InvalidExistsMessage);
+			ThrowParseException(InvalidExistsMessage);
 		}
 
 		const Function func(id.c_str());
 		if (!func.IsValid())
 		{
-			throw ConstructParseException("unknown function");
+			ThrowParseException("unknown function");
 		}
 
 		AdvancePointer();
-		ExpressionValue rslt;
 		if (func == Function::exists)
 		{
-			rslt = ParseIdentifierExpression(evaluate, false, true);
+			CheckStack(StackUsage::ParseIdentifierExpression);
+			ParseIdentifierExpression(rslt, evaluate, false, true);
 		}
 		else
 		{
-			rslt = Parse(evaluate);					// evaluate the first operand
+			CheckStack(StackUsage::ParseInternal);
+			ParseInternal(rslt, evaluate, 0);					// evaluate the first operand
 
 			switch (func.RawValue())
 			{
@@ -856,7 +882,7 @@ ExpressionValue ExpressionParser::ParseIdentifierExpression(bool evaluate, bool
 				default:
 					if (evaluate)
 					{
-						throw ConstructParseException("expected numeric operand");
+						ThrowParseException("expected numeric operand");
 					}
 					rslt.Set((int32_t)0);
 				}
@@ -904,11 +930,13 @@ ExpressionValue ExpressionParser::ParseIdentifierExpression(bool evaluate, bool
 					SkipWhiteSpace();
 					if (CurrentCharacter() != ',')
 					{
-						throw ConstructParseException("expected ','");
+						ThrowParseException("expected ','");
 					}
 					AdvancePointer();
 					SkipWhiteSpace();
-					ExpressionValue nextOperand = Parse(evaluate);
+					ExpressionValue nextOperand;
+					// We recently checked the stack for a call to ParseInternal, no need to do it again
+					ParseInternal(nextOperand, evaluate, 0);
 					ConvertToFloat(nextOperand, evaluate);
 					rslt.fVal = atan2f(rslt.fVal, nextOperand.fVal);
 					rslt.param = MaxFloatDigitsDisplayedAfterPoint;
@@ -960,11 +988,13 @@ ExpressionValue ExpressionParser::ParseIdentifierExpression(bool evaluate, bool
 					SkipWhiteSpace();
 					if (CurrentCharacter() != ',')
 					{
-						throw ConstructParseException("expected ','");
+						ThrowParseException("expected ','");
 					}
 					AdvancePointer();
 					SkipWhiteSpace();
-					ExpressionValue nextOperand = Parse(evaluate);
+					ExpressionValue nextOperand;
+					// We recently checked the stack for a call to ParseInternal, no need to do it again
+					ParseInternal(nextOperand, evaluate, 0);
 					BalanceNumericTypes(rslt, nextOperand, evaluate);
 					if (rslt.GetType() == TypeCode::Float)
 					{
@@ -991,7 +1021,9 @@ ExpressionValue ExpressionParser::ParseIdentifierExpression(bool evaluate, bool
 					}
 					AdvancePointer();
 					SkipWhiteSpace();
-					ExpressionValue nextOperand = Parse(evaluate);
+					ExpressionValue nextOperand;
+					// We recently checked the stack for a call to ParseInternal, no need to do it again
+					ParseInternal(nextOperand, evaluate, 0);
 					BalanceNumericTypes(rslt, nextOperand, evaluate);
 					if (rslt.GetType() == TypeCode::Float)
 					{
@@ -1015,7 +1047,9 @@ ExpressionValue ExpressionParser::ParseIdentifierExpression(bool evaluate, bool
 					}
 					AdvancePointer();
 					SkipWhiteSpace();
-					ExpressionValue nextOperand = Parse(evaluate);
+					ExpressionValue nextOperand;
+					// We recently checked the stack for a call to ParseInternal, no need to do it again
+					ParseInternal(nextOperand, evaluate, 0);
 					BalanceNumericTypes(rslt, nextOperand, evaluate);
 					if (rslt.GetType() == TypeCode::Float)
 					{
@@ -1031,11 +1065,20 @@ ExpressionValue ExpressionParser::ParseIdentifierExpression(bool evaluate, bool
 
 			case Function::random:
 				{
-					const uint32_t limit = (rslt.GetType() == TypeCode::Uint32) ? rslt.uVal
-											: (rslt.GetType() == TypeCode::Int32 && rslt.iVal > 0) ? rslt.iVal
-												: throw ConstructParseException("expected positive integer");
-					rslt.SetType(TypeCode::Int32);
-					rslt.iVal = random(limit);
+					uint32_t limit;
+					if (rslt.GetType() == TypeCode::Uint32)
+					{
+						limit = rslt.uVal;
+					}
+					else if (rslt.GetType() == TypeCode::Int32 && rslt.iVal > 0)
+					{
+						limit = rslt.iVal;
+					}
+					else
+					{
+						ThrowParseException("expected positive integer");
+					}
+					rslt.Set((int32_t)random(limit));
 				}
 				break;
 
@@ -1047,10 +1090,10 @@ ExpressionValue ExpressionParser::ParseIdentifierExpression(bool evaluate, bool
 		SkipWhiteSpace();
 		if (CurrentCharacter() != ')')
 		{
-			throw ConstructParseException("expected ')'");
+			ThrowParseException("expected ')'");
 		}
 		AdvancePointer();
-		return rslt;
+		return;
 	}
 
 	// If we are not evaluating then the object expression doesn't have to exist, so don't retrieve it because that might throw an error
@@ -1059,58 +1102,65 @@ ExpressionValue ExpressionParser::ParseIdentifierExpression(bool evaluate, bool
 		// Check for a parameter, local or global variable
 		if (StringStartsWith(id.c_str(), "param."))
 		{
-			return GetVariableValue(&gb.GetVariables(), id.c_str() + strlen("param."), true, context.WantExists());
+			GetVariableValue(rslt, &gb.GetVariables(), id.c_str() + strlen("param."), true, applyExists);
+			return;
 		}
 
 		if (StringStartsWith(id.c_str(), "global."))
 		{
 			auto vars = reprap.GetGlobalVariablesForReading();
-			return GetVariableValue(vars.Ptr(), id.c_str() + strlen("global."), false, context.WantExists());
+			GetVariableValue(rslt, vars.Ptr(), id.c_str() + strlen("global."), false, applyExists);
+			return;
 		}
 
 		if (StringStartsWith(id.c_str(), "var."))
 		{
-			return GetVariableValue(&gb.GetVariables(), id.c_str() + strlen("var."), false, context.WantExists());
+			GetVariableValue(rslt, &gb.GetVariables(), id.c_str() + strlen("var."), false, applyExists);
+			return;
 		}
 
 		// "exists(var)", "exists(param)" and "exists(global)" should return true.
 		// "exists(global)" will anyway because "global" is a root key in the object model. Handle the other two here.
-		if (context.WantExists() && (strcmp(id.c_str(), "param") == 0 || strcmp(id.c_str(), "var") == 0))
+		if (applyExists && (strcmp(id.c_str(), "param") == 0 || strcmp(id.c_str(), "var") == 0))
 		{
-			return ExpressionValue(true);
+			rslt.Set(true);
+			return;
 		}
 
 		// Else assume an object model value
-		ExpressionValue value = reprap.GetObjectValue(context, nullptr, id.c_str());
+		CheckStack(StackUsage::GetObjectValue_withTable);
+		rslt = reprap.GetObjectValue(context, nullptr, id.c_str(), 0);
 		if (context.ObsoleteFieldQueried() && obsoleteField.IsEmpty())
 		{
 			obsoleteField.copy(id.c_str());
 		}
-		return value;
+		return;
 	}
-	return ExpressionValue(nullptr);
+	rslt.Set(nullptr);
 }
 
 // Get the value of a variable
-ExpressionValue ExpressionParser::GetVariableValue(const VariableSet *vars, const char *name, bool parameter, bool wantExists) THROWS(GCodeException)
+void ExpressionParser::GetVariableValue(ExpressionValue& rslt, const VariableSet *vars, const char *name, bool parameter, bool wantExists) THROWS(GCodeException)
 {
 	const Variable* var = vars->Lookup(name);
 	if (wantExists)
 	{
-		return ExpressionValue(var != nullptr);
+		rslt.Set(var != nullptr);
+		return;
 	}
 
 	if (var != nullptr && (!parameter || var->GetScope() < 0))
 	{
-		return var->GetValue();
+		rslt = var->GetValue();
+		return;
 	}
 
-	throw ConstructParseException((parameter) ? "unknown parameter '%s'" : "unknown variable '%s'", name);
+	ThrowParseException((parameter) ? "unknown parameter '%s'" : "unknown variable '%s'", name);
 }
 
 // Parse a quoted string, given that the current character is double-quote
 // This is almost a copy of InternalGetQuotedString in class StringParser
-ExpressionValue ExpressionParser::ParseQuotedString() THROWS(GCodeException)
+void ExpressionParser::ParseQuotedString(ExpressionValue& rslt) THROWS(GCodeException)
 {
 	String<MaxStringExpressionLength> str;
 	AdvancePointer();
@@ -1120,14 +1170,15 @@ ExpressionValue ExpressionParser::ParseQuotedString() THROWS(GCodeException)
 		AdvancePointer();
 		if (c < ' ')
 		{
-			throw ConstructParseException("control character in string");
+			ThrowParseException("control character in string");
 		}
 		if (c == '"')
 		{
 			if (CurrentCharacter() != c)
 			{
 				StringHandle sh(str.c_str());
-				return ExpressionValue(sh);
+				rslt.Set(sh);
+				return;
 			}
 			AdvancePointer();
 		}
@@ -1147,7 +1198,7 @@ ExpressionValue ExpressionParser::ParseQuotedString() THROWS(GCodeException)
 		}
 		if (str.cat(c))
 		{
-			throw ConstructParseException("string too long");
+			ThrowParseException("string too long");
 		}
 	}
 }
@@ -1163,19 +1214,40 @@ int ExpressionParser::GetColumn() const noexcept
 	return (column < 0) ? column : (currentp - startp) + column;
 }
 
-GCodeException ExpressionParser::ConstructParseException(const char *str) const noexcept
+void ExpressionParser::ThrowParseException(const char *str) const THROWS(GCodeException)
 {
-	return GCodeException(gb.GetLineNumber(), GetColumn(), str);
+	throw GCodeException(gb.GetLineNumber(), GetColumn(), str);
 }
 
-GCodeException ExpressionParser::ConstructParseException(const char *str, const char *param) const noexcept
+void ExpressionParser::ThrowParseException(const char *str, const char *param) const THROWS(GCodeException)
 {
-	return GCodeException(gb.GetLineNumber(), GetColumn(), str, param);
+	throw GCodeException(gb.GetLineNumber(), GetColumn(), str, param);
 }
 
-GCodeException ExpressionParser::ConstructParseException(const char *str, uint32_t param) const noexcept
+void ExpressionParser::ThrowParseException(const char *str, uint32_t param) const THROWS(GCodeException)
 {
-	return GCodeException(gb.GetLineNumber(), GetColumn(), str, param);
+	throw GCodeException(gb.GetLineNumber(), GetColumn(), str, param);
+}
+
+// Call this before making a recursive call, or before calling a function that needs a lot of stack from a recursive function
+void ExpressionParser::CheckStack(uint32_t calledFunctionStackUsage) const THROWS(GCodeException)
+{
+	register const char * stackPtr asm ("sp");
+	const char *stackLimit = (const char*)TaskBase::GetCallerTaskHandle() + sizeof(TaskBase);
+	//debugPrintf("Margin: %u\n", stackPtr - stackLimit);
+	if (stackLimit + calledFunctionStackUsage + (StackUsage::Throw + StackUsage::Margin) <= stackPtr)
+	{
+		return;
+	}
+
+	// The stack is in danger of overflowing. Throw an exception if we have enough stack to do so (ideally, this should always be the case)
+	if (stackLimit + StackUsage::Throw <= stackPtr)
+	{
+		throw GCodeException(gb.GetLineNumber(), GetColumn(), "Expression nesting too deep");
+	}
+
+	// Not enough stack left to throw an exception
+	SoftwareReset(SoftwareResetReason::stackOverflow, (const uint32_t *)stackPtr);
 }
 
 // End
diff --git a/src/GCodes/GCodeBuffer/ExpressionParser.h b/src/GCodes/GCodeBuffer/ExpressionParser.h
index a4e6cb06..778a5b22 100644
--- a/src/GCodes/GCodeBuffer/ExpressionParser.h
+++ b/src/GCodes/GCodeBuffer/ExpressionParser.h
@@ -20,7 +20,6 @@ public:
 	ExpressionParser(const GCodeBuffer& p_gb, const char *text, const char *textLimit, int p_column = -1) noexcept;
 
 	ExpressionValue Parse(bool evaluate = true) THROWS(GCodeException);
-	ExpressionValue ParseSimple() THROWS(GCodeException);
 	bool ParseBoolean() THROWS(GCodeException);
 	float ParseFloat() THROWS(GCodeException);
 	int32_t ParseInteger() THROWS(GCodeException);
@@ -31,22 +30,27 @@ public:
 	const char *GetEndptr() const noexcept { return currentp; }
 
 private:
-	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;
+	[[noreturn]] void __attribute__((noinline)) ThrowParseException(const char *str) const THROWS(GCodeException);
+	[[noreturn]] void __attribute__((noinline)) ThrowParseException(const char *str, const char *param) const THROWS(GCodeException);
+	[[noreturn]] void __attribute__((noinline)) ThrowParseException(const char *str, uint32_t param) const THROWS(GCodeException);
 
-	ExpressionValue ParseInternal(bool evaluate = true, uint8_t priority = 0) THROWS(GCodeException);
-	ExpressionValue ParseExpectKet(bool evaluate, char expectedKet) THROWS(GCodeException);
-	ExpressionValue ParseNumber() noexcept
+	void ParseInternal(ExpressionValue& val, bool evaluate, uint8_t priority) THROWS(GCodeException);
+	void ParseExpectKet(ExpressionValue& rslt, bool evaluate, char expectedKet) THROWS(GCodeException);
+	void __attribute__((noinline)) ParseNumber(ExpressionValue& rslt) noexcept
 		pre(readPointer >= 0; isdigit(gb.buffer[readPointer]));
-	ExpressionValue ParseIdentifierExpression(bool evaluate, bool applyLengthOperator, bool applyExists) THROWS(GCodeException)
+	void __attribute__((noinline)) ParseIdentifierExpression(ExpressionValue& rslt, bool evaluate, bool applyLengthOperator, bool applyExists) THROWS(GCodeException)
 		pre(readPointer >= 0; isalpha(gb.buffer[readPointer]));
-	ExpressionValue ParseQuotedString() THROWS(GCodeException);
-	ExpressionValue GetVariableValue(const VariableSet *vars, const char *name, bool parameter, bool wantExists) THROWS(GCodeException);
+	void __attribute__((noinline)) ParseQuotedString(ExpressionValue& rslt) 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) THROWS(GCodeException);
+	void ConvertToString(ExpressionValue& val, bool evaluate) noexcept;
+
+	void CheckStack(uint32_t calledFunctionStackUsage) const THROWS(GCodeException);
+
+	// The following must be declared 'noinline' because it allocates a large buffer on the stack and its caller is recursive
+	static void __attribute__((noinline)) StringConcat(ExpressionValue &val, ExpressionValue &val2) noexcept;
 
 	void BalanceNumericTypes(ExpressionValue& val1, ExpressionValue& val2, bool evaluate) const THROWS(GCodeException);
 	void BalanceTypes(ExpressionValue& val1, ExpressionValue& val2, bool evaluate) THROWS(GCodeException);
diff --git a/src/GCodes/GCodeBuffer/GCodeBuffer.cpp b/src/GCodes/GCodeBuffer/GCodeBuffer.cpp
index 01466213..fea25c00 100644
--- a/src/GCodes/GCodeBuffer/GCodeBuffer.cpp
+++ b/src/GCodes/GCodeBuffer/GCodeBuffer.cpp
@@ -626,18 +626,18 @@ bool GCodeBuffer::TryGetFloatArray(char c, size_t numVals, float vals[], const S
 
 // Try to get a quoted string after parameter letter.
 // If we found it then set 'seen' true and return true, else leave 'seen' alone and return false
-bool GCodeBuffer::TryGetQuotedString(char c, const StringRef& str, bool& seen) THROWS(GCodeException)
+bool GCodeBuffer::TryGetQuotedString(char c, const StringRef& str, bool& seen, bool allowEmpty) THROWS(GCodeException)
 {
 	if (Seen(c))
 	{
 		seen = true;
-		GetQuotedString(str);
+		GetQuotedString(str, allowEmpty);
 		return true;
 	}
 	return false;
 }
 
-// Try to get a string, which may be quoted, after parameter letter.
+// Try to get a non-empty string, which may be quoted, after parameter letter.
 // If we found it then set 'seen' true and return true, else leave 'seen' alone and return false
 bool GCodeBuffer::TryGetPossiblyQuotedString(char c, const StringRef& str, bool& seen) THROWS(GCodeException)
 {
@@ -1027,9 +1027,9 @@ void GCodeBuffer::AppendFullCommand(const StringRef &s) const noexcept
 	PARSER_OPERATION(AppendFullCommand(s));
 }
 
-void GCodeBuffer::SetParameters(int codeRunning) noexcept
+void GCodeBuffer::AddParameters(VariableSet& vars, int codeRunning) noexcept
 {
-	PARSER_OPERATION(SetParameters(GetVariables(), codeRunning));
+	PARSER_OPERATION(AddParameters(vars, codeRunning));
 }
 
 VariableSet& GCodeBuffer::GetVariables() const noexcept
diff --git a/src/GCodes/GCodeBuffer/GCodeBuffer.h b/src/GCodes/GCodeBuffer/GCodeBuffer.h
index 05f6c284..fb1f7a3f 100644
--- a/src/GCodes/GCodeBuffer/GCodeBuffer.h
+++ b/src/GCodes/GCodeBuffer/GCodeBuffer.h
@@ -110,7 +110,7 @@ public:
 	bool TryGetBValue(char c, bool& val, bool& seen) THROWS(GCodeException);
 	bool TryGetFloatArray(char c, size_t numVals, float vals[], const StringRef& reply, bool& seen, bool doPad = false) THROWS(GCodeException);
 	bool TryGetUIArray(char c, size_t numVals, uint32_t vals[], const StringRef& reply, bool& seen, bool doPad = false) THROWS(GCodeException);
-	bool TryGetQuotedString(char c, const StringRef& str, bool& seen) THROWS(GCodeException);
+	bool TryGetQuotedString(char c, const StringRef& str, bool& seen, bool allowEmpty = false) THROWS(GCodeException);
 	bool TryGetPossiblyQuotedString(char c, const StringRef& str, bool& seen) THROWS(GCodeException);
 
 	bool IsIdle() const noexcept;
@@ -226,7 +226,7 @@ public:
 	void MotionStopped() noexcept { motionCommanded = false; }
 	bool WasMotionCommanded() const noexcept { return motionCommanded; }
 
-	void SetParameters(int codeRunning) noexcept;
+	void AddParameters(VariableSet& vars, int codeRunning) noexcept;
 	VariableSet& GetVariables() const noexcept;
 
 	Mutex mutex;
diff --git a/src/GCodes/GCodeBuffer/StringParser.cpp b/src/GCodes/GCodeBuffer/StringParser.cpp
index 7db2e7b2..abf822e5 100644
--- a/src/GCodes/GCodeBuffer/StringParser.cpp
+++ b/src/GCodes/GCodeBuffer/StringParser.cpp
@@ -413,7 +413,9 @@ bool StringParser::ProcessConditionalGCode(const StringRef& reply, BlockType ski
 		}
 	}
 
-	if (i >= 2 && i < 9 && (gb.buffer[i] == 0 || gb.buffer[i] == ' ' || gb.buffer[i] == '\t' || gb.buffer[i] == '{'))		// if the command word is properly terminated
+	if (i >= 2 && i < 9
+		&& (gb.buffer[i] == 0 || gb.buffer[i] == ' ' || gb.buffer[i] == '\t' || gb.buffer[i] == '{' || gb.buffer[i] == '"' || gb.buffer[i] == '(')	// if the command word is properly terminated
+	   )
 	{
 		readPointer = i;
 		const char * const command = gb.buffer;
@@ -894,10 +896,8 @@ void StringParser::DecodeCommand() noexcept
 	}
 	else if (   hasCommandNumber
 			 && commandLetter == 'G'
-			 && commandNumber <= 3
-			 && (   strchr(reprap.GetGCodes().GetAxisLetters(), cl) != nullptr
-				 || ((cl == 'I' || cl == 'J') && commandNumber >= 2)
-				)
+			 && commandNumber <= 1
+			 && strchr(reprap.GetGCodes().GetAxisLetters(), cl) != nullptr
 			 && (   reprap.GetGCodes().GetMachineType() == MachineType::cnc			// Fanuc style CNC
 				 || reprap.GetGCodes().GetMachineType() == MachineType::laser		// LaserWeb style
 				)
@@ -1757,7 +1757,7 @@ void StringParser::SkipWhiteSpace() noexcept
 	}
 }
 
-void StringParser::SetParameters(VariableSet& vs, int codeRunning) noexcept
+void StringParser::AddParameters(VariableSet& vs, int codeRunning) noexcept
 {
 	parametersPresent.Iterate([this, &vs, codeRunning](unsigned int bit, unsigned int count)
 								{
@@ -1765,13 +1765,18 @@ void StringParser::SetParameters(VariableSet& vs, int codeRunning) noexcept
 									if ((letter != 'P' || codeRunning != 98) && Seen(letter))
 									{
 										ExpressionParser parser(gb, &gb.buffer[readPointer], &gb.buffer[commandEnd]);
+										ExpressionValue ev;
 										try
 										{
-											ExpressionValue ev = parser.Parse();
-											char paramName[2] = { letter, 0 };
-											vs.Insert(new Variable(paramName, ev, -1));
+											ev = parser.Parse();
+										}
+										catch (const GCodeException&)
+										{
+											//TODO can we report the error anywhere?
+											ev.Set(nullptr);
 										}
-										catch (const GCodeException&) { }
+										char paramName[2] = { letter, 0 };
+										vs.Insert(new Variable(paramName, ev, -1));
 									}
 								}
 							  );
diff --git a/src/GCodes/GCodeBuffer/StringParser.h b/src/GCodes/GCodeBuffer/StringParser.h
index 5e744a1f..a4b828fe 100644
--- a/src/GCodes/GCodeBuffer/StringParser.h
+++ b/src/GCodes/GCodeBuffer/StringParser.h
@@ -81,7 +81,7 @@ public:
 
 	void PrintCommand(const StringRef& s) const noexcept;
 	void AppendFullCommand(const StringRef &s) const noexcept;
-	void SetParameters(VariableSet& vs, int codeRunning) noexcept;
+	void AddParameters(VariableSet& vs, int codeRunning) noexcept;
 
 	GCodeException ConstructParseException(const char *str) const noexcept;
 	GCodeException ConstructParseException(const char *str, const char *param) const noexcept;
diff --git a/src/GCodes/GCodeException.h b/src/GCodes/GCodeException.h
index 4fb56d85..67f8f6ad 100644
--- a/src/GCodes/GCodeException.h
+++ b/src/GCodes/GCodeException.h
@@ -10,6 +10,12 @@
 
 #include <RepRapFirmware.h>
 
+namespace StackUsage
+{
+	constexpr uint32_t Throw = 1050;					// how much stack we need to throw an exception
+	constexpr uint32_t Margin = 300;					// the margin we allow for calls to non-recursive functions that can throw
+}
+
 class GCodeException
 {
 public:
diff --git a/src/GCodes/GCodes.cpp b/src/GCodes/GCodes.cpp
index 3fa82655..805aa675 100644
--- a/src/GCodes/GCodes.cpp
+++ b/src/GCodes/GCodes.cpp
@@ -36,6 +36,7 @@
 #include <Platform/Tasks.h>
 #include <Tools/Tool.h>
 #include <Endstops/ZProbe.h>
+#include <ObjectModel/Variable.h>
 
 #if SUPPORT_LED_STRIPS
 # include <Fans/LedStripDriver.h>
@@ -179,7 +180,7 @@ void GCodes::Init() noexcept
 	{
 		f = 0.0;
 	}
-	lastDefaultFanSpeed = pausedDefaultFanSpeed = 0.0;
+	lastDefaultFanSpeed = 0.0;
 
 	lastAuxStatusReportType = -1;						// no status reports requested yet
 
@@ -998,6 +999,7 @@ void GCodes::DoPause(GCodeBuffer& gb, PauseReason reason, const char *msg, uint1
 
 	SaveFanSpeeds();
 	pauseRestorePoint.toolNumber = reprap.GetCurrentToolNumber();
+	pauseRestorePoint.fanSpeed = lastDefaultFanSpeed;
 
 #if HAS_MASS_STORAGE
 	if (simulationMode == 0)
@@ -1180,6 +1182,7 @@ bool GCodes::DoEmergencyPause() noexcept
 
 	SaveFanSpeeds();
 	pauseRestorePoint.toolNumber = reprap.GetCurrentToolNumber();
+	pauseRestorePoint.fanSpeed = lastDefaultFanSpeed;
 	pauseState = PauseState::paused;
 
 	return true;
@@ -1295,8 +1298,8 @@ bool GCodes::ReHomeOnStall(DriversBitmap stalledDrivers) noexcept
 		return false;								// can't handle it yet
 	}
 
-	// Evaluate which machine axes have stalled.
-	AxesBitmap machineAxesNotHomed;
+	// Evaluate which machine axes have stalled and create parameters for them
+	VariableSet vars;
 	for (size_t axis = 0; axis < numVisibleAxes; ++axis)
 	{
 		const AxisDriversConfig& cfg = platform.GetAxisDriversConfig(axis);
@@ -1305,18 +1308,16 @@ bool GCodes::ReHomeOnStall(DriversBitmap stalledDrivers) noexcept
 			//TODO handle remote stalled drivers
 			if (cfg.driverNumbers[i].IsLocal() && stalledDrivers.IsBitSet(cfg.driverNumbers[i].localDriver))
 			{
-				machineAxesNotHomed.SetBit(axis);
+				char str[2] = { axisLetters[axis], 0 };
+				vars.Insert(new Variable(str, ExpressionValue((int32_t)1), -1));		// create a parameter with value 1 for the axis
 				break;
 			}
 		}
 	}
 
-	// Now pass the machine axes to the rehome.g file
-	// TODO
-
 	autoPauseGCode->SetState(GCodeState::resuming1);					// set up to resume after rehoming
 	pauseState = PauseState::resuming;
-	DoFileMacro(*autoPauseGCode, REHOME_G, true, AsyncSystemMacroCode);	// run the SD card rehome-and-resume script
+	DoFileMacro(*autoPauseGCode, REHOME_G, true, AsyncSystemMacroCode, vars);	// run the SD card rehome-and-resume script
 	return true;
 }
 
@@ -2101,15 +2102,20 @@ bool GCodes::DoStraightMove(GCodeBuffer& gb, bool isCoordinated, const char *& e
 		}
 
 		// Apply segmentation if necessary. To speed up simulation on SCARA printers, we don't apply kinematics segmentation when simulating.
-		// Note for when we use RTOS: as soon as we set segmentsLeft nonzero, the Move process will assume that the move is ready to take, so this must be the last thing we do.
+		// As soon as we set segmentsLeft nonzero, the Move process will assume that the move is ready to take, so this must be the last thing we do.
 		const Kinematics& kin = reprap.GetMove().GetKinematics();
-		if (kin.UseSegmentation() && simulationMode != 1 && (moveBuffer.hasPositiveExtrusion || moveBuffer.isCoordinated || !kin.UseRawG0()))
+		const SegmentationType st = kin.GetSegmentationType();
+		if (st.useSegmentation && simulationMode != 1 && (moveBuffer.hasPositiveExtrusion || moveBuffer.isCoordinated || st.useG0Segmentation))
 		{
-			// This kinematics approximates linear motion by means of segmentation.
-			// We assume that the segments will be smaller than the mesh spacing.
-			const float xyLength = fastSqrtf(fsquare(currentUserPosition[X_AXIS] - initialUserPosition[X_AXIS]) + fsquare(currentUserPosition[Y_AXIS] - initialUserPosition[Y_AXIS]));
-			const float moveTime = xyLength/moveBuffer.feedRate;			// this is a best-case time, often the move will take longer
-			moveBuffer.totalSegments = (unsigned int)max<long>(1, lrintf(min<float>(xyLength * kin.GetReciprocalMinSegmentLength(), moveTime * kin.GetSegmentsPerSecond())));
+			// This kinematics approximates linear motion by means of segmentation
+			float moveLengthSquared = fsquare(currentUserPosition[X_AXIS] - initialUserPosition[X_AXIS]) + fsquare(currentUserPosition[Y_AXIS] - initialUserPosition[Y_AXIS]);
+			if (st.useZSegmentation)
+			{
+				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
+			moveBuffer.totalSegments = (unsigned int)max<long>(1, lrintf(min<float>(moveLength * kin.GetReciprocalMinSegmentLength(), moveTime * kin.GetSegmentsPerSecond())));
 		}
 		else
 		{
@@ -2148,9 +2154,10 @@ bool GCodes::DoStraightMove(GCodeBuffer& gb, bool isCoordinated, const char *& e
 // If an error occurs, return true with 'err' assigned
 bool GCodes::DoArcMove(GCodeBuffer& gb, bool clockwise, const char *& err)
 {
+	// The plans are XY, ZX and YZ depending on the G17/G18/G19 setting. We must use ZX instead of XZ to get the correct arc direction.
 	const unsigned int selectedPlane = gb.LatestMachineState().selectedPlane;
-	const unsigned int axis0 = (selectedPlane == 2) ? Y_AXIS : X_AXIS;
-	const unsigned int axis1 = (selectedPlane == 0) ? Y_AXIS : Z_AXIS;
+	const unsigned int axis0 = (unsigned int[]){ X_AXIS, Z_AXIS, Y_AXIS }[selectedPlane];
+	const unsigned int axis1 = (axis0 + 1) % 3;
 
 	if (moveFractionToSkip > 0.0)
 	{
@@ -2257,7 +2264,7 @@ bool GCodes::DoArcMove(GCodeBuffer& gb, bool clockwise, const char *& err)
 	}
 	else
 	{
-		if (gb.Seen('I'))
+		if (gb.Seen((char)('I' + axis0)))
 		{
 			iParam = gb.GetDistance();
 		}
@@ -2266,7 +2273,7 @@ bool GCodes::DoArcMove(GCodeBuffer& gb, bool clockwise, const char *& err)
 			iParam = 0.0;
 		}
 
-		if (gb.Seen('J'))
+		if (gb.Seen((char)('I' + axis1)))
 		{
 			jParam = gb.GetDistance();
 		}
@@ -2275,9 +2282,9 @@ bool GCodes::DoArcMove(GCodeBuffer& gb, bool clockwise, const char *& err)
 			jParam = 0.0;
 		}
 
-		if (iParam == 0.0 && jParam == 0.0)			// at least one of IJ must be specified and nonzero
+		if (iParam == 0.0 && jParam == 0.0)			// at least one of IJK must be specified and nonzero
 		{
-			err = "G2/G3: no I or J or R parameter";
+			err = "G2/G3: no I J K or R parameter";
 			return true;
 		}
 	}
@@ -2721,6 +2728,17 @@ void GCodes::EmergencyStop() noexcept
 #endif
 }
 
+// Simplified version of DoFileMacro, see below
+bool GCodes::DoFileMacro(GCodeBuffer& gb, const char* fileName, bool reportMissing, int codeRunning) noexcept
+{
+	VariableSet vars;
+	if (codeRunning >= 0)
+	{
+		gb.AddParameters(vars, codeRunning);
+	}
+	return DoFileMacro(gb, fileName, reportMissing, codeRunning, vars);
+}
+
 // Run a file macro. Prior to calling this, 'state' must be set to the state we want to enter when the macro has been completed.
 // Return true if the file was found or it wasn't and we were asked to report that fact.
 // 'codeRunning' is the G or M command we are running, or 0 for a tool change file. In particular:
@@ -2728,7 +2746,7 @@ void GCodes::EmergencyStop() noexcept
 // 502 = running M502
 // 98 = running a macro explicitly via M98
 // otherwise it is either the G- or M-code being executed, or ToolChangeMacroCode for a tool change file, or SystemMacroCode for another system file
-bool GCodes::DoFileMacro(GCodeBuffer& gb, const char* fileName, bool reportMissing, int codeRunning) noexcept
+bool GCodes::DoFileMacro(GCodeBuffer& gb, const char* fileName, bool reportMissing, int codeRunning, VariableSet& initialVariables) noexcept
 {
 #if HAS_LINUX_INTERFACE
 	if (reprap.UsingLinuxInterface())
@@ -2752,10 +2770,7 @@ bool GCodes::DoFileMacro(GCodeBuffer& gb, const char* fileName, bool reportMissi
 			gb.AbortFile(false, true);
 			return true;
 		}
-		if (codeRunning >= 0)
-		{
-			gb.SetParameters(codeRunning);
-		}
+		gb.GetVariables().AssignFrom(initialVariables);
 		gb.StartNewFile();
 		if (gb.IsMacroEmpty())
 		{
@@ -2782,10 +2797,7 @@ bool GCodes::DoFileMacro(GCodeBuffer& gb, const char* fileName, bool reportMissi
 			f->Close();
 			return true;
 		}
-		if (codeRunning >= 0)
-		{
-			gb.SetParameters(codeRunning);
-		}
+		gb.GetVariables().AssignFrom(initialVariables);
 		gb.LatestMachineState().fileState.Set(f);
 		gb.StartNewFile();
 		gb.GetFileInput()->Reset(gb.LatestMachineState().fileState);
@@ -3225,7 +3237,7 @@ void GCodes::StartPrinting(bool fromStart) noexcept
 	buildObjects.Init();
 	reprap.GetMove().ResetMoveCounters();
 
-	if (fromStart)													// if not resurrecting a print
+	if (fromStart)															// if not resurrecting a print
 	{
 		fileGCode->LatestMachineState().volumetricExtrusion = false;		// default to non-volumetric extrusion
 		virtualExtruderPosition = 0.0;
@@ -3260,8 +3272,8 @@ void GCodes::StartPrinting(bool fromStart) noexcept
 							reprap.GetPrintMonitor().GetPrintingFilename());
 	if (fromStart)
 	{
-		// Get the fileGCode to execute the start macro so that any M82/M83 codes will be executed in the correct context
-		DoFileMacro(*fileGCode, START_G, false, AsyncSystemMacroCode);
+		fileGCode->LatestMachineState().selectedPlane = 0;					// default G2 and G3 moves to XY plane
+		DoFileMacro(*fileGCode, START_G, false, AsyncSystemMacroCode);		// get fileGCode to execute the start macro so that any M82/M83 codes will be executed in the correct context
 	}
 }
 
@@ -3308,23 +3320,23 @@ GCodeResult GCodes::DoDwell(GCodeBuffer& gb) THROWS(GCodeException)
 	return (gb.DoDwellTime((uint32_t)dwell)) ? GCodeResult::ok : GCodeResult::notFinished;
 }
 
-// Set offset, working and standby temperatures for a tool. I.e. handle a G10.
+// Set offset, working and standby temperatures for a tool. i.e. handle a G10 or M568.
 GCodeResult GCodes::SetOrReportOffsets(GCodeBuffer &gb, const StringRef& reply, int code) THROWS(GCodeException)
 {
-	int32_t toolNumber = 0;
+	uint32_t toolNumber = 0;
 	bool seenP = false;
-	gb.TryGetIValue('P', toolNumber, seenP);
+	gb.TryGetUIValue('P', toolNumber, seenP);
 
 	ReadLockedPointer<Tool> const tool = (seenP) ? reprap.GetTool(toolNumber) : reprap.GetLockedCurrentTool();
 	if (tool.IsNull())
 	{
 		if (seenP)
 		{
-			reply.printf("Attempt to set/report offsets and temperatures for non-existent tool: %" PRIi32, toolNumber);
+			reply.printf("Tool %" PRIu32 " not found", toolNumber);
 		}
 		else
 		{
-			reply.printf("Attempt to set/report offsets and temperatures for no selected tool");
+			reply.printf("No tool selected");
 		}
 		return GCodeResult::error;
 	}
@@ -3384,23 +3396,44 @@ GCodeResult GCodes::SetOrReportOffsets(GCodeBuffer &gb, const StringRef& reply,
 		}
 	}
 
-	bool settingSpindleRpm = false;
-	if (code == 568)					// Only M568 can set spindle RPM
+	bool settingOther = false;
+	if (code == 568)					// Only M568 can set spindle RPM and change tool heater states
 	{
+		// Deal with spindle RPM
 		if (tool->GetSpindleNumber() > -1)
 		{
 			if (gb.Seen('F'))
 			{
-				settingSpindleRpm = true;
+				settingOther = true;
 				if (simulationMode == 0)
 				{
 					tool->SetSpindleRpm(gb.GetUIValue());
 				}
 			}
 		}
+
+		// Deal with tool heater states
+		uint32_t newHeaterState;
+		if (gb.TryGetLimitedUIValue('A', newHeaterState, settingOther, 3))
+		{
+			switch (newHeaterState)
+			{
+			case 0:			// turn heaters off
+				tool->HeatersToOff();
+				break;
+
+			case 1:			// turn heaters to standby, except any that are used by a different active tool
+				tool->HeatersToStandby();
+				break;
+
+			case 2:			// set heaters to their active temperatures, except any that are used by a different active tool
+				tool->HeatersToActive();
+				break;
+			}
+		}
 	}
 
-	if (!settingOffset && !settingTemps && !settingSpindleRpm)
+	if (!settingOffset && !settingTemps && !settingOther)
 	{
 		// Print offsets and temperatures
 		reply.printf("Tool %d offsets:", tool->Number());
@@ -3626,13 +3659,13 @@ bool GCodes::IsMappedFan(unsigned int fanNumber) noexcept
 }
 
 // Save the speeds of all fans
+// The speed of the default printing fan (i.e. S parameter of the last M106 command with no P parameter) is no longer included because we save that in a restore point.
 void GCodes::SaveFanSpeeds() noexcept
 {
 	for (size_t i = 0; i < MaxFans; ++i)
 	{
 		pausedFanSpeeds[i] = reprap.GetFansManager().GetFanValue(i);
 	}
-	pausedDefaultFanSpeed = lastDefaultFanSpeed;
 }
 
 // Handle sending a reply back to the appropriate interface(s) and update lastResult
@@ -4203,6 +4236,7 @@ void GCodes::SavePosition(RestorePoint& rp, const GCodeBuffer& gb) const noexcep
 	rp.virtualExtruderPosition = virtualExtruderPosition;
 	rp.filePos = gb.GetFilePosition();
 	rp.toolNumber = reprap.GetCurrentToolNumber();
+	rp.fanSpeed = lastDefaultFanSpeed;
 
 #if SUPPORT_LASER || SUPPORT_IOBITS
 	rp.laserPwmOrIoBits = moveBuffer.laserPwmOrIoBits;
diff --git a/src/GCodes/GCodes.h b/src/GCodes/GCodes.h
index dd714ab0..933aa4b7 100644
--- a/src/GCodes/GCodes.h
+++ b/src/GCodes/GCodes.h
@@ -338,7 +338,8 @@ private:
 	void StartPrinting(bool fromStart) noexcept;								// Start printing the file already selected
 	void StopPrint(StopPrintReason reason) noexcept;							// Stop the current print
 
-	bool DoFilePrint(GCodeBuffer& gb, const StringRef& reply) noexcept;			// Get G Codes from a file and print them
+	bool DoFilePrint(GCodeBuffer& gb, const StringRef& reply) noexcept;					// Get G Codes from a file and print them
+	bool DoFileMacro(GCodeBuffer& gb, const char* fileName, bool reportMissing, int codeRunning, VariableSet& initialVariables) noexcept;
 	bool DoFileMacro(GCodeBuffer& gb, const char* fileName, bool reportMissing, int codeRunning) noexcept;
 																						// Run a GCode macro file, optionally report error if not found
 	void FileMacroCyclesReturn(GCodeBuffer& gb) noexcept;								// End a macro
@@ -610,8 +611,7 @@ private:
 	AxesBitmap axesVirtuallyHomed;				// same as axesHomed except all bits are set when simulating
 
 	float pausedFanSpeeds[MaxFans];				// Fan speeds when the print was paused or a tool change started
-	float lastDefaultFanSpeed;					// Last speed given in a M106 command with on fan number
-	float pausedDefaultFanSpeed;				// The speed of the default print cooling fan when the print was paused or a tool change started
+	float lastDefaultFanSpeed;					// Last speed given in a M106 command with no fan number
 	float speedFactor;							// speed factor as a fraction (normally 1.0)
 	float extrusionFactors[MaxExtruders];		// extrusion factors (normally 1.0)
 	float volumetricExtrusionFactors[MaxExtruders]; // Volumetric extrusion factors
diff --git a/src/GCodes/GCodes2.cpp b/src/GCodes/GCodes2.cpp
index 6a295391..ec036535 100644
--- a/src/GCodes/GCodes2.cpp
+++ b/src/GCodes/GCodes2.cpp
@@ -144,7 +144,7 @@ bool GCodes::HandleGcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx
 	{
 	case 0: // Rapid move
 	case 1: // Ordinary move
-		if (moveBuffer.segmentsLeft != 0)							// do this check first to avoid locking movement unnecessarily
+		if (moveBuffer.segmentsLeft != 0)								// do this check first to avoid locking movement unnecessarily
 		{
 			return false;
 		}
@@ -161,7 +161,7 @@ bool GCodes::HandleGcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx
 			if (err != nullptr)
 			{
 				gb.SetState(GCodeState::abortWhenMovementFinished);		// empty the queue before ending simulation, and force the user position to be restored
-				gb.LatestMachineState().SetError(err);						// must do this *after* calling SetState
+				gb.LatestMachineState().SetError(err);					// must do this *after* calling SetState
 			}
 		}
 		break;
@@ -537,98 +537,68 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx
 			break;
 
 		case 3: // Spin spindle clockwise
+		case 4: // Spin spindle counter clockwise
+			if (machineType == MachineType::cnc)
 			{
-				if (machineType == MachineType::cnc)
+				// Determine what spindle number we are using
+				Tool * const currentTool = reprap.GetCurrentTool();
+				uint32_t slot;
+				if (gb.Seen('P'))
 				{
-					Tool * const currentTool = reprap.GetCurrentTool();
-					if (gb.Seen('S'))
-					{
-						if (currentTool != nullptr && currentTool->GetSpindleNumber() > -1)
-						{
-							currentTool->SetSpindleRpm(gb.GetUIValue());
-							platform.AccessSpindle(currentTool->GetSpindleNumber()).SetState(SpindleState::forward);
-						}
-						else
-						{
-							const uint32_t slot = gb.GetLimitedUIValue('P', MaxSpindles);			// Direct spindle speed can only be set when slot is provided as well
-							Spindle& spindle = platform.AccessSpindle(slot);
-							spindle.SetConfiguredRpm(gb.GetIValue(), false);
-							spindle.SetState(SpindleState::forward);
-						}
-					}
-					else if (currentTool != nullptr && currentTool->GetSpindleNumber() > -1)
-					{
-						platform.AccessSpindle(currentTool->GetSpindleNumber()).SetState(SpindleState::forward);
-					}
-					else
-					{
-						reply.copy("No spindle selected via P and no active tool with spindle");
-						result = GCodeResult::warning;
-					}
+					slot = gb.GetLimitedUIValue('P', MaxSpindles);
 				}
-				else if (gb.Seen('S'))
+				else if (currentTool != nullptr && currentTool->GetSpindleNumber() >= 0)
 				{
-					switch (machineType)
-					{
-#if SUPPORT_LASER
-					case MachineType::laser:
-						if (moveBuffer.segmentsLeft != 0)
-						{
-							return false;						// don't modify moves that haven't gone yet
-						}
-						moveBuffer.laserPwmOrIoBits.laserPwm = ConvertLaserPwm(gb.GetFValue());
-						break;
-#endif
-
-					default:
-#if SUPPORT_ROLAND
-						if (reprap.GetRoland()->Active())
-						{
-							result = reprap.GetRoland()->ProcessSpindle(gb.GetFValue());
-						}
-						else
-#endif
-						{
-							result = GCodeResult::notSupportedInCurrentMode;
-						}
-						break;
-					}
+					slot = currentTool->GetSpindleNumber();
 				}
 				else
 				{
-					result = GCodeResult::notSupportedInCurrentMode;
+					reply.copy("No P parameter and no active tool with spindle");
+					result = GCodeResult::error;
+					break;
 				}
-			}
-			break;
 
-		case 4: // Spin spindle counter clockwise
-			if (machineType == MachineType::cnc)
-			{
-				Tool * const currentTool = reprap.GetCurrentTool();
+				Spindle& spindle = platform.AccessSpindle(slot);
 				if (gb.Seen('S'))
 				{
-					if (currentTool != nullptr && currentTool->GetSpindleNumber() > -1)
+					const uint32_t rpm = gb.GetUIValue();
+					if (currentTool != nullptr && currentTool->GetSpindleNumber() == (int)slot)
 					{
-						currentTool->SetSpindleRpm(gb.GetUIValue());
-						platform.AccessSpindle(currentTool->GetSpindleNumber()).SetState(SpindleState::reverse);
+						currentTool->SetSpindleRpm(rpm);
 					}
 					else
 					{
-						const uint32_t slot = gb.GetLimitedUIValue('P', MaxSpindles);			// Direct spindle speed can only be set when slot is provided as well
-						Spindle& spindle = platform.AccessSpindle(slot);
-						spindle.SetConfiguredRpm(gb.GetIValue(), false);
-						spindle.SetState(SpindleState::reverse);
+						spindle.SetConfiguredRpm(rpm, false);
 					}
 				}
-				else if (currentTool != nullptr && currentTool->GetSpindleNumber() > -1)
-				{
-					// At this point slot = currentTool->GetSpindleNumber()
-					platform.AccessSpindle(currentTool->GetSpindleNumber()).SetState(SpindleState::reverse);
-				}
-				else
+				spindle.SetState((code == 4) ? SpindleState::reverse : SpindleState::forward);
+			}
+			else if (code == 3 && gb.Seen('S'))
+			{
+				switch (machineType)
 				{
-					reply.copy("No spindle selected via P and no active tool with spindle");
-					result = GCodeResult::warning;
+#if SUPPORT_LASER
+				case MachineType::laser:
+					if (moveBuffer.segmentsLeft != 0)
+					{
+						return false;						// don't modify moves that haven't gone yet
+					}
+					moveBuffer.laserPwmOrIoBits.laserPwm = ConvertLaserPwm(gb.GetFValue());
+					break;
+#endif
+
+				default:
+#if SUPPORT_ROLAND
+					if (reprap.GetRoland()->Active())
+					{
+						result = reprap.GetRoland()->ProcessSpindle(gb.GetFValue());
+					}
+					else
+#endif
+					{
+						result = GCodeResult::notSupportedInCurrentMode;
+					}
+					break;
 				}
 			}
 			else
@@ -642,29 +612,28 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx
 			{
 			case MachineType::cnc:
 				{
+					// Determine what spindle number we are using
+					Tool * const currentTool = reprap.GetCurrentTool();
+					uint32_t slot;
 					if (gb.Seen('P'))
 					{
-						// Turn off specific spindle
-						const uint32_t slot = gb.GetLimitedUIValue('P', MaxSpindles);
-						platform.AccessSpindle(slot).SetState(SpindleState::stopped);
+						slot = gb.GetLimitedUIValue('P', MaxSpindles);
+					}
+					else if (currentTool != nullptr && currentTool->GetSpindleNumber() >= 0)
+					{
+						slot = currentTool->GetSpindleNumber();
 					}
 					else
 					{
-						Tool * const currentTool = reprap.GetCurrentTool();
-						if (currentTool != nullptr && currentTool->GetSpindleNumber() > -1) // Turn off spindle of current tool
+						// Turn off every spindle if no 'P' parameter is present and the current tool does not have a spindle
+						for (size_t i = 0; i < MaxSpindles; i++)
 						{
-							platform.AccessSpindle(currentTool->GetSpindleNumber()).SetState(SpindleState::stopped);
-						}
-						else
-						{
-							// Turn off every spindle if no 'P' parameter is present and the current tool
-							// does not have a spindle
-							for (size_t i = 0; i < MaxSpindles; i++)
-							{
-								platform.AccessSpindle(i).SetState(SpindleState::stopped);
-							}
+							platform.AccessSpindle(i).SetState(SpindleState::stopped);
 						}
+						break;
 					}
+
+					platform.AccessSpindle(slot).SetState(SpindleState::stopped);
 				}
 				break;
 
@@ -1471,7 +1440,8 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx
 					}
 					else
 					{
-						SetMappedFanSpeed(pausedDefaultFanSpeed);
+						const size_t restorePointNumber = gb.GetLimitedUIValue('R', NumRestorePoints);
+						SetMappedFanSpeed(numberedRestorePoints[restorePointNumber].fanSpeed);
 					}
 				}
 			}
@@ -2858,8 +2828,8 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx
 				String<StringLength100> key;
 				String<StringLength20> flags;
 				bool dummy;
-				gb.TryGetQuotedString('K', key.GetRef(), dummy);
-				gb.TryGetQuotedString('F', flags.GetRef(), dummy);
+				gb.TryGetQuotedString('K', key.GetRef(), dummy, true);
+				gb.TryGetQuotedString('F', flags.GetRef(), dummy, true);
 				if (&gb == auxGCode)
 				{
 					lastAuxStatusReportType = ObjectModelAuxStatusReportType;
diff --git a/src/GCodes/GCodes3.cpp b/src/GCodes/GCodes3.cpp
index 0db31705..3ff14db0 100644
--- a/src/GCodes/GCodes3.cpp
+++ b/src/GCodes/GCodes3.cpp
@@ -44,16 +44,11 @@
 // Deal with G60
 GCodeResult GCodes::SavePosition(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeException)
 {
-	const uint32_t sParam = (gb.Seen('S')) ? gb.GetUIValue() : 0;
-	if (sParam < ARRAY_SIZE(numberedRestorePoints))
-	{
-		SavePosition(numberedRestorePoints[sParam], gb);
-		numberedRestorePoints[sParam].toolNumber = reprap.GetCurrentToolNumber();
-		return GCodeResult::ok;
-	}
-
-	reply.copy("Bad restore point number");
-	return GCodeResult::error;
+	uint32_t sParam = 0;
+	bool dummySeen;
+	gb.TryGetLimitedUIValue('S', sParam, dummySeen, NumRestorePoints);
+	SavePosition(numberedRestorePoints[sParam], gb);
+	return GCodeResult::ok;
 }
 
 // This handles G92. Return true if completed, false if it needs to be called again.
diff --git a/src/GCodes/GCodes4.cpp b/src/GCodes/GCodes4.cpp
index c3d2bbbb..244a3a7d 100644
--- a/src/GCodes/GCodes4.cpp
+++ b/src/GCodes/GCodes4.cpp
@@ -310,8 +310,9 @@ void GCodes::RunStateMachine(GCodeBuffer& gb, const StringRef& reply) noexcept
 	case GCodeState::toolChange0: 						// run tfree for the old tool (if any)
 	case GCodeState::m109ToolChange0:					// run tfree for the old tool (if any)
 		doingToolChange = true;
-		SaveFanSpeeds();
 		SavePosition(toolChangeRestorePoint, gb);
+		toolChangeRestorePoint.toolNumber = reprap.GetCurrentToolNumber();
+		toolChangeRestorePoint.fanSpeed = lastDefaultFanSpeed;
 		reprap.SetPreviousToolNumber();
 		gb.AdvanceState();
 
@@ -529,10 +530,8 @@ void GCodes::RunStateMachine(GCodeBuffer& gb, const StringRef& reply) noexcept
 	case GCodeState::resuming3:
 		if (LockMovementAndWaitForStandstill(gb))
 		{
-			for (size_t i = 0; i < MaxFans; ++i)
-			{
-				reprap.GetFansManager().SetFanValue(i, pausedFanSpeeds[i]);
-			}
+			// We no longer restore the paused fan speeds automatically on resuming, because that messes up the print cooling fan speed if a tool change has been done
+			// They can be restored manually in resume.g if required
 			virtualExtruderPosition = pauseRestorePoint.virtualExtruderPosition;	// reset the extruder position in case we are receiving absolute extruder moves
 			moveBuffer.virtualExtruderPosition = pauseRestorePoint.virtualExtruderPosition;
 			fileGCode->LatestMachineState().feedRate = pauseRestorePoint.feedRate;
@@ -554,8 +553,12 @@ void GCodes::RunStateMachine(GCodeBuffer& gb, const StringRef& reply) noexcept
 		{
 			bool updating = false;
 			String<MaxFilenameLength> filenameString;
-			bool dummy;
-			gb.TryGetQuotedString('P', filenameString.GetRef(), dummy);
+			try
+			{
+				bool dummy;
+				gb.TryGetQuotedString('P', filenameString.GetRef(), dummy);
+			}
+			catch (const GCodeException&) { }
 			for (unsigned int module = 1; module < NumFirmwareUpdateModules; ++module)
 			{
 				if (firmwareUpdateModuleMap.IsBitSet(module))
@@ -594,10 +597,14 @@ void GCodes::RunStateMachine(GCodeBuffer& gb, const StringRef& reply) noexcept
 			// Update main firmware
 			firmwareUpdateModuleMap.Clear();
 			String<MaxFilenameLength> filenameString;
-			bool dummy;
-			gb.TryGetQuotedString('P', filenameString.GetRef(), dummy);
-			reprap.UpdateFirmware(filenameString.GetRef());
-			// The above call does not return unless an error occurred
+			try
+			{
+				bool dummy;
+				gb.TryGetQuotedString('P', filenameString.GetRef(), dummy);
+				reprap.UpdateFirmware(filenameString.GetRef());
+				// The above call does not return unless an error occurred
+			}
+			catch (const GCodeException&) { }
 		}
 		isFlashing = false;
 		gb.SetState(GCodeState::normal);
diff --git a/src/GCodes/RestorePoint.cpp b/src/GCodes/RestorePoint.cpp
index 7ba04a31..edbfbf71 100644
--- a/src/GCodes/RestorePoint.cpp
+++ b/src/GCodes/RestorePoint.cpp
@@ -33,6 +33,7 @@ constexpr ObjectModelTableEntry RestorePoint::objectModelTable[] =
 	// 0. LaserFilamentMonitor members
 	{ "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 },
 #if SUPPORT_IOBITS
 	{ "ioBits",				OBJECT_MODEL_FUNC_IF(reprap.GetGCodes().GetMachineType() != MachineType::laser,
@@ -45,7 +46,7 @@ constexpr ObjectModelTableEntry RestorePoint::objectModelTable[] =
 	{ "toolNumber",			OBJECT_MODEL_FUNC((int32_t)self->toolNumber),										ObjectModelEntryFlags::none },
 };
 
-constexpr uint8_t RestorePoint::objectModelTableDescriptor[] = { 1, 4 + SUPPORT_LASER + SUPPORT_IOBITS };
+constexpr uint8_t RestorePoint::objectModelTableDescriptor[] = { 1, 5 + SUPPORT_LASER + SUPPORT_IOBITS };
 
 DEFINE_GET_OBJECT_MODEL_TABLE(RestorePoint)
 
@@ -69,6 +70,7 @@ void RestorePoint::Init() noexcept
 	proportionDone = 0.0;
 	initialUserC0 = initialUserC1 = 0.0;
 	toolNumber = -1;
+	fanSpeed = 0.0;
 
 #if SUPPORT_LASER || SUPPORT_IOBITS
 	laserPwmOrIoBits.Clear();
diff --git a/src/GCodes/RestorePoint.h b/src/GCodes/RestorePoint.h
index ee2a6742..5b581691 100644
--- a/src/GCodes/RestorePoint.h
+++ b/src/GCodes/RestorePoint.h
@@ -21,6 +21,7 @@ public:
 	FilePosition filePos;					// The file position that this move was read from
 	float initialUserC0, initialUserC1;		// If we paused during an arc move and proportionDone is nonzero, the X and Y user coordinates at the start of the move
 	int toolNumber;							// The tool number that was active
+	float fanSpeed;							// the last fan speed that was set by M106 with no P parameter
 
 #if SUPPORT_LASER || SUPPORT_IOBITS
 	LaserPwmOrIoBits laserPwmOrIoBits;		// The output port bits setting for this move, or the laser power
@@ -32,7 +33,6 @@ public:
 protected:
 	DECLARE_OBJECT_MODEL
 	OBJECT_MODEL_ARRAY(coordinates)
-	OBJECT_MODEL_ARRAY(spindleSpeeds)
 };
 
 #endif /* SRC_GCODES_RESTOREPOINT_H_ */
diff --git a/src/Heating/Heat.cpp b/src/Heating/Heat.cpp
index e71c1f19..ad7f22d4 100644
--- a/src/Heating/Heat.cpp
+++ b/src/Heating/Heat.cpp
@@ -932,7 +932,7 @@ GCodeResult Heat::ConfigureSensor(GCodeBuffer& gb, const StringRef& reply) THROW
 
 	if (gb.Seen('P'))
 	{
-		String<StringLength50> portName;							// StringLength20 is too short for "thermocouple-max31856"
+		String<StringLength50> portName;
 		gb.GetReducedString(portName.GetRef());
 #if SUPPORT_CAN_EXPANSION
 		boardAddress = IoPort::RemoveBoardAddress(portName.GetRef());
@@ -958,13 +958,13 @@ GCodeResult Heat::ConfigureSensor(GCodeBuffer& gb, const StringRef& reply) THROW
 
 		DeleteSensor(sensorNum);
 
-		String<StringLength20> typeName;
+		String<StringLength50> typeName;							// StringLength20 is too short for "thermocouple-max31856"
 		gb.GetReducedString(typeName.GetRef());
 
 #if SUPPORT_CAN_EXPANSION
 		if (boardAddress == CanId::NoAddress)
 		{
-			boardAddress = CanInterface::GetCanAddress();		// no port name was given, so default to local
+			boardAddress = CanInterface::GetCanAddress();			// no port name was given, so default to local
 		}
 		TemperatureSensor * const newSensor = TemperatureSensor::Create(sensorNum, boardAddress, typeName.c_str(), reply);
 #else
diff --git a/src/Heating/Heater.cpp b/src/Heating/Heater.cpp
index 670e0b98..fd3742bd 100644
--- a/src/Heating/Heater.cpp
+++ b/src/Heating/Heater.cpp
@@ -440,11 +440,13 @@ GCodeResult Heater::SetFaultDetectionParameters(float pMaxTempExcursion, float p
 GCodeResult Heater::ConfigureMonitor(GCodeBuffer &gb, const StringRef &reply) THROWS(GCodeException)
 {
 	// Get any parameters that have been provided
-	const bool seenP = gb.Seen('P');
-	const size_t index = (seenP) ? gb.GetLimitedUIValue('P', MaxMonitorsPerHeater) : 0;
+	uint32_t index = 0;
+	bool seenP = false;
+	gb.TryGetLimitedUIValue('P', index, seenP, MaxMonitorsPerHeater);
 
-	const bool seenSensor = gb.Seen('T');
-	const int monitoringSensor = (seenSensor) ? gb.GetLimitedUIValue('T', MaxSensors) : GetSensorNumber();
+	bool seenSensor = false;
+	uint32_t monitoringSensor = GetSensorNumber();
+	gb.TryGetLimitedUIValue('T', monitoringSensor, seenSensor, MaxSensors);
 
 	const bool seenAction = gb.Seen('A');
 	const HeaterMonitorAction action = (seenAction)
diff --git a/src/Movement/DDA.cpp b/src/Movement/DDA.cpp
index e4e21d02..0f358034 100644
--- a/src/Movement/DDA.cpp
+++ b/src/Movement/DDA.cpp
@@ -1261,7 +1261,7 @@ void DDA::Prepare(uint8_t simMode, float extrusionPending[]) noexcept
 {
 	flags.wasAccelOnlyMove = IsAccelerationMove();			// save this for the next move to look at
 
-	if (flags.xyMoving && reprap.GetMove().GetShaper().GetType() == InputShaperType::DAA)
+	if (flags.xyMoving && reprap.GetMove().GetShaper().GetType() == InputShaperType::daa)
 	{
 		AdjustAcceleration();
 	}
diff --git a/src/Movement/DDARing.cpp b/src/Movement/DDARing.cpp
index 0b7e10e2..cb765e53 100644
--- a/src/Movement/DDARing.cpp
+++ b/src/Movement/DDARing.cpp
@@ -113,7 +113,7 @@ void DDARing::Exit() noexcept
 	}
 }
 
-GCodeResult DDARing::ConfigureMovementQueue(GCodeBuffer& gb, const StringRef& reply) noexcept
+GCodeResult DDARing::ConfigureMovementQueue(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeException)
 {
 	bool seen = false;
 	uint32_t numDdasWanted = 0, numDMsWanted = 0;
diff --git a/src/Movement/DDARing.h b/src/Movement/DDARing.h
index be3623d0..359baeb6 100644
--- a/src/Movement/DDARing.h
+++ b/src/Movement/DDARing.h
@@ -84,7 +84,7 @@ public:
 
 	bool SetWaitingToEmpty() noexcept;
 
-	GCodeResult ConfigureMovementQueue(GCodeBuffer& gb, const StringRef& reply) noexcept;
+	GCodeResult ConfigureMovementQueue(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeException);
 
 #if SUPPORT_REMOTE_COMMANDS
 	void AddMoveFromRemote(const CanMessageMovementLinear& msg) noexcept;				// add a move from the ATE to the movement queue
diff --git a/src/Movement/InputShaper.cpp b/src/Movement/InputShaper.cpp
index 4caf84da..cc4d5f9a 100644
--- a/src/Movement/InputShaper.cpp
+++ b/src/Movement/InputShaper.cpp
@@ -65,13 +65,21 @@ GCodeResult InputShaper::Configure(GCodeBuffer& gb, const StringRef& reply) THRO
 
 	if (gb.Seen('P'))
 	{
+		String<StringLength20> shaperName;
+		gb.GetReducedString(shaperName.GetRef());
+		const InputShaperType newType(shaperName.c_str());
+		if (!newType.IsValid())
+		{
+			reply.printf("Unsupported input shaper type '%s'", shaperName.c_str());
+			return GCodeResult::error;
+		}
 		seen = true;
-		type = (InputShaperType)gb.GetLimitedUIValue('P', InputShaperType::NumValues);
+		type = newType;
 	}
 	else if (seen && type == InputShaperType::none)
 	{
 		// For backwards compatibility, if we have set input shaping parameters but not defined shaping type, default to DAA for now. Change this when we support better types of input shaping.
-		type = InputShaperType::DAA;
+		type = InputShaperType::daa;
 	}
 
 	if (seen)
@@ -80,7 +88,7 @@ GCodeResult InputShaper::Configure(GCodeBuffer& gb, const StringRef& reply) THRO
 	}
 	else if (type != InputShaperType::none)
 	{
-		reply.printf("%s input shaping at %.1fHz damping factor %.2f, min. acceleration %.1f",
+		reply.printf("Input shaping '%s' at %.1fHz damping factor %.2f, min. acceleration %.1f",
 						type.ToString(), (double)GetFrequency(), (double)GetFloatDamping(), (double)minimumAcceleration);
 	}
 	else
diff --git a/src/Movement/InputShaper.h b/src/Movement/InputShaper.h
index 4ed73304..068c0a24 100644
--- a/src/Movement/InputShaper.h
+++ b/src/Movement/InputShaper.h
@@ -12,12 +12,10 @@
 #include <General/NamedEnum.h>
 #include <ObjectModel/ObjectModel.h>
 
+// These names must be in alphabetical order and lowercase
 NamedEnum(InputShaperType, uint8_t,
+	daa,
 	none,
-	ZVD,
-	ZVDD,
-	EI2,
-	DAA
 );
 
 class InputShaper INHERIT_OBJECT_MODEL
diff --git a/src/Movement/Kinematics/FiveBarScaraKinematics.cpp b/src/Movement/Kinematics/FiveBarScaraKinematics.cpp
index 8fd10f2d..83c9b3ae 100644
--- a/src/Movement/Kinematics/FiveBarScaraKinematics.cpp
+++ b/src/Movement/Kinematics/FiveBarScaraKinematics.cpp
@@ -42,7 +42,7 @@ DEFINE_GET_OBJECT_MODEL_TABLE_WITH_PARENT(FiveBarScaraKinematics, ZLeadscrewKine
 #endif
 
 FiveBarScaraKinematics::FiveBarScaraKinematics() noexcept
-	: ZLeadscrewKinematics(KinematicsType::scara, true, true)
+	: ZLeadscrewKinematics(KinematicsType::scara, SegmentationType(true, false, false))
 {
 	Recalc();
 }
diff --git a/src/Movement/Kinematics/HangprinterKinematics.cpp b/src/Movement/Kinematics/HangprinterKinematics.cpp
index cf8e8d90..3d5c65aa 100644
--- a/src/Movement/Kinematics/HangprinterKinematics.cpp
+++ b/src/Movement/Kinematics/HangprinterKinematics.cpp
@@ -60,7 +60,7 @@ DEFINE_GET_OBJECT_MODEL_TABLE(HangprinterKinematics)
 
 // Constructor
 HangprinterKinematics::HangprinterKinematics() noexcept
-	: RoundBedKinematics(KinematicsType::scara, true, true)
+	: RoundBedKinematics(KinematicsType::hangprinter, SegmentationType(true, true, true))
 {
 	Init();
 }
@@ -226,7 +226,7 @@ bool HangprinterKinematics::Configure(unsigned int mCode, GCodeBuffer& gb, const
 				(double)anchors[C_AXIS][X_AXIS], (double)anchors[C_AXIS][Y_AXIS], (double)anchors[C_AXIS][Z_AXIS],
 				(double)anchors[D_AXIS][X_AXIS], (double)anchors[D_AXIS][Y_AXIS], (double)anchors[D_AXIS][Z_AXIS],
 				(double)printRadius,
-				(int)segmentsPerSecond, (double)minSegmentLength
+				(int)GetSegmentsPerSecond(), (double)GetMinSegmentLength()
 				);
 		}
 	}
diff --git a/src/Movement/Kinematics/Kinematics.cpp b/src/Movement/Kinematics/Kinematics.cpp
index 1a0c7dc1..850308b5 100644
--- a/src/Movement/Kinematics/Kinematics.cpp
+++ b/src/Movement/Kinematics/Kinematics.cpp
@@ -23,8 +23,9 @@
 const char * const Kinematics::HomeAllFileName = "homeall.g";
 
 // Constructor. Pass segsPerSecond <= 0.0 to get non-segmented kinematics.
-Kinematics::Kinematics(KinematicsType t, bool doUseSegmentation, bool doUseRawG0) noexcept
-	: segmentsPerSecond(DefaultSegmentsPerSecond), minSegmentLength(DefaultMinSegmentLength), useSegmentation(doUseSegmentation), useRawG0(doUseRawG0), type(t)
+Kinematics::Kinematics(KinematicsType t, SegmentationType segType) noexcept
+	: segmentsPerSecond(DefaultSegmentsPerSecond), minSegmentLength(DefaultMinSegmentLength), reciprocalMinSegmentLength(1.0/DefaultMinSegmentLength),
+	  segmentationType(segType), type(t)
 {
 }
 
@@ -55,8 +56,8 @@ bool Kinematics::TryConfigureSegmentation(GCodeBuffer& gb) noexcept
 	gb.TryGetFValue('T', minSegmentLength, seen);
 	if (seen)
 	{
-		useSegmentation = minSegmentLength > 0.0 && segmentsPerSecond > 0.0;
-		if (useSegmentation)
+		segmentationType.useSegmentation = minSegmentLength > 0.0 && segmentsPerSecond > 0.0;
+		if (segmentationType.useSegmentation)
 		{
 			reciprocalMinSegmentLength = 1.0 / minSegmentLength;
 		}
diff --git a/src/Movement/Kinematics/Kinematics.h b/src/Movement/Kinematics/Kinematics.h
index 13967567..addd6098 100644
--- a/src/Movement/Kinematics/Kinematics.h
+++ b/src/Movement/Kinematics/Kinematics.h
@@ -63,6 +63,19 @@ enum class LimitPositionResult : uint8_t
 	adjustedAndIntermediateUnreachable	// we adjusted the final position to make it reachable, but intermediate positions are still urreachable
 };
 
+struct SegmentationType
+{
+	uint8_t useSegmentation : 1,
+			useZSegmentation : 1,
+			useG0Segmentation : 1,
+			zero : 5;
+
+	constexpr SegmentationType(bool useSeg, bool useZSeg, bool useG0Seg) noexcept
+		: useSegmentation(useSeg), useZSegmentation(useZSeg), useG0Segmentation(useG0Seg), zero(0)
+	{
+	}
+};
+
 class Kinematics INHERIT_OBJECT_MODEL
 {
 public:
@@ -204,8 +217,7 @@ public:
 	// Functions that return information held in this base class
 	KinematicsType GetKinematicsType() const noexcept { return type; }
 
-	bool UseSegmentation() const noexcept { return useSegmentation; }
-	bool UseRawG0() const noexcept { return useRawG0; }
+	SegmentationType GetSegmentationType() const noexcept { return segmentationType; }
 	float GetSegmentsPerSecond() const noexcept pre(UseSegmentation()) { return segmentsPerSecond; }
 	float GetMinSegmentLength() const noexcept pre(UseSegmentation()) { return minSegmentLength; }
 	float GetReciprocalMinSegmentLength() const noexcept pre(UseSegmentation()) { return reciprocalMinSegmentLength; }
@@ -213,8 +225,7 @@ public:
 protected:
 	DECLARE_OBJECT_MODEL_VIRTUAL
 
-	// Constructor. Pass segsPerSecond <= 0.0 to get non-segmented motion.
-	Kinematics(KinematicsType t, bool doUseSegmentation, bool doUseRawG0) noexcept;
+	Kinematics(KinematicsType t, SegmentationType segType) noexcept;
 
 	// Apply the M208 limits to the Cartesian position that the user wants to move to for all axes from the specified one upwards
 	// Return true if any coordinates were changed
@@ -229,10 +240,6 @@ protected:
 	static void PrintVector(const char *s, const float *v, size_t numElems) noexcept;
 	static void PrintVector(const char *s, const double *v, size_t numElems) noexcept;
 
-	float segmentsPerSecond;				// if we are using segmentation, the target number of segments/second
-	float minSegmentLength;					// if we are using segmentation, the minimum segment size
-	float reciprocalMinSegmentLength;		// if we are using segmentation, the reciprocal of minimum segment size
-
 	static const char * const HomeAllFileName;
 
 private:
@@ -240,8 +247,11 @@ private:
 	static constexpr float DefaultSegmentsPerSecond = 100.0;
 	static constexpr float DefaultMinSegmentLength = 0.2;
 
-	bool useSegmentation;					// true if we have to approximate linear movement using segmentation
-	bool useRawG0;							// true if we normally use segmentation but we do not need to segment travel moves
+	float segmentsPerSecond;				// if we are using segmentation, the target number of segments/second
+	float minSegmentLength;					// if we are using segmentation, the minimum segment size
+	float reciprocalMinSegmentLength;		// if we are using segmentation, the reciprocal of minimum segment size
+
+	SegmentationType segmentationType;		// the type of segmentation we are using
 	KinematicsType type;
 };
 
diff --git a/src/Movement/Kinematics/LinearDeltaKinematics.cpp b/src/Movement/Kinematics/LinearDeltaKinematics.cpp
index f18c804b..7328294c 100644
--- a/src/Movement/Kinematics/LinearDeltaKinematics.cpp
+++ b/src/Movement/Kinematics/LinearDeltaKinematics.cpp
@@ -55,7 +55,7 @@ DEFINE_GET_OBJECT_MODEL_TABLE(LinearDeltaKinematics)
 
 #endif
 
-LinearDeltaKinematics::LinearDeltaKinematics() noexcept : RoundBedKinematics(KinematicsType::linearDelta, false, true), numTowers(UsualNumTowers)
+LinearDeltaKinematics::LinearDeltaKinematics() noexcept : RoundBedKinematics(KinematicsType::linearDelta, SegmentationType(false, false, false)), numTowers(UsualNumTowers)
 {
 	Init();
 }
diff --git a/src/Movement/Kinematics/PolarKinematics.cpp b/src/Movement/Kinematics/PolarKinematics.cpp
index 6716baa1..79935f35 100644
--- a/src/Movement/Kinematics/PolarKinematics.cpp
+++ b/src/Movement/Kinematics/PolarKinematics.cpp
@@ -37,7 +37,7 @@ DEFINE_GET_OBJECT_MODEL_TABLE(PolarKinematics)
 
 // Constructor
 PolarKinematics::PolarKinematics() noexcept
-	: Kinematics(KinematicsType::polar, true, true),
+	: Kinematics(KinematicsType::polar, SegmentationType(true, false, false)),
 	  minRadius(0.0), maxRadius(DefaultMaxRadius), homedRadius(0.0),
 	  maxTurntableSpeed(DefaultMaxTurntableSpeed), maxTurntableAcceleration(DefaultMaxTurntableAcceleration)
 {
@@ -98,7 +98,7 @@ bool PolarKinematics::Configure(unsigned int mCode, GCodeBuffer& gb, const Strin
 		{
 			reply.printf("Kinematics is Polar with radius %.1f to %.1fmm, homed radius %.1fmm, segments/sec %d, min. segment length %.2f",
 							(double)minRadius, (double)maxRadius, (double)homedRadius,
-							(int)segmentsPerSecond, (double)minSegmentLength);
+							(int)GetSegmentsPerSecond(), (double)GetMinSegmentLength());
 		}
 		return seen;
 	}
diff --git a/src/Movement/Kinematics/RotaryDeltaKinematics.cpp b/src/Movement/Kinematics/RotaryDeltaKinematics.cpp
index 0477e53e..53d15d47 100644
--- a/src/Movement/Kinematics/RotaryDeltaKinematics.cpp
+++ b/src/Movement/Kinematics/RotaryDeltaKinematics.cpp
@@ -38,7 +38,7 @@ DEFINE_GET_OBJECT_MODEL_TABLE(RotaryDeltaKinematics)
 #endif
 
 // Constructor
-RotaryDeltaKinematics::RotaryDeltaKinematics() noexcept : RoundBedKinematics(KinematicsType::rotaryDelta, true, false)
+RotaryDeltaKinematics::RotaryDeltaKinematics() noexcept : RoundBedKinematics(KinematicsType::rotaryDelta, SegmentationType(true, true, true))
 {
 	Init();
 }
diff --git a/src/Movement/Kinematics/RoundBedKinematics.cpp b/src/Movement/Kinematics/RoundBedKinematics.cpp
index 82aad6b9..c35d68e3 100644
--- a/src/Movement/Kinematics/RoundBedKinematics.cpp
+++ b/src/Movement/Kinematics/RoundBedKinematics.cpp
@@ -9,8 +9,8 @@
 #include <Platform/RepRap.h>
 #include <Platform/Platform.h>
 
-RoundBedKinematics::RoundBedKinematics(KinematicsType t, bool doUseSegmentation, bool doUseRawG0) noexcept
-: Kinematics(t, doUseSegmentation, doUseRawG0), printRadiusSquared(0.0)
+RoundBedKinematics::RoundBedKinematics(KinematicsType t, SegmentationType segType) noexcept
+	: Kinematics(t, segType), printRadiusSquared(0.0)
 {
 }
 
diff --git a/src/Movement/Kinematics/RoundBedKinematics.h b/src/Movement/Kinematics/RoundBedKinematics.h
index b69d56a1..29db1b7e 100644
--- a/src/Movement/Kinematics/RoundBedKinematics.h
+++ b/src/Movement/Kinematics/RoundBedKinematics.h
@@ -19,7 +19,7 @@ public:
 	void LimitSpeedAndAcceleration(DDA& dda, const float *normalisedDirectionVector, size_t numVisibleAxes, bool continuousRotationShortcut) const noexcept override;
 	AxesBitmap GetLinearAxes() const noexcept override;
 protected:
-	RoundBedKinematics(KinematicsType t, bool doUseSegmentation, bool doUseRawG0) noexcept;
+	RoundBedKinematics(KinematicsType t, SegmentationType segType) noexcept;
 
 	float printRadiusSquared;
 };
diff --git a/src/Movement/Kinematics/ScaraKinematics.cpp b/src/Movement/Kinematics/ScaraKinematics.cpp
index 63d62347..29b9650d 100644
--- a/src/Movement/Kinematics/ScaraKinematics.cpp
+++ b/src/Movement/Kinematics/ScaraKinematics.cpp
@@ -37,7 +37,7 @@ DEFINE_GET_OBJECT_MODEL_TABLE_WITH_PARENT(ScaraKinematics, ZLeadscrewKinematics)
 #endif
 
 ScaraKinematics::ScaraKinematics() noexcept
-	: ZLeadscrewKinematics(KinematicsType::scara, true, true),
+	: ZLeadscrewKinematics(KinematicsType::scara, SegmentationType(true, false, false)),
 	  proximalArmLength(DefaultProximalArmLength), distalArmLength(DefaultDistalArmLength), xOffset(0.0), yOffset(0.0)
 {
 	thetaLimits[0] = DefaultMinTheta;
@@ -229,7 +229,7 @@ bool ScaraKinematics::Configure(unsigned int mCode, GCodeBuffer& gb, const Strin
 							(double)distalArmLength, (double)psiLimits[0], (double)psiLimits[1], (supportsContinuousRotation[0]) ? " (continuous)" : "",
 							(double)crosstalk[0], (double)crosstalk[1], (double)crosstalk[2],
 							(double)xOffset, (double)yOffset,
-							(int)segmentsPerSecond, (double)minSegmentLength);
+							(int)GetSegmentsPerSecond(), (double)GetMinSegmentLength());
 		}
 		return seen;
 	}
diff --git a/src/Movement/Kinematics/ZLeadscrewKinematics.cpp b/src/Movement/Kinematics/ZLeadscrewKinematics.cpp
index 2880c999..61293b5c 100644
--- a/src/Movement/Kinematics/ZLeadscrewKinematics.cpp
+++ b/src/Movement/Kinematics/ZLeadscrewKinematics.cpp
@@ -69,12 +69,12 @@ DEFINE_GET_OBJECT_MODEL_TABLE(ZLeadscrewKinematics)
 #endif
 
 ZLeadscrewKinematics::ZLeadscrewKinematics(KinematicsType k) noexcept
-	: Kinematics(k, false, true), numLeadscrews(0), correctionFactor(1.0), maxCorrection(1.0), screwPitch(M3ScrewPitch)
+	: Kinematics(k, SegmentationType(false, false, false)), numLeadscrews(0), correctionFactor(1.0), maxCorrection(1.0), screwPitch(M3ScrewPitch)
 {
 }
 
-ZLeadscrewKinematics::ZLeadscrewKinematics(KinematicsType k, bool doUseSegmentation, bool doUseRawG0) noexcept
-	: Kinematics(k, doUseSegmentation, doUseRawG0), numLeadscrews(0), correctionFactor(1.0), maxCorrection(1.0), screwPitch(M3ScrewPitch)
+ZLeadscrewKinematics::ZLeadscrewKinematics(KinematicsType k, SegmentationType segType) noexcept
+	: Kinematics(k, segType), numLeadscrews(0), correctionFactor(1.0), maxCorrection(1.0), screwPitch(M3ScrewPitch)
 {
 }
 
diff --git a/src/Movement/Kinematics/ZLeadscrewKinematics.h b/src/Movement/Kinematics/ZLeadscrewKinematics.h
index 6f096bcc..5c8d0834 100644
--- a/src/Movement/Kinematics/ZLeadscrewKinematics.h
+++ b/src/Movement/Kinematics/ZLeadscrewKinematics.h
@@ -15,7 +15,7 @@ class ZLeadscrewKinematics : public Kinematics
 {
 public:
 	ZLeadscrewKinematics(KinematicsType k) noexcept;
-	ZLeadscrewKinematics(KinematicsType t, bool doUseSegmentation, bool doUseRawG0) noexcept;
+	ZLeadscrewKinematics(KinematicsType t, SegmentationType segType) noexcept;
 
 	bool Configure(unsigned int mCode, GCodeBuffer& gb, const StringRef& reply, bool& error) THROWS(GCodeException) override;
 	bool SupportsAutoCalibration() const noexcept override;
@@ -33,7 +33,7 @@ protected:
 private:
 	void AppendCorrections(const floatc_t corrections[], const StringRef& reply) const noexcept;
 
-	static const unsigned int MaxLeadscrews = 8;			// some Folgertech FT5 printers have 8 bed adjusting screws
+	static const unsigned int MaxLeadscrews = 4;			// the maximum we support for auto bed levelling
 
 	unsigned int numLeadscrews;
 	float leadscrewX[MaxLeadscrews], leadscrewY[MaxLeadscrews];
diff --git a/src/Movement/Move.cpp b/src/Movement/Move.cpp
index ee139290..2feadeb5 100644
--- a/src/Movement/Move.cpp
+++ b/src/Movement/Move.cpp
@@ -624,6 +624,37 @@ void Move::InverseAxisTransform(float xyzPoint[MaxAxes], const Tool *tool) const
 	}
 }
 
+// Compute the height correction needed at a point, ignoring taper
+float Move::ComputeHeightCorrection(float xyzPoint[MaxAxes], const Tool *tool) const noexcept
+{
+	float zCorrection = 0.0;
+	unsigned int numCorrections = 0;
+	const GridDefinition& grid = GetGrid();
+	const AxesBitmap axis1Axes = Tool::GetAxisMapping(tool, grid.GetAxisNumber(1));
+
+	// Transform the Z coordinate based on the average correction for each axis used as an X or Y axis.
+	Tool::GetAxisMapping(tool, grid.GetAxisNumber(0))
+		.Iterate([this, xyzPoint, tool, axis1Axes, &zCorrection, &numCorrections](unsigned int axis0Axis, unsigned int)
+					{
+						const float axis0Coord = xyzPoint[axis0Axis] + Tool::GetOffset(tool, axis0Axis);
+						axis1Axes.Iterate([this, xyzPoint, tool, axis0Coord, &zCorrection, &numCorrections](unsigned int axis1Axis, unsigned int)
+											{
+												const float axis1Coord = xyzPoint[axis1Axis] + Tool::GetOffset(tool, axis1Axis);
+												zCorrection += heightMap.GetInterpolatedHeightError(axis0Coord, axis1Coord);
+												++numCorrections;
+											}
+										);
+					}
+				);
+
+	if (numCorrections > 1)
+	{
+		zCorrection /= numCorrections;			// take an average
+	}
+
+	return zCorrection + zShift;
+}
+
 // Do the bed transform AFTER the axis transform
 void Move::BedTransform(float xyzPoint[MaxAxes], const Tool *tool) const noexcept
 {
@@ -632,32 +663,7 @@ void Move::BedTransform(float xyzPoint[MaxAxes], const Tool *tool) const noexcep
 		const float toolHeight = xyzPoint[Z_AXIS] + Tool::GetOffset(tool, Z_AXIS);
 		if (!useTaper || toolHeight < taperHeight)
 		{
-			float zCorrection = 0.0;
-			unsigned int numCorrections = 0;
-			const GridDefinition& grid = GetGrid();
-			const AxesBitmap axis1Axes = Tool::GetAxisMapping(tool, grid.GetAxisNumber(1));
-
-			// Transform the Z coordinate based on the average correction for each axis used as an X or Y axis.
-			Tool::GetAxisMapping(tool, grid.GetAxisNumber(0))
-				.Iterate([this, xyzPoint, tool, axis1Axes, &zCorrection, &numCorrections](unsigned int axis0Axis, unsigned int)
-							{
-								const float axis0Coord = xyzPoint[axis0Axis] + Tool::GetOffset(tool, axis0Axis);
-								axis1Axes.Iterate([this, xyzPoint, tool, axis0Coord, &zCorrection, &numCorrections](unsigned int axis1Axis, unsigned int)
-													{
-														const float axis1Coord = xyzPoint[axis1Axis] + Tool::GetOffset(tool, axis1Axis);
-														zCorrection += heightMap.GetInterpolatedHeightError(axis0Coord, axis1Coord);
-														++numCorrections;
-													}
-												);
-							}
-						);
-
-			if (numCorrections > 1)
-			{
-				zCorrection /= numCorrections;			// take an average
-			}
-
-			zCorrection += zShift;
+			const float zCorrection = ComputeHeightCorrection(xyzPoint, tool);
 			xyzPoint[Z_AXIS] += (useTaper && zCorrection < taperHeight) ? (taperHeight - toolHeight) * recipTaperHeight * zCorrection : zCorrection;
 		}
 	}
@@ -668,33 +674,7 @@ void Move::InverseBedTransform(float xyzPoint[MaxAxes], const Tool *tool) const
 {
 	if (usingMesh)
 	{
-		float zCorrection = 0.0;
-		unsigned int numCorrections = 0;
-		const GridDefinition& grid = GetGrid();
-		const AxesBitmap axis1Axes = Tool::GetAxisMapping(tool, grid.GetAxisNumber(1));
-
-		// Transform the Z coordinate based on the average correction for each axis used as an X or Y axis.
-		Tool::GetAxisMapping(tool, grid.GetAxisNumber(0))
-			.Iterate([this, xyzPoint, tool, axis1Axes, &zCorrection, &numCorrections](unsigned int axis0Axis, unsigned int)
-						{
-							const float axis0Coord = xyzPoint[axis0Axis] + Tool::GetOffset(tool, axis0Axis);
-							axis1Axes.Iterate([this, xyzPoint, tool, axis0Coord, &zCorrection, &numCorrections](unsigned int axis1Axis, unsigned int)
-												{
-													const float axis1Coord = xyzPoint[axis1Axis] + Tool::GetOffset(tool, axis1Axis);
-													zCorrection += heightMap.GetInterpolatedHeightError(axis0Coord, axis1Coord);
-													++numCorrections;
-												}
-											);
-						}
-					);
-
-		if (numCorrections > 1)
-		{
-			zCorrection /= numCorrections;				// take an average
-		}
-
-		zCorrection += zShift;
-
+		const float zCorrection = ComputeHeightCorrection(xyzPoint, tool);
 		if (!useTaper || zCorrection >= taperHeight)	// need check on zCorrection to avoid possible divide by zero
 		{
 			xyzPoint[Z_AXIS] -= zCorrection;
@@ -989,7 +969,7 @@ bool Move::WriteResumeSettings(FileStore *f) const noexcept
 #endif
 
 // Process M204
-GCodeResult Move::ConfigureAccelerations(GCodeBuffer&gb, const StringRef& reply) noexcept
+GCodeResult Move::ConfigureAccelerations(GCodeBuffer&gb, const StringRef& reply) THROWS(GCodeException)
 {
 	bool seen = false;
 	if (gb.Seen('S'))
@@ -1020,7 +1000,7 @@ GCodeResult Move::ConfigureAccelerations(GCodeBuffer&gb, const StringRef& reply)
 }
 
 // Process M595
-GCodeResult Move::ConfigureMovementQueue(GCodeBuffer& gb, const StringRef& reply) noexcept
+GCodeResult Move::ConfigureMovementQueue(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeException)
 {
 	const size_t ringNumber = (gb.Seen('Q')) ? gb.GetLimitedUIValue('Q', ARRAY_SIZE(rings)) : 0;
 	return rings[ringNumber].ConfigureMovementQueue(gb, reply);
diff --git a/src/Movement/Move.h b/src/Movement/Move.h
index a4559968..5cfcf29e 100644
--- a/src/Movement/Move.h
+++ b/src/Movement/Move.h
@@ -93,8 +93,8 @@ public:
 
 	float PushBabyStepping(size_t axis, float amount) noexcept;				// Try to push some babystepping through the lookahead queue
 
-	GCodeResult ConfigureAccelerations(GCodeBuffer&gb, const StringRef& reply) noexcept;		// process M204
-	GCodeResult ConfigureMovementQueue(GCodeBuffer& gb, const StringRef& reply) noexcept;		// process M595
+	GCodeResult ConfigureAccelerations(GCodeBuffer&gb, const StringRef& reply) THROWS(GCodeException);		// process M204
+	GCodeResult ConfigureMovementQueue(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeException);		// process M595
 
 	float GetMaxPrintingAcceleration() const noexcept { return maxPrintingAcceleration; }
 	float GetMaxTravelAcceleration() const noexcept { return maxTravelAcceleration; }
@@ -120,7 +120,7 @@ public:
 	bool IsRawMotorMove(uint8_t moveType) const noexcept;									// Return true if this is a raw motor move
 
 	float IdleTimeout() const noexcept;														// Returns the idle timeout in seconds
-	void SetIdleTimeout(float timeout) noexcept;												// Set the idle timeout in seconds
+	void SetIdleTimeout(float timeout) noexcept;											// Set the idle timeout in seconds
 
 	void Simulate(uint8_t simMode) noexcept;												// Enter or leave simulation mode
 	float GetSimulationTime() const noexcept { return mainDDARing.GetSimulationTime(); }	// Get the accumulated simulation time
@@ -214,10 +214,11 @@ private:
 		timing			// no moves being executed or in queue, motors are at full current
 	};
 
-	void BedTransform(float move[MaxAxes], const Tool *tool) const noexcept;			// Take a position and apply the bed compensations
-	void InverseBedTransform(float move[MaxAxes],const  Tool *tool) const noexcept;		// Go from a bed-transformed point back to user coordinates
-	void AxisTransform(float move[MaxAxes], const Tool *tool) const noexcept;			// Take a position and apply the axis-angle compensations
-	void InverseAxisTransform(float move[MaxAxes], const Tool *tool) const noexcept;	// Go from an axis transformed point back to user coordinates
+	void BedTransform(float xyzPoint[MaxAxes], const Tool *tool) const noexcept;				// Take a position and apply the bed compensations
+	void InverseBedTransform(float xyzPoint[MaxAxes], const Tool *tool) const noexcept;			// Go from a bed-transformed point back to user coordinates
+	void AxisTransform(float xyzPoint[MaxAxes], const Tool *tool) const noexcept;				// Take a position and apply the axis-angle compensations
+	void InverseAxisTransform(float xyzPoint[MaxAxes], const Tool *tool) const noexcept;		// Go from an axis transformed point back to user coordinates
+	float ComputeHeightCorrection(float xyzPoint[MaxAxes], const Tool *tool) const noexcept;	// Compute the height correction needed at a point, ignoring taper
 
 	const char *GetCompensationTypeString() const noexcept;
 
diff --git a/src/Networking/ESP8266WiFi/WiFiInterface.cpp b/src/Networking/ESP8266WiFi/WiFiInterface.cpp
index 6f46ccc8..8be20e71 100644
--- a/src/Networking/ESP8266WiFi/WiFiInterface.cpp
+++ b/src/Networking/ESP8266WiFi/WiFiInterface.cpp
@@ -1149,35 +1149,7 @@ GCodeResult WiFiInterface::HandleWiFiCode(int mcode, GCodeBuffer &gb, const Stri
 		}
 
 	case 589:	// Configure access point
-		if (gb.Seen('T'))
-		{
-			// Special code to set max transmitter power, 0 to 20.5dBm
-			const float powerTimes4 = gb.GetFValue() * 4;
-			if (powerTimes4 < 0.0 || powerTimes4 > 82.0)
-			{
-				reply.copy("Power setting out of range");
-			}
-			else
-			{
-				const int32_t rslt = SendCommand(NetworkCommand::networkSetTxPower, 0, (uint8_t)powerTimes4, 0, nullptr, 0, nullptr, 0);
-				if (rslt == ResponseEmpty)
-				{
-					return GCodeResult::ok;
-				}
-				reply.printf("Failed to set maximum transmit power: %s", TranslateWiFiResponse(rslt));
-			}
-		}
-		else if (gb.Seen('C'))
-		{
-			const uint32_t clockVal = gb.GetUIValue();
-			const int32_t rslt = SendCommand(NetworkCommand::networkSetClockControl, 0, 0, clockVal, nullptr, 0, nullptr, 0);
-			if (rslt == ResponseEmpty)
-			{
-				return GCodeResult::ok;
-			}
-			reply.printf("Failed to set clock: %s", TranslateWiFiResponse(rslt));
-		}
-		else if (gb.Seen('S'))
+		if (gb.Seen('S'))
 		{
 			// Configure access point parameters
 			WirelessConfigurationData config;
@@ -1217,6 +1189,35 @@ GCodeResult WiFiInterface::HandleWiFiCode(int mcode, GCodeBuffer &gb, const Stri
 
 			reply.printf("Failed to configure access point parameters: %s", TranslateWiFiResponse(rslt));
 		}
+		else if (gb.Seen('T'))
+		{
+			// Special code to set max transmitter power, 0 to 20.5dBm
+			const float powerTimes4 = gb.GetFValue() * 4;
+			if (powerTimes4 < 0.0 || powerTimes4 > 82.0)
+			{
+				reply.copy("Power setting out of range");
+			}
+			else
+			{
+				const int32_t rslt = SendCommand(NetworkCommand::networkSetTxPower, 0, (uint8_t)powerTimes4, 0, nullptr, 0, nullptr, 0);
+				if (rslt == ResponseEmpty)
+				{
+					return GCodeResult::ok;
+				}
+				reply.printf("Failed to set maximum transmit power: %s", TranslateWiFiResponse(rslt));
+			}
+		}
+		else if (gb.Seen('L'))
+		{
+			// Special code to configure SPI clock speed
+			const uint32_t clockVal = gb.GetUIValue();
+			const int32_t rslt = SendCommand(NetworkCommand::networkSetClockControl, 0, 0, clockVal, nullptr, 0, nullptr, 0);
+			if (rslt == ResponseEmpty)
+			{
+				return GCodeResult::ok;
+			}
+			reply.printf("Failed to set clock: %s", TranslateWiFiResponse(rslt));
+		}
 		else
 		{
 			// Report access point parameters
diff --git a/src/ObjectModel/GlobalVariables.cpp b/src/ObjectModel/GlobalVariables.cpp
index 0d4cd7bf..0e532135 100644
--- a/src/ObjectModel/GlobalVariables.cpp
+++ b/src/ObjectModel/GlobalVariables.cpp
@@ -20,16 +20,15 @@ void GlobalVariables::ReportAsJson(OutputBuffer *buf, ObjectExplorationContext&
 	buf->cat('{');
 	if (context.IncreaseDepth())
 	{
-		bool needComma = false;
 		{
 			ReadLocker locker(lock);			// make sure that no other task modifies the list while we are traversing it
-
-			for (const Variable *v = vars.GetRoot(); v != nullptr; v = v->GetNext())
-			{
-				buf->catf((needComma) ? ",\"%s\":" : "\"%s\":", v->GetName().Ptr());
-				ReportItemAsJsonFull(buf, context, classDescriptor, v->GetValue(), filter);
-				needComma = true;
-			}
+			vars.IterateWhile([this, buf, &context, classDescriptor, filter](unsigned int index, const Variable& v) noexcept -> bool
+								{
+									buf->catf((index != 0) ? ",\"%s\":" : "\"%s\":", v.GetName().Ptr());
+									ReportItemAsJsonFull(buf, context, classDescriptor, v.GetValue(), filter);
+									return true;
+								}
+							 );
 		}
 		context.DecreaseDepth();
 	}
diff --git a/src/ObjectModel/ObjectModel.cpp b/src/ObjectModel/ObjectModel.cpp
index 14f458d0..6a1dfd51 100644
--- a/src/ObjectModel/ObjectModel.cpp
+++ b/src/ObjectModel/ObjectModel.cpp
@@ -15,6 +15,13 @@
 #include <cstring>
 #include <General/SafeStrtod.h>
 #include <General/IP4String.h>
+#include <Hardware/ExceptionHandlers.h>
+
+namespace StackUsage
+{
+	constexpr uint32_t GetObjectValue_noTable = 56;
+	constexpr uint32_t GetObjectValue_withTable = 48;
+}
 
 ExpressionValue::ExpressionValue(const MacAddress& mac) noexcept : type((uint32_t)TypeCode::MacAddress), param(mac.HighWord()), uVal(mac.LowWord())
 {
@@ -179,7 +186,7 @@ void ExpressionValue::Release() noexcept
 
 #if SUPPORT_CAN_EXPANSION
 
-// Given that this is a CanExpansionBoardDetails value, extract the part requested according to the parameter
+// Given that this is a CanExpansionBoardDetails value, extract the part requested according to the parameter and append it to the string
 // sVal is a string of the form shortName|version
 void ExpressionValue::ExtractRequestedPart(const StringRef& rslt) const noexcept
 {
@@ -192,22 +199,15 @@ void ExpressionValue::ExtractRequestedPart(const StringRef& rslt) const noexcept
 		switch((ExpansionDetail)param)
 		{
 		case ExpansionDetail::shortName:
-			rslt.copy(sVal, indexOfDivider);
+			rslt.catn(sVal, indexOfDivider);
 			break;
 
 		case ExpansionDetail::firmwareVersion:
-			if (p == nullptr)
-			{
-				rslt.Clear();
-			}
-			else
-			{
-				rslt.copy(sVal + indexOfDivider + 1);
-			}
+			rslt.cat((p == nullptr) ? "unknown" : sVal + indexOfDivider + 1);
 			break;
 
 		case ExpansionDetail::firmwareFileName:
-			rslt.copy("Duet3Firmware_");
+			rslt.cat("Duet3Firmware_");
 			rslt.catn(sVal, indexOfDivider);
 			rslt.cat(".bin");
 			break;
@@ -361,6 +361,26 @@ GCodeException ObjectExplorationContext::ConstructParseException(const char *msg
 	return GCodeException(line, column, msg, sparam);
 }
 
+// Call this before making a recursive call, or before calling a function that needs a lot of stack from a recursive function
+void ObjectExplorationContext::CheckStack(uint32_t calledFunctionStackUsage) const THROWS(GCodeException)
+{
+	register const char * stackPtr asm ("sp");
+	const char *stackLimit = (const char*)TaskBase::GetCallerTaskHandle() + sizeof(TaskBase);
+	if (stackLimit + calledFunctionStackUsage + (StackUsage::Throw + StackUsage::Margin) < stackPtr)
+	{
+		return;
+	}
+
+	// The stack is in danger of overflowing. Throw an exception if we have enough stack to do so (ideally, this should always be the case)
+	if (stackLimit + StackUsage::Throw <= stackPtr)
+	{
+		throw GCodeException(line, column, "Expression nesting too deep");
+	}
+
+	// Not enough stack left to throw an exception
+	SoftwareReset(SoftwareResetReason::stackOverflow, (const uint32_t *)stackPtr);
+}
+
 // Report this object
 void ObjectModel::ReportAsJson(OutputBuffer* buf, ObjectExplorationContext& context, const ObjectModelClassDescriptor * null classDescriptor,
 								uint8_t tableNumber, const char* filter) const THROWS(GCodeException)
@@ -850,7 +870,8 @@ int ObjectModelTableEntry::IdCompare(const char *id) const noexcept
 }
 
 // Get the value of an object
-ExpressionValue ObjectModel::GetObjectValue(ObjectExplorationContext& context, const ObjectModelClassDescriptor * null classDescriptor, const char *idString, uint8_t tableNumber) const
+ExpressionValue ObjectModel::GetObjectValue(ObjectExplorationContext& context, const ObjectModelClassDescriptor * null classDescriptor, const char *idString, uint8_t tableNumber) const THROWS(GCodeException)
+decrease(strlen(idString))	// recursion variant
 {
 	if (classDescriptor == nullptr)
 	{
@@ -868,6 +889,7 @@ ExpressionValue ObjectModel::GetObjectValue(ObjectExplorationContext& context, c
 			}
 			idString = GetNextElement(idString);
 			const ExpressionValue val = e->func(this, context);
+			context.CheckStack(StackUsage::GetObjectValue_noTable);
 			return GetObjectValue(context, classDescriptor, val, idString);
 		}
 		if (tableNumber != 0)
@@ -885,7 +907,8 @@ ExpressionValue ObjectModel::GetObjectValue(ObjectExplorationContext& context, c
 	throw context.ConstructParseException("unknown value '%s'", idString);
 }
 
-ExpressionValue ObjectModel::GetObjectValue(ObjectExplorationContext& context, const ObjectModelClassDescriptor *classDescriptor, const ExpressionValue& val, const char *idString) const
+ExpressionValue ObjectModel::GetObjectValue(ObjectExplorationContext& context, const ObjectModelClassDescriptor *classDescriptor, const ExpressionValue& val, const char *idString) const THROWS(GCodeException)
+decrease(strlen(idString))	// recursion variant
 {
 	if (*idString == 0 && context.WantExists() && val.GetType() != TypeCode::None)
 	{
@@ -923,6 +946,7 @@ ExpressionValue ObjectModel::GetObjectValue(ObjectExplorationContext& context, c
 			}
 
 			const ExpressionValue arrayElement = val.omadVal->GetElement(this, context);
+			context.CheckStack(StackUsage::GetObjectValue_noTable);
 			return GetObjectValue(context, classDescriptor, arrayElement, idString + 1);
 		}
 
@@ -932,6 +956,7 @@ ExpressionValue ObjectModel::GetObjectValue(ObjectExplorationContext& context, c
 		case 0:
 			return val;
 		case '.':
+			context.CheckStack(StackUsage::GetObjectValue_withTable);
 			return val.omVal->GetObjectValue(context, (val.omVal == this) ? classDescriptor : nullptr, idString + 1, val.param);
 		case '^':
 			throw context.ConstructParseException("object is not an array");
diff --git a/src/ObjectModel/ObjectModel.h b/src/ObjectModel/ObjectModel.h
index 24f810d3..6a2858e0 100644
--- a/src/ObjectModel/ObjectModel.h
+++ b/src/ObjectModel/ObjectModel.h
@@ -147,9 +147,11 @@ struct ExpressionValue
 	void Set(bool b) noexcept { Release(); type = (uint32_t)TypeCode::Bool; bVal = b; }
 	void Set(char c) noexcept { Release(); type = (uint32_t)TypeCode::Char; cVal = c; }
 	void Set(int32_t i) noexcept { Release(); type = (uint32_t)TypeCode::Int32; iVal = i; }
-	void Set(float f) noexcept { Release(); type = (uint32_t)TypeCode::Float; fVal = f; param = 1; }
+	void Set(float f) noexcept { Release(); type = (uint32_t)TypeCode::Float; fVal = f; param = MaxFloatDigitsDisplayedAfterPoint; }
+	void Set(float f, uint32_t digits) noexcept { Release(); type = (uint32_t)TypeCode::Float; fVal = f; param = digits; }
 	void Set(const char *s) noexcept { Release(); type = (uint32_t)TypeCode::CString; sVal = s; }
 	void Set(StringHandle sh) noexcept { Release(); type = (uint32_t)TypeCode::HeapString; shVal = sh; }
+	void Set(nullptr_t dummy) noexcept { Release();  type = (uint32_t)TypeCode::None; }
 
 	// Store a 56-bit value
 	void Set56BitValue(uint64_t v) { Release(); param = (uint32_t)(v >> 32) & 0x00FFFFFF; uVal = (uint32_t)v; }
@@ -219,6 +221,7 @@ public:
 
 	GCodeException ConstructParseException(const char *msg) const noexcept;
 	GCodeException ConstructParseException(const char *msg, const char *sparam) const noexcept;
+	void CheckStack(uint32_t calledFunctionStackUsage) const THROWS(GCodeException);
 
 private:
 	static constexpr size_t MaxIndices = 4;			// max depth of array nesting
@@ -266,7 +269,7 @@ public:
 	void ReportAsJson(OutputBuffer *buf, const char *filter, const char *reportFlags, bool wantArrayLength) const THROWS(GCodeException);
 
 	// Get the value of an object via the table
-	ExpressionValue GetObjectValue(ObjectExplorationContext& context, const ObjectModelClassDescriptor * null classDescriptor, const char *idString, uint8_t tableNumber = 0) const THROWS(GCodeException);
+	ExpressionValue GetObjectValue(ObjectExplorationContext& context, const ObjectModelClassDescriptor * null classDescriptor, const char *idString, uint8_t tableNumber) const THROWS(GCodeException);
 
 	// Function to report a value or object as JSON. This does not need to handle 'var' or 'global' because those are checked for before this is called.
 	void ReportItemAsJson(OutputBuffer *buf, ObjectExplorationContext& context, const ObjectModelClassDescriptor *classDescriptor,
diff --git a/src/ObjectModel/Variable.cpp b/src/ObjectModel/Variable.cpp
index 85c143f8..65e6b55f 100644
--- a/src/ObjectModel/Variable.cpp
+++ b/src/ObjectModel/Variable.cpp
@@ -118,4 +118,25 @@ VariableSet::~VariableSet()
 	Clear();
 }
 
+// Delete any existing variables and instead use the variables from the argument, taking ownership of them
+void VariableSet::AssignFrom(VariableSet& other) noexcept
+{
+	Clear();
+	root = other.root;
+	other.root = nullptr;
+}
+
+void VariableSet::IterateWhile(stdext::inplace_function<bool(unsigned int, const Variable&) /*noexcept*/ > func) const noexcept
+{
+	unsigned int num = 0;
+	for (const Variable *v = root; v != nullptr; v = v->GetNext())
+	{
+		if (!func(num, *v))
+		{
+			break;
+		}
+		++num;
+	}
+}
+
 // End
diff --git a/src/ObjectModel/Variable.h b/src/ObjectModel/Variable.h
index c40cff59..28717688 100644
--- a/src/ObjectModel/Variable.h
+++ b/src/ObjectModel/Variable.h
@@ -45,15 +45,20 @@ class VariableSet
 public:
 	VariableSet() noexcept : root(nullptr) { }
 	~VariableSet();
+	VariableSet(const VariableSet&) = delete;
+	VariableSet& operator=(const VariableSet& other) = delete;
+
+	void AssignFrom(VariableSet& other) noexcept;
 
 	Variable *Lookup(const char *str) noexcept;
 	const Variable *Lookup(const char *str) const noexcept;
-	const Variable *GetRoot() const noexcept { return root; }
 	void Insert(Variable *toInsert) noexcept;
 	void EndScope(uint8_t blockNesting) noexcept;
 	void Delete(const char *str) noexcept;
 	void Clear() noexcept;
 
+	void IterateWhile(stdext::inplace_function<bool(unsigned int index, const Variable& v) /*noexcept*/ > func) const noexcept;
+
 private:
 	Variable *root;
 };
diff --git a/src/Platform/Heap.cpp b/src/Platform/Heap.cpp
index 1f7f389a..5f34a6f6 100644
--- a/src/Platform/Heap.cpp
+++ b/src/Platform/Heap.cpp
@@ -353,6 +353,29 @@ StringHandle::StringHandle(const char *s, size_t len) noexcept
 	}
 }
 
+#if 0	// This constructor is currently unused, but may be useful in future
+// Build a handle by concatenating two strings
+StringHandle::StringHandle(const char *s1, const char *s2) noexcept
+{
+	const size_t len = strlen(s1) + strlen(s2);
+	if (len == 0)
+	{
+		slotPtr = nullptr;
+	}
+	else
+	{
+		WriteLocker locker(heapLock);							// prevent other tasks modifying the heap
+		IndexSlot * const slot = AllocateHandle();
+		StorageSpace * const space = AllocateSpace(len + 1);
+		SafeStrncpy(space->data, s1, space->length);
+		SafeStrncat(space->data, s2, space->length);
+		slot->storage = space;
+		slot->refCount = 1;
+		slotPtr = slot;
+	}
+}
+#endif
+
 void StringHandle::Assign(const char *s) noexcept
 {
 	Delete();
@@ -366,11 +389,9 @@ void StringHandle::Assign(const char *s) noexcept
 
 void StringHandle::InternalAssign(const char *s, size_t len) noexcept
 {
-	IndexSlot * const slot = AllocateHandle();					// allocate a handle
+	IndexSlot * const slot = AllocateHandle();
 	StorageSpace * const space = AllocateSpace(len + 1);
-	const size_t lengthToCopy = min<size_t>(len, space->length - 1);	// truncate the string if it won't fit
-	memcpy(space->data, s, lengthToCopy);
-	space->data[lengthToCopy] = 0;
+	SafeStrncpy(space->data, s, space->length);
 	slot->storage = space;
 	slot->refCount = 1;
 	slotPtr = slot;
@@ -380,7 +401,7 @@ void StringHandle::Delete() noexcept
 {
 	if (slotPtr != nullptr)
 	{
-		ReadLocker locker(heapLock);						// prevent other tasks modifying the heap
+		ReadLocker locker(heapLock);							// prevent other tasks modifying the heap
 		InternalDelete();
 	}
 }
diff --git a/src/Platform/Heap.h b/src/Platform/Heap.h
index 5fdca862..4d6009f5 100644
--- a/src/Platform/Heap.h
+++ b/src/Platform/Heap.h
@@ -26,6 +26,10 @@ public:
 	StringHandle(const char *s) noexcept;
 	StringHandle(const char *s, size_t len) noexcept;
 
+#if 0	// unused
+	StringHandle(const char *s1, const char *s2) noexcept;
+#endif
+
 	ReadLockedPointer<const char> Get() const noexcept;
 	size_t GetLength() const noexcept;
 	void Delete() noexcept;
diff --git a/src/Tools/Tool.cpp b/src/Tools/Tool.cpp
index 3a39e39c..6806207a 100644
--- a/src/Tools/Tool.cpp
+++ b/src/Tools/Tool.cpp
@@ -442,7 +442,7 @@ void Tool::Activate() noexcept
 	state = ToolState::active;
 }
 
-void Tool::Standby() noexcept
+void Tool::HeatersToStandby() const noexcept
 {
 	const Tool * const currentTool = reprap.GetCurrentTool();
 	for (size_t heater = 0; heater < heaterCount; heater++)
@@ -463,6 +463,48 @@ void Tool::Standby() noexcept
 			}
 		}
 	}
+}
+
+void Tool::HeatersToActive() const noexcept
+{
+	const Tool * const currentTool = reprap.GetCurrentTool();
+	for (size_t heater = 0; heater < heaterCount; heater++)
+	{
+		// Don't switch a heater to active if the active tool is using it and is different from this tool
+		if (currentTool == this || currentTool == nullptr || !currentTool->UsesHeater(heater))
+		{
+			try
+			{
+				reprap.GetHeat().SetActiveTemperature(heaters[heater], activeTemperatures[heater]);
+				String<1> dummy;
+				(void)reprap.GetHeat().Activate(heaters[heater], dummy.GetRef());
+			}
+			catch (const GCodeException& exc)
+			{
+				String<StringLength100> message;
+				exc.GetMessage(message.GetRef(), nullptr);
+				reprap.GetPlatform().Message(ErrorMessage, message.c_str());
+			}
+		}
+	}
+}
+
+void Tool::HeatersToOff() const noexcept
+{
+	const Tool * const currentTool = reprap.GetCurrentTool();
+	for (size_t heater = 0; heater < heaterCount; heater++)
+	{
+		// Don't switch a heater to standby if the active tool is using it and is different from this tool
+		if (currentTool == this || currentTool == nullptr || !currentTool->UsesHeater(heater))
+		{
+			reprap.GetHeat().SwitchOff(heaters[heater]);
+		}
+	}
+}
+
+void Tool::Standby() noexcept
+{
+	HeatersToStandby();
 
 	// NIST Standard M6 says "When the tool change is complete: * The spindle will be stopped. [...]"
 	// We don't have M6 but Tn already does tool change so we need
@@ -472,6 +514,7 @@ void Tool::Standby() noexcept
 		Spindle& spindle = reprap.GetPlatform().AccessSpindle(spindleNumber);
 		spindle.SetState(SpindleState::stopped);
 	}
+
 	state = ToolState::standby;
 }
 
diff --git a/src/Tools/Tool.h b/src/Tools/Tool.h
index 377ce0bf..6c8f01a0 100644
--- a/src/Tools/Tool.h
+++ b/src/Tools/Tool.h
@@ -116,6 +116,10 @@ public:
 
 	void SetFansPwm(float f) const noexcept;
 
+	void HeatersToOff() const noexcept;
+	void HeatersToActive() const noexcept;
+	void HeatersToStandby() const noexcept;
+
 protected:
 	DECLARE_OBJECT_MODEL
 	OBJECT_MODEL_ARRAY(activeTemps)
diff --git a/src/Version.h b/src/Version.h
index c1a1566a..0cd87871 100644
--- a/src/Version.h
+++ b/src/Version.h
@@ -9,7 +9,7 @@
 #define SRC_VERSION_H_
 
 #ifndef VERSION
-# define MAIN_VERSION	"3.3RC1"
+# define MAIN_VERSION	"3.3RC2+1"
 # ifdef USE_CAN0
 #  define VERSION_SUFFIX	" (CAN0)"
 # else