Version 1.20beta2

New features: On SCARA printera arm position limits are applied as well as XY size limits Heater 0 values are sent to to PanelDue even if there is no heated bed When logging is enabled, log entries are now written for when the date/time/date is set When logging is enabled, "Maximum open file count exceeded" messages are logged Loss of power is now handled much faster. The print is paused in the middle of a move if necessary. The M991 parameters are changed to facilitate this. When resuming a print after loss of power, the head is now moved, sideways and finally down when restoring position Following a power failure, M916 can now be used to resume the print instead of using M98 Presurrect.g The heater control now switches to fast PID parameters when the temperature is within 3C of target, instead of within 1C The TMC2660 Stallguard detection and Coolstep parameters may now be configured using M915. Currently, no action os performed when a stall is signalled. If a heater fault occurs, the print is paused instead of cancelled All error messages relating to incorrect use of a G- or M-code now include the G- or M-number of the command that caused them Increased ADC oversample bits to 2 Duet WiFi: M122 diagnostics now include the wifi module sleep mode and additional network diagnostics You can now disable monitorng of TMC2660 drivers rthat are not in use by using parameter R-1 in the corresponding M569 command The M585 (probe tool) command is now implemented (thanks chrrishamm) If axis lengths are adjusted by probing, a subsequent M500 command saves them in config-override.g If tool offsets are adjusted by probing, a subsequent M500 command saves them in config-override.g The layer counting mechanism has been modified to better handle GCode files that use a different layer height when printing support Debug messages sent to the USB port are truncated or thrown away if a software watchdog reset is imminent XY speed limiting is now done separate for each kinematics, in particular for CoreXY printers Support for Polar kinematics has been added but not tested (see M669 command) The TMC2660 drivers are configured to detect short-to-ground conditions faster The parameters in rr_ http commands are now all order-independent Bug fixes An error in computing the time taken to execute moves that were not yet frozen caused the first movement on a SCARA printer following homing to be jerky An extra space in the output from the M114 command confused Printerface, causing it to print exception messages When tuning a heater, any previous maximum PWM that was set is now ignored
author: David Crocker <dcrocker@eschertech.com> 2017-10-26 00:36:48 +0300
committer: David Crocker <dcrocker@eschertech.com> 2017-10-26 00:37:05 +0300
commit: ad73549a01954de291cea135cde2527d1883596e (patch)
tree: 0fcd0f7b05c9e40e986851c5f6455c0f1f589108
parent: dff15aadac70efe87d3ec952af8b6a3d9acbcf66 (diff)
76 files changed, 3500 insertions, 1874 deletions
diff --git a/Release/Duet-Ethernet/Edge/DuetEthernetFirmware-1.20alpha7.bin b/Release/Duet-Ethernet/Edge/DuetEthernetFirmware-1.20alpha7.bin
deleted file mode 100644
index 32671020..00000000
--- a/Release/Duet-Ethernet/Edge/DuetEthernetFirmware-1.20alpha7.bin
+++ /dev/null
diff --git a/Release/Duet-Ethernet/Edge/DuetEthernetFirmware-1.20beta2.bin b/Release/Duet-Ethernet/Edge/DuetEthernetFirmware-1.20beta2.bin
new file mode 100644
index 00000000..73a3aceb
--- /dev/null
+++ b/Release/Duet-Ethernet/Edge/DuetEthernetFirmware-1.20beta2.bin
diff --git a/Release/Duet-WiFi/Edge/DuetWiFiFirmware-1.20alpha7.bin b/Release/Duet-WiFi/Edge/DuetWiFiFirmware-1.20alpha7.bin
deleted file mode 100644
index c46746be..00000000
--- a/Release/Duet-WiFi/Edge/DuetWiFiFirmware-1.20alpha7.bin
+++ /dev/null
diff --git a/Release/Duet-WiFi/Edge/DuetWiFiFirmware-1.20beta2.bin b/Release/Duet-WiFi/Edge/DuetWiFiFirmware-1.20beta2.bin
new file mode 100644
index 00000000..b0086459
--- /dev/null
+++ b/Release/Duet-WiFi/Edge/DuetWiFiFirmware-1.20beta2.bin
diff --git a/Release/Duet-WiFi/Edge/DuetWiFiServer-1.20beta2.bin b/Release/Duet-WiFi/Edge/DuetWiFiServer-1.20beta2.bin
new file mode 100644
index 00000000..47bb4ec1
--- /dev/null
+++ b/Release/Duet-WiFi/Edge/DuetWiFiServer-1.20beta2.bin
diff --git a/src/Alligator/Pins_Alligator.h b/src/Alligator/Pins_Alligator.h
index d8eb673b..1167e492 100644
--- a/src/Alligator/Pins_Alligator.h
+++ b/src/Alligator/Pins_Alligator.h
@@ -7,6 +7,9 @@
 #define HAS_LWIP_NETWORKING		1
 #define HAS_CPU_TEMP_SENSOR		1
 #define HAS_HIGH_SPEED_SD		0
+#define HAS_SMART_DRIVERS		0
+#define HAS_VOLTAGE_MONITOR		0
+#define ACTIVE_LOW_HEAT_ON		0
 
 const size_t NumFirmwareUpdateModules = 1;
 #define IAP_UPDATE_FILE "iapalligator.bin"
@@ -40,7 +43,7 @@ const size_t NUM_SERIAL_CHANNELS = 3;			// The number of serial IO channels (USB
 #define SERIAL_AUX_DEVICE Serial
 #define SERIAL_AUX2_DEVICE Serial1
 
-// The numbers of entries in each array must correspond with the values of DRIVES, AXES, or HEATERS. Set values to -1 to flag unavailability.
+// The numbers of entries in each array must correspond with the values of DRIVES, AXES, or HEATERS. Set values to NoPin to flag unavailability.
 // DRIVES
 const Pin ENABLE_PINS[DRIVES] = { 24, NoPin, NoPin, NoPin, NoPin, NoPin, NoPin };
 const Pin STEP_PINS[DRIVES] = { X16, X14, X1, 5, 28, 11, X9 };
@@ -65,7 +68,6 @@ const size_t MaxSpiDac = 2;
 const Pin SPI_DAC_CS[MaxSpiDac] = { 53, 6 };
 
 // HEATERS
-const bool HEAT_ON = true;												// Not inverted heater for Alligator
 const Pin TEMP_SENSE_PINS[Heaters] = HEATERS_(1, 0, 2, 3, 4, f, g, h);	// Analogue pin numbers
 
 // h1,h2,h3,h4: X2,8,9,X8 is hardware PWM
diff --git a/src/Configuration.h b/src/Configuration.h
index 5d641841..5f659f4b 100644
--- a/src/Configuration.h
+++ b/src/Configuration.h
@@ -131,16 +131,18 @@ constexpr unsigned int DefaultPinWritePwmFreq = 500;	// default PWM frequency fo
 //     Using single-precision maths and up to 9-factor calibration: (9 + 5) * 4 bytes per point
 //     Using double-precision maths and up to 9-factor calibration: (9 + 5) * 8 bytes per point
 //   So 32 points using double precision arithmetic need 3584 bytes of stack space.
-#ifdef DUET_NG
+#if SAM4E || SAM4S
 constexpr size_t MaxGridProbePoints = 441;				// 441 allows us to probe e.g. 400x400 at 20mm intervals
 constexpr size_t MaxXGridPoints = 41;					// Maximum number of grid points in one X row
 constexpr size_t MaxProbePoints = 32;					// Maximum number of G30 probe points
 constexpr size_t MaxDeltaCalibrationPoints = 32;		// Should a power of 2 for speed
-#else
+#elif SAM3XA
 constexpr size_t MaxGridProbePoints = 121;				// 121 allows us to probe 200x200 at 20mm intervals
 constexpr size_t MaxXGridPoints = 21;					// Maximum number of grid points in one X row
 constexpr size_t MaxProbePoints = 32;					// Maximum number of G30 probe points
 constexpr size_t MaxDeltaCalibrationPoints = 32;		// Should a power of 2 for speed
+#else
+# error
 #endif
 
 const float DefaultGridSpacing = 20.0;				// Default bed probing grid spacing in mm
@@ -171,14 +173,16 @@ constexpr size_t FILENAME_LENGTH = 100;
 constexpr size_t MaxHeaterNameLength = 20;				// Maximum number of characters in a heater name
 
 // Output buffer lengths
-#ifdef DUET_NG
+#if SAM4E || SAM4S
 constexpr uint16_t OUTPUT_BUFFER_SIZE = 256;			// How many bytes does each OutputBuffer hold?
 constexpr size_t OUTPUT_BUFFER_COUNT = 32;				// How many OutputBuffer instances do we have?
 constexpr size_t RESERVED_OUTPUT_BUFFERS = 1;			// Number of reserved output buffers after long responses. Must be enough for an HTTP header
-#else
+#elif SAM3XA
 constexpr uint16_t OUTPUT_BUFFER_SIZE = 128;			// How many bytes does each OutputBuffer hold?
 constexpr size_t OUTPUT_BUFFER_COUNT = 32;				// How many OutputBuffer instances do we have?
 constexpr size_t RESERVED_OUTPUT_BUFFERS = 2;			// Number of reserved output buffers after long responses. Must be enough for an HTTP header
+#else
+# error
 #endif
 
 // Move system
diff --git a/src/Duet/Pins_Duet.h b/src/Duet/Pins_Duet.h
index 768f686e..76623c34 100644
--- a/src/Duet/Pins_Duet.h
+++ b/src/Duet/Pins_Duet.h
@@ -7,6 +7,9 @@
 #define HAS_LWIP_NETWORKING		1
 #define HAS_CPU_TEMP_SENSOR		1
 #define HAS_HIGH_SPEED_SD		1
+#define HAS_SMART_DRIVERS		0
+#define HAS_VOLTAGE_MONITOR		0
+#define ACTIVE_LOW_HEAT_ON		1
 
 const size_t NumFirmwareUpdateModules = 1;
 #define IAP_UPDATE_FILE "iap.bin"
@@ -40,9 +43,9 @@ const size_t NUM_SERIAL_CHANNELS = 3;			// The number of serial IO channels (USB
 #define SERIAL_AUX_DEVICE Serial
 #define SERIAL_AUX2_DEVICE Serial1
 
-// The numbers of entries in each array must correspond with the values of DRIVES, AXES, or HEATERS. Set values to -1 to flag unavailability.
+// The numbers of entries in each array must correspond with the values of DRIVES, AXES, or HEATERS. Set values to NoPin to flag unavailability.
 
-// DRIVES
+// Drives
 
 const Pin ENABLE_PINS[DRIVES] = { 29, 27, X1, X0, 37, X8, 50, 47, X13 };
 const Pin STEP_PINS[DRIVES] = { 14, 25, 5, X2, 41, 39, X4, 49, X10 };
@@ -61,9 +64,6 @@ const float STEPPER_DAC_VOLTAGE_RANGE = 2.02;							// Stepper motor current ref
 const float STEPPER_DAC_VOLTAGE_OFFSET = -0.025;						// Stepper motor current offset voltage for E1 if using a DAC
 
 // HEATERS
-
-const bool HEAT_ON = false;												// false for inverted heater (e.g. Duet v0.6), true for not (e.g. Duet v0.4)
-
 const Pin TEMP_SENSE_PINS[Heaters] = { 5, 4, 0, 7, 8, 9, 11 };			// Analogue pin numbers
 const Pin HEAT_ON_PINS[Heaters] = { 6, X5, X7, 7, 8, 9, X17 };			// Heater Channel 7 (pin X17) is shared with Fan1
 
diff --git a/src/Duet/Webserver.cpp b/src/Duet/Webserver.cpp
index 27392548..bbb431a3 100644
--- a/src/Duet/Webserver.cpp
+++ b/src/Duet/Webserver.cpp
@@ -831,10 +831,14 @@ void Webserver::HttpInterpreter::SendJsonResponse(const char* command)
 			return;
 		}
 
-		if (StringEquals(command, "download") && StringEquals(qualifiers[0].key, "name"))
+		if (StringEquals(command, "download"))
 		{
-			SendFile(qualifiers[0].value, false);
-			return;
+			const char* const filename = GetKeyValue("name");
+			if (filename != nullptr)
+			{
+				SendFile(filename, false);
+				return;
+			}
 		}
 	}
 
@@ -905,11 +909,12 @@ void Webserver::HttpInterpreter::GetJsonResponse(const char* request, OutputBuff
 			if (Authenticate())
 			{
 				// See if we can update the current RTC date and time
-				if (numQualKeys > 1 && StringEquals(qualifiers[1].key, "time") && !platform->IsDateTimeSet())
+				const char* const timeString = GetKeyValue("time");
+				if (timeString != nullptr && !platform->IsDateTimeSet())
 				{
 					struct tm timeInfo;
 					memset(&timeInfo, 0, sizeof(timeInfo));
-					if (strptime(qualifiers[1].value, "%Y-%m-%dT%H:%M:%S", &timeInfo) != nullptr)
+					if (strptime(timeString, "%Y-%m-%dT%H:%M:%S", &timeInfo) != nullptr)
 					{
 						time_t newTime = mktime(&timeInfo);
 						platform->SetDateTime(newTime);
@@ -1527,7 +1532,8 @@ bool Webserver::HttpInterpreter::ProcessMessage()
 								  || (commandWords[1][0] == '/' && StringEquals(commandWords[1] + 1, KO_START "upload"));
 		if (isUploadRequest)
 		{
-			if (numQualKeys > 0 && StringEquals(qualifiers[0].key, "name"))
+			const char* const filename = GetKeyValue("name");
+			if (filename != nullptr)
 			{
 				// We cannot upload more than one file at once
 				if (IsUploading())
@@ -1537,7 +1543,7 @@ bool Webserver::HttpInterpreter::ProcessMessage()
 
 				// See how many bytes we expect to read
 				bool contentLengthFound = false;
-				for(size_t i=0; i<numHeaderKeys; i++)
+				for (size_t i = 0; i < numHeaderKeys; i++)
 				{
 					if (StringEquals(headers[i].key, "Content-Length"))
 					{
@@ -1554,18 +1560,19 @@ bool Webserver::HttpInterpreter::ProcessMessage()
 				}
 
 				// Start a new file upload
-				FileStore *file = platform->GetFileStore(FS_PREFIX, qualifiers[0].value, OpenMode::write);
-				if (!StartUpload(file, qualifiers[0].value))
+				FileStore *file = platform->GetFileStore(FS_PREFIX, filename, OpenMode::write);
+				if (!StartUpload(file, filename))
 				{
 					return RejectMessage("could not start file upload");
 				}
 
 				// Try to get the last modified file date and time
-				if (numQualKeys > 1 && StringEquals(qualifiers[1].key, "time"))
+				const char* const lastModifiedString = GetKeyValue("time");
+				if (lastModifiedString != nullptr)
 				{
 					struct tm timeInfo;
 					memset(&timeInfo, 0, sizeof(timeInfo));
-					if (strptime(qualifiers[1].value, "%Y-%m-%dT%H:%M:%S", &timeInfo) != nullptr)
+					if (strptime(lastModifiedString, "%Y-%m-%dT%H:%M:%S", &timeInfo) != nullptr)
 					{
 						fileLastModified  = mktime(&timeInfo);
 					}
@@ -1581,7 +1588,7 @@ bool Webserver::HttpInterpreter::ProcessMessage()
 
 				if (reprap.Debug(moduleWebserver))
 				{
-					platform->MessageF(UsbMessage, "Start uploading file %s length %lu\n", qualifiers[0].value, postFileLength);
+					platform->MessageF(UsbMessage, "Start uploading file %s length %lu\n", filename, postFileLength);
 				}
 				uploadedBytes = 0;
 
diff --git a/src/DuetNG/DuetWiFi/Network.cpp b/src/DuetNG/DuetWiFi/Network.cpp
index 3bb96387..b72e992d 100644
--- a/src/DuetNG/DuetWiFi/Network.cpp
+++ b/src/DuetNG/DuetWiFi/Network.cpp
@@ -376,6 +376,7 @@ void Network::Start()
 	// The ESP takes about 300ms before it starts talking to us, so don't wait for it here, do that in Spin()
 	spiTxUnderruns = spiRxOverruns = 0;
 	reconnectCount = 0;
+	transferAlreadyPendingCount = readyTimeoutCount = responseTimeoutCount = 0;
 
 	lastTickMillis = millis();
 	state = NetworkState::starting1;
@@ -475,6 +476,7 @@ void Network::Spin(bool full)
 		{
 			if (espStatusChanged && digitalRead(EspTransferRequestPin))
 			{
+				Platform::softwareResetDebugInfo = 1;
 				if (reprap.Debug(moduleNetwork))
 				{
 					debugPrintf("ESP reported status change\n");
@@ -487,6 +489,7 @@ void Network::Spin(bool full)
 					 && currentMode != WiFiState::autoReconnecting
 					)
 			{
+				Platform::softwareResetDebugInfo = 2;
 				// Tell the wifi module to change mode
 				int32_t rslt = ResponseUnknownError;
 				if (currentMode != WiFiState::idle)
@@ -515,6 +518,7 @@ void Network::Spin(bool full)
 			}
 			else if (currentMode == WiFiState::connected || currentMode == WiFiState::runningAsAccessPoint)
 			{
+				Platform::softwareResetDebugInfo = 3;
 				// Find the next socket to poll
 				const size_t startingSocket = currentSocket;
 				do
@@ -549,6 +553,7 @@ void Network::Spin(bool full)
 						{
 							nr = responders;		// 'responders' can't be null at this point
 						}
+						Platform::softwareResetDebugInfo = 4;
 						doneSomething = nr->Spin();
 						nr = nr->GetNext();
 					} while (!doneSomething && nr != nextResponderToPoll);
@@ -558,6 +563,7 @@ void Network::Spin(bool full)
 		}
 		else if (currentMode == requestedMode && (currentMode == WiFiState::connected || currentMode == WiFiState::runningAsAccessPoint))
 		{
+			Platform::softwareResetDebugInfo = 5;
 			sockets[currentSocket].Poll(full);
 			++currentSocket;
 			if (currentSocket == NumTcpSockets)
@@ -608,6 +614,7 @@ void Network::Spin(bool full)
 		break;
 	}
 
+	Platform::softwareResetDebugInfo = 0;
 	if (full)
 	{
 		platform.ClassReport(longWait);
@@ -651,10 +658,13 @@ void Network::Diagnostics(MessageType mtype)
 {
 	platform.MessageF(mtype, "Network state is %s\n", TranslateNetworkState());
 	platform.MessageF(mtype, "WiFi module is %s\n", TranslateWiFiState(currentMode));
+	platform.MessageF(mtype, "Failed messages: pending %u, notready %u, noresp %u\n", transferAlreadyPendingCount, readyTimeoutCount, responseTimeoutCount);
+
 #if 0
 	// The underrun/overrun counters don't work at present
 	platform.MessageF(mtype, "SPI underruns %u, overruns %u\n", spiTxUnderruns, spiRxOverruns);
 #endif
+
 	if (state != NetworkState::disabled && state != NetworkState::starting1 && state != NetworkState::starting2)
 	{
 		Receiver<NetworkStatusResponse> status;
@@ -675,7 +685,8 @@ void Network::Diagnostics(MessageType mtype)
 
 			if (currentMode == WiFiState::connected)
 			{
-				platform.MessageF(mtype, "WiFi signal strength %ddBm\nReconnections %u\n", (int)r.rssi, reconnectCount);
+				const char* const sleepMode = (r.sleepMode == 1) ? "none" : (r.sleepMode == 2) ? "light" : (r.sleepMode == 3) ? "modem" : "unknown";
+				platform.MessageF(mtype, "WiFi signal strength %ddBm, reconnections %u, sleep mode %s\n", (int)r.rssi, reconnectCount, sleepMode);
 			}
 			else if (currentMode == WiFiState::runningAsAccessPoint)
 			{
@@ -1192,6 +1203,7 @@ int32_t Network::SendCommand(NetworkCommand cmd, SocketNumber socketNum, uint8_t
 		{
 			debugPrintf("ResponseBusy\n");
 		}
+		++transferAlreadyPendingCount;
 		return ResponseBusy;
 	}
 
@@ -1206,6 +1218,7 @@ int32_t Network::SendCommand(NetworkCommand cmd, SocketNumber socketNum, uint8_t
 				{
 					debugPrintf("ResponseBusy\n");
 				}
+				++readyTimeoutCount;
 				return ResponseBusy;
 			}
 		}
@@ -1255,6 +1268,7 @@ int32_t Network::SendCommand(NetworkCommand cmd, SocketNumber socketNum, uint8_t
 				}
 				transferPending = false;
 				spi_dma_disable();
+				++responseTimeoutCount;
 				return ResponseTimeout;
 			}
 		}
diff --git a/src/DuetNG/DuetWiFi/Network.h b/src/DuetNG/DuetWiFi/Network.h
index 4c97f7e8..fa89cb0b 100644
--- a/src/DuetNG/DuetWiFi/Network.h
+++ b/src/DuetNG/DuetWiFi/Network.h
@@ -158,6 +158,9 @@ private:
 	unsigned int spiTxUnderruns;
 	unsigned int spiRxOverruns;
 	unsigned int reconnectCount;
+	unsigned int transferAlreadyPendingCount;
+	unsigned int readyTimeoutCount;
+	unsigned int responseTimeoutCount;
 
 	char wiFiServerVersion[16];
 };
diff --git a/src/DuetNG/HttpResponder.cpp b/src/DuetNG/HttpResponder.cpp
index 484b0d50..cb25d9d8 100644
--- a/src/DuetNG/HttpResponder.cpp
+++ b/src/DuetNG/HttpResponder.cpp
@@ -463,11 +463,12 @@ bool HttpResponder::GetJsonResponse(const char* request, OutputBuffer *&response
 		}
 
 		// See if we can update the current RTC date and time
-		if (numQualKeys > 1 && StringEquals(qualifiers[1].key, "time") && !GetPlatform().IsDateTimeSet())
+		const char* const timeString = GetKeyValue("time");
+		if (timeString != nullptr && !GetPlatform().IsDateTimeSet())
 		{
 			struct tm timeInfo;
 			memset(&timeInfo, 0, sizeof(timeInfo));
-			if (strptime(qualifiers[1].value, "%Y-%m-%dT%H:%M:%S", &timeInfo) != nullptr)
+			if (strptime(timeString, "%Y-%m-%dT%H:%M:%S", &timeInfo) != nullptr)
 			{
 				time_t newTime = mktime(&timeInfo);
 				GetPlatform().SetDateTime(newTime);
@@ -876,10 +877,14 @@ void HttpResponder::SendJsonResponse(const char* command)
 			return;
 		}
 
-		if (StringEquals(command, "download") && StringEquals(qualifiers[0].key, "name"))
+		if (StringEquals(command, "download"))
 		{
-			SendFile(qualifiers[0].value, false);
-			return;
+			const char* const filename = GetKeyValue("name");
+			if (filename != nullptr)
+			{
+				SendFile(filename, false);
+				return;
+			}
 		}
 	}
 
@@ -996,7 +1001,8 @@ void HttpResponder::ProcessMessage()
 								  || (commandWords[1][0] == '/' && StringEquals(commandWords[1] + 1, KO_START "upload"));
 		if (isUploadRequest)
 		{
-			if (numQualKeys > 0 && StringEquals(qualifiers[0].key, "name"))
+			const char* const filename = GetKeyValue("name");
+			if (filename != nullptr)
 			{
 				// See how many bytes we expect to read
 				bool contentLengthFound = false;
@@ -1018,21 +1024,22 @@ void HttpResponder::ProcessMessage()
 				}
 
 				// Start a new file upload
-				FileStore *file = GetPlatform().GetFileStore(FS_PREFIX, qualifiers[0].value, OpenMode::write);
+				FileStore *file = GetPlatform().GetFileStore(FS_PREFIX, filename, OpenMode::write);
 				if (file == nullptr)
 				{
 					RejectMessage("could not create file");
 					return;
 
 				}
-				StartUpload(file, qualifiers[0].value);
+				StartUpload(file, filename);
 
 				// Try to get the last modified file date and time
-				if (numQualKeys > 1 && StringEquals(qualifiers[1].key, "time"))
+				const char* const lastModifiedString = GetKeyValue("time");
+				if (lastModifiedString != nullptr)
 				{
 					struct tm timeInfo;
 					memset(&timeInfo, 0, sizeof(timeInfo));
-					if (strptime(qualifiers[1].value, "%Y-%m-%dT%H:%M:%S", &timeInfo) != nullptr)
+					if (strptime(lastModifiedString, "%Y-%m-%dT%H:%M:%S", &timeInfo) != nullptr)
 					{
 						fileLastModified  = mktime(&timeInfo);
 					}
@@ -1048,7 +1055,7 @@ void HttpResponder::ProcessMessage()
 
 				if (reprap.Debug(moduleWebserver))
 				{
-					GetPlatform().MessageF(UsbMessage, "Start uploading file %s length %lu\n", qualifiers[0].value, postFileLength);
+					GetPlatform().MessageF(UsbMessage, "Start uploading file %s length %lu\n", filename, postFileLength);
 				}
 				uploadedBytes = 0;
 
diff --git a/src/DuetNG/Pins_DuetNG.h b/src/DuetNG/Pins_DuetNG.h
index 85b02396..047dfc18 100644
--- a/src/DuetNG/Pins_DuetNG.h
+++ b/src/DuetNG/Pins_DuetNG.h
@@ -25,6 +25,9 @@ const size_t NumFirmwareUpdateModules = 1;		// 1 module
 #define HAS_LWIP_NETWORKING		0
 #define HAS_CPU_TEMP_SENSOR		1
 #define HAS_HIGH_SPEED_SD		1
+#define HAS_SMART_DRIVERS		1
+#define HAS_VOLTAGE_MONITOR		1
+#define ACTIVE_LOW_HEAT_ON		1
 
 #define IAP_UPDATE_FILE		"iap4e.bin"			// using the same IAP file for both Duet WiFi and Duet Ethernet
 
@@ -73,7 +76,6 @@ constexpr Pin DueX_INT = 17;						// DueX interrupt pin = PA17 (was E6_STOP)
 constexpr Pin END_STOP_PINS[DRIVES] = { 46, 02, 93, 74, 48, 200, 203, 202, 201, 213, 39, 8 };
 
 // HEATERS
-constexpr bool HEAT_ON = false;												// false for inverted heater (e.g. Duet v0.6), true for not (e.g. Duet v0.4)
 constexpr Pin TEMP_SENSE_PINS[Heaters] = { 45, 47, 44, 61, 62, 63, 59, 18 }; // Thermistor pin numbers
 constexpr Pin HEAT_ON_PINS[Heaters] = { 19, 20, 16, 35, 37, 40, 43, 15 };	// Heater pin numbers (heater 7 pin TBC)
 
diff --git a/src/DuetNG/TMC2660.cpp b/src/DuetNG/TMC2660.cpp
index fb0c9477..12f2f4fb 100644
--- a/src/DuetNG/TMC2660.cpp
+++ b/src/DuetNG/TMC2660.cpp
@@ -85,6 +85,7 @@ const uint32_t TMC_DRVCTRL_INTPOL = 1 << 9;
 const uint32_t TMC_SGCSCONF_CS_MASK = 31;
 #define TMC_SGCSCONF_CS(n) ((((uint32_t)n) & 31) << 0)
 const uint32_t TMC_SGCSCONF_SGT_MASK = 127 << 8;
+const uint32_t TMC_SGCSCONF_SGT_SHIFT = 8;
 #define TMC_SGCSCONF_SGT(n) ((((uint32_t)n) & 127) << 8)
 const uint32_t TMC_SGCSCONF_SGT_SFILT = 1 << 16;
 
@@ -126,6 +127,7 @@ const uint32_t defaultSgscConfReg =
 const uint32_t defaultDrvConfReg =
 	TMC_REG_DRVCONF
 	| TMC_DRVCONF_VSENSE				// use high sensitivity range
+	| TMC_DRVCONF_TS2G_0P8				// fast short-to-ground detection
 	| 0;
 
 // Driver control register
@@ -137,7 +139,7 @@ const uint32_t defaultDrvCtrlReg =
 // coolStep control register
 const uint32_t defaultSmartEnReg =
 	  TMC_REG_SMARTEN
-	| 0;								// disable coolStep, we already do this in the main firmware
+	| 0;								// disable coolStep, it needs to be tuned ot the motor to work properly
 
 //----------------------------------------------------------------------------------------------------------------------------------
 // Private types and methods
@@ -161,6 +163,10 @@ struct TmcDriverState
 	void Enable(bool en);
 	void SpiSendWord(uint32_t dataOut);
 	uint32_t ReadStatus();
+	void SetStallThreshold(int sgThreshold);
+	void SetStallFilter(bool sgFilter);
+	void SetCoolStep(uint16_t coolStepConfig);
+	void AppendStallConfig(StringRef& reply);
 };
 
 // Initialise the state of the driver.
@@ -270,11 +276,48 @@ uint32_t TmcDriverState::ReadStatus()
 	return lastReadValue & (TMC_RR_SG | TMC_RR_OT | TMC_RR_OTPW | TMC_RR_S2G | TMC_RR_OLA | TMC_RR_OLB | TMC_RR_STST);
 }
 
+void TmcDriverState::SetStallThreshold(int sgThreshold)
+{
+	const uint32_t sgVal = ((uint32_t)constrain<int>(sgThreshold, -64, 63)) & 127;
+	sgcsConfReg = (sgcsConfReg & ~TMC_SGCSCONF_SGT_MASK) | (sgVal << TMC_SGCSCONF_SGT_SHIFT);
+	SpiSendWord(sgcsConfReg);
+}
+
+void TmcDriverState::SetStallFilter(bool sgFilter)
+{
+	if (sgFilter)
+	{
+		sgcsConfReg |= TMC_SGCSCONF_SGT_SFILT;
+	}
+	else
+	{
+		sgcsConfReg &= ~TMC_SGCSCONF_SGT_SFILT;
+	}
+	SpiSendWord(sgcsConfReg);
+}
+
+void TmcDriverState::SetCoolStep(uint16_t coolStepConfig)
+{
+	smartEnReg = TMC_REG_SMARTEN | coolStepConfig;
+	SpiSendWord(smartEnReg);
+}
+
+void TmcDriverState::AppendStallConfig(StringRef& reply)
+{
+	const bool filtered = ((sgcsConfReg & TMC_SGCSCONF_SGT_SFILT) != 0);
+	int threshold = (int)((sgcsConfReg & TMC_SGCSCONF_SGT_MASK) >> TMC_SGCSCONF_SGT_SHIFT);
+	if (threshold >= 64)
+	{
+		threshold -= 128;
+	}
+	reply.catf("stall threshold %d, filter %s, coolstep %" PRIx32, threshold, ((filtered) ? "on" : "off"), smartEnReg & 0xFFFF);
+}
+
 static TmcDriverState driverStates[DRIVES];
 
 //--------------------------- Public interface ---------------------------------
 
-namespace TMC2660
+namespace SmartDrivers
 {
 	// Initialise the driver interface and the drivers, leaving each drive disabled.
 	// It is assumed that the drivers are not powered, so driversPowered(true) must be called after calling this before the motors can be moved.
@@ -434,6 +477,38 @@ namespace TMC2660
 		}
 	}
 
+	void SetStallThreshold(size_t drive, int sgThreshold)
+	{
+		if (drive < numTmc2660Drivers)
+		{
+			driverStates[drive].SetStallThreshold(sgThreshold);
+		}
+	}
+
+	void SetStallFilter(size_t drive, bool sgFilter)
+	{
+		if (drive < numTmc2660Drivers)
+		{
+			driverStates[drive].SetStallFilter(sgFilter);
+		}
+	}
+
+	void SetCoolStep(size_t drive, uint16_t coolStepConfig)
+	{
+		if (drive < numTmc2660Drivers)
+		{
+			driverStates[drive].SetCoolStep(coolStepConfig);
+		}
+	}
+
+	void AppendStallConfig(size_t drive, StringRef& reply)
+	{
+		if (drive < numTmc2660Drivers)
+		{
+			driverStates[drive].AppendStallConfig(reply);
+		}
+	}
+
 };	// end namespace
 
 // End
diff --git a/src/DuetNG/TMC2660.h b/src/DuetNG/TMC2660.h
index 5284789b..91f4b40c 100644
--- a/src/DuetNG/TMC2660.h
+++ b/src/DuetNG/TMC2660.h
@@ -9,6 +9,7 @@
 #define TMC2660_H_
 
 #include "Pins.h"
+#include "Libraries/General/StringRef.h"
 
 // TMC2660 Read response. The microstep counter can also be read, but we don't include that here.
 const uint32_t TMC_RR_SG = 1 << 0;		// stall detected
@@ -19,7 +20,7 @@ const uint32_t TMC_RR_OLA = 1 << 5;		// open load A
 const uint32_t TMC_RR_OLB = 1 << 6;		// open load B
 const uint32_t TMC_RR_STST = 1 << 7;	// standstill detected
 
-namespace TMC2660
+namespace SmartDrivers
 {
 	void Init(const Pin[DRIVES], size_t numTmcDrivers)
 		pre(numTmcDrivers <= DRIVES);
@@ -29,6 +30,10 @@ namespace TMC2660
 	bool SetMicrostepping(size_t drive, int microsteps, int mode);
 	unsigned int GetMicrostepping(size_t drive, int mode, bool& interpolation);
 	void SetDriversPowered(bool powered);
+	void SetStallThreshold(size_t drive, int sgThreshold);
+	void SetStallFilter(size_t drive, bool sgFilter);
+	void SetCoolStep(size_t drive, uint16_t coolStepConfig);
+	void AppendStallConfig(size_t drive, StringRef& reply);
 };
 
 #endif /* TMC2660_H_ */
diff --git a/src/Fan.cpp b/src/Fan.cpp
index 155ad99e..3b792b4a 100644
--- a/src/Fan.cpp
+++ b/src/Fan.cpp
@@ -204,7 +204,7 @@ void Fan::SetHeatersMonitored(uint16_t h)
 void Fan::Refresh()
 {
 	float reqVal;
-#ifdef DUET_NG
+#if HAS_SMART_DRIVERS
 	uint32_t driverChannelsMonitored = 0;
 #endif
 
@@ -247,7 +247,7 @@ void Fan::Refresh()
 						// If the fan is on, add a hysteresis before turning it off
 						reqVal = max<float>(reqVal, (bangBangMode) ? max<float>(0.5, val) : minVal);
 					}
-#ifdef DUET_NG
+#if HAS_SMART_DRIVERS
 					const unsigned int channel = reprap.GetHeat().GetHeaterChannel(heaterHumber);
 					if (channel >= FirstTmcDriversSenseChannel && channel < FirstTmcDriversSenseChannel + NumTmcDriversSenseChannels)
 					{
@@ -269,7 +269,7 @@ void Fan::Refresh()
 		if (lastVal == 0.0)
 		{
 			// We are turning this fan on
-#ifdef DUET_NG
+#if HAS_SMART_DRIVERS
 			if (driverChannelsMonitored != 0)
 			{
 				reprap.GetPlatform().DriverCoolingFansOn(driverChannelsMonitored);		// tell Platform that we have started a fan that cools drivers
diff --git a/src/GCodes/GCodeBuffer.cpp b/src/GCodes/GCodeBuffer.cpp
index bea16d10..5c444a9d 100644
--- a/src/GCodes/GCodeBuffer.cpp
+++ b/src/GCodes/GCodeBuffer.cpp
@@ -27,18 +27,19 @@ void GCodeBuffer::Reset()
 
 void GCodeBuffer::Init()
 {
-	gcodePointer = 0;
+	gcodeLineEnd = 0;
 	commandLength = 0;
 	readPointer = -1;
-	inQuotes = inComment = timerRunning = false;
-	bufferState = GCodeBufferState::idle;
+	hadLineNumber = hadChecksum = timerRunning = false;
+	computedChecksum = 0;
+	bufferState = GCodeBufferState::parseNotStarted;
 }
 
 void GCodeBuffer::Diagnostics(MessageType mtype)
 {
 	switch (bufferState)
 	{
-	case GCodeBufferState::idle:
+	case GCodeBufferState::parseNotStarted:
 		scratchString.printf("%s is idle", identity);
 		break;
 
@@ -48,6 +49,10 @@ void GCodeBuffer::Diagnostics(MessageType mtype)
 
 	case GCodeBufferState::executing:
 		scratchString.printf("%s is doing \"%s\"", identity, Buffer());
+		break;
+
+	default:
+		scratchString.printf("%s is assembling a command", identity);
 	}
 
 	scratchString.cat(" in state(s)");
@@ -62,14 +67,18 @@ void GCodeBuffer::Diagnostics(MessageType mtype)
 	reprap.GetPlatform().Message(mtype, scratchString.Pointer());
 }
 
-int GCodeBuffer::CheckSum() const
+inline void GCodeBuffer::AddToChecksum(char c)
+{
+	computedChecksum ^= (uint8_t)c;
+}
+
+inline void GCodeBuffer::StoreAndAddToChecksum(char c)
 {
-	uint8_t cs = 0;
-	for (size_t i = 0; gcodeBuffer[i] != '*' && gcodeBuffer[i] != 0; i++)
+	computedChecksum ^= (uint8_t)c;
+	if (gcodeLineEnd < ARRAY_SIZE(gcodeBuffer))
 	{
-		cs = cs ^ (uint8_t)gcodeBuffer[i];
+		gcodeBuffer[gcodeLineEnd++] = c;
 	}
-	return (int)cs;
 }
 
 // Add a byte to the code being assembled.  If false is returned, the code is
@@ -81,88 +90,262 @@ bool GCodeBuffer::Put(char c)
 		++commandLength;
 	}
 
-	if (!inQuotes && ((c == ';') || (gcodePointer == 0 && c == '(')))
+	if (c == 0 || c == '\n' || c == '\r')
 	{
-		inComment = true;
+		return LineFinished();
 	}
-	else if (c == '\n' || c == '\r' || c == 0)
+
+	// Process the incoming character in a state machine
+	bool again;
+	do
 	{
-		gcodeBuffer[gcodePointer] = 0;
-		if (reprap.Debug(moduleGcodes) && gcodeBuffer[0] != 0 && !writingFileDirectory)		// don't bother echoing blank/comment lines
+		again = false;
+		switch (bufferState)
 		{
-			reprap.GetPlatform().MessageF(DebugMessage, "%s: %s\n", identity, gcodeBuffer);
-		}
+		case GCodeBufferState::parseNotStarted:				// we haven't started parsing yet
+			switch (c)
+			{
+			case 'N':
+			case 'n':
+				hadLineNumber = true;
+				AddToChecksum(c);
+				bufferState = GCodeBufferState::parsingLineNumber;
+				lineNumber = 0;
+				break;
 
-		// Deal with line numbers and checksums
-		if (Seen('*'))
-		{
-			const int csSent = GetIValue();
-			const int csHere = CheckSum();
-			if (csSent != csHere)
+			case ' ':
+			case '\t':
+				AddToChecksum(c);
+				break;
+
+			default:
+				bufferState = GCodeBufferState::parsingGCode;
+				commandStart = 0;
+				again = true;
+				break;
+			}
+			break;
+
+		case GCodeBufferState::parsingLineNumber:			// we saw N at the start and we are parsing the line number
+			if (isDigit(c))
 			{
-				if (Seen('N'))
-				{
-					snprintf(gcodeBuffer, GCODE_LENGTH, "M998 P%ld", GetIValue());
-				}
-				Init();
-				return true;
+				AddToChecksum(c);
+				lineNumber = (10 * lineNumber) + (c - '0');
+				break;
+			}
+			else
+			{
+				bufferState = GCodeBufferState::parsingWhitespace;
+				again = true;
+			}
+			break;
+
+		case GCodeBufferState::parsingWhitespace:
+			switch (c)
+			{
+			case ' ':
+			case '\t':
+				AddToChecksum(c);
+				break;
+
+			default:
+				bufferState = GCodeBufferState::parsingGCode;
+				commandStart = 0;
+				again = true;
+				break;
+			}
+			break;
+
+		case GCodeBufferState::parsingGCode:				// parsing GCode words
+			switch (c)
+			{
+			case '*':
+				declaredChecksum = 0;
+				hadChecksum = true;
+				bufferState = GCodeBufferState::parsingChecksum;
+				break;
+
+			case ';':
+				bufferState = GCodeBufferState::discarding;
+				break;
+
+			case '(':
+				AddToChecksum(c);
+				bufferState = GCodeBufferState::parsingBracketedComment;
+				break;
+
+			case '"':
+				StoreAndAddToChecksum(c);
+				bufferState = GCodeBufferState::parsingQuotedString;
+				break;
+
+			default:
+				StoreAndAddToChecksum(c);
 			}
+			break;
 
-			// Strip out the line number and checksum
-			gcodePointer = 0;
-			while (gcodeBuffer[gcodePointer] != ' ' && gcodeBuffer[gcodePointer] != 0)
+		case GCodeBufferState::parsingBracketedComment:		// inside a (...) comment
+			AddToChecksum(c);
+			if (c == ')')
 			{
-				gcodePointer++;
+				bufferState = GCodeBufferState::parsingGCode;
 			}
+			break;
 
-			// Anything there?
-			if (gcodeBuffer[gcodePointer] == 0)
+		case GCodeBufferState::parsingQuotedString:			// inside a double-quoted string
+			StoreAndAddToChecksum(c);
+			if (c == '"')
 			{
-				// No...
-				gcodeBuffer[0] = 0;
-				Init();
-				return false;
+				bufferState = GCodeBufferState::parsingGCode;
 			}
+			break;
 
-			// Yes...
-			gcodePointer++;
-			int gp2 = 0;
-			while (gcodeBuffer[gcodePointer] != '*' && gcodeBuffer[gcodePointer] != 0)
+		case GCodeBufferState::parsingChecksum:				// parsing the checksum after '*'
+			if (isDigit(c))
+			{
+				declaredChecksum = (10 * declaredChecksum) + (c - '0');
+			}
+			else
 			{
-				gcodeBuffer[gp2] = gcodeBuffer[gcodePointer++];
-				gp2++;
+				bufferState = GCodeBufferState::discarding;
+				again = true;
 			}
-			gcodeBuffer[gp2] = 0;
+			break;
+
+		case GCodeBufferState::discarding:					// discarding characters after the checksum or an end-of-line comment
+		default:
+			// throw the character away
+			break;
+		}
+	} while (again);
+
+	return false;
+}
+
+// This is called when we are fed a null, CR or LF character.
+// Return true if there is a completed command ready to be executed.
+bool GCodeBuffer::LineFinished()
+{
+	if (gcodeLineEnd == 0)
+	{
+		// Empty line
+		Init();
+		return false;
+	}
+
+	if (gcodeLineEnd == ARRAY_SIZE(gcodeBuffer))
+	{
+		reprap.GetPlatform().MessageF(ErrorMessage, "G-Code buffer '%s' length overflow\n", identity);
+		Init();
+		return false;
+	}
+
+	gcodeBuffer[gcodeLineEnd] = 0;
+	const bool badChecksum = (hadChecksum && computedChecksum != declaredChecksum);
+	const bool missingChecksum = (checksumRequired && !hadChecksum && machineState->previous == nullptr);
+	if (reprap.Debug(moduleGcodes) && !writingFileDirectory)
+	{
+		reprap.GetPlatform().MessageF(DebugMessage, "%s%s: %s\n", identity, ((badChecksum) ? "(bad-csum)" : (missingChecksum) ? "(no-csum)" : ""), gcodeBuffer);
+	}
+
+	if (badChecksum)
+	{
+		if (hadLineNumber)
+		{
+			snprintf(gcodeBuffer, ARRAY_SIZE(gcodeBuffer), "M998 P%u", lineNumber);	// request resend
 		}
-		else if ((checksumRequired && machineState->previous == nullptr) || IsEmpty())
+		else
 		{
-			// Checksum not found or buffer empty - cannot do anything
-			gcodeBuffer[0] = 0;
 			Init();
 			return false;
 		}
-		readPointer = -1;;
-		bufferState = GCodeBufferState::ready;
-		return true;
 	}
-	else if (!inComment || writingFileDirectory)
+	else if (missingChecksum)
+	{
+		// Checksum required but none was provided
+		Init();
+		return false;
+	}
+
+	commandStart = 0;
+	DecodeCommand();
+	return true;
+}
+
+// Decode this command command and find the start of the next one on the same line.
+// On entry, 'commandStart' has already been set to the address the start of where the command should be.
+// On return, the state must be set to 'ready' to indicate that a command is available and we should stop adding characters.
+void GCodeBuffer::DecodeCommand()
+{
+	// Check for a valid command letter at the start
+	commandLetter = toupper(gcodeBuffer[commandStart]);
+	hasCommandNumber = false;
+	commandNumber = -1;
+	commandFraction = -1;
+	if (commandLetter == 'G' || commandLetter == 'M' || commandLetter == 'T')
 	{
-		if (gcodePointer >= (int)GCODE_LENGTH)
+		parameterStart = commandStart + 1;
+		const bool negative = (gcodeBuffer[parameterStart] == '-');
+		if (negative)
 		{
-			reprap.GetPlatform().MessageF(ErrorMessage, "G-Code buffer '%s' length overflow\n", identity);
-			Init();
+			++parameterStart;
 		}
-		else
+		if (isdigit(gcodeBuffer[parameterStart]))
+		{
+			hasCommandNumber = true;
+			// Read the number after the command letter
+			commandNumber = 0;
+			do
+			{
+				commandNumber = (10 * commandNumber) + (gcodeBuffer[parameterStart] - '0');
+				++parameterStart;
+			}
+			while (isdigit(gcodeBuffer[parameterStart]));
+			if (negative)
+			{
+				commandNumber = -commandNumber;
+			}
+
+			// Read the fractional digit, if any
+			if (gcodeBuffer[parameterStart] == '.')
+			{
+				++parameterStart;
+				if (isdigit(gcodeBuffer[parameterStart]))
+				{
+					commandFraction = gcodeBuffer[parameterStart] - '0';
+					++parameterStart;
+				}
+			}
+		}
+
+		// Find where the end of the command is. We assume that a G or M preceded by a space and not inside quotes is the start of a new command.
+		bool inQuotes = false;
+		bool primed = false;
+		for (commandEnd = parameterStart; commandEnd < gcodeLineEnd; ++commandEnd)
 		{
-			gcodeBuffer[gcodePointer++] = c;
-			if (c == '"' && !inComment)
+			const char c = gcodeBuffer[commandEnd];
+			char c2;
+			if (c == '"')
 			{
 				inQuotes = !inQuotes;
+				primed = false;
+			}
+			else if (!inQuotes)
+			{
+				if (primed && ((c2 = toupper(c)) == 'G' || c2 == 'M'))
+				{
+					break;
+				}
+				primed = (c == ' ' || c == '\t');
 			}
 		}
 	}
-
-	return false;
+	else
+	{
+		parameterStart = commandStart;
+		commandEnd = gcodeLineEnd;
+	}
+	bufferState = GCodeBufferState::ready;
 }
 
 // Add an entire string, overwriting any existing content
@@ -184,8 +367,16 @@ void GCodeBuffer::SetFinished(bool f)
 {
 	if (f)
 	{
-		bufferState = GCodeBufferState::idle;
-		Init();
+		if (commandEnd < gcodeLineEnd)
+		{
+			// There is another command in the same line of gcode
+			commandStart = commandEnd;
+			DecodeCommand();
+		}
+		else
+		{
+			Init();
+		}
 	}
 	else
 	{
@@ -198,88 +389,32 @@ FilePosition GCodeBuffer::GetFilePosition(size_t bytesCached) const
 {
 	if (machineState->fileState.IsLive())
 	{
-		return machineState->fileState.GetPosition() - bytesCached - commandLength;
+		return machineState->fileState.GetPosition() - bytesCached - commandLength + commandStart;
 	}
 	return noFilePosition;
 }
 
-// Does this buffer contain any code?
-bool GCodeBuffer::IsEmpty() const
-{
-	const char *buf = gcodeBuffer;
-	while (*buf != 0 && strchr(" \t\n\r", *buf) != nullptr)
-	{
-		buf++;
-	}
-	return *buf == 0;
-}
-
 // Is 'c' in the G Code string?
 // Leave the pointer there for a subsequent read.
 bool GCodeBuffer::Seen(char c)
 {
-	readPointer = 0;
 	bool inQuotes = false;
-	for (;;)
+	for (readPointer = parameterStart; (unsigned int)readPointer < commandEnd; ++readPointer)
 	{
 		const char b = gcodeBuffer[readPointer];
-		if (b == 0)
-		{
-			break;
-		}
 		if (b == '"')
 		{
 			inQuotes = !inQuotes;
 		}
-		else if (!inQuotes)
+		else if (!inQuotes && toupper(b) == c)
 		{
-			if (b == ';')
-			{
-				break;
-			}
-			if (toupper(b) == c)
-			{
-				return true;
-			}
+			return true;
 		}
-		++readPointer;
 	}
 	readPointer = -1;
 	return false;
 }
 
-// Return the first G, M or T command letter. Needed so that we don't pick up a spurious command letter from inside a string parameter.
-char GCodeBuffer::GetCommandLetter()
-{
-	readPointer = 0;
-	for (;;)
-	{
-		char b = gcodeBuffer[readPointer];
-		if (b == 0 || b == ';' || b == '"' || b == '(')
-		{
-			break;
-		}
-		b = (char)toupper(b);
-		if (b == 'T')
-		{
-			return b;
-		}
-		if (b == 'G' || b == 'M')
-		{
-			// Check that the command letter is followed by a digit. This is mostly to avoid a malformed string in which the first letter is M being interpreted as M0.
-			const char b2 = gcodeBuffer[readPointer + 1];
-			if (b2 >= '0' && b2 <= '9')
-			{
-				return b;
-			}
-			break;
-		}
-		++readPointer;
-	}
-	readPointer = -1;
-	return 0;
-}
-
 // Get a float after a G Code letter found by a call to Seen()
 float GCodeBuffer::GetFValue()
 {
@@ -290,11 +425,11 @@ float GCodeBuffer::GetFValue()
 		return result;
 	}
 
-	reprap.GetPlatform().Message(ErrorMessage, "GCodes: Attempt to read a GCode float before a search.\n");
+	INTERNAL_ERROR;
 	return 0.0;
 }
 
-// Get a :-separated list of floats after a key letter
+// Get a colon-separated list of floats after a key letter
 const void GCodeBuffer::GetFloatArray(float a[], size_t& returnedLength, bool doPad)
 {
 	if (readPointer >= 0)
@@ -340,7 +475,7 @@ const void GCodeBuffer::GetFloatArray(float a[], size_t& returnedLength, bool do
 	}
 	else
 	{
-		reprap.GetPlatform().Message(ErrorMessage, "GCodes: Attempt to read a GCode float array before a search.\n");
+		INTERNAL_ERROR;
 		returnedLength = 0;
 	}
 }
@@ -377,7 +512,8 @@ const void GCodeBuffer::GetLongArray(long l[], size_t& returnedLength)
 	}
 	else
 	{
-		reprap.GetPlatform().Message(ErrorMessage, "GCodes: Attempt to read a GCode long array before a search.\n");
+		INTERNAL_ERROR;
+		returnedLength = 0;
 	}
 }
 
@@ -388,12 +524,13 @@ const char* GCodeBuffer::GetString()
 {
 	if (readPointer >= 0)
 	{
+		commandEnd = gcodeLineEnd;				// the string is the remainder of the line of gcode
 		const char* const result = &gcodeBuffer[readPointer + 1];
 		readPointer = -1;
 		return result;
 	}
 
-	reprap.GetPlatform().Message(ErrorMessage, "GCodes: Attempt to read a GCode string before a search.\n");
+	INTERNAL_ERROR;
 	return "";
 }
 
@@ -440,7 +577,7 @@ bool GCodeBuffer::GetQuotedString(const StringRef& str)
 		return false;
 	}
 
-	reprap.GetPlatform().Message(ErrorMessage, "GCodes: Attempt to read a GCode string before a search.\n");
+	INTERNAL_ERROR;
 	return false;
 }
 
@@ -454,6 +591,7 @@ bool GCodeBuffer::GetPossiblyQuotedString(const StringRef& str)
 			return GetQuotedString(str);
 		}
 
+		commandEnd = gcodeLineEnd;				// the string is the remainder of the line of gcode
 		str.Clear();
 		for (;;)
 		{
@@ -469,43 +607,33 @@ bool GCodeBuffer::GetPossiblyQuotedString(const StringRef& str)
 		return !str.IsEmpty();
 	}
 
-	reprap.GetPlatform().Message(ErrorMessage, "GCodes: Attempt to read a possibly quoted GCode string before a search.\n");
+	INTERNAL_ERROR;
 	return false;
 }
 
-// This returns a pointer to the end of the buffer where a
-// string starts.  It assumes that an M or G search has
-// been done followed by a GetIValue(), so readPointer will
-// be -1.  It absorbs "M/Gnnn " (including the space) from the
-// start and returns a pointer to the next location.
-
-// This is provided for legacy use, in particular in the M23
+// This returns a pointer to the end of the buffer where a string starts.
+// It is provided for legacy use, in particular in the M23
 // command that sets the name of a file to be printed.  In
 // preference use GetString() which requires the string to have
 // been preceded by a tag letter.
-
 // If no string was provided, it produces an error message if the string was not optional, and returns nullptr.
 const char* GCodeBuffer::GetUnprecedentedString(bool optional)
 {
-	readPointer = 0;
-	while (gcodeBuffer[readPointer] != 0 && gcodeBuffer[readPointer] != ' ')
-	{
-		readPointer++;
-	}
+	commandEnd = gcodeLineEnd;					// the string is the remainder of the line
+	size_t i;
+	char c;
+	for (i = parameterStart; i < commandEnd && ((c = gcodeBuffer[i]) == ' ' || c == '\t'); ++i) { }
 
-	if (gcodeBuffer[readPointer] == 0)
+	if (i == commandEnd)
 	{
-		readPointer = -1;
 		if (!optional)
 		{
-			reprap.GetPlatform().Message(ErrorMessage, "GCodes: String expected but not seen.\n");
+			reprap.GetPlatform().MessageF(ErrorMessage, "%c%d: String expected but not seen.\n", commandLetter, commandNumber);
 		}
 		return nullptr;
 	}
 
-	const char* const result = &gcodeBuffer[readPointer + 1];
-	readPointer = -1;
-	return result;
+	return &gcodeBuffer[i];
 }
 
 // Get an int32 after a G Code letter
@@ -518,8 +646,7 @@ int32_t GCodeBuffer::GetIValue()
 		return result;
 	}
 
-	reprap.GetPlatform().Message(ErrorMessage, "GCodes: Attempt to read a GCode int before a search.\n");
-	readPointer = -1;
+	INTERNAL_ERROR;
 	return 0;
 }
 
@@ -533,8 +660,7 @@ uint32_t GCodeBuffer::GetUIValue()
 		return result;
 	}
 
-	reprap.GetPlatform().Message(ErrorMessage, "GCodes: Attempt to read a GCode unsigned int before a search.\n");
-	readPointer = -1;
+	INTERNAL_ERROR;
 	return 0;
 }
 
@@ -619,9 +745,10 @@ bool GCodeBuffer::GetIPAddress(uint8_t ip[4])
 {
 	if (readPointer < 0)
 	{
-		reprap.GetPlatform().Message(ErrorMessage, "GCodes: Attempt to read a GCode string before a search.\n");
+		INTERNAL_ERROR;
 		return false;
 	}
+
 	const char* p = &gcodeBuffer[readPointer + 1];
 	unsigned int n = 0;
 	for (;;)
@@ -654,6 +781,12 @@ bool GCodeBuffer::GetIPAddress(uint8_t ip[4])
 // Get an IP address quad after a key letter
 bool GCodeBuffer::GetIPAddress(uint32_t& ip)
 {
+	if (readPointer < 0)
+	{
+		INTERNAL_ERROR;
+		return false;
+	}
+
 	uint8_t ipa[4];
 	const bool ok = GetIPAddress(ipa);
 	if (ok)
@@ -697,6 +830,7 @@ bool GCodeBuffer::PushState()
 	ms->axesRelative = machineState->axesRelative;
 	ms->doingFileMacro = machineState->doingFileMacro;
 	ms->waitWhileCooling = machineState->waitWhileCooling;
+	ms->runningM501 = machineState->runningM501;
 	ms->runningM502 = machineState->runningM502;
 	ms->volumetricExtrusion = false;
 	ms->messageAcknowledged = false;
diff --git a/src/GCodes/GCodeBuffer.h b/src/GCodes/GCodeBuffer.h
index c31c6340..f917f1d4 100644
--- a/src/GCodes/GCodeBuffer.h
+++ b/src/GCodes/GCodeBuffer.h
@@ -22,10 +22,13 @@ public:
 	void Diagnostics(MessageType mtype);				// Write some debug info
 	bool Put(char c) __attribute__((hot));				// Add a character to the end
 	bool Put(const char *str, size_t len);				// Add an entire string, overwriting any existing content
-	bool IsEmpty() const;								// Does this buffer contain any code?
 	bool Seen(char c) __attribute__((hot));				// Is a character present?
 
-	char GetCommandLetter() __attribute__((hot));		// Find the first G, M or T command
+	char GetCommandLetter() const { return commandLetter; }
+	bool HasCommandNumber() const { return hasCommandNumber; }
+	int GetCommandNumber() const { return commandNumber; }
+	int8_t GetCommandFraction() const { return commandFraction; }
+
 	float GetFValue() __attribute__((hot));				// Get a float after a key letter
 	int32_t GetIValue() __attribute__((hot));			// Get an integer after a key letter
 	uint32_t GetUIValue();								// Get an unsigned integer value
@@ -55,7 +58,7 @@ public:
 	int GetToolNumberAdjust() const { return toolNumberAdjust; }
 	void SetToolNumberAdjust(int arg) { toolNumberAdjust = arg; }
 	void SetCommsProperties(uint32_t arg) { checksumRequired = (arg & 1); }
-	bool StartingNewCode() const { return gcodePointer == 0; }
+	bool StartingNewCode() const { return gcodeLineEnd == 0; }
 	MessageType GetResponseMessageType() const { return responseMessageType; }
 	GCodeMachineState& MachineState() const { return *machineState; }
 	GCodeMachineState& OriginalMachineState() const;
@@ -79,28 +82,49 @@ public:
 
 private:
 
-	enum class GCodeBufferState
+	enum class GCodeBufferState : uint8_t
 	{
-		idle,			// we don't have a complete gcode ready
-		ready,			// we have a complete gcode but haven't started executing it
-		executing		// we have a complete gcode and have started executing it
+		parseNotStarted,								// we haven't started parsing yet
+		parsingLineNumber,								// we saw N at the start and we are parsing the line number
+		parsingWhitespace,								// parsing whitespace after the line number
+		parsingGCode,									// parsing GCode words
+		parsingBracketedComment,						// inside a (...) comment
+		parsingQuotedString,							// inside a double-quoted string
+		parsingChecksum,								// parsing the checksum after '*'
+		discarding,										// discarding characters after the checksum or an end-of-line comment
+		ready,											// we have a complete gcode but haven't started executing it
+		executing										// we have a complete gcode and have started executing it
 	};
 
-	int CheckSum() const;								// Compute the checksum (if any) at the end of the G Code
+	void AddToChecksum(char c);
+	void StoreAndAddToChecksum(char c);
+	bool LineFinished();								// Deal with receiving end-of-line and return true if we have a command
+	void DecodeCommand();
 
 	GCodeMachineState *machineState;					// Machine state for this gcode source
 	char gcodeBuffer[GCODE_LENGTH];						// The G Code
-	const char* identity;								// Where we are from (web, file, serial line etc)
-	int gcodePointer;									// Index in the buffer
+	const char* const identity;							// Where we are from (web, file, serial line etc)
+	unsigned int commandStart;							// Index in the buffer of the command letter of this command
+	unsigned int parameterStart;
+	unsigned int commandEnd;							// Index in the buffer of one past the last character of this command
 	unsigned int commandLength;							// Number of characters we read to build this command including the final \r or \n
+	unsigned int gcodeLineEnd;							// Number of characters in the entire line of gcode
 	int readPointer;									// Where in the buffer to read next
-	bool inQuotes;										// Are we inside double quotation marks?
-	bool inComment;										// Are we after a ';' character?
 	bool checksumRequired;								// True if we only accept commands with a valid checksum
 	GCodeBufferState bufferState;						// Idle, executing or paused
 	const char* writingFileDirectory;					// If the G Code is going into a file, where that is
 	int toolNumberAdjust;								// The adjustment to tool numbers in commands we receive
 	const MessageType responseMessageType;				// The message type we use for responses to commands coming from this channel
+	unsigned int lineNumber;
+	unsigned int declaredChecksum;
+	uint8_t computedChecksum;
+	bool hadLineNumber;
+	bool hadChecksum;
+	bool hasCommandNumber;
+	char commandLetter;
+	int commandNumber;
+	int8_t commandFraction;
+
 	bool queueCodes;									// Can we queue certain G-codes from this source?
 	bool binaryWriting;									// Executing gcode or writing binary file?
 	uint32_t crc32;										// crc32 of the binary file
@@ -133,7 +157,7 @@ inline const char* GCodeBuffer::Buffer() const
 
 inline bool GCodeBuffer::IsIdle() const
 {
-	return bufferState == GCodeBufferState::idle;
+	return bufferState != GCodeBufferState::ready && bufferState != GCodeBufferState::executing;
 }
 
 inline bool GCodeBuffer::IsReady() const
diff --git a/src/GCodes/GCodeInput.cpp b/src/GCodes/GCodeInput.cpp
index 3a6f6227..e4787fbd 100644
--- a/src/GCodes/GCodeInput.cpp
+++ b/src/GCodes/GCodeInput.cpp
@@ -13,7 +13,7 @@
 
 bool GCodeInput::FillBuffer(GCodeBuffer *gb)
 {
-	size_t bytesToPass = min<size_t>(BytesCached(), GCODE_LENGTH);
+	const size_t bytesToPass = min<size_t>(BytesCached(), GCODE_LENGTH);
 	for (size_t i = 0; i < bytesToPass; i++)
 	{
 		const char c = ReadByte();
diff --git a/src/GCodes/GCodeMachineState.cpp b/src/GCodes/GCodeMachineState.cpp
index 5b21a612..d4a600eb 100644
--- a/src/GCodes/GCodeMachineState.cpp
+++ b/src/GCodes/GCodeMachineState.cpp
@@ -13,7 +13,7 @@ unsigned int GCodeMachineState::numAllocated = 0;
 // Create a default initialised GCodeMachineState
 GCodeMachineState::GCodeMachineState()
 	: previous(nullptr), feedrate(DefaultFeedrate * SecondsToMinutes), fileState(), lockedResources(0), state(GCodeState::normal),
-	  drivesRelative(false), axesRelative(false), doingFileMacro(false), runningM502(false), volumetricExtrusion(false), waitingForAcknowledgement(false), messageAcknowledged(false)
+	  drivesRelative(false), axesRelative(false), doingFileMacro(false), runningM501(false), runningM502(false), volumetricExtrusion(false), waitingForAcknowledgement(false), messageAcknowledged(false)
 {
 }
 
diff --git a/src/GCodes/GCodeMachineState.h b/src/GCodes/GCodeMachineState.h
index ce5694cd..0848f369 100644
--- a/src/GCodes/GCodeMachineState.h
+++ b/src/GCodes/GCodeMachineState.h
@@ -16,7 +16,9 @@ enum class GCodeState : uint8_t
 {
 	normal,												// not doing anything and ready to process a new GCode
 
-	waitingForSpecialMoveToComplete,							// doing a special move, so we must wait for it to finish before processing another GCode
+	waitingForSpecialMoveToComplete,					// doing a special move, so we must wait for it to finish before processing another GCode
+
+	probingToolOffset,
 
 	homing1,
 	homing2,
@@ -35,11 +37,14 @@ enum class GCodeState : uint8_t
 
 	pausing1,
 	pausing2,
+
 	resuming1,
 	resuming2,
 	resuming3,
+
 	flashing1,
 	flashing2,
+
 	stopping,
 	sleeping,
 
@@ -64,7 +69,11 @@ enum class GCodeState : uint8_t
 	doingFirmwareRetraction,
 	doingFirmwareUnRetraction,
 	loadingFilament,
-	unloadingFilament
+	unloadingFilament,
+
+#if HAS_VOLTAGE_MONITOR
+	powerFailPausing1
+#endif
 };
 
 // Class to hold the state of gcode execution for some input source
@@ -79,11 +88,14 @@ public:
 	FileData fileState;
 	ResourceBitmap lockedResources;
 	GCodeState state;
+	uint8_t toolChangeParam;
+	int16_t newToolNumber;
 	unsigned int
 		drivesRelative : 1,
 		axesRelative : 1,
 		doingFileMacro : 1,
 		waitWhileCooling : 1,
+		runningM501 : 1,
 		runningM502 : 1,
 		volumetricExtrusion : 1,
 		// Caution: these next 3 will be modified out-of-process when we use RTOS, so they will need to be individual bool variables
diff --git a/src/GCodes/GCodeQueue.cpp b/src/GCodes/GCodeQueue.cpp
index 69b35d87..0e701acf 100644
--- a/src/GCodes/GCodeQueue.cpp
+++ b/src/GCodes/GCodeQueue.cpp
@@ -49,21 +49,19 @@ bool GCodeQueue::QueueCode(GCodeBuffer &gb, uint32_t segmentsLeft)
 	{
 	case 'G':
 		{
-			const int code = gb.GetIValue();
+			const int code = gb.GetCommandNumber();
+			queueCode = (code == 10 && gb.Seen('P'));	// Set active/standby temperatures
 
-			// Set active/standby temperatures
-			queueCode = (code == 10 && gb.Seen('P'));
 		}
 		break;
 
 	case 'M':
 		{
-			const int code = gb.GetIValue();
-			switch(code)
+			switch (gb.GetCommandNumber())
 			{
 			case 3:		// spindle control
-			case 4:
-			case 5:
+			case 4:		// spindle control
+			case 5:		// spindle control
 			case 42:	// set IO pin
 			case 106:	// fan control
 			case 107:	// fan off
diff --git a/src/GCodes/GCodes.cpp b/src/GCodes/GCodes.cpp
index fb2c42b5..6acf9fb7 100644
--- a/src/GCodes/GCodes.cpp
+++ b/src/GCodes/GCodes.cpp
@@ -44,8 +44,11 @@ const char GCodes::axisLetters[MaxAxes] = AXES_('X', 'Y', 'Z', 'U', 'V', 'W', 'A
 const size_t gcodeReplyLength = 2048;			// long enough to pass back a reasonable number of files in response to M20
 
 GCodes::GCodes(Platform& p) :
-	platform(p), machineType(MachineType::fff), active(false), isFlashing(false),
-	fileBeingHashed(nullptr), lastWarningMillis(0)
+	platform(p), machineType(MachineType::fff), active(false),
+#if HAS_VOLTAGE_MONITOR
+	powerFailScript(nullptr),
+#endif
+	isFlashing(false), fileBeingHashed(nullptr), lastWarningMillis(0)
 {
 	httpInput = new RegularGCodeInput(true);
 	telnetInput = new RegularGCodeInput(true);
@@ -60,9 +63,7 @@ GCodes::GCodes(Platform& p) :
 	auxGCode = new GCodeBuffer("aux", LcdMessage, false);
 	daemonGCode = new GCodeBuffer("daemon", GenericMessage, false);
 	queuedGCode = new GCodeBuffer("queue", GenericMessage, false);
-#ifdef DUET_NG
 	autoPauseGCode = new GCodeBuffer("autopause", GenericMessage, false);
-#endif
 	codeQueue = new GCodeQueue();
 }
 
@@ -89,7 +90,6 @@ void GCodes::Init()
 	eofStringLength = strlen(eofString);
 	runningConfigFile = false;
 	doingToolChange = false;
-	toolChangeParam = DefaultToolChangeParam;
 	active = true;
 	fileSize = 0;
 	longWait = millis();
@@ -128,9 +128,7 @@ void GCodes::Reset()
 	auxGCode->SetCommsProperties(1);					// by default, we require a checksum on the aux port
 	daemonGCode->Reset();
 	queuedGCode->Reset();
-#ifdef DUET_NG
 	autoPauseGCode->Reset();
-#endif
 
 	nextGcodeSource = 0;
 
@@ -178,8 +176,8 @@ void GCodes::Reset()
 	exitSimulationWhenFileComplete = false;
 	simulationTime = 0.0;
 	isPaused = false;
-#ifdef DUET_NG
-	isAutoPaused = false;
+#if HAS_VOLTAGE_MONITOR
+	isPowerFailPaused = false;
 #endif
 	doingToolChange = false;
 	doingManualBedProbe = false;
@@ -259,9 +257,20 @@ void GCodes::Spin()
 	}
 
 	CheckTriggers();
+	CheckFilament();
 
-	// Get the GCodeBuffer that we want to work from
-	GCodeBuffer& gb = *(gcodeSources[nextGcodeSource]);
+	// Get the GCodeBuffer that we want to process a command from. Give priority to auto-pause.
+	GCodeBuffer *gbp = autoPauseGCode;
+	if (gbp->GetState() == GCodeState::normal && gbp->IsIdle())
+	{
+		gbp = gcodeSources[nextGcodeSource];
+		++nextGcodeSource;											// move on to the next gcode source ready for next time
+		if (nextGcodeSource == ARRAY_SIZE(gcodeSources) - 1)		// the last one is autoPauseGCode, so don't do it again
+		{
+			nextGcodeSource = 0;
+		}
+	}
+	GCodeBuffer& gb = *gbp;
 
 	// Set up a buffer for the reply
 	char replyBuffer[gcodeReplyLength];
@@ -294,888 +303,921 @@ void GCodes::Spin()
 	}
 	else
 	{
-		// Perform the next operation of the state machine for this gcode source
-		bool error = false;
+		RunStateMachine(gb, reply);			// Execute the state machine
+	}
 
-		switch (gb.GetState())
+	// Check if we need to display a warning
+	const uint32_t now = millis();
+	if (now - lastWarningMillis >= MinimumWarningInterval)
+	{
+		if (displayNoToolWarning)
 		{
-		case GCodeState::waitingForSpecialMoveToComplete:
-			if (LockMovementAndWaitForStandstill(gb))		// movement should already be locked, but we need to wait for standstill and fetch the current position
+			platform.Message(ErrorMessage, "Attempting to extrude with no tool selected.\n");
+			displayNoToolWarning = false;
+			lastWarningMillis = now;
+		}
+		if (displayDeltaNotHomedWarning)
+		{
+			platform.Message(ErrorMessage, "Attempt to move the head of a Delta or SCARA printer before homing it\n");
+			displayDeltaNotHomedWarning = false;
+			lastWarningMillis = now;
+		}
+	}
+	platform.ClassReport(longWait);
+}
+
+// Execute a step of the state machine
+void GCodes::RunStateMachine(GCodeBuffer& gb, StringRef& reply)
+{
+	// Perform the next operation of the state machine for this gcode source
+	bool error = false;
+
+	switch (gb.GetState())
+	{
+	case GCodeState::waitingForSpecialMoveToComplete:
+		if (LockMovementAndWaitForStandstill(gb))		// movement should already be locked, but we need to wait for standstill and fetch the current position
+		{
+			// Check whether we made any G1 S3 moves and need to set the axis limits
+			for (size_t axis = 0; axis < numVisibleAxes; ++axis)
 			{
-				// Check whether we made any G1 S3 moves and need to set the axis limits
-				for (size_t axis = 0; axis < numVisibleAxes; ++axis)
+				if (IsBitSet<AxesBitmap>(axesToSenseLength, axis))
 				{
-					if (IsBitSet<AxesBitmap>(axesToSenseLength, axis))
+					EndStopType stopType;
+					bool dummy;
+					platform.GetEndStopConfiguration(axis, stopType, dummy);
+					if (stopType == EndStopType::highEndStop)
 					{
-						EndStopType stopType;
-						bool dummy;
-						platform.GetEndStopConfiguration(axis, stopType, dummy);
-						if (stopType == EndStopType::highEndStop)
-						{
-							platform.SetAxisMaximum(axis, moveBuffer.coords[axis]);
-						}
-						else if (stopType == EndStopType::lowEndStop)
-						{
-							platform.SetAxisMinimum(axis, moveBuffer.coords[axis]);
-						}
+						platform.SetAxisMaximum(axis, moveBuffer.coords[axis], true);
+					}
+					else if (stopType == EndStopType::lowEndStop)
+					{
+						platform.SetAxisMinimum(axis, moveBuffer.coords[axis], true);
 					}
 				}
-				gb.SetState(GCodeState::normal);
 			}
-			break;
+			gb.SetState(GCodeState::normal);
+		}
+		break;
 
-		case GCodeState::homing1:
-			if (toBeHomed == 0)
-			{
-				gb.SetState(GCodeState::normal);
-			}
-			else
+	case GCodeState::probingToolOffset:
+		if (LockMovementAndWaitForStandstill(gb))
+		{
+			Tool * const currentTool = reprap.GetCurrentTool();
+			if (currentTool != nullptr)
 			{
-				AxesBitmap mustHomeFirst;
-				const char *nextHomingFile = reprap.GetMove().GetKinematics().GetHomingFileName(toBeHomed, axesHomed, numVisibleAxes, mustHomeFirst);
-				if (nextHomingFile == nullptr)
+				for (size_t axis = 0; axis < numTotalAxes; ++axis)
 				{
-					// Error, can't home this axes
-					reply.copy("Must home these axes:");
-					AppendAxes(reply, mustHomeFirst);
-					reply.cat(" before homing these:");
-					AppendAxes(reply, toBeHomed);
-					error = true;
-					toBeHomed = 0;
-					gb.SetState(GCodeState::normal);
-				}
-				else
-				{
-					gb.SetState(GCodeState::homing2);
-					if (!DoFileMacro(gb, nextHomingFile, false))
+					if (gb.Seen(axisLetters[axis]))
 					{
-						reply.printf("Homing file %s not found", nextHomingFile);
-						error = true;
-						gb.SetState(GCodeState::normal);
+						// We get here when the tool probe has been activated. In this case we know how far we
+						// went (i.e. the difference between our start and end positions) and if we need to
+						// incorporate any correction factors. That's why we only need to set the final tool
+						// offset to this value in order to finish the tool probing.
+						currentTool->SetOffset(axis, (currentUserPosition[axis] - toolChangeRestorePoint.moveCoords[axis]) + gb.GetFValue(), true);
+						break;
 					}
 				}
 			}
-			break;
+			gb.SetState(GCodeState::normal);
+		}
+		break;
 
-		case GCodeState::homing2:
-			if (LockMovementAndWaitForStandstill(gb))		// movement should already be locked, but we need to wait for the previous homing move to complete
+	case GCodeState::homing1:
+		if (toBeHomed == 0)
+		{
+			gb.SetState(GCodeState::normal);
+		}
+		else
+		{
+			AxesBitmap mustHomeFirst;
+			const char *nextHomingFile = reprap.GetMove().GetKinematics().GetHomingFileName(toBeHomed, axesHomed, numVisibleAxes, mustHomeFirst);
+			if (nextHomingFile == nullptr)
 			{
-				// Test whether the previous homing move homed any axes
-				if ((toBeHomed & axesHomed) == 0)
+				// Error, can't home this axes
+				reply.copy("Must home these axes:");
+				AppendAxes(reply, mustHomeFirst);
+				reply.cat(" before homing these:");
+				AppendAxes(reply, toBeHomed);
+				error = true;
+				toBeHomed = 0;
+				gb.SetState(GCodeState::normal);
+			}
+			else
+			{
+				gb.SetState(GCodeState::homing2);
+				if (!DoFileMacro(gb, nextHomingFile, false))
 				{
-					reply.copy("Homing failed");
+					reply.printf("Homing file %s not found", nextHomingFile);
 					error = true;
 					gb.SetState(GCodeState::normal);
 				}
-				else
-				{
-					toBeHomed &= ~axesHomed;
-					gb.SetState((toBeHomed == 0) ? GCodeState::normal : GCodeState::homing1);
-				}
 			}
-			break;
+		}
+		break;
 
-		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();
-			memcpy(toolChangeRestorePoint.moveCoords, currentUserPosition, MaxAxes * sizeof(currentUserPosition[0]));
-			toolChangeRestorePoint.feedRate = gb.MachineState().feedrate;
-			gb.AdvanceState();
-			if ((toolChangeParam & TFreeBit) != 0)
+	case GCodeState::homing2:
+		if (LockMovementAndWaitForStandstill(gb))		// movement should already be locked, but we need to wait for the previous homing move to complete
+		{
+			// Test whether the previous homing move homed any axes
+			if ((toBeHomed & axesHomed) == 0)
 			{
-				const Tool * const oldTool = reprap.GetCurrentTool();
-				if (oldTool != nullptr && AllAxesAreHomed())
-				{
-					scratchString.printf("tfree%d.g", oldTool->Number());
-					DoFileMacro(gb, scratchString.Pointer(), false);
-				}
+				reply.copy("Homing failed");
+				error = true;
+				gb.SetState(GCodeState::normal);
 			}
-			break;
-
-		case GCodeState::toolChange1:		// Release the old tool (if any), then run tpre for the new tool
-		case GCodeState::m109ToolChange1:	// Release the old tool (if any), then run tpre for the new tool
+			else
 			{
-				const Tool * const oldTool = reprap.GetCurrentTool();
-				if (oldTool != nullptr)
-				{
-					reprap.StandbyTool(oldTool->Number());
-				}
+				toBeHomed &= ~axesHomed;
+				gb.SetState((toBeHomed == 0) ? GCodeState::normal : GCodeState::homing1);
 			}
-			gb.AdvanceState();
-			if (reprap.GetTool(newToolNumber) != nullptr && AllAxesAreHomed() && (toolChangeParam & TPreBit) != 0)
+		}
+		break;
+
+	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();
+		memcpy(toolChangeRestorePoint.moveCoords, currentUserPosition, MaxAxes * sizeof(currentUserPosition[0]));
+		toolChangeRestorePoint.feedRate = gb.MachineState().feedrate;
+		gb.AdvanceState();
+		if ((gb.MachineState().toolChangeParam & TFreeBit) != 0)
+		{
+			const Tool * const oldTool = reprap.GetCurrentTool();
+			if (oldTool != nullptr && AllAxesAreHomed())
 			{
-				scratchString.printf("tpre%d.g", newToolNumber);
+				scratchString.printf("tfree%d.g", oldTool->Number());
 				DoFileMacro(gb, scratchString.Pointer(), false);
 			}
-			break;
+		}
+		break;
 
-		case GCodeState::toolChange2:		// Select the new tool (even if it doesn't exist - that just deselects all tools) and run tpost
-		case GCodeState::m109ToolChange2:	// Select the new tool (even if it doesn't exist - that just deselects all tools) and run tpost
-			reprap.SelectTool(newToolNumber, simulationMode != 0);
-			GetCurrentUserPosition();									// get the actual position of the new tool
+	case GCodeState::toolChange1:		// Release the old tool (if any), then run tpre for the new tool
+	case GCodeState::m109ToolChange1:	// Release the old tool (if any), then run tpre for the new tool
+		{
+			const Tool * const oldTool = reprap.GetCurrentTool();
+			if (oldTool != nullptr)
+			{
+				reprap.StandbyTool(oldTool->Number());
+			}
+		}
+		gb.AdvanceState();
+		if (reprap.GetTool(gb.MachineState().newToolNumber) != nullptr && AllAxesAreHomed() && (gb.MachineState().toolChangeParam & TPreBit) != 0)
+		{
+			scratchString.printf("tpre%d.g", gb.MachineState().newToolNumber);
+			DoFileMacro(gb, scratchString.Pointer(), false);
+		}
+		break;
 
-			gb.AdvanceState();
-			if (AllAxesAreHomed())
+	case GCodeState::toolChange2:		// Select the new tool (even if it doesn't exist - that just deselects all tools) and run tpost
+	case GCodeState::m109ToolChange2:	// Select the new tool (even if it doesn't exist - that just deselects all tools) and run tpost
+		reprap.SelectTool(gb.MachineState().newToolNumber, simulationMode != 0);
+		GetCurrentUserPosition();									// get the actual position of the new tool
+
+		gb.AdvanceState();
+		if (AllAxesAreHomed())
+		{
+			if (reprap.GetTool(gb.MachineState().newToolNumber) != nullptr && (gb.MachineState().toolChangeParam & TPostBit) != 0)
 			{
-				if (reprap.GetTool(newToolNumber) != nullptr && (toolChangeParam & TPostBit) != 0)
-				{
-					scratchString.printf("tpost%d.g", newToolNumber);
-					DoFileMacro(gb, scratchString.Pointer(), false);
-				}
+				scratchString.printf("tpost%d.g", gb.MachineState().newToolNumber);
+				DoFileMacro(gb, scratchString.Pointer(), false);
 			}
-			break;
+		}
+		break;
 
-		case GCodeState::toolChangeComplete:
-		case GCodeState::m109ToolChangeComplete:
-			// Restore the original Z axis user position, so that different tool Z offsets work even if the first move after the tool change doesn't have a Z coordinate
-			currentUserPosition[Z_AXIS] = toolChangeRestorePoint.moveCoords[Z_AXIS];
-			gb.MachineState().feedrate = toolChangeRestorePoint.feedRate;
-			// We don't restore the default fan speed in case the user wants to use a different one for the new tool
-			doingToolChange = false;
+	case GCodeState::toolChangeComplete:
+	case GCodeState::m109ToolChangeComplete:
+		// Restore the original Z axis user position, so that different tool Z offsets work even if the first move after the tool change doesn't have a Z coordinate
+		currentUserPosition[Z_AXIS] = toolChangeRestorePoint.moveCoords[Z_AXIS];
+		gb.MachineState().feedrate = toolChangeRestorePoint.feedRate;
+		// We don't restore the default fan speed in case the user wants to use a different one for the new tool
+		doingToolChange = false;
 
-			if (gb.GetState() == GCodeState::toolChangeComplete)
+		if (gb.GetState() == GCodeState::toolChangeComplete)
+		{
+			gb.SetState(GCodeState::normal);
+		}
+		else
+		{
+			UnlockAll(gb);									// allow movement again
+			if (cancelWait || ToolHeatersAtSetTemperatures(reprap.GetCurrentTool(), gb.MachineState().waitWhileCooling))
 			{
+				cancelWait = isWaiting = false;
 				gb.SetState(GCodeState::normal);
 			}
 			else
 			{
-				UnlockAll(gb);									// allow movement again
-				if (cancelWait || ToolHeatersAtSetTemperatures(reprap.GetCurrentTool(), gb.MachineState().waitWhileCooling))
-				{
-					cancelWait = isWaiting = false;
-					gb.SetState(GCodeState::normal);
-				}
-				else
-				{
-					CheckReportDue(gb, reply);
-					isWaiting = true;
-				}
+				CheckReportDue(gb, reply);
+				isWaiting = true;
 			}
-			break;
+		}
+		break;
 
-		case GCodeState::pausing1:
-			if (LockMovementAndWaitForStandstill(gb))
+	case GCodeState::pausing1:
+		if (LockMovementAndWaitForStandstill(gb))
+		{
+			gb.SetState(GCodeState::pausing2);
+			if (AllAxesAreHomed())
 			{
-				gb.SetState(GCodeState::pausing2);
-				if (AllAxesAreHomed())
-				{
-#ifdef DUET_NG
-					DoFileMacro(gb, (isAutoPaused) ? POWER_FAIL_G : PAUSE_G, true);
-#else
-					DoFileMacro(gb, PAUSE_G, true);
-#endif
-				}
+				DoFileMacro(gb, PAUSE_G, true);
 			}
-			break;
+		}
+		break;
 
-		case GCodeState::pausing2:
-			if (LockMovementAndWaitForStandstill(gb))
+	case GCodeState::pausing2:
+		if (LockMovementAndWaitForStandstill(gb))
+		{
+			reply.copy("Printing paused");
+			platform.Message(LogMessage, "Printing paused\n");
+			gb.SetState(GCodeState::normal);
+		}
+		break;
+
+	case GCodeState::resuming1:
+	case GCodeState::resuming2:
+		// Here when we have just finished running the resume macro file.
+		// Move the head back to the paused location
+		if (LockMovementAndWaitForStandstill(gb))
+		{
+			float currentZ = moveBuffer.coords[Z_AXIS];
+			for (size_t drive = 0; drive < numVisibleAxes; ++drive)
 			{
-#ifdef DUET_NG
-				if (isAutoPaused)
-				{
-					platform.Message(LoggedGenericMessage, "Print auto-paused due to low voltage\n");
-					reprap.GetHeat().SuspendHeaters(false);			// resume the heaters, we may have turned them off while we executed the pause macro
-				}
-				else
-#endif
-				{
-					reply.copy("Printing paused");
-					platform.Message(LogMessage, "Printing paused\n");
-				}
-				gb.SetState(GCodeState::normal);
+				currentUserPosition[drive] =  pauseRestorePoint.moveCoords[drive];
 			}
-			break;
-
-		case GCodeState::resuming1:
-		case GCodeState::resuming2:
-			// Here when we have just finished running the resume macro file.
-			// Move the head back to the paused location
-			if (LockMovementAndWaitForStandstill(gb))
+			ToolOffsetTransform(currentUserPosition, moveBuffer.coords);
+			for (size_t drive = numTotalAxes; drive < DRIVES; ++drive)
 			{
-				float currentZ = moveBuffer.coords[Z_AXIS];
-				for (size_t drive = 0; drive < numVisibleAxes; ++drive)
-				{
-					currentUserPosition[drive] =  pauseRestorePoint.moveCoords[drive];
-				}
-				ToolOffsetTransform(currentUserPosition, moveBuffer.coords);
-				for (size_t drive = numTotalAxes; drive < DRIVES; ++drive)
-				{
-					moveBuffer.coords[drive] = 0.0;
-				}
-				moveBuffer.feedRate = DefaultFeedrate * SecondsToMinutes;	// ask for a good feed rate, we may have paused during a slow move
-				moveBuffer.moveType = 0;
-				moveBuffer.isCoordinated = false;
-				moveBuffer.endStopsToCheck = 0;
-				moveBuffer.usePressureAdvance = false;
-				moveBuffer.filePos = noFilePosition;
-				if (gb.GetState() == GCodeState::resuming1 && currentZ > pauseRestorePoint.moveCoords[Z_AXIS])
-				{
-					// First move the head to the correct XY point, then move it down in a separate move
-					moveBuffer.coords[Z_AXIS] = currentZ;
-					gb.SetState(GCodeState::resuming2);
-				}
-				else
-				{
-					// Just move to the saved position in one go
-					gb.SetState(GCodeState::resuming3);
-				}
-				segmentsLeft = 1;
+				moveBuffer.coords[drive] = 0.0;
 			}
-			break;
-
-		case GCodeState::resuming3:
-			if (LockMovementAndWaitForStandstill(gb))
+			moveBuffer.feedRate = DefaultFeedrate * SecondsToMinutes;	// ask for a good feed rate, we may have paused during a slow move
+			moveBuffer.moveType = 0;
+			moveBuffer.isCoordinated = false;
+			moveBuffer.endStopsToCheck = 0;
+			moveBuffer.usePressureAdvance = false;
+			moveBuffer.filePos = noFilePosition;
+			if (gb.GetState() == GCodeState::resuming1 && currentZ > pauseRestorePoint.moveCoords[Z_AXIS])
 			{
-				for (size_t i = 0; i < NUM_FANS; ++i)
-				{
-					platform.SetFanValue(i, pausedFanSpeeds[i]);
-				}
-				virtualExtruderPosition = pauseRestorePoint.virtualExtruderPosition;	// reset the extruder position in case we are receiving absolute extruder moves
-				moveBuffer.virtualExtruderPosition = pauseRestorePoint.virtualExtruderPosition;
-				fileGCode->MachineState().feedrate = pauseRestorePoint.feedRate;
-				isPaused = false;
-				reply.copy("Printing resumed");
-				platform.Message(LogMessage, "Printing resumed\n");
-				gb.SetState(GCodeState::normal);
+				// First move the head to the correct XY point, then move it down in a separate move
+				moveBuffer.coords[Z_AXIS] = currentZ;
+				gb.SetState(GCodeState::resuming2);
 			}
-			break;
-
-		case GCodeState::flashing1:
-#ifdef DUET_NG
-			// Update additional modules before the main firmware
-			if (FirmwareUpdater::IsReady())
+			else
 			{
-				bool updating = false;
-				for (unsigned int module = 1; module < NumFirmwareUpdateModules; ++module)
-				{
-					if ((firmwareUpdateModuleMap & (1u << module)) != 0)
-					{
-						firmwareUpdateModuleMap &= ~(1u << module);
-						FirmwareUpdater::UpdateModule(module);
-						updating = true;
-						break;
-					}
-				}
-				if (!updating)
-				{
-					gb.SetState(GCodeState::flashing2);
-				}
+				// Just move to the saved position in one go
+				gb.SetState(GCodeState::resuming3);
 			}
-#else
-			gb.SetState(GCodeState::flashing2);
-#endif
-			break;
+			totalSegments = 1;
+			segmentsLeft = 1;
+		}
+		break;
 
-		case GCodeState::flashing2:
-			if ((firmwareUpdateModuleMap & 1) != 0)
+	case GCodeState::resuming3:
+		if (LockMovementAndWaitForStandstill(gb))
+		{
+			for (size_t i = 0; i < NUM_FANS; ++i)
 			{
-				// Update main firmware
-				firmwareUpdateModuleMap = 0;
-				platform.UpdateFirmware();
-				// The above call does not return unless an error occurred
+				platform.SetFanValue(i, pausedFanSpeeds[i]);
 			}
-			isFlashing = false;
+			virtualExtruderPosition = pauseRestorePoint.virtualExtruderPosition;	// reset the extruder position in case we are receiving absolute extruder moves
+			moveBuffer.virtualExtruderPosition = pauseRestorePoint.virtualExtruderPosition;
+			fileGCode->MachineState().feedrate = pauseRestorePoint.feedRate;
+			isPaused = false;
+			reply.copy("Printing resumed");
+			platform.Message(LogMessage, "Printing resumed\n");
 			gb.SetState(GCodeState::normal);
-			break;
+		}
+		break;
 
-		case GCodeState::stopping:		// MO after executing stop.g if present
-		case GCodeState::sleeping:		// M1 after executing sleep.g if present
-			// Deselect the active tool and turn off all heaters, unless parameter Hn was used with n > 0
-			if (!gb.Seen('H') || gb.GetIValue() <= 0)
+	case GCodeState::flashing1:
+#ifdef DUET_NG
+		// Update additional modules before the main firmware
+		if (FirmwareUpdater::IsReady())
+		{
+			bool updating = false;
+			for (unsigned int module = 1; module < NumFirmwareUpdateModules; ++module)
 			{
-				Tool* tool = reprap.GetCurrentTool();
-				if (tool != nullptr)
+				if ((firmwareUpdateModuleMap & (1u << module)) != 0)
 				{
-					reprap.StandbyTool(tool->Number());
+					firmwareUpdateModuleMap &= ~(1u << module);
+					FirmwareUpdater::UpdateModule(module);
+					updating = true;
+					break;
 				}
-				reprap.GetHeat().SwitchOffAll();
 			}
-
-			// chrishamm 2014-18-10: Although RRP says M0 is supposed to turn off all drives and heaters,
-			// I think M1 is sufficient for this purpose. Leave M0 for a normal reset.
-			if (gb.GetState() == GCodeState::sleeping)
+			if (!updating)
 			{
-				DisableDrives();
+				gb.SetState(GCodeState::flashing2);
 			}
-			else
-			{
-				platform.SetDriversIdle();
-			}
-			gb.SetState(GCodeState::normal);
-			break;
+		}
+#else
+		gb.SetState(GCodeState::flashing2);
+#endif
+		break;
 
-		// States used for grid probing
-		case GCodeState::gridProbing1:	// ready to move to next grid probe point
-			{
-				// Move to the current probe point
-				Move& move = reprap.GetMove();
-				const GridDefinition& grid = move.AccessHeightMap().GetGrid();
-				const float x = grid.GetXCoordinate(gridXindex);
-				const float y = grid.GetYCoordinate(gridYindex);
-				if (grid.IsInRadius(x, y))
-				{
-					if (move.IsAccessibleProbePoint(x, y))
-					{
-						moveBuffer.moveType = 0;
-						moveBuffer.isCoordinated = false;
-						moveBuffer.endStopsToCheck = 0;
-						moveBuffer.usePressureAdvance = false;
-						moveBuffer.filePos = noFilePosition;
-						moveBuffer.coords[X_AXIS] = x - platform.GetCurrentZProbeParameters().xOffset;
-						moveBuffer.coords[Y_AXIS] = y - platform.GetCurrentZProbeParameters().yOffset;
-						moveBuffer.coords[Z_AXIS] = platform.GetZProbeStartingHeight();
-						moveBuffer.feedRate = platform.GetZProbeTravelSpeed();
-						moveBuffer.xAxes = DefaultXAxisMapping;
-						moveBuffer.yAxes = DefaultYAxisMapping;
-						segmentsLeft = 1;
-						gb.AdvanceState();
-					}
-					else
-					{
-						platform.MessageF(WarningMessage, "Skipping grid point (%.1f, %.1f) because Z probe cannot reach it\n", (double)x, (double)y);
-						gb.SetState(GCodeState::gridProbing5);
-					}
-				}
-				else
-				{
-					gb.SetState(GCodeState::gridProbing5);
-				}
-			}
-			break;
+	case GCodeState::flashing2:
+		if ((firmwareUpdateModuleMap & 1) != 0)
+		{
+			// Update main firmware
+			firmwareUpdateModuleMap = 0;
+			platform.UpdateFirmware();
+			// The above call does not return unless an error occurred
+		}
+		isFlashing = false;
+		gb.SetState(GCodeState::normal);
+		break;
 
-		case GCodeState::gridProbing2:		// ready to probe the current grid probe point
-			if (LockMovementAndWaitForStandstill(gb))
+	case GCodeState::stopping:		// MO after executing stop.g if present
+	case GCodeState::sleeping:		// M1 after executing sleep.g if present
+		// Deselect the active tool and turn off all heaters, unless parameter Hn was used with n > 0
+		if (!gb.Seen('H') || gb.GetIValue() <= 0)
+		{
+			Tool* tool = reprap.GetCurrentTool();
+			if (tool != nullptr)
 			{
-				lastProbedTime = millis();
-				gb.AdvanceState();
+				reprap.StandbyTool(tool->Number());
 			}
-			break;
+			reprap.GetHeat().SwitchOffAll();
+		}
+
+		// chrishamm 2014-18-10: Although RRP says M0 is supposed to turn off all drives and heaters,
+		// I think M1 is sufficient for this purpose. Leave M0 for a normal reset.
+		if (gb.GetState() == GCodeState::sleeping)
+		{
+			DisableDrives();
+		}
+		else
+		{
+			platform.SetDriversIdle();
+		}
+		gb.SetState(GCodeState::normal);
+		break;
 
-		case GCodeState::gridProbing3:	// ready to probe the current grid probe point
-			if (millis() - lastProbedTime >= (uint32_t)(reprap.GetPlatform().GetCurrentZProbeParameters().recoveryTime * SecondsToMillis))
+	// States used for grid probing
+	case GCodeState::gridProbing1:	// ready to move to next grid probe point
+		{
+			// Move to the current probe point
+			Move& move = reprap.GetMove();
+			const GridDefinition& grid = move.AccessHeightMap().GetGrid();
+			const float x = grid.GetXCoordinate(gridXindex);
+			const float y = grid.GetYCoordinate(gridYindex);
+			if (grid.IsInRadius(x, y))
 			{
-				// Probe the bed at the current XY coordinates
-				// Check for probe already triggered at start
-				if (platform.GetZProbeType() == 0)
-				{
-					// No Z probe, so do manual mesh levelling instead
-					UnlockAll(gb);															// release the movement lock to allow manual Z moves
-					gb.AdvanceState();														// resume at next state when user has finished adjusting the height
-					doingManualBedProbe = true;												// suspend the Z movement limit
-					DoManualProbe(gb);
-				}
-				else if (reprap.GetPlatform().GetZProbeResult() == EndStopHit::lowHit)
-				{
-					platform.Message(ErrorMessage, "Z probe already triggered before probing move started");
-					gb.SetState(GCodeState::normal);
-					if (platform.GetZProbeType() != 0 && !probeIsDeployed)
-					{
-						DoFileMacro(gb, RETRACTPROBE_G, false);
-					}
-					break;
-				}
-				else
+				if (move.IsAccessibleProbePoint(x, y))
 				{
-					zProbeTriggered = false;
-					platform.SetProbing(true);
 					moveBuffer.moveType = 0;
 					moveBuffer.isCoordinated = false;
-					moveBuffer.endStopsToCheck = ZProbeActive;
+					moveBuffer.endStopsToCheck = 0;
 					moveBuffer.usePressureAdvance = false;
 					moveBuffer.filePos = noFilePosition;
-					moveBuffer.coords[Z_AXIS] = -platform.GetZProbeDiveHeight();
-					moveBuffer.feedRate = platform.GetCurrentZProbeParameters().probeSpeed;
+					moveBuffer.coords[X_AXIS] = x - platform.GetCurrentZProbeParameters().xOffset;
+					moveBuffer.coords[Y_AXIS] = y - platform.GetCurrentZProbeParameters().yOffset;
+					moveBuffer.coords[Z_AXIS] = platform.GetZProbeStartingHeight();
+					moveBuffer.feedRate = platform.GetZProbeTravelSpeed();
 					moveBuffer.xAxes = DefaultXAxisMapping;
 					moveBuffer.yAxes = DefaultYAxisMapping;
+					totalSegments = 1;
 					segmentsLeft = 1;
 					gb.AdvanceState();
 				}
+				else
+				{
+					platform.MessageF(WarningMessage, "Skipping grid point (%.1f, %.1f) because Z probe cannot reach it\n", (double)x, (double)y);
+					gb.SetState(GCodeState::gridProbing5);
+				}
 			}
-			break;
+			else
+			{
+				gb.SetState(GCodeState::gridProbing5);
+			}
+		}
+		break;
+
+	case GCodeState::gridProbing2:		// ready to probe the current grid probe point
+		if (LockMovementAndWaitForStandstill(gb))
+		{
+			lastProbedTime = millis();
+			gb.AdvanceState();
+		}
+		break;
 
-		case GCodeState::gridProbing4:	// ready to lift the probe after probing the current grid probe point
-			if (LockMovementAndWaitForStandstill(gb))
+	case GCodeState::gridProbing3:	// ready to probe the current grid probe point
+		if (millis() - lastProbedTime >= (uint32_t)(reprap.GetPlatform().GetCurrentZProbeParameters().recoveryTime * SecondsToMillis))
+		{
+			// Probe the bed at the current XY coordinates
+			// Check for probe already triggered at start
+			if (platform.GetZProbeType() == 0)
 			{
-				doingManualBedProbe = false;
-				float heightError;
-				if (platform.GetZProbeType() == 0)
-				{
-					// No Z probe, so we are doing manual mesh levelling. Take the current Z height as the height error.
-					heightError = moveBuffer.coords[Z_AXIS];
-				}
-				else
+				// No Z probe, so do manual mesh levelling instead
+				UnlockAll(gb);															// release the movement lock to allow manual Z moves
+				gb.AdvanceState();														// resume at next state when user has finished adjusting the height
+				doingManualBedProbe = true;												// suspend the Z movement limit
+				DoManualProbe(gb);
+			}
+			else if (reprap.GetPlatform().GetZProbeResult() == EndStopHit::lowHit)
+			{
+				platform.Message(ErrorMessage, "Z probe already triggered before probing move started");
+				gb.SetState(GCodeState::normal);
+				if (platform.GetZProbeType() != 0 && !probeIsDeployed)
 				{
-					platform.SetProbing(false);
-					if (!zProbeTriggered)
-					{
-						platform.Message(ErrorMessage, "Z probe was not triggered during probing move\n");
-						gb.SetState(GCodeState::normal);
-						if (platform.GetZProbeType() != 0 && !probeIsDeployed)
-						{
-							DoFileMacro(gb, RETRACTPROBE_G, false);
-						}
-						break;
-					}
-
-					heightError = moveBuffer.coords[Z_AXIS] - platform.ZProbeStopHeight();
+					DoFileMacro(gb, RETRACTPROBE_G, false);
 				}
-				reprap.GetMove().AccessHeightMap().SetGridHeight(gridXindex, gridYindex, heightError);
-
-				// Move back up to the dive height
+				break;
+			}
+			else
+			{
+				zProbeTriggered = false;
+				platform.SetProbing(true);
 				moveBuffer.moveType = 0;
 				moveBuffer.isCoordinated = false;
-				moveBuffer.endStopsToCheck = 0;
+				moveBuffer.endStopsToCheck = ZProbeActive;
 				moveBuffer.usePressureAdvance = false;
 				moveBuffer.filePos = noFilePosition;
-				moveBuffer.coords[Z_AXIS] = platform.GetZProbeStartingHeight();
-				moveBuffer.feedRate = platform.GetZProbeTravelSpeed();
+				moveBuffer.coords[Z_AXIS] = -platform.GetZProbeDiveHeight();
+				moveBuffer.feedRate = platform.GetCurrentZProbeParameters().probeSpeed;
 				moveBuffer.xAxes = DefaultXAxisMapping;
 				moveBuffer.yAxes = DefaultYAxisMapping;
+				totalSegments = 1;
 				segmentsLeft = 1;
-				gb.SetState(GCodeState::gridProbing5);
+				gb.AdvanceState();
 			}
-			break;
+		}
+		break;
 
-		case GCodeState::gridProbing5:	// ready to compute the next probe point
-			if (LockMovementAndWaitForStandstill(gb))
+	case GCodeState::gridProbing4:	// ready to lift the probe after probing the current grid probe point
+		if (LockMovementAndWaitForStandstill(gb))
+		{
+			doingManualBedProbe = false;
+			float heightError;
+			if (platform.GetZProbeType() == 0)
 			{
-				const GridDefinition& grid = reprap.GetMove().AccessHeightMap().GetGrid();
-				if (gridYindex & 1)
-				{
-					// Odd row, so decreasing X
-					if (gridXindex == 0)
-					{
-						++gridYindex;
-					}
-					else
-					{
-						--gridXindex;
-					}
-				}
-				else
-				{
-					// Even row, so increasing X
-					if (gridXindex + 1 == grid.NumXpoints())
-					{
-						++gridYindex;
-					}
-					else
-					{
-						++gridXindex;
-					}
-				}
-				if (gridYindex == grid.NumYpoints())
+				// No Z probe, so we are doing manual mesh levelling. Take the current Z height as the height error.
+				heightError = moveBuffer.coords[Z_AXIS];
+			}
+			else
+			{
+				platform.SetProbing(false);
+				if (!zProbeTriggered)
 				{
-					// Done all the points
-					gb.AdvanceState();
+					platform.Message(ErrorMessage, "Z probe was not triggered during probing move\n");
+					gb.SetState(GCodeState::normal);
 					if (platform.GetZProbeType() != 0 && !probeIsDeployed)
 					{
 						DoFileMacro(gb, RETRACTPROBE_G, false);
 					}
+					break;
+				}
+
+				heightError = moveBuffer.coords[Z_AXIS] - platform.ZProbeStopHeight();
+			}
+			reprap.GetMove().AccessHeightMap().SetGridHeight(gridXindex, gridYindex, heightError);
+
+			// Move back up to the dive height
+			moveBuffer.moveType = 0;
+			moveBuffer.isCoordinated = false;
+			moveBuffer.endStopsToCheck = 0;
+			moveBuffer.usePressureAdvance = false;
+			moveBuffer.filePos = noFilePosition;
+			moveBuffer.coords[Z_AXIS] = platform.GetZProbeStartingHeight();
+			moveBuffer.feedRate = platform.GetZProbeTravelSpeed();
+			moveBuffer.xAxes = DefaultXAxisMapping;
+			moveBuffer.yAxes = DefaultYAxisMapping;
+			totalSegments = 1;
+			segmentsLeft = 1;
+			gb.SetState(GCodeState::gridProbing5);
+		}
+		break;
+
+	case GCodeState::gridProbing5:	// ready to compute the next probe point
+		if (LockMovementAndWaitForStandstill(gb))
+		{
+			const GridDefinition& grid = reprap.GetMove().AccessHeightMap().GetGrid();
+			if (gridYindex & 1)
+			{
+				// Odd row, so decreasing X
+				if (gridXindex == 0)
+				{
+					++gridYindex;
 				}
 				else
 				{
-					gb.SetState(GCodeState::gridProbing1);
+					--gridXindex;
 				}
 			}
-			break;
-
-		case GCodeState::gridprobing6:
-			// Finished probing the grid, and retracted the probe if necessary
+			else
 			{
-				float mean, deviation;
-				const uint32_t numPointsProbed = reprap.GetMove().AccessHeightMap().GetStatistics(mean, deviation);
-				if (numPointsProbed >= 4)
+				// Even row, so increasing X
+				if (gridXindex + 1 == grid.NumXpoints())
 				{
-					reply.printf("%" PRIu32 " points probed, mean error %.3f, deviation %.3f\n", numPointsProbed, (double)mean, (double)deviation);
-					error = SaveHeightMap(gb, reply);
-					reprap.GetMove().AccessHeightMap().ExtrapolateMissing();
-					reprap.GetMove().UseMesh(true);
+					++gridYindex;
 				}
 				else
 				{
-					reply.copy("Too few points probed");
-					error = true;
+					++gridXindex;
 				}
 			}
-			gb.SetState(GCodeState::normal);
-			break;
+			if (gridYindex == grid.NumYpoints())
+			{
+				// Done all the points
+				gb.AdvanceState();
+				if (platform.GetZProbeType() != 0 && !probeIsDeployed)
+				{
+					DoFileMacro(gb, RETRACTPROBE_G, false);
+				}
+			}
+			else
+			{
+				gb.SetState(GCodeState::gridProbing1);
+			}
+		}
+		break;
+
+	case GCodeState::gridprobing6:
+		// Finished probing the grid, and retracted the probe if necessary
+		{
+			float mean, deviation;
+			const uint32_t numPointsProbed = reprap.GetMove().AccessHeightMap().GetStatistics(mean, deviation);
+			if (numPointsProbed >= 4)
+			{
+				reply.printf("%" PRIu32 " points probed, mean error %.3f, deviation %.3f\n", numPointsProbed, (double)mean, (double)deviation);
+				error = SaveHeightMap(gb, reply);
+				reprap.GetMove().AccessHeightMap().ExtrapolateMissing();
+				reprap.GetMove().UseMesh(true);
+			}
+			else
+			{
+				reply.copy("Too few points probed");
+				error = true;
+			}
+		}
+		gb.SetState(GCodeState::normal);
+		break;
 
-		// States used for G30 probing
-		case GCodeState::probingAtPoint0:
-			// Initial state when executing G30 with a P parameter. Start by moving to the dive height at the current position.
+	// States used for G30 probing
+	case GCodeState::probingAtPoint0:
+		// Initial state when executing G30 with a P parameter. Start by moving to the dive height at the current position.
+		moveBuffer.moveType = 0;
+		moveBuffer.isCoordinated = false;
+		moveBuffer.endStopsToCheck = 0;
+		moveBuffer.usePressureAdvance = false;
+		moveBuffer.filePos = noFilePosition;
+		moveBuffer.coords[Z_AXIS] = platform.GetZProbeStartingHeight();
+		moveBuffer.feedRate = platform.GetZProbeTravelSpeed();
+		moveBuffer.xAxes = DefaultXAxisMapping;
+		moveBuffer.yAxes = DefaultYAxisMapping;
+		totalSegments = 1;
+		segmentsLeft = 1;
+		gb.AdvanceState();
+		break;
+
+	case GCodeState::probingAtPoint1:
+		// The move to raise/lower the head to the correct dive height has been commanded.
+		if (LockMovementAndWaitForStandstill(gb))
+		{
+			// Head is at the dive height but needs to be moved to the correct XY position.
+			// The XY coordinates have already been stored.
 			moveBuffer.moveType = 0;
 			moveBuffer.isCoordinated = false;
 			moveBuffer.endStopsToCheck = 0;
 			moveBuffer.usePressureAdvance = false;
 			moveBuffer.filePos = noFilePosition;
+			(void)reprap.GetMove().GetProbeCoordinates(g30ProbePointIndex, moveBuffer.coords[X_AXIS], moveBuffer.coords[Y_AXIS], true);
 			moveBuffer.coords[Z_AXIS] = platform.GetZProbeStartingHeight();
 			moveBuffer.feedRate = platform.GetZProbeTravelSpeed();
 			moveBuffer.xAxes = DefaultXAxisMapping;
 			moveBuffer.yAxes = DefaultYAxisMapping;
+			totalSegments = 1;
 			segmentsLeft = 1;
+
 			gb.AdvanceState();
-			break;
+		}
+		break;
 
-		case GCodeState::probingAtPoint1:
-			// The move to raise/lower the head to the correct dive height has been commanded.
-			if (LockMovementAndWaitForStandstill(gb))
+	case GCodeState::probingAtPoint2:
+		// Executing G30 with a P parameter. The move to put the head at the specified XY coordinates has been commanded.
+		// OR initial state when executing G30 with no P parameter
+		if (LockMovementAndWaitForStandstill(gb))
+		{
+			// Head has finished moving to the correct XY position
+			lastProbedTime = millis();			// start the probe recovery timer
+			gb.AdvanceState();
+		}
+		break;
+
+	case GCodeState::probingAtPoint3:
+		// Executing G30 with a P parameter. The move to put the head at the specified XY coordinates has been completed and the recovery timer started.
+		// OR executing G30 without a P parameter, and the recovery timer has been started.
+		if (millis() - lastProbedTime >= (uint32_t)(platform.GetCurrentZProbeParameters().recoveryTime * SecondsToMillis))
+		{
+			// The probe recovery time has elapsed, so we can start the probing  move
+			if (platform.GetZProbeType() == 0)
+			{
+				// No Z probe, so we are doing manual 'probing'
+				UnlockAll(gb);															// release the movement lock to allow manual Z moves
+				gb.AdvanceState();														// resume at the next state when the user has finished
+				doingManualBedProbe = true;												// suspend the Z movement limit
+				DoManualProbe(gb);
+			}
+			else if (reprap.GetPlatform().GetZProbeResult() == EndStopHit::lowHit)		// check for probe already triggered at start
+			{
+				// Z probe is already triggered at the start of the move, so abandon the probe and record an error
+				platform.Message(ErrorMessage, "Z probe already triggered at start of probing move\n");
+				if (g30ProbePointIndex >= 0)
+				{
+					reprap.GetMove().SetZBedProbePoint(g30ProbePointIndex, platform.GetZProbeDiveHeight(), true, true);
+				}
+				if (platform.GetZProbeType() != 0 && !probeIsDeployed)
+				{
+					DoFileMacro(gb, RETRACTPROBE_G, false);
+				}
+				gb.SetState(GCodeState::normal);										// no point in doing anything else
+			}
+			else
 			{
-				// Head is at the dive height but needs to be moved to the correct XY position.
-				// The XY coordinates have already been stored.
+				zProbeTriggered = false;
+				platform.SetProbing(true);
 				moveBuffer.moveType = 0;
 				moveBuffer.isCoordinated = false;
-				moveBuffer.endStopsToCheck = 0;
+				moveBuffer.endStopsToCheck = ZProbeActive;
 				moveBuffer.usePressureAdvance = false;
 				moveBuffer.filePos = noFilePosition;
-				(void)reprap.GetMove().GetProbeCoordinates(g30ProbePointIndex, moveBuffer.coords[X_AXIS], moveBuffer.coords[Y_AXIS], true);
-				moveBuffer.coords[Z_AXIS] = platform.GetZProbeStartingHeight();
-				moveBuffer.feedRate = platform.GetZProbeTravelSpeed();
+				moveBuffer.coords[Z_AXIS] = (GetAxisIsHomed(Z_AXIS))
+											? -platform.GetZProbeDiveHeight()			// Z axis has been homed, so no point in going very far
+											: -1.1 * platform.AxisTotalLength(Z_AXIS);	// Z axis not homed yet, so treat this as a homing move
+				moveBuffer.feedRate = platform.GetCurrentZProbeParameters().probeSpeed;
 				moveBuffer.xAxes = DefaultXAxisMapping;
 				moveBuffer.yAxes = DefaultYAxisMapping;
+				totalSegments = 1;
 				segmentsLeft = 1;
-
 				gb.AdvanceState();
 			}
-			break;
+		}
+		break;
 
-		case GCodeState::probingAtPoint2:
-			// Executing G30 with a P parameter. The move to put the head at the specified XY coordinates has been commanded.
-			// OR initial state when executing G30 with no P parameter
-			if (LockMovementAndWaitForStandstill(gb))
+	case GCodeState::probingAtPoint4:
+		// Executing G30. The probe wasn't triggered at the start of the move, and the probing move has been commanded.
+		if (LockMovementAndWaitForStandstill(gb))
+		{
+			// Probing move has stopped
+			doingManualBedProbe = false;
+			bool probingError = false;
+			if (platform.GetZProbeType() == 0)
 			{
-				// Head has finished moving to the correct XY position
-				lastProbedTime = millis();			// start the probe recovery timer
-				gb.AdvanceState();
+				// No Z probe, so we are doing manual mesh levelling. Take the current Z height as the height error.
+				g30zHeightError = moveBuffer.coords[Z_AXIS];
 			}
-			break;
-
-		case GCodeState::probingAtPoint3:
-			// Executing G30 with a P parameter. The move to put the head at the specified XY coordinates has been completed and the recovery timer started.
-			// OR executing G30 without a P parameter, and the recovery timer has been started.
-			if (millis() - lastProbedTime >= (uint32_t)(platform.GetCurrentZProbeParameters().recoveryTime * SecondsToMillis))
+			else
 			{
-				// The probe recovery time has elapsed, so we can start the probing  move
-				if (platform.GetZProbeType() == 0)
+				platform.SetProbing(false);
+				if (!zProbeTriggered)
 				{
-					// No Z probe, so we are doing manual 'probing'
-					UnlockAll(gb);															// release the movement lock to allow manual Z moves
-					gb.AdvanceState();														// resume at the next state when the user has finished
-					doingManualBedProbe = true;												// suspend the Z movement limit
-					DoManualProbe(gb);
-				}
-				else if (reprap.GetPlatform().GetZProbeResult() == EndStopHit::lowHit)		// check for probe already triggered at start
-				{
-					// Z probe is already triggered at the start of the move, so abandon the probe and record an error
-					platform.Message(ErrorMessage, "Z probe already triggered at start of probing move\n");
-					if (g30ProbePointIndex >= 0)
-					{
-						reprap.GetMove().SetZBedProbePoint(g30ProbePointIndex, platform.GetZProbeDiveHeight(), true, true);
-					}
-					if (platform.GetZProbeType() != 0 && !probeIsDeployed)
-					{
-						DoFileMacro(gb, RETRACTPROBE_G, false);
-					}
-					gb.SetState(GCodeState::normal);										// no point in doing anything else
+					platform.Message(ErrorMessage, "Z probe was not triggered during probing move\n");
+					g30zHeightError = 0.0;
+					probingError = true;
 				}
 				else
 				{
-					zProbeTriggered = false;
-					platform.SetProbing(true);
-					moveBuffer.moveType = 0;
-					moveBuffer.isCoordinated = false;
-					moveBuffer.endStopsToCheck = ZProbeActive;
-					moveBuffer.usePressureAdvance = false;
-					moveBuffer.filePos = noFilePosition;
-					moveBuffer.coords[Z_AXIS] = (GetAxisIsHomed(Z_AXIS))
-												? -platform.GetZProbeDiveHeight()			// Z axis has been homed, so no point in going very far
-												: -1.1 * platform.AxisTotalLength(Z_AXIS);	// Z axis not homed yet, so treat this as a homing move
-					moveBuffer.feedRate = platform.GetCurrentZProbeParameters().probeSpeed;
-					moveBuffer.xAxes = DefaultXAxisMapping;
-					moveBuffer.yAxes = DefaultYAxisMapping;
-					segmentsLeft = 1;
-					gb.AdvanceState();
+					// Successful probing
+					float heightAdjust = 0.0;
+					bool dummy;
+					gb.TryGetFValue('H', heightAdjust, dummy);
+					float m[MaxAxes];
+					reprap.GetMove().GetCurrentMachinePosition(m, false);		// get height without bed compensation
+					g30zStoppedHeight = m[Z_AXIS] - heightAdjust;				// save for later
+					g30zHeightError = g30zStoppedHeight - platform.ZProbeStopHeight();
 				}
 			}
-			break;
 
-		case GCodeState::probingAtPoint4:
-			// Executing G30. The probe wasn't triggered at the start of the move, and the probing move has been commanded.
-			if (LockMovementAndWaitForStandstill(gb))
+			if (g30ProbePointIndex < 0)
 			{
-				// Probing move has stopped
-				doingManualBedProbe = false;
-				bool probingError = false;
-				if (platform.GetZProbeType() == 0)
+				// Simple G30 probing move
+				gb.SetState(GCodeState::normal);								// usually nothing more to do except perhaps retract the probe
+				if (!probingError)
 				{
-					// No Z probe, so we are doing manual mesh levelling. Take the current Z height as the height error.
-					g30zHeightError = moveBuffer.coords[Z_AXIS];
-				}
-				else
-				{
-					platform.SetProbing(false);
-					if (!zProbeTriggered)
+					if (g30SValue == -1 || g30SValue == -2)
 					{
-						platform.Message(ErrorMessage, "Z probe was not triggered during probing move\n");
-						g30zHeightError = 0.0;
-						probingError = true;
+						// G30 S-1 or S-2 command
+						gb.SetState(GCodeState::probingAtPoint7);				// special state for reporting the stopped height at the end
 					}
 					else
 					{
-						// Successful probing
-						float heightAdjust = 0.0;
-						bool dummy;
-						gb.TryGetFValue('H', heightAdjust, dummy);
-						float m[MaxAxes];
-						reprap.GetMove().GetCurrentMachinePosition(m, false);		// get height without bed compensation
-						g30zStoppedHeight = m[Z_AXIS] - heightAdjust;				// save for later
-						g30zHeightError = g30zStoppedHeight - platform.ZProbeStopHeight();
-					}
-				}
-
-				if (g30ProbePointIndex < 0)
-				{
-					// Simple G30 probing move
-					gb.SetState(GCodeState::normal);								// usually nothing more to do except perhaps retract the probe
-					if (!probingError)
-					{
-						if (g30SValue == -1 || g30SValue == -2)
-						{
-							// G30 S-1 or S-2 command
-							gb.SetState(GCodeState::probingAtPoint7);				// special state for reporting the stopped height at the end
-						}
-						else
-						{
-							// Reset the Z axis origin according to the height error
-							moveBuffer.coords[Z_AXIS] -= g30zHeightError;
-							reprap.GetMove().SetNewPosition(moveBuffer.coords, false);
-							ToolOffsetInverseTransform(moveBuffer.coords, currentUserPosition);
-							SetAxisIsHomed(Z_AXIS);									//TODO this is only correct if the Z axis is Cartesian-like!
-						}
-					}
-
-					if (platform.GetZProbeType() != 0 && !probeIsDeployed)
-					{
-						DoFileMacro(gb, RETRACTPROBE_G, false);						// retract the probe before moving to the new state
-					}
-				}
-				else
-				{
-					// Probing with a probe point index number
-					if (!GetAxisIsHomed(Z_AXIS) && !probingError)
-					{
-						// The Z axis has not yet been homed, so treat this probe as a homing move.
-						moveBuffer.coords[Z_AXIS] -= g30zHeightError;			// reset the Z origin
+						// Reset the Z axis origin according to the height error
+						moveBuffer.coords[Z_AXIS] -= g30zHeightError;
 						reprap.GetMove().SetNewPosition(moveBuffer.coords, false);
 						ToolOffsetInverseTransform(moveBuffer.coords, currentUserPosition);
-						SetAxisIsHomed(Z_AXIS);
-						g30zHeightError = 0.0;
+						SetAxisIsHomed(Z_AXIS);									//TODO this is only correct if the Z axis is Cartesian-like!
 					}
-					reprap.GetMove().SetZBedProbePoint(g30ProbePointIndex, g30zHeightError, true, probingError);
-
-					// Move back up to the dive height before we change anything, in particular before we adjust leadscrews
-					moveBuffer.moveType = 0;
-					moveBuffer.isCoordinated = false;
-					moveBuffer.endStopsToCheck = 0;
-					moveBuffer.usePressureAdvance = false;
-					moveBuffer.filePos = noFilePosition;
-					moveBuffer.coords[Z_AXIS] = platform.GetZProbeStartingHeight();
-					moveBuffer.feedRate = platform.GetZProbeTravelSpeed();
-					moveBuffer.xAxes = DefaultXAxisMapping;
-					moveBuffer.yAxes = DefaultYAxisMapping;
-					segmentsLeft = 1;
-					gb.AdvanceState();
 				}
-			}
-			break;
 
-		case GCodeState::probingAtPoint5:
-			// Here when we were probing at a numbered point and we have moved the head back up to the dive height
-			if (LockMovementAndWaitForStandstill(gb))
-			{
-				gb.AdvanceState();
 				if (platform.GetZProbeType() != 0 && !probeIsDeployed)
 				{
-					DoFileMacro(gb, RETRACTPROBE_G, false);
+					DoFileMacro(gb, RETRACTPROBE_G, false);						// retract the probe before moving to the new state
 				}
 			}
-			break;
-
-		case GCodeState::probingAtPoint6:
-			// Here when we have finished probing with a P parameter and have retracted the probe if necessary
-			if (LockMovementAndWaitForStandstill(gb))		// retracting the Z probe
+			else
 			{
-				if (g30SValue == 1)
+				// Probing with a probe point index number
+				if (!GetAxisIsHomed(Z_AXIS) && !probingError)
 				{
-					// G30 with a silly Z value and S=1 is equivalent to G30 with no parameters in that it sets the current Z height
-					// This is useful because it adjusts the XY position to account for the probe offset.
-					moveBuffer.coords[Z_AXIS] -= g30zHeightError;
+					// The Z axis has not yet been homed, so treat this probe as a homing move.
+					moveBuffer.coords[Z_AXIS] -= g30zHeightError;			// reset the Z origin
 					reprap.GetMove().SetNewPosition(moveBuffer.coords, false);
 					ToolOffsetInverseTransform(moveBuffer.coords, currentUserPosition);
+					SetAxisIsHomed(Z_AXIS);
+					g30zHeightError = 0.0;
 				}
-				else if (g30SValue >= -1)
-				{
-					error = reprap.GetMove().FinishedBedProbing(g30SValue, reply);
-				}
+				reprap.GetMove().SetZBedProbePoint(g30ProbePointIndex, g30zHeightError, true, probingError);
+
+				// Move back up to the dive height before we change anything, in particular before we adjust leadscrews
+				moveBuffer.moveType = 0;
+				moveBuffer.isCoordinated = false;
+				moveBuffer.endStopsToCheck = 0;
+				moveBuffer.usePressureAdvance = false;
+				moveBuffer.filePos = noFilePosition;
+				moveBuffer.coords[Z_AXIS] = platform.GetZProbeStartingHeight();
+				moveBuffer.feedRate = platform.GetZProbeTravelSpeed();
+				moveBuffer.xAxes = DefaultXAxisMapping;
+				moveBuffer.yAxes = DefaultYAxisMapping;
+				totalSegments = 1;
+				segmentsLeft = 1;
+				gb.AdvanceState();
 			}
+		}
+		break;
 
-			gb.SetState(GCodeState::normal);
-			break;
+	case GCodeState::probingAtPoint5:
+		// Here when we were probing at a numbered point and we have moved the head back up to the dive height
+		if (LockMovementAndWaitForStandstill(gb))
+		{
+			gb.AdvanceState();
+			if (platform.GetZProbeType() != 0 && !probeIsDeployed)
+			{
+				DoFileMacro(gb, RETRACTPROBE_G, false);
+			}
+		}
+		break;
 
-		case GCodeState::probingAtPoint7:
-			// Here when we have finished executing G30 S-1 or S-2 including retracting the probe if necessary
-			if (g30SValue == -2)
+	case GCodeState::probingAtPoint6:
+		// Here when we have finished probing with a P parameter and have retracted the probe if necessary
+		if (LockMovementAndWaitForStandstill(gb))		// retracting the Z probe
+		{
+			if (g30SValue == 1)
 			{
-				// Adjust the Z offset of the current tool to account for the height error
-				Tool *const tool = reprap.GetCurrentTool();
-				if (tool == nullptr)
-				{
-					platform.Message(ErrorMessage, "G30 S-2 commanded with no tool selected");
-					error = true;
-				}
-				else
-				{
-					const float *offsets = tool->GetOffsets();
-					tool->SetOffset(Z_AXIS, offsets[Z_AXIS] + g30zHeightError);
-				}
+				// G30 with a silly Z value and S=1 is equivalent to G30 with no parameters in that it sets the current Z height
+				// This is useful because it adjusts the XY position to account for the probe offset.
+				moveBuffer.coords[Z_AXIS] -= g30zHeightError;
+				reprap.GetMove().SetNewPosition(moveBuffer.coords, false);
+				ToolOffsetInverseTransform(moveBuffer.coords, currentUserPosition);
+			}
+			else if (g30SValue >= -1)
+			{
+				error = reprap.GetMove().FinishedBedProbing(g30SValue, reply);
+			}
+		}
+
+		gb.SetState(GCodeState::normal);
+		break;
+
+	case GCodeState::probingAtPoint7:
+		// Here when we have finished executing G30 S-1 or S-2 including retracting the probe if necessary
+		if (g30SValue == -2)
+		{
+			// Adjust the Z offset of the current tool to account for the height error
+			Tool *const tool = reprap.GetCurrentTool();
+			if (tool == nullptr)
+			{
+				platform.Message(ErrorMessage, "G30 S-2 commanded with no tool selected");
+				error = true;
 			}
 			else
 			{
-				// Just print the stop height
-				reply.printf("Stopped at height %.3f mm", (double)g30zStoppedHeight);
+				const float *offsets = tool->GetOffsets();
+				tool->SetOffset(Z_AXIS, offsets[Z_AXIS] + g30zHeightError, true);
+			}
+		}
+		else
+		{
+			// Just print the stop height
+			reply.printf("Stopped at height %.3f mm", (double)g30zStoppedHeight);
+		}
+		gb.SetState(GCodeState::normal);
+		break;
+
+	// Firmware retraction/un-retraction states
+	case GCodeState::doingFirmwareRetraction:
+		// We just did the retraction part of a firmware retraction, now we need to do the Z hop
+		if (segmentsLeft == 0)
+		{
+			const AxesBitmap xAxes = reprap.GetCurrentXAxes();
+			const AxesBitmap yAxes = reprap.GetCurrentYAxes();
+			reprap.GetMove().GetCurrentUserPosition(moveBuffer.coords, 0, xAxes, yAxes);
+			for (size_t i = numTotalAxes; i < DRIVES; ++i)
+			{
+				moveBuffer.coords[i] = 0.0;
 			}
+			moveBuffer.feedRate = platform.MaxFeedrate(Z_AXIS);
+			moveBuffer.coords[Z_AXIS] += retractHop;
+			currentZHop = retractHop;
+			moveBuffer.moveType = 0;
+			moveBuffer.isCoordinated = false;
+			moveBuffer.isFirmwareRetraction = true;
+			moveBuffer.usePressureAdvance = false;
+			moveBuffer.filePos = (&gb == fileGCode) ? gb.GetFilePosition(fileInput->BytesCached()) : noFilePosition;
+			moveBuffer.canPauseAfter = false;			// don't pause after a retraction because that could cause too much retraction
+			moveBuffer.xAxes = xAxes;
+			moveBuffer.yAxes = yAxes;
+			totalSegments = 1;
+			segmentsLeft = 1;
 			gb.SetState(GCodeState::normal);
-			break;
+		}
+		break;
 
-		// Firmware retraction/un-retraction states
-		case GCodeState::doingFirmwareRetraction:
-			// We just did the retraction part of a firmware retraction, now we need to do the Z hop
-			if (segmentsLeft == 0)
+	case GCodeState::doingFirmwareUnRetraction:
+		// We just undid the Z-hop part of a firmware un-retraction, now we need to do the un-retract
+		if (segmentsLeft == 0)
+		{
+			const Tool * const tool = reprap.GetCurrentTool();
+			if (tool != nullptr)
 			{
-				const AxesBitmap xAxes = reprap.GetCurrentXAxes();
-				const AxesBitmap yAxes = reprap.GetCurrentYAxes();
+				const uint32_t xAxes = reprap.GetCurrentXAxes();
+				const uint32_t yAxes = reprap.GetCurrentYAxes();
 				reprap.GetMove().GetCurrentUserPosition(moveBuffer.coords, 0, xAxes, yAxes);
 				for (size_t i = numTotalAxes; i < DRIVES; ++i)
 				{
 					moveBuffer.coords[i] = 0.0;
 				}
-				moveBuffer.feedRate = platform.MaxFeedrate(Z_AXIS);
-				moveBuffer.coords[Z_AXIS] += retractHop;
-				currentZHop = retractHop;
+				for (size_t i = 0; i < tool->DriveCount(); ++i)
+				{
+					moveBuffer.coords[numTotalAxes + tool->Drive(i)] = retractLength + retractExtra;
+				}
+				moveBuffer.feedRate = unRetractSpeed;
 				moveBuffer.moveType = 0;
 				moveBuffer.isCoordinated = false;
 				moveBuffer.isFirmwareRetraction = true;
 				moveBuffer.usePressureAdvance = false;
-				moveBuffer.filePos = (&gb == fileGCode) ? gb.GetFilePosition(fileInput->BytesCached()) : noFilePosition;
-				moveBuffer.canPauseAfter = false;			// don't pause after a retraction because that could cause too much retraction
+				moveBuffer.filePos = (&gb == fileGCode) ? gb.MachineState().fileState.GetPosition() - fileInput->BytesCached() : noFilePosition;
+				moveBuffer.canPauseAfter = true;
 				moveBuffer.xAxes = xAxes;
 				moveBuffer.yAxes = yAxes;
+				totalSegments = 1;
 				segmentsLeft = 1;
-				gb.SetState(GCodeState::normal);
 			}
-			break;
-
-		case GCodeState::doingFirmwareUnRetraction:
-			// We just undid the Z-hop part of a firmware un-retraction, now we need to do the un-retract
-			if (segmentsLeft == 0)
-			{
-				const Tool * const tool = reprap.GetCurrentTool();
-				if (tool != nullptr)
-				{
-					const uint32_t xAxes = reprap.GetCurrentXAxes();
-					const uint32_t yAxes = reprap.GetCurrentYAxes();
-					reprap.GetMove().GetCurrentUserPosition(moveBuffer.coords, 0, xAxes, yAxes);
-					for (size_t i = numTotalAxes; i < DRIVES; ++i)
-					{
-						moveBuffer.coords[i] = 0.0;
-					}
-					for (size_t i = 0; i < tool->DriveCount(); ++i)
-					{
-						moveBuffer.coords[numTotalAxes + tool->Drive(i)] = retractLength + retractExtra;
-					}
-					moveBuffer.feedRate = unRetractSpeed;
-					moveBuffer.moveType = 0;
-					moveBuffer.isCoordinated = false;
-					moveBuffer.isFirmwareRetraction = true;
-					moveBuffer.usePressureAdvance = false;
-					moveBuffer.filePos = (&gb == fileGCode) ? gb.MachineState().fileState.GetPosition() - fileInput->BytesCached() : noFilePosition;
-					moveBuffer.canPauseAfter = true;
-					moveBuffer.xAxes = xAxes;
-					moveBuffer.yAxes = yAxes;
-					segmentsLeft = 1;
-				}
-				gb.SetState(GCodeState::normal);
-			}
-			break;
+			gb.SetState(GCodeState::normal);
+		}
+		break;
 
-		case GCodeState::loadingFilament:
-			// We just returned from the filament load macro
-			if (reprap.GetCurrentTool() != nullptr)
+	case GCodeState::loadingFilament:
+		// We just returned from the filament load macro
+		if (reprap.GetCurrentTool() != nullptr)
+		{
+			reprap.GetCurrentTool()->GetFilament()->Load(filamentToLoad);
+			if (reprap.Debug(moduleGcodes))
 			{
-				reprap.GetCurrentTool()->GetFilament()->Load(filamentToLoad);
-				if (reprap.Debug(moduleGcodes))
-				{
-					platform.MessageF(LoggedGenericMessage, "Filament %s loaded", filamentToLoad);
-				}
+				platform.MessageF(LoggedGenericMessage, "Filament %s loaded", filamentToLoad);
 			}
-			gb.SetState(GCodeState::normal);
-			break;
+		}
+		gb.SetState(GCodeState::normal);
+		break;
 
-		case GCodeState::unloadingFilament:
-			// We just returned from the filament unload macro
-			if (reprap.GetCurrentTool() != nullptr)
+	case GCodeState::unloadingFilament:
+		// We just returned from the filament unload macro
+		if (reprap.GetCurrentTool() != nullptr)
+		{
+			if (reprap.Debug(moduleGcodes))
 			{
-				if (reprap.Debug(moduleGcodes))
-				{
-					platform.MessageF(LoggedGenericMessage, "Filament %s unloaded", reprap.GetCurrentTool()->GetFilament()->GetName());
-				}
-				reprap.GetCurrentTool()->GetFilament()->Unload();
+				platform.MessageF(LoggedGenericMessage, "Filament %s unloaded", reprap.GetCurrentTool()->GetFilament()->GetName());
 			}
-			gb.SetState(GCodeState::normal);
-			break;
-
-		default:				// should not happen
-			platform.Message(ErrorMessage, "Undefined GCodeState\n");
-			gb.SetState(GCodeState::normal);
-			break;
+			reprap.GetCurrentTool()->GetFilament()->Unload();
 		}
+		gb.SetState(GCodeState::normal);
+		break;
 
-		if (gb.GetState() == GCodeState::normal)
+#if HAS_VOLTAGE_MONITOR
+	case GCodeState::powerFailPausing1:
+		if (gb.IsReady() || gb.IsExecuting())
 		{
-			// We completed a command, so unlock resources and tell the host about it
-			gb.timerRunning = false;
-			UnlockAll(gb);
-			HandleReply(gb, error, reply.Pointer());
+			gb.SetFinished(ActOnCode(gb, reply));							// execute the pause script
 		}
-	}
+		else
+		{
+			SaveResumeInfo(true);												// create the resume file so that we can resume after power down
+			platform.Message(LoggedGenericMessage, "Print auto-paused due to low voltage\n");
+			gb.SetState(GCodeState::normal);
+		}
+		break;
+#endif
 
-	// Move on to the next gcode source ready for next time
-	++nextGcodeSource;
-	if (nextGcodeSource == ARRAY_SIZE(gcodeSources))
-	{
-		nextGcodeSource = 0;
+	default:				// should not happen
+		platform.Message(ErrorMessage, "Undefined GCodeState\n");
+		gb.SetState(GCodeState::normal);
+		break;
 	}
 
-	// Check if we need to display a warning
-	const uint32_t now = millis();
-	if (now - lastWarningMillis >= MinimumWarningInterval)
+	if (gb.GetState() == GCodeState::normal)
 	{
-		if (displayNoToolWarning)
-		{
-			platform.Message(ErrorMessage, "Attempting to extrude with no tool selected.\n");
-			displayNoToolWarning = false;
-			lastWarningMillis = now;
-		}
-		if (displayDeltaNotHomedWarning)
-		{
-			platform.Message(ErrorMessage, "Attempt to move the head of a Delta or SCARA printer before homing it\n");
-			displayDeltaNotHomedWarning = false;
-			lastWarningMillis = now;
-		}
+		// We completed a command, so unlock resources and tell the host about it
+		gb.timerRunning = false;
+		UnlockAll(gb);
+		HandleReply(gb, error, reply.Pointer());
 	}
-	platform.ClassReport(longWait);
 }
 
 // Start a new gcode, or continue to execute one that has already been started:
@@ -1350,7 +1392,7 @@ void GCodes::CheckTriggers()
 			else if (LockMovement(*daemonGCode))					// need to lock movement before executing the pause macro
 			{
 				ClearBit(triggersPending, lowestTriggerPending);	// clear the trigger
-				DoPause(*daemonGCode, false);
+				DoPause(*daemonGCode, PauseReason::trigger, nullptr);
 			}
 		}
 		else
@@ -1362,15 +1404,18 @@ void GCodes::CheckTriggers()
 			DoFileMacro(*daemonGCode, filename.Pointer(), true);
 		}
 	}
-	else if (   lastFilamentError != FilamentSensorStatus::ok		// check for a filament error
-			 && reprap.GetPrintMonitor().IsPrinting()
-			 && !isPaused
-			 && LockMovement(*daemonGCode)							// need to lock movement before executing the pause macro
-		 	)
+}
+
+// Check for and respond to filament errors
+void GCodes::CheckFilament()
+{
+	if (   lastFilamentError != FilamentSensorStatus::ok			// check for a filament error
+		&& reprap.GetPrintMonitor().IsPrinting()
+		&& !isPaused
+		&& LockMovement(*autoPauseGCode)							// need to lock movement before executing the pause macro
+	   )
 	{
-		scratchString.printf("Extruder %u reports %s", lastFilamentErrorExtruder, FilamentSensor::GetErrorMessage(lastFilamentError));
-		platform.SendAlert(GenericMessage, scratchString.Pointer(), "Filament error", 1, 0.0, 0);
-		DoPause(*daemonGCode, false);
+		DoPause(*autoPauseGCode, PauseReason::filament, "Extruder %u reports %s", lastFilamentErrorExtruder, FilamentSensor::GetErrorMessage(lastFilamentError));
 		lastFilamentError = FilamentSensorStatus::ok;
 	}
 }
@@ -1394,7 +1439,7 @@ void GCodes::DoEmergencyStop()
 }
 
 // Pause the print. Before calling this, check that we are doing a file print that isn't already paused and get the movement lock.
-void GCodes::DoPause(GCodeBuffer& gb, bool isAuto)
+void GCodes::DoPause(GCodeBuffer& gb, PauseReason reason, const char *msg, ...)
 {
 	if (&gb == fileGCode)
 	{
@@ -1403,18 +1448,14 @@ void GCodes::DoPause(GCodeBuffer& gb, bool isAuto)
 	}
 	else
 	{
-		// Pausing a file print via another input source
+		// Pausing a file print via another input source or for some other reason
 		const bool movesSkipped = reprap.GetMove().PausePrint(pauseRestorePoint);		// tell Move we wish to pause the current print
 
 		if (movesSkipped)
 		{
 			// The PausePrint call has filled in the restore point with machine coordinates
 			ToolOffsetInverseTransform(pauseRestorePoint.moveCoords, currentUserPosition);	// transform the returned coordinates to user coordinates
-
-			if (segmentsLeft != 0)
-			{
-				ClearMove();
-			}
+			ClearMove();
 		}
 		else if (segmentsLeft != 0 && moveBuffer.canPauseBefore)
 		{
@@ -1442,14 +1483,12 @@ void GCodes::DoPause(GCodeBuffer& gb, bool isAuto)
 #endif
 		}
 
-		// Replace the pauses machine coordinates by user coordinates, which we updated earlier
+		// Replace the paused machine coordinates by user coordinates, which we updated earlier
 		for (size_t axis = 0; axis < numVisibleAxes; ++axis)
 		{
 			pauseRestorePoint.moveCoords[axis] = currentUserPosition[axis];
 		}
 
-		pauseRestorePoint.virtualExtruderPosition = virtualExtruderPosition;
-
 		// If we skipped any moves, reset the file pointer to the start of the first move we need to replay
 		// The following could be delayed until we resume the print
 		FileData& fdata = fileGCode->MachineState().fileState;
@@ -1470,15 +1509,20 @@ void GCodes::DoPause(GCodeBuffer& gb, bool isAuto)
 	SaveFanSpeeds();
 	if (simulationMode == 0)
 	{
-		SaveResumeInfo();																// create the resume file so that we can resume after power down
+		SaveResumeInfo(false);															// create the resume file so that we can resume after power down
 	}
 
 	gb.SetState(GCodeState::pausing1);
 	isPaused = true;
 
-#ifdef DUET_NG
-	isAutoPaused = isAuto;
-#endif
+	if (msg != nullptr)
+	{
+		va_list vargs;
+		va_start(vargs, msg);
+		scratchString.vprintf(msg, vargs);
+		va_end(vargs);
+		platform.SendAlert(GenericMessage, scratchString.Pointer(), "Printing paused", 1, 0.0, 0);
+	}
 }
 
 bool GCodes::IsPaused() const
@@ -1488,8 +1532,19 @@ bool GCodes::IsPaused() const
 
 bool GCodes::IsPausing() const
 {
-	const GCodeState topState = fileGCode->OriginalMachineState().state;
-	return topState == GCodeState::pausing1 || topState == GCodeState::pausing2;
+	GCodeState topState = fileGCode->OriginalMachineState().state;
+	if (topState == GCodeState::pausing1 || topState == GCodeState::pausing2)
+	{
+		return true;
+	}
+#if HAS_VOLTAGE_MONITOR
+	topState = autoPauseGCode->OriginalMachineState().state;
+	if (topState == GCodeState::powerFailPausing1)
+	{
+		return true;
+	}
+#endif
+	return false;
 }
 
 bool GCodes::IsResuming() const
@@ -1503,54 +1558,114 @@ bool GCodes::IsRunning() const
 	return !IsPaused() && !IsPausing() && !IsResuming();
 }
 
-#ifdef DUET_NG
+#if HAS_VOLTAGE_MONITOR
 
 // Try to pause the current SD card print, returning true if successful, false if needs to be called again
-bool GCodes::AutoPause()
+bool GCodes::LowVoltagePause()
 {
-	if (IsPausing() || IsResuming())
+	if (simulationMode != 0)
 	{
+		return true;								// ignore the low voltage indication
+	}
+
+	reprap.GetHeat().SuspendHeaters(true);			// turn heaters off to conserve power for the motors to execute the pause
+	if (IsResuming())
+	{
+		// This is an unlucky situation, because the resume macro is probably being run, which will probably lower the head back on to the print.
+		// It may well be that the power loss will prevent the resume macro being completed. If not, try again when the print has been resumed.
 		return false;
 	}
 
-	if (!isPaused && reprap.GetPrintMonitor().IsPrinting())
+	if (IsPausing())
 	{
-		if (!LockMovement(*autoPauseGCode))
-		{
-			return false;
-		}
-		reprap.GetHeat().SuspendHeaters(true);			// turn heaters off to conserve power for the motors to execute the pause
-		DoPause(*autoPauseGCode, true);
+		// We are in the process of pausing already, so the resume info has already been saved.
+		// With luck the retraction and lifting of the head in the pause.g file has been done already.
+		return true;
 	}
-	return true;
-}
 
-// Suspend the printer, only ever called after it has been auto paused
-bool GCodes::AutoShutdown()
-{
-	if (isPaused && isAutoPaused)
+	if (IsPaused())
+	{
+		// Resume info has already been saved, and resuming will be prevented while the power is low
+		return true;
+	}
+
+	if (reprap.GetPrintMonitor().IsPrinting())
 	{
-		if (!LockMovementAndWaitForStandstill(*autoPauseGCode))
+		if (!autoPauseGCode->IsIdle() || autoPauseGCode->OriginalMachineState().state != GCodeState::normal)
 		{
-			return false;
+			return false;							// we can't pause if the auto pause thread is busy already
 		}
-		CancelPrint(false, false);
-		platform.Message(WarningMessage, "Print cancelled due to low voltage\n");
-		return true;
+
+		// Save the resume info, stop movement immediately and run the low voltage pause script to lift the nozzle etc.
+		GrabMovement(*autoPauseGCode);
+
+		const bool movesSkipped = reprap.GetMove().LowPowerPause(pauseRestorePoint);
+		if (movesSkipped)
+		{
+			// The PausePrint call has filled in the restore point with machine coordinates
+			ToolOffsetInverseTransform(pauseRestorePoint.moveCoords, currentUserPosition);	// transform the returned coordinates to user coordinates
+			ClearMove();
+		}
+		else if (segmentsLeft != 0 && moveBuffer.filePos != noFilePosition)
+		{
+			// We were not able to skip any moves, however we can skip the remaining segments of this current move
+			ToolOffsetInverseTransform(moveBuffer.initialCoords, currentUserPosition);
+			pauseRestorePoint.feedRate = moveBuffer.feedRate;
+			pauseRestorePoint.virtualExtruderPosition = moveBuffer.virtualExtruderPosition;
+			pauseRestorePoint.filePos = moveBuffer.filePos;
+			pauseRestorePoint.proportionDone = (float)(totalSegments - segmentsLeft)/(float)totalSegments;
+#if SUPPORT_IOBITS
+			pauseRestorePoint.ioBits = moveBuffer.ioBits;
+#endif
+			ClearMove();
+		}
+		else
+		{
+			// We were not able to skip any moves, and if there is a move waiting then we can't skip that one either
+			pauseRestorePoint.feedRate = fileGCode->MachineState().feedrate;
+			pauseRestorePoint.virtualExtruderPosition = virtualExtruderPosition;
+
+			// TODO: when we use RTOS there is a possible race condition in the following,
+			// because we might try to pause when a waiting move has just been added but before the gcode buffer has been re-initialised ready for the next command
+			pauseRestorePoint.filePos = fileGCode->GetFilePosition(fileInput->BytesCached());
+			pauseRestorePoint.proportionDone = 0.0;
+#if SUPPORT_IOBITS
+			pauseRestorePoint.ioBits = moveBuffer.ioBits;
+#endif
+		}
+
+		// Replace the paused machine coordinates by user coordinates, which we updated earlier
+		for (size_t axis = 0; axis < numVisibleAxes; ++axis)
+		{
+			pauseRestorePoint.moveCoords[axis] = currentUserPosition[axis];
+		}
+
+		SaveFanSpeeds();
+
+		// Run the auto-pause script
+		if (powerFailScript != nullptr)
+		{
+			autoPauseGCode->Put(powerFailScript, strlen(powerFailScript) + 1);
+		}
+		autoPauseGCode->SetState(GCodeState::powerFailPausing1);
+		isPaused = true;
+		isPowerFailPaused = true;
+
+		// Don't do any more here, we want the auto pause thread to run as soon as possible
 	}
+
 	return true;
 }
 
-// Resume printing, normally only ever called after it has been auto paused
-bool GCodes::AutoResume()
+// Resume printing, normally only ever called after it has been paused because if low voltage.
+// If the pause was short enough, resume automatically.
+bool GCodes::LowVoltageResume()
 {
-	if (isPaused && isAutoPaused)
+	reprap.GetHeat().SuspendHeaters(false);			// turn heaters off to conserve power for the motors to execute the pause
+	if (isPaused && isPowerFailPaused)
 	{
-		autoPauseGCode->SetState(GCodeState::resuming1);
-		if (AllAxesAreHomed())
-		{
-			DoFileMacro(*autoPauseGCode, POWER_RESTORE_G, true);
-		}
+		// Run resurrect.g automatically
+		//TODO qq;
 		platform.Message(LoggedGenericMessage, "Print auto-resumed\n");
 	}
 	return true;
@@ -1564,7 +1679,7 @@ bool GCodes::AutoResumeAfterShutdown()
 }
 #endif
 
-void GCodes::SaveResumeInfo()
+void GCodes::SaveResumeInfo(bool wasPowerFailure)
 {
 	const char* const printingFilename = reprap.GetPrintMonitor().GetPrintingFilename();
 	if (printingFilename != nullptr)
@@ -1580,7 +1695,7 @@ void GCodes::SaveResumeInfo()
 			StringRef buf(bufferSpace, ARRAY_SIZE(bufferSpace));
 
 			// Write the header comment
-			buf.printf("; File \"%s\" resume print after auto-pause", printingFilename);
+			buf.printf("; File \"%s\" resume print after %s", printingFilename, (wasPowerFailure) ? "power failure" : "print paused");
 			if (reprap.GetPlatform().IsDateTimeSet())
 			{
 				time_t timeNow = reprap.GetPlatform().GetDateTime();
@@ -1606,8 +1721,8 @@ void GCodes::SaveResumeInfo()
 			}
 			if (ok)
 			{
-				buf.printf("M116\nM290 S%.3f\nM23 %s\nM26 S%" PRIu32 "\n", (double)currentBabyStepZOffset, printingFilename, pauseRestorePoint.filePos);
-				ok = f->Write(buf.Pointer());								// write baby stepping offset, filename and file position
+				buf.printf("M116\nM290 S%.3f\n", (double)currentBabyStepZOffset);
+				ok = f->Write(buf.Pointer());								// write baby stepping offset
 			}
 			if (ok && fileGCode->OriginalMachineState().volumetricExtrusion)
 			{
@@ -1628,9 +1743,14 @@ void GCodes::SaveResumeInfo()
 			}
 			if (ok)
 			{
+				buf.printf("M23 %s\nM26 S%" PRIu32 " P%.3f\n", printingFilename, pauseRestorePoint.filePos, (double)pauseRestorePoint.proportionDone);
+				ok = f->Write(buf.Pointer());								// write filename and file position
+			}
+			if (ok)
+			{
 				// Build the commands to restore the head position. These assume that we are working in mm.
 				// Start with a vertical move to 2mm above the final Z position
-				buf.printf("G0 F1200 Z%.3f\n", (double)(pauseRestorePoint.moveCoords[Z_AXIS] + 2.0));
+				buf.printf("G0 F6000 Z%.3f\n", (double)(pauseRestorePoint.moveCoords[Z_AXIS] + 2.0));
 
 				// Now set all the other axes
 				buf.cat("G0 F6000");
@@ -1643,7 +1763,7 @@ void GCodes::SaveResumeInfo()
 				}
 
 				// Now move down to the correct Z height
-				buf.catf("\nG0 F1200 Z%.3f\n", (double)pauseRestorePoint.moveCoords[Z_AXIS]);
+				buf.catf("\nG0 F6000 Z%.3f\n", (double)pauseRestorePoint.moveCoords[Z_AXIS]);
 
 				// Set the feed rate
 				buf.catf("G1 F%.1f", (double)(pauseRestorePoint.feedRate * MinutesToSeconds));
@@ -1999,12 +2119,12 @@ bool GCodes::DoStraightMove(GCodeBuffer& gb, StringRef& reply, bool isCoordinate
 	if (moveBuffer.moveType != 0)
 	{
 		// It's a raw motor move, so do it in a single segment and wait for it to complete
-		segmentsLeft = 1;
+		totalSegments = 1;
 		gb.SetState(GCodeState::waitingForSpecialMoveToComplete);
 	}
 	else if (axesMentioned == 0)
 	{
-		segmentsLeft = 1;														// extruder-only movement
+		totalSegments = 1;
 	}
 	else
 	{
@@ -2014,7 +2134,7 @@ bool GCodes::DoStraightMove(GCodeBuffer& gb, StringRef& reply, bool isCoordinate
 		{
 			ClearBit(effectiveAxesHomed, Z_AXIS);								// if doing a manual Z probe, don't limit the Z movement
 		}
-		if (limitAxes && reprap.GetMove().GetKinematics().LimitPosition(moveBuffer.coords, numVisibleAxes, effectiveAxesHomed))
+		if (limitAxes && reprap.GetMove().GetKinematics().LimitPosition(moveBuffer.coords, numVisibleAxes, effectiveAxesHomed, moveBuffer.isCoordinated))
 		{
 			ToolOffsetInverseTransform(moveBuffer.coords, currentUserPosition);	// make sure the limits are reflected in the user position
 		}
@@ -2025,7 +2145,7 @@ bool GCodes::DoStraightMove(GCodeBuffer& gb, StringRef& reply, bool isCoordinate
 		{
 			AxesBitmap axesMentionedExceptZ = axesMentioned;
 			ClearBit(axesMentionedExceptZ, Z_AXIS);
-			moveBuffer.usePressureAdvance = (axesMentionedExceptZ != 0);
+			moveBuffer.usePressureAdvance = moveBuffer.hasExtrusion && (axesMentionedExceptZ != 0);
 		}
 
 		// Apply segmentation if necessary. To speed up simulation on SCARA printers, we don't apply kinematics segmentation when simulating.
@@ -2037,19 +2157,20 @@ bool GCodes::DoStraightMove(GCodeBuffer& gb, StringRef& reply, bool isCoordinate
 			// We assume that the segments will be smaller than the mesh spacing.
 			const float xyLength = sqrtf(fsquare(currentUserPosition[X_AXIS] - initialX) + fsquare(currentUserPosition[Y_AXIS] - initialY));
 			const float moveTime = xyLength/moveBuffer.feedRate;			// this is a best-case time, often the move will take longer
-			segmentsLeft = (unsigned int)max<int>(1, min<int>(rintf(xyLength/kin.GetMinSegmentLength()), rintf(moveTime * kin.GetSegmentsPerSecond())));
+			totalSegments = (unsigned int)max<int>(1, min<int>(rintf(xyLength/kin.GetMinSegmentLength()), rintf(moveTime * kin.GetSegmentsPerSecond())));
 		}
 		else if (reprap.GetMove().IsUsingMesh())
 		{
 			const HeightMap& heightMap = reprap.GetMove().AccessHeightMap();
-			segmentsLeft = max<unsigned int>(1, heightMap.GetMinimumSegments(currentUserPosition[X_AXIS] - initialX, currentUserPosition[Y_AXIS] - initialY));
+			totalSegments = max<unsigned int>(1, heightMap.GetMinimumSegments(currentUserPosition[X_AXIS] - initialX, currentUserPosition[Y_AXIS] - initialY));
 		}
 		else
 		{
-			segmentsLeft = 1;
+			totalSegments = 1;
 		}
 	}
 
+	segmentsLeft = totalSegments;
 	return false;
 }
 
@@ -2102,7 +2223,7 @@ bool GCodes::DoArcMove(GCodeBuffer& gb, bool clockwise)
 	}
 
 	ToolOffsetTransform(currentUserPosition, moveBuffer.coords);			// set the final position
-	if (limitAxes && reprap.GetMove().GetKinematics().LimitPosition(moveBuffer.coords, numVisibleAxes, axesHomed))
+	if (limitAxes && reprap.GetMove().GetKinematics().LimitPosition(moveBuffer.coords, numVisibleAxes, axesHomed, true))
 	{
 		ToolOffsetInverseTransform(moveBuffer.coords, currentUserPosition);	// make sure the limits are reflected in the user position
 	}
@@ -2118,7 +2239,6 @@ bool GCodes::DoArcMove(GCodeBuffer& gb, bool clockwise)
 	moveBuffer.yAxes = reprap.GetCurrentYAxes();
 	moveBuffer.virtualExtruderPosition = virtualExtruderPosition;
 	moveBuffer.endStopsToCheck = 0;
-	moveBuffer.usePressureAdvance = true;
 	moveBuffer.isCoordinated = true;
 	moveBuffer.filePos = (&gb == fileGCode) ? gb.GetFilePosition(fileInput->BytesCached()) : noFilePosition;
 
@@ -2138,6 +2258,7 @@ bool GCodes::DoArcMove(GCodeBuffer& gb, bool clockwise)
 	}
 
 	LoadExtrusionAndFeedrateFromGCode(gb, moveBuffer.moveType);
+	moveBuffer.usePressureAdvance = moveBuffer.hasExtrusion;
 
 	arcRadius = sqrtf(iParam * iParam + jParam * jParam);
 	arcCurrentAngle = atan2(-jParam, -iParam);
@@ -2150,15 +2271,15 @@ bool GCodes::DoArcMove(GCodeBuffer& gb, bool clockwise)
 	}
 
 	// Compute how many segments we need to move, but don't store it yet
-	const unsigned int segsLeft = max<unsigned int>((unsigned int)((arcRadius * totalArc)/arcSegmentLength + 0.8), 1u);
-	arcAngleIncrement = totalArc/segsLeft;
+	totalSegments = max<unsigned int>((unsigned int)((arcRadius * totalArc)/arcSegmentLength + 0.8), 1u);
+	arcAngleIncrement = totalArc/totalSegments;
 	if (clockwise)
 	{
 		arcAngleIncrement = -arcAngleIncrement;
 	}
 	doingArcMove = true;
 
-	segmentsLeft = segsLeft;		// must do this last for RTOS
+	segmentsLeft = totalSegments;		// must do this last for RTOS
 //	debugPrintf("Radius %.2f, initial angle %.1f, increment %.1f, segments %u\n",
 //				arcRadius, arcCurrentAngle * RadiansToDegrees, arcAngleIncrement * RadiansToDegrees, segmentsLeft);
 	return false;
@@ -2178,12 +2299,14 @@ bool GCodes::ReadMove(RawMove& m)
 	{
 		// If there is just 1 segment left, it doesn't matter if it is an arc move or not, just move to the end position
 		ClearMove();
+		m.proportionRemaining = 0;
 	}
 	else
 	{
 		// This move needs to be divided into 2 or more segments
-		m.canPauseAfter = false;							// we can only pause after the final segment
-		moveBuffer.canPauseBefore = false;					// we can't pause before any of the remaining segments
+		m.canPauseAfter = false;							// we can only do a controlled pause pause after the final segment
+		moveBuffer.canPauseBefore = false;					// we can't do a controlled pause before any of the remaining segments
+		m.proportionRemaining = (uint8_t)min<unsigned int>((((totalSegments - segmentsLeft - 1) * 256) + (totalSegments/2))/totalSegments, 255);
 
 		// Do the axes
 		if (doingArcMove)
@@ -2223,6 +2346,11 @@ bool GCodes::ReadMove(RawMove& m)
 		}
 
 		--segmentsLeft;
+		if ((unsigned int)moveFractionToSkip + (unsigned int)m.proportionRemaining > 256)
+		{
+			// We are resuming a print part way through a move and we printed this segment already
+			return false;
+		}
 	}
 
 	return true;
@@ -2230,16 +2358,17 @@ bool GCodes::ReadMove(RawMove& m)
 
 void GCodes::ClearMove()
 {
+	segmentsLeft = 0;
 	doingArcMove = false;
 	moveBuffer.endStopsToCheck = 0;
 	moveBuffer.moveType = 0;
 	moveBuffer.isFirmwareRetraction = false;
-	segmentsLeft = 0;				// do this last
+	moveFractionToSkip = 0;
 }
 
 // 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.
-bool GCodes::DoFileMacro(GCodeBuffer& gb, const char* fileName, bool reportMissing, bool runningM502)
+bool GCodes::DoFileMacro(GCodeBuffer& gb, const char* fileName, bool reportMissing, int codeRunning)
 {
 	FileStore * const f = platform.GetFileStore(platform.GetSysDir(), fileName, OpenMode::read);
 	if (f == nullptr)
@@ -2259,7 +2388,8 @@ bool GCodes::DoFileMacro(GCodeBuffer& gb, const char* fileName, bool reportMissi
 	}
 	gb.MachineState().fileState.Set(f);
 	gb.MachineState().doingFileMacro = true;
-	gb.MachineState().runningM502 = runningM502;
+	gb.MachineState().runningM501 = (codeRunning == 501);
+	gb.MachineState().runningM502 = (codeRunning == 502);
 	gb.SetState(GCodeState::normal);
 	gb.Init();
 	return true;
@@ -2297,7 +2427,7 @@ GCodeResult GCodes::SetPositions(GCodeBuffer& gb)
 				}
 			}
 			SetBit(axesIncluded, axis);
-			currentUserPosition[axis] = axisValue;
+			currentUserPosition[axis] = axisValue * distanceScale;
 		}
 	}
 
@@ -2310,7 +2440,7 @@ GCodeResult GCodes::SetPositions(GCodeBuffer& gb)
 	if (axesIncluded != 0)
 	{
 		ToolOffsetTransform(currentUserPosition, moveBuffer.coords);
-		if (reprap.GetMove().GetKinematics().LimitPosition(moveBuffer.coords, numVisibleAxes, LowestNBits<AxesBitmap>(numVisibleAxes)))	// pretend that all axes are homed
+		if (reprap.GetMove().GetKinematics().LimitPosition(moveBuffer.coords, numVisibleAxes, LowestNBits<AxesBitmap>(numVisibleAxes), false))	// pretend that all axes are homed
 		{
 			ToolOffsetInverseTransform(moveBuffer.coords, currentUserPosition);		// make sure the limits are reflected in the user position
 		}
@@ -2635,13 +2765,13 @@ GCodeResult GCodes::ProbeGrid(GCodeBuffer& gb, StringRef& reply)
 {
 	if (!defaultGrid.IsValid())
 	{
-		reply.copy("No valid grid defined for G29 bed probing");
+		reply.copy("No valid grid defined for bed probing");
 		return GCodeResult::error;
 	}
 
 	if (!AllAxesAreHomed())
 	{
-		reply.copy("Must home printer before G29 bed probing");
+		reply.copy("Must home printer before bed probing");
 		return GCodeResult::error;
 	}
 
@@ -2750,11 +2880,12 @@ void GCodes::GetCurrentCoordinates(StringRef& s) const
 	s.Clear();
 	for (size_t axis = 0; axis < numVisibleAxes; ++axis)
 	{
-		s.catf("%c: %.3f ", axisLetters[axis], (double)liveCoordinates[axis]);
+		// Don't put a space after the colon in the response, it confuses Pronterface
+		s.catf("%c:%.3f ", axisLetters[axis], (double)liveCoordinates[axis]);
 	}
 	for (size_t i = numTotalAxes; i < DRIVES; i++)
 	{
-		s.catf("E%u: %.1f ", i - numTotalAxes, (double)liveCoordinates[i]);
+		s.catf("E%u:%.1f ", i - numTotalAxes, (double)liveCoordinates[i]);
 	}
 
 	// Print the axis stepper motor positions as Marlin does, as an aid to debugging.
@@ -2907,6 +3038,7 @@ bool GCodes::QueueFileToPrint(const char* fileName, StringRef& reply)
 
 		fileToPrint.Set(f);
 		fileOffsetToPrint = 0;
+		moveFractionToStartAt = 0;
 		return true;
 	}
 
@@ -3338,7 +3470,7 @@ void GCodes::HandleReply(GCodeBuffer& gb, bool error, const char* reply)
 
 	const Compatibility c = (&gb == serialGCode || &gb == telnetGCode) ? platform.Emulating() : me;
 	const MessageType type = gb.GetResponseMessageType();
-	const char* const response = (gb.GetCommandLetter() == 'M' && gb.GetIValue() == 998) ? "rs " : "ok";
+	const char* const response = (gb.GetCommandLetter() == 'M' && gb.GetCommandNumber() == 998) ? "rs " : "ok";
 	const char* emulationType = nullptr;
 
 	switch (c)
@@ -4123,10 +4255,17 @@ bool GCodes::WriteConfigOverrideFile(StringRef& reply, const char *fileName) con
 	{
 		ok = reprap.GetHeat().WriteModelParameters(f);
 	}
+
+	if (ok)
+	{
+		ok = platform.WritePlatformParameters(f);
+	}
+
 	if (ok)
 	{
-		ok = platform.WriteZProbeParameters(f);
+		ok = reprap.WriteToolParameters(f);
 	}
+
 	if (!f->Close())
 	{
 		ok = false;
@@ -4255,6 +4394,26 @@ bool GCodes::LockResource(const GCodeBuffer& gb, Resource r)
 	return false;
 }
 
+// Grab the movement lock even if another GCode source has it
+// CAUTION: this will be unsafe when we move to RTOS
+void GCodes::GrabResource(const GCodeBuffer& gb, Resource r)
+{
+	if (resourceOwners[r] != &gb)
+	{
+		if (resourceOwners[r] != nullptr)
+		{
+			GCodeMachineState *m = &(resourceOwners[r]->MachineState());
+			do
+			{
+				ClearBit(m->lockedResources, r);
+				m = m->previous;
+			}
+			while (m != nullptr);
+		}
+		resourceOwners[r] = &gb;
+	}
+}
+
 bool GCodes::LockHeater(const GCodeBuffer& gb, int heater)
 {
 	if (heater >= 0 && heater < (int)Heaters)
@@ -4285,6 +4444,11 @@ bool GCodes::LockMovement(const GCodeBuffer& gb)
 	return LockResource(gb, MoveResource);
 }
 
+void GCodes::GrabMovement(const GCodeBuffer& gb)
+{
+	GrabResource(gb, MoveResource);
+}
+
 // Release all locks, except those that were owned when the current macro was started
 void GCodes::UnlockAll(const GCodeBuffer& gb)
 {
@@ -4328,4 +4492,13 @@ const char* GCodes::GetMachineModeString() const
 	}
 }
 
+// Respond to a heater fault. The heater has already been turned off and its status set to 'fault' when this is called.
+void GCodes::HandleHeaterFault(int heater)
+{
+	if (reprap.GetPrintMonitor().IsPrinting() && !isPaused)
+	{
+		DoPause(*autoPauseGCode, PauseReason::heaterFault, "Fault on heater %d", heater);
+	}
+}
+
 // End
diff --git a/src/GCodes/GCodes.h b/src/GCodes/GCodes.h
index aa06db0a..bbe13e46 100644
--- a/src/GCodes/GCodes.h
+++ b/src/GCodes/GCodes.h
@@ -41,6 +41,7 @@ typedef AxesBitmap EndstopChecks;						// must be large enough to hold a bitmap
 const EndstopChecks ZProbeActive = 1 << 31;				// must be distinct from 1 << (any drive number)
 const EndstopChecks HomeAxes = 1 << 30;					// must be distinct from 1 << (any drive number)
 const EndstopChecks LogProbeChanges = 1 << 29;			// must be distinct from 1 << (any drive number)
+const EndstopChecks UseSpecialEndstop = 1 << 28;		// must be distinct from 1 << (any drive number)
 
 typedef uint32_t TriggerInputsBitmap;					// Bitmap of input pins that a single trigger number responds to
 typedef uint32_t TriggerNumbersBitmap;					// Bitmap of trigger numbers
@@ -64,10 +65,10 @@ struct Trigger
 };
 
 // Bits for T-code P-parameter to specify which macros are supposed to be run
-const int TFreeBit = 1 << 0;
-const int TPreBit = 1 << 1;
-const int TPostBit = 1 << 2;
-const int DefaultToolChangeParam = TFreeBit | TPreBit | TPostBit;
+constexpr uint8_t TFreeBit = 1 << 0;
+constexpr uint8_t TPreBit = 1 << 1;
+constexpr uint8_t TPostBit = 1 << 2;
+constexpr uint8_t DefaultToolChangeParam = TFreeBit | TPreBit | TPostBit;
 
 // Machine type enumeration. The numeric values must be in the same order as the corresponding M451..M453 commands.
 enum class MachineType : uint8_t
@@ -77,6 +78,21 @@ enum class MachineType : uint8_t
 	cnc = 2
 };
 
+enum class PauseReason
+{
+	user,			// M25 command received
+	gcode,			// M25 or M226 command encountered in the file being printed
+	trigger,		// external switch
+	heaterFault,	// heater fault detected
+	filament,		// filament monitor
+#if HAS_SMART_DRIVERS
+	stall,			// motor stall detected
+#endif
+#if HAS_VOLTAGE_MONITOR
+	lowVoltage		// VIN voltage dropped below configured minimum
+#endif
+};
+
 //****************************************************************************************************
 
 // The GCode interpreter
@@ -98,6 +114,8 @@ public:
 		IoBits_t ioBits;												// I/O bits to set/clear at the start of this move
 #endif
 		uint8_t moveType;												// the S parameter from the G0 or G1 command, 0 for a normal move
+		uint8_t proportionRemaining;									// what proportion of the entire move remains after this segment
+
 		uint8_t isFirmwareRetraction : 1;								// true if this is a firmware retraction/un-retraction move
 		uint8_t usePressureAdvance : 1;									// true if we want to us extruder pressure advance, if there is any extrusion
 		uint8_t canPauseBefore : 1;										// true if we can pause before this move
@@ -161,11 +179,11 @@ public:
 	size_t GetNumExtruders() const { return numExtruders; }
 
 	void FilamentError(size_t extruder, FilamentSensorStatus fstat);
+	void HandleHeaterFault(int heater);											// Respond to a heater fault
 
-#ifdef DUET_NG
-	bool AutoPause();
-	bool AutoShutdown();
-	bool AutoResume();
+#if HAS_VOLTAGE_MONITOR
+	bool LowVoltagePause();
+	bool LowVoltageResume();
 	bool AutoResumeAfterShutdown();
 #endif
 
@@ -193,11 +211,14 @@ private:
 	bool LockFileSystem(const GCodeBuffer& gb);							// Lock the unshareable parts of the file system
 	bool LockMovement(const GCodeBuffer& gb);							// Lock movement
 	bool LockMovementAndWaitForStandstill(const GCodeBuffer& gb);		// Lock movement and wait for pending moves to finish
+	void GrabResource(const GCodeBuffer& gb, Resource r);				// Grab a resource even if it is already owned
+	void GrabMovement(const GCodeBuffer& gb);							// Grab the movement lock even if it is already owned
 	void UnlockAll(const GCodeBuffer& gb);								// Release all locks
 
 	void StartNextGCode(GCodeBuffer& gb, StringRef& reply);				// Fetch a new or old GCode and process it
+	void RunStateMachine(GCodeBuffer& gb, StringRef& reply);			// Execute a step of the state machine
 	void DoFilePrint(GCodeBuffer& gb, StringRef& reply);				// Get G Codes from a file and print them
-	bool DoFileMacro(GCodeBuffer& gb, const char* fileName, bool reportMissing, bool runningM502 = false);
+	bool DoFileMacro(GCodeBuffer& gb, const char* fileName, bool reportMissing, int codeRunning = 0);
 																		// Run a GCode macro file, optionally report error if not found
 	void FileMacroCyclesReturn(GCodeBuffer& gb);						// End a macro
 
@@ -256,9 +277,10 @@ private:
 	bool ChangeMicrostepping(size_t drive, int microsteps, int mode) const;	// Change microstepping on the specified drive
 	void ListTriggers(StringRef reply, TriggerInputsBitmap mask);		// Append a list of trigger inputs to a message
 	void CheckTriggers();												// Check for and execute triggers
+	void CheckFilament();												// Check for and respond to filament errors
 	void DoEmergencyStop();												// Execute an emergency stop
 
-	void DoPause(GCodeBuffer& gb, bool isAuto)							// Pause the print
+	void DoPause(GCodeBuffer& gb, PauseReason reason, const char *msg, ...) __attribute__ ((format (printf, 4, 5)));	// Pause the print
 	pre(resourceOwners[movementResource] = &gb);
 
 	void SetMappedFanSpeed();											// Set the speeds of fans mapped for the current tool
@@ -282,7 +304,7 @@ private:
 	void EndSimulation(GCodeBuffer *gb);								// Restore positions etc. when exiting simulation mode
 	bool IsCodeQueueIdle() const;										// Return true if the code queue is idle
 
-	void SaveResumeInfo();
+	void SaveResumeInfo(bool wasPowerFailure);
 
 	const char* GetMachineModeString() const;							// Get the name of the current machine mode
 
@@ -294,12 +316,7 @@ private:
 	StreamGCodeInput* serialInput;										// ...
 	StreamGCodeInput* auxInput;											// ...for the GCodeBuffers below
 
-#ifdef DUET_NG
 	GCodeBuffer* gcodeSources[8];										// The various sources of gcodes
-	GCodeBuffer*& autoPauseGCode = gcodeSources[7];						// GCode state machine used to run pause.g and resume.g
-#else
-	GCodeBuffer* gcodeSources[7];										// The various sources of gcodes
-#endif
 
 	GCodeBuffer*& httpGCode = gcodeSources[0];
 	GCodeBuffer*& telnetGCode = gcodeSources[1];
@@ -308,6 +325,8 @@ private:
 	GCodeBuffer*& auxGCode = gcodeSources[4];							// This one is for the LCD display on the async serial interface
 	GCodeBuffer*& daemonGCode = gcodeSources[5];						// Used for executing config.g and trigger macro files
 	GCodeBuffer*& queuedGCode = gcodeSources[6];
+	GCodeBuffer*& autoPauseGCode = gcodeSources[7];						// ***THIS ONE MUST BE LAST*** GCode state machine used to run macros on power fail, heater faults and filament out
+
 	size_t nextGcodeSource;												// The one to check next
 
 	const GCodeBuffer* resourceOwners[NumResources];					// Which gcode buffer owns each resource
@@ -316,18 +335,24 @@ private:
 	bool active;								// Live and running?
 	bool isPaused;								// true if the print has been paused manually or automatically
 	bool pausePending;							// true if we have been asked to pause but we are running a macro
-#ifdef DUET_NG
-	bool isAutoPaused;							// true if the print was paused automatically
-#endif
 	bool runningConfigFile;						// We are running config.g during the startup process
 	bool doingToolChange;						// We are running tool change macros
 
+	uint8_t moveFractionToStartAt;				// how much of the next move was printed before the power failure
+	uint8_t moveFractionToSkip;
+
+	#if HAS_VOLTAGE_MONITOR
+	bool isPowerFailPaused;						// true if the print was paused automatically because of a power failure
+	char *powerFailScript;						// the commands run when there is a power failure
+#endif
+
 	float currentUserPosition[MaxAxes];			// The current position of the axes as commanded by the input gcode, before accounting for tool offset and Z hop
 	float currentZHop;							// The amount of Z hop that is currently applied
 
 	// The following contain the details of moves that the Move module fetches
 	RawMove moveBuffer;							// Move details to pass to Move class
 	unsigned int segmentsLeft;					// The number of segments left to do in the current move, or 0 if no move available
+	unsigned int totalSegments;					// The total number of segments left in the complete move
 	float arcCentre[MaxAxes];
 	float arcRadius;
 	float arcCurrentAngle;
@@ -355,9 +380,6 @@ private:
 	FileStore* fileBeingWritten;				// A file to write G Codes (or sometimes HTML) to
 	FilePosition fileSize;						// Size of the file being written
 
-	int oldToolNumber, newToolNumber;			// Tools being changed
-	int toolChangeParam;						// Bitmap of all the macros to be run during a tool change
-
 	const char* eofString;						// What's at the end of an HTML file?
 	uint8_t eofStringCounter;					// Check the...
 	uint8_t eofStringLength;					// ... EoF string as we read.
@@ -443,20 +465,14 @@ private:
 	static constexpr const char* CONFIG_OVERRIDE_G = "config-override.g";
 	static constexpr const char* DEPLOYPROBE_G = "deployprobe.g";
 	static constexpr const char* RETRACTPROBE_G = "retractprobe.g";
-	static constexpr const char* RESUME_PROLOGUE_G = "resurrect-prologue.g";
 	static constexpr const char* PAUSE_G = "pause.g";
 	static constexpr const char* HOME_ALL_G = "homeall.g";
 	static constexpr const char* HOME_DELTA_G = "homedelta.g";
 	static constexpr const char* DefaultHeightMapFile = "heightmap.csv";
 	static constexpr const char* LOAD_FILAMENT_G = "load.g";
 	static constexpr const char* UNLOAD_FILAMENT_G = "unload.g";
-
-#ifdef DUET_NG
-	static constexpr const char* POWER_FAIL_G = "powerfail.g";
-	static constexpr const char* POWER_RESTORE_G = "powerrestore.g";
-#endif
-
 	static constexpr const char* RESUME_AFTER_POWER_FAIL_G = "resurrect.g";
+	static constexpr const char* RESUME_PROLOGUE_G = "resurrect-prologue.g";
 
 	static constexpr const float MinServoPulseWidth = 544.0, MaxServoPulseWidth = 2400.0;
 	static const uint16_t ServoRefreshFrequency = 50;
diff --git a/src/GCodes/GCodes2.cpp b/src/GCodes/GCodes2.cpp
index f30cc1bb..8767b4a1 100644
--- a/src/GCodes/GCodes2.cpp
+++ b/src/GCodes/GCodes2.cpp
@@ -29,17 +29,13 @@
 # include "FirmwareUpdater.h"
 #endif
 
+#include <algorithm>			// for std::swap
+
 // If the code to act on is completed, this returns true,
 // otherwise false.  It is called repeatedly for a given
 // code until it returns true for that code.
 bool GCodes::ActOnCode(GCodeBuffer& gb, StringRef& reply)
 {
-	// Discard empty buffers right away
-	if (gb.IsEmpty())
-	{
-		return true;
-	}
-
 	// Can we queue this code?
 	if (gb.CanQueueCodes() && codeQueue->QueueCode(gb, segmentsLeft))
 	{
@@ -52,22 +48,35 @@ bool GCodes::ActOnCode(GCodeBuffer& gb, StringRef& reply)
 	switch (gb.GetCommandLetter())
 	{
 	case 'G':
-		return HandleGcode(gb, reply);
+		if (gb.HasCommandNumber())
+		{
+			return HandleGcode(gb, reply);
+		}
+		break;
+
 	case 'M':
-		return HandleMcode(gb, reply);
+		if (gb.HasCommandNumber())
+		{
+			return HandleMcode(gb, reply);
+		}
+		break;
+
 	case 'T':
 		return HandleTcode(gb, reply);
+
 	default:
-		// An invalid command gets discarded
-		HandleReply(gb, false, "");
-		return true;
+		break;
 	}
+
+	reply.printf("Bad command: %s", gb.Buffer());
+	HandleReply(gb, true, reply.Pointer());
+	return true;
 }
 
 bool GCodes::HandleGcode(GCodeBuffer& gb, StringRef& reply)
 {
 	GCodeResult result = GCodeResult::ok;
-	const int code = gb.GetIValue();
+	const int code = gb.GetCommandNumber();
 	if (simulationMode != 0 && code > 4 && code != 10 && code != 11 && code != 20 && code != 21 && code != 90 && code != 91 && code != 92)
 	{
 		return true;			// we only simulate some gcodes
@@ -108,7 +117,7 @@ bool GCodes::HandleGcode(GCodeBuffer& gb, StringRef& reply)
 		}
 		if (DoArcMove(gb, code == 2))
 		{
-			reply.copy("Invalid G2 or G3 command");
+			reply.copy("Invalid arc parameters");
 			result = GCodeResult::error;
 		}
 		break;
@@ -229,7 +238,7 @@ bool GCodes::HandleGcode(GCodeBuffer& gb, StringRef& reply)
 		break;
 
 	default:
-		reply.printf("invalid G Code: %s", gb.Buffer());
+		reply.copy("Unsupported command");
 		result = GCodeResult::error;
 	}
 
@@ -246,6 +255,11 @@ bool GCodes::HandleGcode(GCodeBuffer& gb, StringRef& reply)
 	if (gb.GetState() == GCodeState::normal)
 	{
 		UnlockAll(gb);
+		if (result == GCodeResult::error)
+		{
+			scratchString.printf("G%d: ", code);
+			reply.Prepend(scratchString.Pointer());
+		}
 		HandleReply(gb, result != GCodeResult::ok, reply.Pointer());
 	}
 	return true;
@@ -255,7 +269,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply)
 {
 	GCodeResult result = GCodeResult::ok;
 
-	const int code = gb.GetIValue();
+	const int code = gb.GetCommandNumber();
 	if (   simulationMode != 0
 		&& (code < 20 || code > 37)
 		&& code != 0 && code != 1 && code != 82 && code != 83 && code != 105 && code != 109 && code != 111 && code != 112 && code != 122
@@ -496,7 +510,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply)
 		}
 		{
 			const size_t card = (gb.Seen('P')) ? gb.GetIValue() : 0;
-			result = GetGCodeResultFromError(platform.GetMassStorage()->Mount(card, reply, true)) ;
+			result = platform.GetMassStorage()->Mount(card, reply, true);
 		}
 		break;
 
@@ -507,7 +521,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply)
 		}
 		{
 			const size_t card = (gb.Seen('P')) ? gb.GetIValue() : 0;
-			result = GetGCodeResultFromError(platform.GetMassStorage()->Unmount(card, reply));
+			result = platform.GetMassStorage()->Unmount(card, reply);
 		}
 		break;
 
@@ -556,35 +570,63 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply)
 		break;
 
 	case 24: // Print/resume-printing the selected file
-		if (!LockMovementAndWaitForStandstill(gb))
+		if (IsPausing() || IsResuming())
 		{
-			return false;
+			// ignore the resume request
 		}
-
-		if (isPaused)
+		else
 		{
-			gb.SetState(GCodeState::resuming1);
-			if (AllAxesAreHomed())
+			if (!LockMovementAndWaitForStandstill(gb))
 			{
-				DoFileMacro(gb, RESUME_G, true);
+				return false;
 			}
-		}
-		else if (!fileToPrint.IsLive())
-		{
-			reply.copy("Cannot print, because no file is selected!");
-			result = GCodeResult::error;
-		}
-		else
-		{
-			if (fileOffsetToPrint != 0)
+
+			if (IsPaused())
+			{
+#if HAS_VOLTAGE_MONITOR
+				if (!platform.IsPowerOk())
+				{
+					reply.copy("Cannot resume while power voltage is low");
+					result = GCodeResult::error;
+				}
+				else
+#endif
+				{
+					gb.SetState(GCodeState::resuming1);
+					if (AllAxesAreHomed())
+					{
+						DoFileMacro(gb, RESUME_G, true);
+					}
+				}
+			}
+			else if (!fileToPrint.IsLive())
 			{
-				// We executed M23 to set the file offset, which normally means that we are executing resurrect.g.
-				// We need to copy the absolute/relative and volumetric extrusion flags over
-				fileGCode->OriginalMachineState().drivesRelative = gb.MachineState().drivesRelative;
-				fileGCode->OriginalMachineState().volumetricExtrusion = gb.MachineState().volumetricExtrusion;
-				fileToPrint.Seek(fileOffsetToPrint);
+				reply.copy("Cannot print, because no file is selected!");
+				result = GCodeResult::error;
+			}
+			else
+			{
+#if HAS_VOLTAGE_MONITOR
+				if (!platform.IsPowerOk())
+				{
+					reply.copy("Cannot start a print while power voltage is low");
+					result = GCodeResult::error;
+				}
+				else
+#endif
+				{
+					if (fileOffsetToPrint != 0)
+					{
+						// We executed M23 to set the file offset, which normally means that we are executing resurrect.g.
+						// We need to copy the absolute/relative and volumetric extrusion flags over
+						fileGCode->OriginalMachineState().drivesRelative = gb.MachineState().drivesRelative;
+						fileGCode->OriginalMachineState().volumetricExtrusion = gb.MachineState().volumetricExtrusion;
+						fileToPrint.Seek(fileOffsetToPrint);
+						moveFractionToSkip = moveFractionToStartAt;
+					}
+					StartPrinting();
+				}
 			}
-			StartPrinting();
 		}
 		break;
 
@@ -601,7 +643,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply)
 				{
 					return false;
 				}
-				DoPause(gb, false);
+				DoPause(gb, PauseReason::gcode, nullptr);
 			}
 		}
 		break;
@@ -627,7 +669,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply)
 			{
 				return false;
 			}
-			DoPause(gb, false);
+			DoPause(gb, PauseReason::user, nullptr);
 		}
 		break;
 
@@ -635,8 +677,11 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply)
 		// This is used between executing M23 to set up the file to print, and M25 to print it
 		if (gb.Seen('S'))
 		{
-			// Ideally we would get an unsigned value here in case of the file offset being >2Gb
-			fileOffsetToPrint = (FilePosition)gb.GetIValue();
+			fileOffsetToPrint = (FilePosition)gb.GetUIValue();
+			if (gb.Seen('P'))
+			{
+				moveFractionToStartAt = (uint8_t)constrain<unsigned long>(lrintf(gb.GetFValue()), 0, 255);
+			}
 		}
 		break;
 
@@ -939,16 +984,17 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply)
 		FileMacroCyclesReturn(gb);
 		break;
 
-	case 101: // Un-retract
+	case 101: // Un-retract, generated by S3D if "Include M101/101/103" is enabled
 		result = RetractFilament(gb, false);
 		break;
 
 	case 102:
-		// S3D generates this command once at the start of he print job if firmware retraction is enabled.
+		// S3D generates this command just before each explicit retraction command if both explicit retraction and "Include M101/101/103" are enabled.
+		// Old versions of S3D also generate it once at the start of each prnit file if "Include M101/101/103" is enabled.
 		// It's not documented, so we just ignore it rather than generate an error message.
 		break;
 
-	case 103: // Retract
+	case 103: // Retract, generated by S3D if "Include M101/101/103" is enabled
 		result = RetractFilament(gb, true);
 		break;
 
@@ -1077,8 +1123,8 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply)
 					return false;
 				}
 
-				newToolNumber = tool->Number();
-				toolChangeParam = (simulationMode == 0) ? 0 : DefaultToolChangeParam;
+				gb.MachineState().newToolNumber = tool->Number();
+				gb.MachineState().toolChangeParam = (simulationMode == 0) ? 0 : DefaultToolChangeParam;
 				gb.SetState(GCodeState::m109ToolChange0);
 			}
 			else
@@ -1272,7 +1318,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply)
 				}
 				else if (bedHeater >= (int)Heaters)
 				{
-					reply.copy("Invalid heater number");
+					reply.printf("Invalid heater number '%d'", bedHeater);
 					result = GCodeResult::error;
 					break;
 				}
@@ -1331,7 +1377,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply)
 				}
 				else if (heater >= (int)Heaters)
 				{
-					reply.copy("Bad heater number specified!");
+					reply.printf("Bad heater number '%d'", heater);
 					result = GCodeResult::error;
 					break;
 				}
@@ -1362,7 +1408,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply)
 				}
 				else
 				{
-					reply.copy("No chamber heater has been set up yet!");
+					reply.copy("No chamber heater has been configured");
 					result = GCodeResult::error;
 				}
 			}
@@ -1376,7 +1422,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply)
 				}
 				else
 				{
-					reply.copy("No chamber heater has been configured yet.");
+					reply.copy("No chamber heater has been configured");
 				}
 			}
 		}
@@ -1387,7 +1433,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply)
 			const int heater = (gb.Seen('H')) ? gb.GetIValue() : 1;		// default to extruder 1 if no heater number provided
 			if (heater < 0 || heater >= (int)Heaters)
 			{
-				reply.copy("Invalid heater number");
+				reply.printf("Invalid heater number '%d'", heater);
 				result = GCodeResult::error;
 			}
 			else if (gb.Seen('S'))
@@ -1656,11 +1702,11 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply)
 					const float value = gb.GetFValue() * distanceScale;
 					if (setMin)
 					{
-						platform.SetAxisMinimum(axis, value);
+						platform.SetAxisMinimum(axis, value, gb.MachineState().runningM501);
 					}
 					else
 					{
-						platform.SetAxisMaximum(axis, value);
+						platform.SetAxisMaximum(axis, value, gb.MachineState().runningM501);
 					}
 					seen = true;
 				}
@@ -1709,7 +1755,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply)
 			}
 			else
 			{
-				reply.printf("Invalid speed factor specified.");
+				reply.printf("Invalid speed factor");
 				result = GCodeResult::error;
 			}
 		}
@@ -1921,7 +1967,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply)
 										: heater == reprap.GetHeat().GetBedHeater() ? 75.0
 										: heater == reprap.GetHeat().GetChamberHeater() ? 50.0
 										: 200.0;
-			const float maxPwm = (gb.Seen('P')) ? gb.GetFValue() : 1.0;
+			const float maxPwm = (gb.Seen('P')) ? gb.GetFValue() : reprap.GetHeat().GetHeaterModel(heater).GetMaxPwm();
 			if (heater < 0 || heater >= (int)Heaters)
 			{
 				reply.copy("Bad heater number in M303 command");
@@ -2198,7 +2244,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply)
 			}
 			else
 			{
-				reply.copy("Bad M452 P parameter");
+				reply.copy("Bad P parameter");
 				result = GCodeResult::error;
 			}
 		}
@@ -2233,7 +2279,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply)
 			}
 			else
 			{
-				reply.copy("Bad M453 P parameter");
+				reply.copy("Bad P parameter");
 				result = GCodeResult::error;
 			}
 		}
@@ -2252,14 +2298,14 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply)
 		break;
 
 	case 501: // Load parameters from EEPROM
-		DoFileMacro(gb, "config-override.g", true);
+		DoFileMacro(gb, "config-override.g", true, code);
 		break;
 
 	case 502: // Revert to default "factory settings"
 		reprap.GetHeat().ResetHeaterModels();							// in case some heaters have no M307 commands in config.g
 		reprap.GetMove().GetKinematics().SetCalibrationDefaults();		// in case M665/M666/M667/M669 in config.g don't define all the parameters
 		platform.SetZProbeDefaults();
-		DoFileMacro(gb, "config.g", true, true);
+		DoFileMacro(gb, "config.g", true, code);
 		break;
 
 	case 503: // List variable settings
@@ -2281,7 +2327,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply)
 			FileStore * const f = platform.GetFileStore(platform.GetSysDir(), platform.GetConfigFile(), OpenMode::read);
 			if (f == nullptr)
 			{
-				reply.copy("Configuration file not found!");
+				reply.copy("Configuration file not found");
 				result = GCodeResult::error;
 			}
 			else
@@ -2368,7 +2414,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply)
 				StringRef ssid(ssidBuffer, ARRAY_SIZE(ssidBuffer));
 				if (gb.Seen('P') && !gb.GetQuotedString(ssid))
 				{
-					reply.copy("Bad or missing SSID in M552 command");
+					reply.copy("Bad or missing SSID");
 					result = GCodeResult::error;
 				}
 				else
@@ -2635,7 +2681,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply)
 			}
 			else
 			{
-				reply.copy("Invalid heater number.\n");
+				reply.printf("Invalid heater number '%d'", heater);
 				result = GCodeResult::error;
 			}
 		}
@@ -2758,7 +2804,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply)
 					{
 						return false;
 					}
-					platform.SetEnableValue(drive, gb.GetIValue() != 0);
+					platform.SetEnableValue(drive, (int8_t)gb.GetIValue());
 					seen = true;
 				}
 				if (gb.Seen('T'))
@@ -3011,7 +3057,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply)
 					const size_t eDrive = eDrives[extruder];
 					if (eDrive >= MaxExtruders)
 					{
-						reply.copy("Invalid extruder drive specified!");
+						reply.printf("Invalid extruder drive '%u'", eDrive);
 						result = GCodeResult::error;
 						break;
 					}
@@ -3261,7 +3307,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply)
 
 			if (badDrive)
 			{
-				reply.copy("Invalid drive number in M584 command");
+				reply.copy("Invalid drive number");
 				result = GCodeResult::error;
 			}
 			else
@@ -3276,7 +3322,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply)
 					}
 					else
 					{
-						reply.copy("Invalid number of visible axes in M584 command");
+						reply.copy("Invalid number of visible axes");
 						result = GCodeResult::error;
 					}
 				}
@@ -3307,6 +3353,79 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply)
 		}
 		break;
 
+	case 585: // Probe Tool
+		if (reprap.GetCurrentTool() == nullptr)
+		{
+			reply.copy("No tool selected!");
+			result = GCodeResult::error;
+			break;
+		}
+
+		if (LockMovementAndWaitForStandstill(gb))
+		{
+			for (size_t axis = 0; axis < numTotalAxes; axis++)
+			{
+				if (gb.Seen(axisLetters[axis]))
+				{
+					// Get parameters first and check them
+					const int endStopToUse = gb.Seen('E') ? gb.GetIValue() : 0;
+					if (endStopToUse < 0 || endStopToUse > (int)DRIVES)
+					{
+						reply.copy("Invalid endstop number");
+						result = GCodeResult::error;
+						break;
+					}
+
+					// Save the current axis coordinates
+					memcpy(toolChangeRestorePoint.moveCoords, currentUserPosition, ARRAY_SIZE(currentUserPosition) * sizeof(currentUserPosition[0]));
+
+					// Prepare another move similar to G1 .. S3
+					moveBuffer.moveType = 3;
+					if (endStopToUse == 0)
+					{
+						moveBuffer.endStopsToCheck = 0;
+						SetBit(moveBuffer.endStopsToCheck, axis);
+					}
+					else
+					{
+						moveBuffer.endStopsToCheck = UseSpecialEndstop;
+						SetBit(moveBuffer.endStopsToCheck, endStopToUse);
+					}
+					moveBuffer.xAxes = DefaultXAxisMapping;
+					moveBuffer.yAxes = DefaultYAxisMapping;
+					moveBuffer.usePressureAdvance = false;
+					moveBuffer.filePos = noFilePosition;
+					moveBuffer.canPauseAfter = false;
+					moveBuffer.canPauseBefore = true;
+
+					// Decide which way and how far to go
+					const float axisLength = platform.AxisMaximum(axis) - platform.AxisMinimum(axis) + 5.0;
+					moveBuffer.coords[axis] = (gb.Seen('S') && gb.GetIValue() == 1) ? axisLength * -1.0 : axisLength;
+
+					// Zero every extruder drive
+					for (size_t drive = numTotalAxes; drive < DRIVES; drive++)
+					{
+						moveBuffer.coords[drive] = 0.0;
+					}
+					moveBuffer.hasExtrusion = false;
+
+					// Deal with feed rate
+					if (gb.Seen(feedrateLetter))
+					{
+						const float rate = gb.GetFValue() * distanceScale;
+						gb.MachineState().feedrate = rate * SecondsToMinutes;	// don't apply the speed factor to homing and other special moves
+					}
+					moveBuffer.feedRate = gb.MachineState().feedrate;
+
+					// Kick off new movement
+					segmentsLeft = 1;
+					gb.SetState(GCodeState::probingToolOffset);
+					break;
+				}
+			}
+		}
+		break;
+
 	case 586: // Configure network protocols
 		if (gb.Seen('P'))
 		{
@@ -3373,7 +3492,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply)
 			}
 			else
 			{
-				reply.copy("Bad parameter in M587 command");
+				reply.copy("Bad parameter");
 				result = GCodeResult::error;
 			}
 		}
@@ -3462,7 +3581,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply)
 			}
 			else
 			{
-				reply.copy("Bad parameter in M588 command");
+				reply.copy("Bad parameter");
 				result = GCodeResult::error;
 			}
 		}
@@ -3513,7 +3632,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply)
 			}
 			else
 			{
-				reply.copy("Bad or missing parameter in M589 command");
+				reply.copy("Bad or missing parameter");
 				result = GCodeResult::error;
 			}
 		}
@@ -3603,7 +3722,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply)
 			}
 			if (changed || changedMode)
 			{
-				if (reprap.GetMove().GetKinematics().LimitPosition(moveBuffer.coords, numVisibleAxes, axesHomed))
+				if (reprap.GetMove().GetKinematics().LimitPosition(moveBuffer.coords, numVisibleAxes, axesHomed, false))
 				{
 					ToolOffsetInverseTransform(moveBuffer.coords, currentUserPosition);	// make sure the limits are reflected in the user position
 				}
@@ -3661,7 +3780,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply)
 					break;
 
 				default:
-					reply.printf("Mode %d is not valid in M667 command\n", mode);
+					reply.printf("Mode %d is not valid", mode);
 					result = GCodeResult::error;
 					break;
 				}
@@ -3688,7 +3807,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply)
 						move.GetKinematics().GetAssumedInitialPosition(numVisibleAxes, moveBuffer.coords);
 						ToolOffsetInverseTransform(moveBuffer.coords, currentUserPosition);
 					}
-					if (reprap.GetMove().GetKinematics().LimitPosition(moveBuffer.coords, numVisibleAxes, axesHomed))
+					if (reprap.GetMove().GetKinematics().LimitPosition(moveBuffer.coords, numVisibleAxes, axesHomed, false))
 					{
 						ToolOffsetInverseTransform(moveBuffer.coords, currentUserPosition);	// make sure the limits are reflected in the user position
 					}
@@ -3714,7 +3833,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply)
 				const int nk = gb.GetIValue();
 				if (nk < 0 || nk >= (int)KinematicsType::unknown || !move.SetKinematics(static_cast<KinematicsType>(nk)))
 				{
-					reply.printf("Unknown kinematics type %d in M669 command", nk);
+					reply.printf("Unknown kinematics type %d", nk);
 					result = GCodeResult::error;
 					break;
 				}
@@ -3735,7 +3854,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply)
 					move.GetKinematics().GetAssumedInitialPosition(numVisibleAxes, moveBuffer.coords);
 					ToolOffsetInverseTransform(moveBuffer.coords, currentUserPosition);
 				}
-				if (reprap.GetMove().GetKinematics().LimitPosition(moveBuffer.coords, numVisibleAxes, axesHomed))
+				if (reprap.GetMove().GetKinematics().LimitPosition(moveBuffer.coords, numVisibleAxes, axesHomed, false))
 				{
 					ToolOffsetInverseTransform(moveBuffer.coords, currentUserPosition);	// make sure the limits are reflected in the user position
 				}
@@ -3795,7 +3914,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply)
 		}
 		else
 		{
-			reply.copy("Invalid source for this M-code");
+			reply.copy("Invalid source");
 			result = GCodeResult::error;
 		}
 		break;
@@ -3946,7 +4065,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply)
 				const char * const dateString = gb.GetString();
 				if (strptime(dateString, "%Y-%m-%d", &timeInfo) == nullptr)
 				{
-					reply.copy("M905: Invalid date format");
+					reply.copy("Invalid date format");
 					result = GCodeResult::error;
 					break;
 				}
@@ -3960,7 +4079,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply)
 				const char * const timeString = gb.GetString();
 				if (strptime(timeString, "%H:%M:%S", &timeInfo) == nullptr)
 				{
-					reply.copy("M905: Invalid time format");
+					reply.copy("Invalid time format");
 					result = GCodeResult::error;
 					break;
 				}
@@ -4052,12 +4171,57 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply)
 		}
 		break;
 
-#ifdef DUET_NG
+#if HAS_VOLTAGE_MONITOR
 	case 911: // Enable auto save
-		result = GetGCodeResultFromError(platform.ConfigureAutoSave(gb, reply));
+		if (gb.Seen('S'))
+		{
+			const float saveVoltage = gb.GetFValue();
+			if (saveVoltage < 10.0)
+			{
+				platform.DisableAutoSave();
+			}
+			else
+			{
+				float resumeVoltage = saveVoltage + 1.0;		// set up default resume voltage
+				bool dummy;
+				gb.TryGetFValue('R', resumeVoltage, dummy);
+
+				String<80> powerFailString;
+				bool seenCommandString = false;
+				gb.TryGetQuotedString('P', powerFailString.GetRef(), seenCommandString);
+				if (seenCommandString)
+				{
+					// Replace the power fail script atomically
+					char *newPowerFailScript = new char[powerFailString.strlen() + 1];
+					strcpy(newPowerFailScript, powerFailString.c_str());
+					std::swap(newPowerFailScript, powerFailScript);
+					delete[] newPowerFailScript;
+				}
+				else if (powerFailScript == nullptr)
+				{
+					reply.copy("No power fail script provided");
+					result = GCodeResult::error;
+					break;
+				}
+				platform.EnableAutoSave(saveVoltage, resumeVoltage);
+			}
+		}
+		else
+		{
+			float saveVoltage, resumeVoltage;
+			if (platform.GetAutoSaveSettings(saveVoltage, resumeVoltage))
+			{
+				reply.printf("Auto save voltage %.1fV, resume %.1fV, script \"%s\"", (double)saveVoltage, (double)resumeVoltage, (powerFailScript == nullptr) ? "" : powerFailScript);
+			}
+			else
+			{
+				reply.copy("Auto save is disabled");
+			}
+		}
 		break;
 #endif
 
+#if HAS_CPU_TEMP_SENSOR
 	case 912: // Set electronics temperature monitor adjustment
 		// Currently we ignore the P parameter (i.e. temperature measurement channel)
 		if (gb.Seen('S'))
@@ -4069,6 +4233,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply)
 			reply.printf("MCU temperature calibration adjustment is %.1f" DEGREE_SYMBOL "C", (double)platform.GetMcuTemperatureAdjust());
 		}
 		break;
+#endif
 
 	// For case 913, see 906
 
@@ -4098,6 +4263,31 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply)
 		break;
 #endif
 
+#if HAS_SMART_DRIVERS
+	case 915:
+		result = GetGCodeResultFromError(platform.ConfigureStallDetection(gb, reply));
+		break;
+#endif
+
+#if HAS_VOLTAGE_MONITOR
+	case 916:
+		if (!platform.GetMassStorage()->FileExists(platform.GetSysDir(), RESUME_AFTER_POWER_FAIL_G))
+		{
+			reply.copy("No resume file found");
+			result = GCodeResult::error;
+		}
+		else if (!platform.GetMassStorage()->FileExists(platform.GetSysDir(), RESUME_PROLOGUE_G))
+		{
+			reply.printf("Resume prologue file '%s' not found", RESUME_PROLOGUE_G);
+			result = GCodeResult::error;
+		}
+		else
+		{
+			DoFileMacro(gb, RESUME_AFTER_POWER_FAIL_G, true);
+		}
+		break;
+#endif
+
 	case 929: // Start/stop event logging
 		result = GetGCodeResultFromError(platform.ConfigureLogging(gb, reply));
 		break;
@@ -4191,7 +4381,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply)
 		break;
 
 	default:
-		reply.printf("Unsupported command: %s", gb.Buffer());
+		reply.copy("Unsupported command");
 		result = GCodeResult::error;
 		break;
 	}
@@ -4210,6 +4400,11 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply)
 	{
 		gb.timerRunning = false;
 		UnlockAll(gb);
+		if (result == GCodeResult::error)
+		{
+			scratchString.printf("M%d: ", code);
+			reply.Prepend(scratchString.Pointer());
+		}
 		HandleReply(gb, result != GCodeResult::ok, reply.Pointer());
 	}
 	return true;
@@ -4222,24 +4417,25 @@ bool GCodes::HandleTcode(GCodeBuffer& gb, StringRef& reply)
 		return true;			// when running M502 we don't execute T commands
 	}
 
-	if (!LockMovementAndWaitForStandstill(gb))
+	if (gb.HasCommandNumber())
 	{
-		return false;
-	}
+		if (!LockMovementAndWaitForStandstill(gb))
+		{
+			return false;
+		}
 
-	if (strlen(gb.Buffer()) > 1)
-	{
 		// See if the tool can be changed
-		newToolNumber = gb.GetIValue();
-		newToolNumber += gb.GetToolNumberAdjust();
+		int toolNum = gb.GetCommandNumber();
+		toolNum += gb.GetToolNumberAdjust();
 
 		const Tool * const oldTool = reprap.GetCurrentTool();
 		// If old and new are the same we no longer follow the sequence. User can deselect and then reselect the tool if he wants the macros run.
-		if (oldTool == nullptr || oldTool->Number() != newToolNumber)
+		if (oldTool == nullptr || oldTool->Number() != toolNum)
 		{
-			toolChangeParam = (simulationMode != 0) ? 0
-								: gb.Seen('P') ? gb.GetIValue()
-									: DefaultToolChangeParam;
+			gb.MachineState().newToolNumber = toolNum;
+			gb.MachineState().toolChangeParam = (simulationMode != 0) ? 0
+													: gb.Seen('P') ? gb.GetUIValue()
+														: DefaultToolChangeParam;
 			gb.SetState(GCodeState::toolChange0);
 			return true;							// proceeding with state machine, so don't unlock or send a reply
 		}
@@ -4250,11 +4446,11 @@ bool GCodes::HandleTcode(GCodeBuffer& gb, StringRef& reply)
 		const Tool * const tool = reprap.GetCurrentTool();
 		if (tool == nullptr)
 		{
-			reply.copy("No tool is selected.");
+			reply.copy("No tool is selected");
 		}
 		else
 		{
-			reply.printf("Tool %d is selected.", tool->Number());
+			reply.printf("Tool %d is selected", tool->Number());
 		}
 	}
 
diff --git a/src/GCodes/RestorePoint.cpp b/src/GCodes/RestorePoint.cpp
index 011fec02..977730de 100644
--- a/src/GCodes/RestorePoint.cpp
+++ b/src/GCodes/RestorePoint.cpp
@@ -20,6 +20,7 @@ void RestorePoint::Init()
 	}
 	feedRate = DefaultFeedrate * SecondsToMinutes;
 	virtualExtruderPosition = 0.0;
+	proportionDone = 0.0;
 	filePos = noFilePosition;
 #if SUPPORT_IOBITS
 	ioBits = 0;
diff --git a/src/GCodes/RestorePoint.h b/src/GCodes/RestorePoint.h
index fd9ca828..129d6100 100644
--- a/src/GCodes/RestorePoint.h
+++ b/src/GCodes/RestorePoint.h
@@ -16,13 +16,15 @@
 
 struct RestorePoint
 {
-	float moveCoords[MaxAxes];
-	float feedRate;
-	float virtualExtruderPosition;
-	FilePosition filePos;
+	float moveCoords[MaxAxes];				// The axis locations when we paused
+	float feedRate;							// The feed rate for the current move
+	float virtualExtruderPosition;			// The virtual extruder position at the start of this move
+	float proportionDone;					// How much of this move we have already done (zero unless we interrupts a move)
+	FilePosition filePos;					// The file position that this move was read from
 #if SUPPORT_IOBITS
-	IoBits_t ioBits;
+	IoBits_t ioBits;						// The output port bits setting for this move
 #endif
+
 	RestorePoint();
 	void Init();
 };
diff --git a/src/Heating/Heat.cpp b/src/Heating/Heat.cpp
index 841f79b4..b67a8abc 100644
--- a/src/Heating/Heat.cpp
+++ b/src/Heating/Heat.cpp
@@ -105,7 +105,7 @@ void Heat::Init()
 	virtualHeaterSensors[0] = TemperatureSensor::Create(CpuTemperatureSenseChannel);
 	virtualHeaterSensors[0]->SetHeaterName("MCU");				// name this virtual heater so that it appears in DWC
 #endif
-#ifdef DUET_NG
+#if HAS_SMART_DRIVERS
 	virtualHeaterSensors[1] = TemperatureSensor::Create(FirstTmcDriversSenseChannel);
 	virtualHeaterSensors[2] = TemperatureSensor::Create(FirstTmcDriversSenseChannel + 1);
 #endif
@@ -488,7 +488,7 @@ float Heat::GetTemperature(size_t heater, TemperatureError& err)
 	return t;
 }
 
-#ifdef DUET_NG
+#if HAS_VOLTAGE_MONITOR
 
 // Suspend the heaters to conserve power
 void Heat::SuspendHeaters(bool sus)
diff --git a/src/Heating/Heat.h b/src/Heating/Heat.h
index cfa81fc4..cdfc9c99 100644
--- a/src/Heating/Heat.h
+++ b/src/Heating/Heat.h
@@ -130,7 +130,7 @@ public:
 
 	bool WriteBedAndChamberTempSettings(FileStore *f) const;	// Save some resume information
 
-#ifdef DUET_NG
+#if HAS_VOLTAGE_MONITOR
 	void SuspendHeaters(bool sus);								// Suspend the heaters to conserve power
 #endif
 
diff --git a/src/Heating/Pid.cpp b/src/Heating/Pid.cpp
index 0fef6e59..236981bb 100644
--- a/src/Heating/Pid.cpp
+++ b/src/Heating/Pid.cpp
@@ -74,7 +74,7 @@ void PID::Reset()
 	averagePWM = lastPwm = 0.0;
 	heatingFaultCount = 0;
 	temperature = BAD_ERROR_TEMPERATURE;
-#ifdef DUET_NG
+#if HAS_VOLTAGE_MONITOR
 	suspended = false;
 #endif
 }
@@ -188,16 +188,16 @@ void PID::Spin()
 {
 	if (model.IsEnabled())
 	{
-#ifdef DUET_NG
+		// Read the temperature even if the heater is suspended
+		const TemperatureError err = ReadTemperature();
+
+#if HAS_VOLTAGE_MONITOR
 		if (suspended)
 		{
 			SetHeater(0.0);
 			return;
 		}
 #endif
-		// Read the temperature
-		const TemperatureError err = ReadTemperature();
-
 		// Handle any temperature reading error and calculate the temperature rate of change, if possible
 		if (err != TemperatureError::success)
 		{
@@ -267,7 +267,7 @@ void PID::Spin()
 							{
 								SetHeater(0.0);					// do this here just to be sure
 								mode = HeaterMode::fault;
-								reprap.GetGCodes().CancelPrint(false, false);
+								reprap.GetGCodes().HandleHeaterFault(heater);
 								platform.MessageF(ErrorMessage,
 											"Heating fault on heater %d, temperature rising much more slowly than the expected %.1f" DEGREE_SYMBOL "C/sec\n",
 											heater, (double)expectedRate);
@@ -294,7 +294,7 @@ void PID::Spin()
 					{
 						SetHeater(0.0);					// do this here just to be sure
 						mode = HeaterMode::fault;
-						reprap.GetGCodes().CancelPrint(false, false);
+						reprap.GetGCodes().HandleHeaterFault(heater);
 						platform.MessageF(ErrorMessage, "Heating fault on heater %d, temperature excursion exceeded %.1f" DEGREE_SYMBOL "C\n",
 											heater, (double)maxTempExcursion);
 					}
@@ -498,7 +498,7 @@ void PID::StartAutoTune(float targetTemp, float maxPwm, StringRef& reply)
 			tuningTempReadings = new float[MaxTuningTempReadings];
 			tuningTempReadings[0] = temperature;
 			tuningReadingInterval = platform.HeatSampleInterval();
-			tuningPwm = min<float>(maxPwm, model.GetMaxPwm());
+			tuningPwm = maxPwm;
 			tuningTargetTemp = targetTemp;
 			reply.printf("Auto tuning heater %d using target temperature %.1f" DEGREE_SYMBOL "C and PWM %.2f - do not leave printer unattended", heater, (double)targetTemp, (double)maxPwm);
 		}
@@ -857,7 +857,7 @@ void PID::DisplayBuffer(const char *intro)
 	}
 }
 
-#ifdef DUET_NG
+#if HAS_VOLTAGE_MONITOR
 
 // Suspend the heater to conserve power, or resume it
 void PID::Suspend(bool sus)
diff --git a/src/Heating/Pid.h b/src/Heating/Pid.h
index 8907063a..1ca9b5d3 100644
--- a/src/Heating/Pid.h
+++ b/src/Heating/Pid.h
@@ -16,8 +16,6 @@
 #include "FOPDT.h"
 #include "TemperatureError.h"
 
-#define NEW_TUNING	(1)
-
 class PID
 {
 	enum class HeaterMode : uint8_t
@@ -32,12 +30,8 @@ class PID
 		tuning0,
 		tuning1,
 		tuning2,
-#ifdef NEW_TUNING
 		tuning3,
 		lastTuningMode = tuning3
-#else
-		lastTuningMode = tuning2
-#endif
 	};
 
 	static const size_t NumPreviousTemperatures = 4; // How many samples we average the temperature derivative over
@@ -86,7 +80,7 @@ public:
 	void SetM301PidParameters(const M301PidParameters& params)
 		{ model.SetM301PidParameters(params); }
 
-#ifdef DUET_NG
+#if HAS_VOLTAGE_MONITOR
 	void Suspend(bool sus);							// Suspend the heater to conserve power
 #endif
 
@@ -98,14 +92,9 @@ private:
 	void DoTuningStep();							// Called on each temperature sample when auto tuning
 	static bool ReadingsStable(size_t numReadings, float maxDiff)
 		pre(numReadings >= 2; numReadings <= MaxTuningTempReadings);
-#ifdef NEW_TUNING
 	static int GetPeakTempIndex();					// Auto tune helper function
 	static int IdentifyPeak(size_t numToAverage);	// Auto tune helper function
 	void CalculateModel();							// Calculate G, td and tc from the accumulated readings
-#else
-	static size_t GetMaxRateIndex();				// Auto tune helper function
-	void FitCurve();								// Calculate G, td and tc from the accumulated readings
-#endif
 	void DisplayBuffer(const char *intro);			// Debug helper
 	float GetExpectedHeatingRate() const;			// Get the minimum heating rate we expect
 
@@ -132,7 +121,7 @@ private:
 	HeaterMode mode;								// Current state of the heater
 	bool active;									// Are we active or standby?
 	bool tuned;										// True if tuning was successful
-#ifdef DUET_NG
+#if HAS_VOLTAGE_MONITOR
 	bool suspended;									// True if suspended to save power
 #endif
 	uint8_t badTemperatureCount;					// Count of sequential dud readings
@@ -150,16 +139,10 @@ private:
 	static uint32_t tuningPhaseStartTime;			// when we started the current tuning phase
 	static uint32_t tuningReadingInterval;			// how often we are sampling, in milliseconds
 	static size_t tuningReadingsTaken;				// how many temperature samples we have taken
-
-#ifdef NEW_TUNING
 	static float tuningHeaterOffTemp;				// the temperature when we turned the heater off
 	static float tuningPeakTemperature;				// the peak temperature reached, averaged over 3 readings (so slightly less than the true peak)
 	static uint32_t tuningHeatingTime;				// how long we had the heating on for
 	static uint32_t tuningPeakDelay;				// how many milliseconds the temperature continues to rise after turning the heater off
-#else
-	static float tuningTimeOfFastestRate;			// how long after turn-on the fastest temperature rise occurred
-	static float tuningFastestRate;					// the fastest temperature rise
-#endif
 };
 
 
diff --git a/src/Heating/Sensors/TemperatureSensor.cpp b/src/Heating/Sensors/TemperatureSensor.cpp
index d6054d2f..852c9796 100644
--- a/src/Heating/Sensors/TemperatureSensor.cpp
+++ b/src/Heating/Sensors/TemperatureSensor.cpp
@@ -12,6 +12,9 @@
 
 #ifdef DUET_NG
 #include "DhtSensor.h"
+#endif
+
+#if HAS_SMART_DRIVERS
 #include "TmcDriverTemperatureSensor.h"
 #endif
 
@@ -115,7 +118,7 @@ TemperatureSensor *TemperatureSensor::Create(unsigned int channel)
 		ts = new CpuTemperatureSensor(channel);
 	}
 #endif
-#ifdef DUET_NG
+#if HAS_SMART_DRIVERS
 	else if (channel >= FirstTmcDriversSenseChannel && channel < FirstTmcDriversSenseChannel + 2)
 	{
 		ts = new TmcDriverTemperatureSensor(channel);
diff --git a/src/Heating/Sensors/Thermistor.h b/src/Heating/Sensors/Thermistor.h
index 1c002c34..74774f2b 100644
--- a/src/Heating/Sensors/Thermistor.h
+++ b/src/Heating/Sensors/Thermistor.h
@@ -41,7 +41,7 @@ private:
 	void SetHighOffset(int8_t p_offset) { adcHighOffset = p_offset; }
 
 	// For the theory behind ADC oversampling, see http://www.atmel.com/Images/doc8003.pdf
-	static const unsigned int AdcOversampleBits = 1	;						// we use 1-bit oversampling
+	static const unsigned int AdcOversampleBits = 2;						// we use 2-bit oversampling
 
 	void CalcDerivedParameters();											// calculate shA and shB
 
diff --git a/src/Heating/Sensors/TmcDriverTemperatureSensor.cpp b/src/Heating/Sensors/TmcDriverTemperatureSensor.cpp
index fe2ccbf9..ac1bd464 100644
--- a/src/Heating/Sensors/TmcDriverTemperatureSensor.cpp
+++ b/src/Heating/Sensors/TmcDriverTemperatureSensor.cpp
@@ -9,7 +9,7 @@
 #include "Platform.h"
 #include "RepRap.h"
 
-#ifdef DUET_NG
+#if HAS_SMART_DRIVERS
 
 TmcDriverTemperatureSensor::TmcDriverTemperatureSensor(unsigned int channel) : TemperatureSensor(channel, "TMC2660 temperature warnings")
 {
diff --git a/src/Heating/Sensors/TmcDriverTemperatureSensor.h b/src/Heating/Sensors/TmcDriverTemperatureSensor.h
index be18dd5c..099ec962 100644
--- a/src/Heating/Sensors/TmcDriverTemperatureSensor.h
+++ b/src/Heating/Sensors/TmcDriverTemperatureSensor.h
@@ -10,7 +10,7 @@
 
 #include "TemperatureSensor.h"
 
-#ifdef DUET_NG
+#if HAS_SMART_DRIVERS
 
 class TmcDriverTemperatureSensor : public TemperatureSensor
 {
diff --git a/src/Libraries/General/StringRef.cpp b/src/Libraries/General/StringRef.cpp
index a05e6070..7b0840c1 100644
--- a/src/Libraries/General/StringRef.cpp
+++ b/src/Libraries/General/StringRef.cpp
@@ -8,6 +8,7 @@
 #include "StringRef.h"
 #include <cstring>
 #include <cstdio>
+#include "WMath.h"
 
 // Need to define strnlen here because it isn't ISO standard
 size_t strnlen(const char *s, size_t n)
@@ -110,6 +111,16 @@ size_t StringRef::StripTrailingSpaces() const
 	return slen;
 }
 
+size_t StringRef::Prepend(const char *src) const
+{
+	const size_t slen = ::strlen(src);
+	const size_t dlen = strlen();
+	const size_t newLen = min<size_t>(dlen + slen, len - 1);
+	memmove(p + slen, p, newLen - slen + 1);
+	memcpy(p, src, slen);
+	return newLen;
+}
+
 // End
 
 
diff --git a/src/Libraries/General/StringRef.h b/src/Libraries/General/StringRef.h
index 9660389a..09ddaa24 100644
--- a/src/Libraries/General/StringRef.h
+++ b/src/Libraries/General/StringRef.h
@@ -33,7 +33,7 @@ public:
 
 	void Clear() const { p[0] = 0; }
 
-	int printf(const char *fmt, ...) const  __attribute__ ((format (printf, 2, 3)));
+	int printf(const char *fmt, ...) const __attribute__ ((format (printf, 2, 3)));
 	int vprintf(const char *fmt, va_list vargs) const;
 	int catf(const char *fmt, ...) const __attribute__ ((format (printf, 2, 3)));
 	int vcatf(const char *fmt, va_list vargs) const;
@@ -41,6 +41,7 @@ public:
 	size_t cat(const char *src) const;
 	size_t cat(char c) const;
 	size_t StripTrailingSpaces() const;
+	size_t Prepend(const char *src) const;
 
 	bool IsEmpty() const { return p[0] == 0; }
 };
diff --git a/src/Logger.cpp b/src/Logger.cpp
index e472f291..7d19237a 100644
--- a/src/Logger.cpp
+++ b/src/Logger.cpp
@@ -22,7 +22,7 @@ private:
 	bool& b;
 };
 
-Logger::Logger() : logFile(), dirty(false), inLogger(false)
+Logger::Logger() : logFile(), lastFlushTime(0), lastFlushFileSize(0), dirty(false), inLogger(false)
 {
 }
 
@@ -35,7 +35,8 @@ void Logger::Start(time_t time, const StringRef& filename)
 		if (f != nullptr)
 		{
 			logFile.Set(f);
-			logFile.Seek(logFile.Length());
+			lastFlushFileSize = logFile.Length();
+			logFile.Seek(lastFlushFileSize);
 			InternalLogMessage(time, "Event logging started\n");
 		}
 	}
@@ -109,13 +110,26 @@ void Logger::InternalLogMessage(time_t time, const char *message)
 	}
 }
 
+// This is called regularly by Platform to give the logger an opportunity to flush the file buffer
 void Logger::Flush()
 {
 	if (logFile.IsLive() && dirty && !inLogger)
 	{
-		Lock loggerLock(inLogger);
-		logFile.Flush();
-		dirty = false;
+		// Log file is dirty and can be flushed.
+		// To avoid excessive disk write operations, flush it only if one of the following is true:
+		// 1. We have possibly allocated a new cluster since the last flush. To avoid lost clusters if we power down before flushing,
+		//    we should flush early in this case. Rather than determine the cluster size, we flush if we have started a new 512-byte sector.
+		// 2. If it hasn't been flushed for LogFlushInterval milliseconds.
+		const FilePosition currentPos = logFile.GetPosition();
+		const uint32_t now = millis();
+		if (now - lastFlushTime >= LogFlushInterval || currentPos/512 != lastFlushFileSize/512)
+		{
+			Lock loggerLock(inLogger);
+			logFile.Flush();
+			lastFlushTime = millis();
+			lastFlushFileSize = currentPos;
+			dirty = false;
+		}
 	}
 }
 
diff --git a/src/Logger.h b/src/Logger.h
index 79b8df19..04397efe 100644
--- a/src/Logger.h
+++ b/src/Logger.h
@@ -30,6 +30,8 @@ private:
 	void InternalLogMessage(time_t time, const char *message);
 
 	FileData logFile;
+	uint32_t lastFlushTime;
+	FilePosition lastFlushFileSize;
 	bool dirty;
 	bool inLogger;
 };
diff --git a/src/Movement/DDA.cpp b/src/Movement/DDA.cpp
index a76a25c6..896e588c 100644
--- a/src/Movement/DDA.cpp
+++ b/src/Movement/DDA.cpp
@@ -26,17 +26,30 @@ struct MoveParameters
 	float accelDistance;
 	float steadyDistance;
 	float decelDistance;
+	float requestedSpeed;
 	float startSpeed;
 	float topSpeed;
 	float endSpeed;
+	float targetNextSpeed;
+	uint32_t endstopChecks;
+	uint16_t flags;
 
 	MoveParameters()
 	{
-		accelDistance = steadyDistance = decelDistance = startSpeed = topSpeed = endSpeed = 0.0;
+		accelDistance = steadyDistance = decelDistance = requestedSpeed = startSpeed = topSpeed = endSpeed = targetNextSpeed = 0.0;
+		endstopChecks = 0;
+		flags = 0;
+	}
+
+	void DebugPrint() const
+	{
+		reprap.GetPlatform().MessageF(DebugMessage, "%f,%f,%f,%f,%f,%f,%f,%f,%08" PRIX32 ",%04x\n",
+								(double)accelDistance, (double)steadyDistance, (double)decelDistance, (double)requestedSpeed, (double)startSpeed, (double)topSpeed, (double)endSpeed,
+								(double)targetNextSpeed, endstopChecks, flags);
 	}
 };
 
-const size_t NumSavedMoves = 256;
+const size_t NumSavedMoves = 128;
 
 static MoveParameters savedMoves[NumSavedMoves];
 static size_t savedMovePointer = 0;
@@ -47,8 +60,7 @@ static size_t savedMovePointer = 0;
 	// Print the saved moved in CSV format
 	for (size_t i = 0; i < NumSavedMoves; ++i)
 	{
-		const MoveParameters& m = savedMoves[savedMovePointer];
-		reprap.GetPlatform().MessageF(DebugMessage, "%f,%f,%f,%f,%f,%f\n", m.accelDistance, m.steadyDistance, m.decelDistance, m.startSpeed, m.topSpeed, m.endSpeed);
+		savedMoves[savedMovePointer].DebugPrint();
 		savedMovePointer = (savedMovePointer + 1) % NumSavedMoves;
 	}
 }
@@ -132,22 +144,21 @@ inline void DDA::InsertDM(DriveMovement *dm)
 	*dmp = dm;
 }
 
-// Remove this drive from the list of drives with steps due, and return its DM or nullptr if not there
+// Remove this drive from the list of drives with steps due
 // Called from the step ISR only.
-DriveMovement *DDA::RemoveDM(size_t drive)
+void DDA::RemoveDM(size_t drive)
 {
 	DriveMovement **dmp = &firstDM;
 	while (*dmp != nullptr)
 	{
-		DriveMovement *dm = *dmp;
+		DriveMovement * const dm = *dmp;
 		if (dm->drive == drive)
 		{
 			(*dmp) = dm->nextDM;
-			return dm;
+			break;
 		}
 		dmp = &(dm->nextDM);
 	}
-	return nullptr;
 }
 
 void DDA::DebugPrintVector(const char *name, const float *vec, size_t len) const
@@ -225,7 +236,7 @@ bool DDA::Init(GCodes::RawMove &nextMove, bool doMotorMapping)
 	const Move& move = reprap.GetMove();
 	if (doMotorMapping)
 	{
-		if (!move.CartesianToMotorSteps(nextMove.coords, endPoint, !nextMove.isCoordinated))		// transform the axis coordinates if on a delta or CoreXY printer
+		if (!move.CartesianToMotorSteps(nextMove.coords, endPoint, nextMove.isCoordinated))		// transform the axis coordinates if on a delta or CoreXY printer
 		{
 			return false;												// throw away the move if it couldn't be transformed
 		}
@@ -237,7 +248,6 @@ bool DDA::Init(GCodes::RawMove &nextMove, bool doMotorMapping)
 		isDeltaMovement = false;
 	}
 
-	isPrintingMove = false;
 	xyMoving = false;
 	bool extruding = false;												// we set this true if extrusion was commanded, even if it is too small to do
 	bool realMove = false;
@@ -338,6 +348,7 @@ bool DDA::Init(GCodes::RawMove &nextMove, bool doMotorMapping)
 	isPrintingMove = xyMoving && extruding;
 	usePressureAdvance = nextMove.usePressureAdvance;
 	virtualExtruderPosition = nextMove.virtualExtruderPosition;
+	proportionRemaining = nextMove.proportionRemaining;
 	hadLookaheadUnderrun = false;
 	isLeadscrewAdjustmentMove = false;
 	goingSlow = false;
@@ -417,41 +428,13 @@ bool DDA::Init(GCodes::RawMove &nextMove, bool doMotorMapping)
 	// for diagonal moves. On a delta, this is not OK and any movement in the XY plane should be limited to the X/Y axis values, which we assume to be equal.
 	if (doMotorMapping)
 	{
-		switch (reprap.GetMove().GetKinematics().GetKinematicsType())
-		{
-		case KinematicsType::cartesian:
-			// On a Cartesian printer the axes accelerate independently
-			break;
-
-		case KinematicsType::coreXY:
-		case KinematicsType::coreXYU:
-			// Preferably we would specialise these, but for now fall through to the default implementation
-		default:
-			// On other types of printer, the relationship between motor movement and head movement is complex.
-			// Limit all moves to the lower of X and Y speeds and accelerations.
-			{
-				const float xyFactor = sqrtf(fsquare(normalisedDirectionVector[X_AXIS]) + fsquare(normalisedDirectionVector[Y_AXIS]));
-				const float * const maxSpeeds = reprap.GetPlatform().MaxFeedrates();
-				const float maxSpeed = min<float>(maxSpeeds[X_AXIS], maxSpeeds[Y_AXIS]);
-				if (requestedSpeed * xyFactor > maxSpeed)
-				{
-					requestedSpeed = maxSpeed/xyFactor;
-				}
-
-				const float maxAcceleration = min<float>(normalAccelerations[X_AXIS], normalAccelerations[Y_AXIS]);
-				if (acceleration * xyFactor > maxAcceleration)
-				{
-					acceleration = maxAcceleration/xyFactor;
-				}
-			}
-			break;
-		}
+		reprap.GetMove().GetKinematics().LimitSpeedAndAcceleration(*this, normalisedDirectionVector);	// give the kinematics the chance to further restrict the speed and acceleration
 	}
 
 	// 7. Calculate the provisional accelerate and decelerate distances and the top speed
 	endSpeed = 0.0;					// until the next move asks us to adjust it
 
-	if (prev->state != provisional || isPrintingMove != prev->isPrintingMove)
+	if (prev->state != provisional || isPrintingMove != prev->isPrintingMove || xyMoving != prev->xyMoving)
 	{
 		// There is no previous move that we can adjust, so this move must start at zero speed.
 		startSpeed = 0.0;
@@ -460,7 +443,7 @@ bool DDA::Init(GCodes::RawMove &nextMove, bool doMotorMapping)
 	{
 		// Try to meld this move to the previous move to avoid stop/start
 		// Assuming that this move ends with zero speed, calculate the maximum possible starting speed: u^2 = v^2 - 2as
-		prev->targetNextSpeed = min<float>(sqrtf(acceleration * totalDistance * 2.0), requestedSpeed);
+		prev->targetNextSpeed = sqrtf(acceleration * totalDistance * 2.0);
 		DoLookahead(prev);
 		startSpeed = prev->targetNextSpeed;
 	}
@@ -562,61 +545,73 @@ inline bool DDA::IsDecelerationMove() const
 				&& prev->decelDistance > 0.0);				// if the previous move has no deceleration phase then no point in adjus6ting it
 }
 
-// Try to increase the ending speed of this move to allow the next move to start at targetNextSpeed
+// Return true if there is no reason to delay preparing this move
+bool DDA::IsGoodToPrepare() const
+{
+	return endSpeed >= topSpeed;							// if it never decelerates, we can't improve it
+}
+
+// Try to increase the ending speed of this move to allow the next move to start at targetNextSpeed.
+// Only called if this move ands the next one are both printing moves.
 /*static*/ void DDA::DoLookahead(DDA *laDDA)
 pre(state == provisional)
 {
 //	if (reprap.Debug(moduleDda)) debugPrintf("Adjusting, %f\n", laDDA->targetNextSpeed);
 	unsigned int laDepth = 0;
-	bool goingUp = true;
+	bool recurse = true;
 
 	for(;;)					// this loop is used to nest lookahead without making recursive calls
 	{
-		bool recurse = false;
-		if (goingUp)
+		if (recurse)
 		{
 			// We have been asked to adjust the end speed of this move to match the next move starting at targetNextSpeed
+			if (laDDA->targetNextSpeed > laDDA->requestedSpeed)
+			{
+				laDDA->targetNextSpeed = laDDA->requestedSpeed;
+			}
 			if (laDDA->topSpeed >= laDDA->requestedSpeed)
 			{
 				// This move already reaches its top speed, so just need to adjust the deceleration part
-				laDDA->endSpeed = laDDA->requestedSpeed;		// remove the deceleration phase
-				laDDA->CalcNewSpeeds();							// put it back if necessary
+				laDDA->endSpeed = laDDA->targetNextSpeed;						// ideally, maintain constant speed between the two moves
+				laDDA->CalcNewSpeeds();											// adjust it if necessary
+				recurse = false;
 			}
 			else if (laDDA->IsDecelerationMove())
 			{
 				// This is a deceleration-only move, so we may have to adjust the previous move as well to get optimum behaviour
-				if (laDDA->prev->state == provisional)
+				if (laDDA->prev->state == provisional && laDDA->prev->isPrintingMove == laDDA->isPrintingMove && laDDA->prev->xyMoving == laDDA->xyMoving)
 				{
-					laDDA->endSpeed = laDDA->requestedSpeed;
+					laDDA->endSpeed = laDDA->targetNextSpeed;
 					laDDA->CalcNewSpeeds();
 					const float maxStartSpeed = sqrtf(fsquare(laDDA->endSpeed) + (2 * laDDA->acceleration * laDDA->totalDistance));
-					if (maxStartSpeed < laDDA->requestedSpeed)
-					{
-						laDDA->prev->targetNextSpeed = maxStartSpeed;
-						// Still provisionally a decelerate-only move
-					}
-					else
-					{
-						laDDA->prev->targetNextSpeed = laDDA->requestedSpeed;
-					}
-					recurse = true;
+					laDDA->prev->targetNextSpeed = min<float>(maxStartSpeed, laDDA->requestedSpeed);
+					// leave 'recurse' true
 				}
 				else
 				{
 					// This move is a deceleration-only move but we can't adjust the previous one
 					laDDA->hadLookaheadUnderrun = true;
-					laDDA->endSpeed = min<float>(sqrtf(fsquare(laDDA->startSpeed) + (2 * laDDA->acceleration * laDDA->totalDistance)),
-													laDDA->requestedSpeed);
+					laDDA->endSpeed = min<float>(sqrtf(fsquare(laDDA->startSpeed) + (2 * laDDA->acceleration * laDDA->totalDistance)), laDDA->requestedSpeed);
 					laDDA->CalcNewSpeeds();
+					recurse = false;
 				}
 			}
 			else
 			{
 				// This move doesn't reach its requested speed, but it isn't a deceleration-only move
 				// Set its end speed to the minimum of the requested speed and the highest we can reach
-				laDDA->endSpeed = min<float>(sqrtf(fsquare(laDDA->startSpeed) + (2 * laDDA->acceleration * laDDA->totalDistance)),
-												laDDA->requestedSpeed);
+				const float maxReachableSpeed = sqrtf(fsquare(laDDA->startSpeed) + (2 * laDDA->acceleration * laDDA->totalDistance));
+				if (maxReachableSpeed >= laDDA->targetNextSpeed)
+				{
+					laDDA->endSpeed = laDDA->targetNextSpeed;
+				}
+				else
+				{
+					// Looks like this is an acceleration segment, so to ensure smooth acceleration we should reduce targetNextSpeed to endSpeed as well
+					laDDA->targetNextSpeed = laDDA->endSpeed = maxReachableSpeed;
+				}
 				laDDA->CalcNewSpeeds();
+				recurse = false;
 			}
 		}
 		else
@@ -646,7 +641,7 @@ pre(state == provisional)
 		}
 		else
 		{
-			// Either just stopped going up. or going down
+			// Either just stopped going up, or going down
 			laDDA->RecalculateMove();
 
 			if (laDepth == 0)
@@ -657,7 +652,6 @@ pre(state == provisional)
 
 			laDDA = laDDA->next;
 			--laDepth;
-			goingUp = false;
 		}
 	}
 }
@@ -758,18 +752,15 @@ float DDA::AdvanceBabyStepping(float amount)
 					pdm->state = DMState::moving;
 				}
 
-				if (pdm != nullptr)
+				if (steps >= 0)
 				{
-					if (steps >= 0)
-					{
-						pdm->direction = true;
-						pdm->totalSteps = (uint32_t)steps;
-					}
-					else
-					{
-						pdm->direction = false;
-						pdm->totalSteps = (uint32_t)(-steps);
-					}
+					pdm->direction = true;
+					pdm->totalSteps = (uint32_t)steps;
+				}
+				else
+				{
+					pdm->direction = false;
+					pdm->totalSteps = (uint32_t)(-steps);
 				}
 			}
 		}
@@ -785,19 +776,19 @@ float DDA::AdvanceBabyStepping(float amount)
 // This may cause a move that we intended to be a deceleration-only move to have a tiny acceleration segment at the start
 void DDA::RecalculateMove()
 {
-	accelDistance = (fsquare(requestedSpeed) - fsquare(startSpeed))/(2.0 * acceleration);
-	decelDistance = (fsquare(requestedSpeed) - fsquare(endSpeed))/(2.0 * acceleration);
-	if (accelDistance + decelDistance >= totalDistance)
+	const float accelDiff = fsquare(requestedSpeed) - fsquare(startSpeed);
+	const float decelDiff = fsquare(requestedSpeed) - fsquare(endSpeed);
+	if (accelDiff + decelDiff >= acceleration * totalDistance * 2)
 	{
-		// This move has no steady-speed phase, so it's accelerate-decelerate or accelerate-only move.
+		// This move has no steady-speed phase, so it's accelerate-decelerate or accelerate-only or decelerate-only move.
 		// If V is the peak speed, then (V^2 - u^2)/2a + (V^2 - v^2)/2a = distance.
 		// So (2V^2 - u^2 - v^2)/2a = distance
 		// So V^2 = a * distance + 0.5(u^2 + v^2)
-		float vsquared = (acceleration * totalDistance) + 0.5 * (fsquare(startSpeed) + fsquare(endSpeed));
+		const float vsquared = (acceleration * totalDistance) + 0.5 * (fsquare(startSpeed) + fsquare(endSpeed));
 		// Calculate accelerate distance from: V^2 = u^2 + 2as
 		if (vsquared >= 0.0)
 		{
-			accelDistance = max<float>((vsquared - fsquare(startSpeed))/(2.0 * acceleration), 0.0);
+			accelDistance = max<float>((vsquared - fsquare(startSpeed))/(2 * acceleration), 0.0);
 			decelDistance = totalDistance - accelDistance;
 			topSpeed = sqrtf(vsquared);
 		}
@@ -811,20 +802,22 @@ void DDA::RecalculateMove()
 				accelDistance = totalDistance;
 				decelDistance = 0.0;
 				topSpeed = endSpeed;
-				acceleration = (fsquare(endSpeed) - fsquare(startSpeed))/(2.0 * totalDistance);
+				acceleration = (fsquare(endSpeed) - fsquare(startSpeed))/(2 * totalDistance);
 			}
 			else
 			{
 				accelDistance = 0.0;
 				decelDistance = totalDistance;
 				topSpeed = startSpeed;
-				acceleration = (fsquare(startSpeed) - fsquare(endSpeed))/(2.0 * totalDistance);
+				acceleration = (fsquare(startSpeed) - fsquare(endSpeed))/(2 * totalDistance);
 			}
 		}
 	}
 	else
 	{
 		topSpeed = requestedSpeed;
+		accelDistance = accelDiff/(2 * acceleration);
+		decelDistance = decelDiff/(2 * acceleration);
 	}
 
 	if (canPauseAfter && endSpeed != 0.0)
@@ -839,6 +832,12 @@ void DDA::RecalculateMove()
 			}
 		}
 	}
+
+	// We need to set the number of clocks needed here because we use it before the move has been frozen
+	const float accelStopTime = (topSpeed - startSpeed)/acceleration;
+	const float decelStartTime = accelStopTime + (totalDistance - accelDistance - decelDistance)/topSpeed;
+	const float totalTime = decelStartTime + (topSpeed - endSpeed)/acceleration;
+	clocksNeeded = (uint32_t)(totalTime * stepClockRate);
 }
 
 // Decide what speed we would really like this move to end at.
@@ -963,12 +962,6 @@ pre(disableDeltaMapping || drive < MaxAxes)
 	}
 }
 
-// Calculate the time needed for this move
-float DDA::CalcTime() const
-{
-	return (float)clocksNeeded/stepClockRate;
-}
-
 // Prepare this DDA for execution.
 // This must not be called with interrupts disabled, because it calls Platform::EnableDrive.
 void DDA::Prepare(uint8_t simMode)
@@ -976,98 +969,26 @@ void DDA::Prepare(uint8_t simMode)
 	PrepParams params;
 	params.decelStartDistance = totalDistance - decelDistance;
 
-	// Convert the accelerate/decelerate distances to times
-	const float accelStopTime = (topSpeed - startSpeed)/acceleration;
-	const float decelStartTime = accelStopTime + (params.decelStartDistance - accelDistance)/topSpeed;
-	const float totalTime = decelStartTime + (topSpeed - endSpeed)/acceleration;
-
-	clocksNeeded = (uint32_t)(totalTime * stepClockRate);
-
 	if (simMode == 0)
 	{
 		if (isDeltaMovement)
 		{
-			// This code used to be in DDA::Init but we moved it here so that we can implement babystepping in it,
-			// also it speeds up simulation because we no longer execute this code when simulating.
-			// However, this code assumes that the previous move in the DDA ring is the previously-executed move, because it fetches the X and Y end coordinates from that move.
+			// This code assumes that the previous move in the DDA ring is the previously-executed move, because it fetches the X and Y end coordinates from that move.
 			// Therefore the Move code must not store a new move in that entry until this one has been prepared! (It took me ages to track this down.)
-			// Ideally we would store the initial X any Y coordinates in the DDA, but we need to be economical with memory in the Duet 06/085 build.
-			a2plusb2 = fsquare(directionVector[X_AXIS]) + fsquare(directionVector[Y_AXIS]);
+			// Ideally we would store the initial X and Y coordinates in the DDA, but we need to be economical with memory in the Duet 06/085 build.
 			cKc = (int32_t)(directionVector[Z_AXIS] * DriveMovement::Kc);
-
-			const float initialX = prev->GetEndCoordinate(X_AXIS, false);
-			const float initialY = prev->GetEndCoordinate(Y_AXIS, false);
-			const LinearDeltaKinematics& dparams = (const LinearDeltaKinematics&)reprap.GetMove().GetKinematics();
-			const float diagonalSquared = dparams.GetDiagonalSquared();
-			const float a2b2D2 = a2plusb2 * diagonalSquared;
-
-			for (size_t drive = 0; drive < DELTA_AXES; ++drive)
-			{
-				const float A = initialX - dparams.GetTowerX(drive);
-				const float B = initialY - dparams.GetTowerY(drive);
-				const float stepsPerMm = reprap.GetPlatform().DriveStepsPerUnit(drive);
-				DriveMovement* const pdm = pddm[drive];
-				if (pdm != nullptr)
-				{
-					const float aAplusbB = A * directionVector[X_AXIS] + B * directionVector[Y_AXIS];
-					const float dSquaredMinusAsquaredMinusBsquared = diagonalSquared - fsquare(A) - fsquare(B);
-					const float h0MinusZ0 = sqrtf(dSquaredMinusAsquaredMinusBsquared);
-					pdm->mp.delta.hmz0sK = (int32_t)(h0MinusZ0 * stepsPerMm * DriveMovement::K2);
-					pdm->mp.delta.minusAaPlusBbTimesKs = -(int32_t)(aAplusbB * stepsPerMm * DriveMovement::K2);
-					pdm->mp.delta.dSquaredMinusAsquaredMinusBsquaredTimesKsquaredSsquared =
-							(int64_t)(dSquaredMinusAsquaredMinusBsquared * fsquare(stepsPerMm * DriveMovement::K2));
-
-					// Calculate the distance at which we need to reverse direction.
-					if (a2plusb2 <= 0.0)
-					{
-						// Pure Z movement. We can't use the main calculation because it divides by a2plusb2.
-						pdm->direction = (directionVector[Z_AXIS] >= 0.0);
-						pdm->reverseStartStep = pdm->totalSteps + 1;
-					}
-					else
-					{
-						// The distance to reversal is the solution to a quadratic equation. One root corresponds to the carriages being below the bed,
-						// the other root corresponds to the carriages being above the bed.
-						const float drev = ((directionVector[Z_AXIS] * sqrtf(a2b2D2 - fsquare(A * directionVector[Y_AXIS] - B * directionVector[X_AXIS])))
-											- aAplusbB)/a2plusb2;
-						if (drev > 0.0 && drev < totalDistance)		// if the reversal point is within range
-						{
-							// Calculate how many steps we need to move up before reversing
-							const float hrev = directionVector[Z_AXIS] * drev + sqrtf(dSquaredMinusAsquaredMinusBsquared - 2 * drev * aAplusbB - a2plusb2 * fsquare(drev));
-							const int32_t numStepsUp = (int32_t)((hrev - h0MinusZ0) * stepsPerMm);
-
-							// We may be almost at the peak height already, in which case we don't really have a reversal.
-							if (numStepsUp < 1 || (pdm->direction && (uint32_t)numStepsUp <= pdm->totalSteps))
-							{
-								pdm->reverseStartStep = pdm->totalSteps + 1;
-							}
-							else
-							{
-								pdm->reverseStartStep = (uint32_t)numStepsUp + 1;
-
-								// Correct the initial direction and the total number of steps
-								if (pdm->direction)
-								{
-									// Net movement is up, so we will go up a bit and then down by a lesser amount
-									pdm->totalSteps = (2 * numStepsUp) - pdm->totalSteps;
-								}
-								else
-								{
-									// Net movement is down, so we will go up first and then down by a greater amount
-									pdm->direction = true;
-									pdm->totalSteps = (2 * numStepsUp) + pdm->totalSteps;
-								}
-							}
-						}
-						else
-						{
-							pdm->reverseStartStep = pdm->totalSteps + 1;
-						}
-					}
-				}
-			}
+			params.a2plusb2 = fsquare(directionVector[X_AXIS]) + fsquare(directionVector[Y_AXIS]);
+			params.initialX = prev->GetEndCoordinate(X_AXIS, false);
+			params.initialY = prev->GetEndCoordinate(Y_AXIS, false);
+			params.dparams = static_cast<const LinearDeltaKinematics*>(&(reprap.GetMove().GetKinematics()));
+			params.diagonalSquared = params.dparams->GetDiagonalSquared();
+			params.a2b2D2 = params.a2plusb2 * params.diagonalSquared;
 		}
 
+		// Convert the accelerate/decelerate distances to times
+		const float accelStopTime = (topSpeed - startSpeed)/acceleration;
+		const float decelStartTime = accelStopTime + (params.decelStartDistance - accelDistance)/topSpeed;
+
 		params.startSpeedTimesCdivA = (uint32_t)((startSpeed * stepClockRate)/acceleration);
 		params.topSpeedTimesCdivA = (uint32_t)((topSpeed * stepClockRate)/acceleration);
 		params.decelStartClocks = (uint32_t)(decelStartTime * stepClockRate);
@@ -1138,7 +1059,7 @@ void DDA::Prepare(uint8_t simMode)
 				// Prepare for the first step
 				pdm->nextStep = 0;
 				pdm->nextStepTime = 0;
-				pdm->stepInterval = 999999;						// initialise to a large value so that we will calculate the time for just one step
+				pdm->stepInterval = 999999;							// initialise to a large value so that we will calculate the time for just one step
 				pdm->stepsTillRecalc = 0;							// so that we don't skip the calculation
 				const bool stepsToDo = (isDeltaMovement && drive < numAxes)
 										? pdm->CalcNextStepTimeDelta(*this, false)
@@ -1164,9 +1085,13 @@ void DDA::Prepare(uint8_t simMode)
 		m.accelDistance = accelDistance;
 		m.decelDistance = decelDistance;
 		m.steadyDistance = totalDistance - accelDistance - decelDistance;
+		m.requestedSpeed = requestedSpeed;
 		m.startSpeed = startSpeed;
 		m.topSpeed = topSpeed;
 		m.endSpeed = endSpeed;
+		m.targetNextSpeed = targetNextSpeed;
+		m.endstopChecks = endStopsToCheck;
+		m.flags = flags;
 		savedMovePointer = (savedMovePointer + 1) % NumSavedMoves;
 #endif
 	}
@@ -1285,7 +1210,7 @@ void DDA::CheckEndstops(Platform& platform)
 		case EndStopHit::lowHit:
 			MoveAborted();											// set the state to completed and recalculate the endpoints
 			reprap.GetGCodes().MoveStoppedByZProbe();
-			break;
+			return;
 
 		case EndStopHit::lowNear:
 			ReduceHomingSpeed();
@@ -1334,12 +1259,19 @@ void DDA::CheckEndstops(Platform& platform)
 			{
 			case EndStopHit::lowHit:
 			case EndStopHit::highHit:
+				if ((endStopsToCheck & UseSpecialEndstop) != 0)		// use only one (probably non-default) endstop while probing a tool offset
+				{
+					MoveAborted();
+					return;
+				}
+				else
 				{
 					ClearBit(endStopsToCheck, drive);					// clear this check so that we can check for more
 					const Kinematics& kin = reprap.GetMove().GetKinematics();
 					if (endStopsToCheck == 0 || kin.QueryTerminateHomingMove(drive))
 					{
 						MoveAborted();									// no more endstops to check, or this axis uses shared motors, so stop the entire move
+						return;
 					}
 					else
 					{
@@ -1354,8 +1286,7 @@ void DDA::CheckEndstops(Platform& platform)
 				break;
 
 			case EndStopHit::lowNear:
-				// Only reduce homing speed if there are no more axes to be homed.
-				// This allows us to home X and Y simultaneously.
+				// Only reduce homing speed if there are no more axes to be homed. This allows us to home X and Y simultaneously.
 				if (endStopsToCheck == MakeBitmap<AxesBitmap>(drive))
 				{
 					ReduceHomingSpeed();
@@ -1453,7 +1384,7 @@ pre(state == frozen)
 	return true;	// schedule another interrupt immediately
 }
 
-extern uint32_t maxReps;
+uint32_t DDA::maxReps = 0;		// this holds he maximum ISR loop count
 
 // This is called by the interrupt service routine to execute steps.
 // It returns true if it needs to be called again on the DDA of the new current move, otherwise false.
@@ -1564,16 +1495,17 @@ bool DDA::Step()
 	return false;
 }
 
-// Stop a drive and re-calculate the corresponding endpoint
+// Stop a drive and re-calculate the corresponding endpoint.
+// For extruder drivers, we need to be able to calculate how much of the extrusion was completed after calling this.
 void DDA::StopDrive(size_t drive)
 {
 	DriveMovement* const pdm = pddm[drive];
-	if (pdm->state == DMState::moving)
+	if (pdm != nullptr && pdm->state == DMState::moving)
 	{
-		endPoint[drive] -= pdm->GetNetStepsLeft();
 		pdm->state = DMState::idle;
 		if (drive < reprap.GetGCodes().GetTotalAxes())
 		{
+			endPoint[drive] -= pdm->GetNetStepsLeft();
 			endCoordinatesValid = false;			// the XYZ position is no longer valid
 		}
 		RemoveDM(drive);
@@ -1585,7 +1517,8 @@ void DDA::StopDrive(size_t drive)
 }
 
 // This is called when we abort a move because we have hit an endstop.
-// It adjusts the end points of the current move to account for how far through the move we got.
+// It stop all drives and adjusts the end points of the current move to account for how far through the move we got.
+// The caller must call MoveCompleted at some point after calling this.
 void DDA::MoveAborted()
 {
 	if (state == executing)
@@ -1598,6 +1531,40 @@ void DDA::MoveAborted()
 	state = completed;
 }
 
+// Return the approximate (to within 1%) proportion of extrusion for the complete multi-segment move that remains to be done.
+// The move was either not started or was aborted.
+float DDA::GetProportionDone() const
+{
+	// Get the proportion of extrusion already done at the start of this segment
+	unsigned int proportionDone = (filePos != noFilePosition && filePos == prev->filePos)
+									? 256 - prev->proportionRemaining
+										: 0;
+	if (state == completed)
+	{
+		// The move was aborted, so subtract how much was done
+		const unsigned int proportionDoneAtEnd = 256 - proportionRemaining;
+		if (proportionDoneAtEnd > proportionDone)
+		{
+			int32_t taken = 0, left = 0;
+			for (size_t drive = reprap.GetGCodes().GetTotalAxes(); drive < DRIVES; ++drive)
+			{
+				const DriveMovement* const pdm = pddm[drive];
+				if (pdm != nullptr)								// if this extruder is active
+				{
+					taken += pdm->GetNetStepsTaken();
+					left += pdm->GetNetStepsLeft();
+				}
+			}
+			const int32_t total = taken + left;
+			if (total > 0)								// if the move has net extrusion
+			{
+				proportionDone += (((proportionDoneAtEnd - proportionDone) * taken) + (total/2)) / total;
+			}
+		}
+	}
+	return (float)proportionDone/256.0;
+}
+
 // Reduce the speed of this move to the indicated speed.
 // This is called from the ISR, so interrupts are disabled and nothing else can mess with us.
 // As this is only called for homing moves and with very low speeds, we assume that we don't need acceleration or deceleration phases.
@@ -1611,7 +1578,7 @@ void DDA::ReduceHomingSpeed()
 		for (size_t drive = 0; drive < DRIVES; ++drive)
 		{
 			DriveMovement* const pdm = pddm[drive];
-			if (pdm->state == DMState::moving)
+			if (pdm != nullptr && pdm->state == DMState::moving)
 			{
 				pdm->ReduceSpeed(*this, factor);
 				RemoveDM(pdm->drive);
@@ -1657,4 +1624,16 @@ int32_t DDA::GetStepsTaken(size_t drive) const
 	return (dmp != nullptr) ? dmp->GetNetStepsTaken() : 0;
 }
 
+void DDA::LimitSpeedAndAcceleration(float maxSpeed, float maxAcceleration)
+{
+	if (requestedSpeed > maxSpeed)
+	{
+		requestedSpeed = maxSpeed;
+	}
+	if (acceleration > maxAcceleration)
+	{
+		acceleration = maxAcceleration;
+	}
+}
+
 // End
diff --git a/src/Movement/DDA.h b/src/Movement/DDA.h
index ebb560dc..83a8fc9c 100644
--- a/src/Movement/DDA.h
+++ b/src/Movement/DDA.h
@@ -41,14 +41,13 @@ public:
 	bool Init(GCodes::RawMove &nextMove, bool doMotorMapping) __attribute__ ((hot));	// Set up a new move, returning true if it represents real movement
 	bool Init(const float_t steps[DRIVES]);							// Set up a raw (unmapped) motor move
 	void Init();													// Set up initial positions for machine startup
-	bool Start(uint32_t tim);										// Start executing the DDA, i.e. move the move.
+	bool Start(uint32_t tim) __attribute__ ((hot));					// Start executing the DDA, i.e. move the move.
 	bool Step() __attribute__ ((hot));								// Take one step of the DDA, called by timed interrupt.
 	void SetNext(DDA *n) { next = n; }
 	void SetPrevious(DDA *p) { prev = p; }
 	void Complete() { state = completed; }
 	bool Free();
 	void Prepare(uint8_t simMode) __attribute__ ((hot));			// Calculate all the values and freeze this DDA
-	float CalcTime() const;											// Calculate the time needed for this move (used for simulation)
 	bool HasStepError() const;
 	bool CanPauseAfter() const { return canPauseAfter; }
 	bool CanPauseBefore() const { return canPauseBefore; }
@@ -71,8 +70,15 @@ public:
 	bool IsHomingAxes() const { return (endStopsToCheck & HomeAxes) != 0; }
 	uint32_t GetXAxes() const { return xAxes; }
 	uint32_t GetYAxes() const { return yAxes; }
+	float GetTotalDistance() const { return totalDistance; }
+	void LimitSpeedAndAcceleration(float maxSpeed, float maxAcceleration);	// Limit the speed an acceleration of this move
 
 	int32_t GetStepsTaken(size_t drive) const;
+	float GetProportionDone() const;								// Return the proportion of extrusion for the complete multi-segment move already done
+
+	void MoveAborted();
+
+	bool IsGoodToPrepare() const;
 
 #if SUPPORT_IOBITS
 	uint32_t GetMoveStartTime() const { return moveStartTime; }
@@ -91,14 +97,14 @@ public:
 	// the calculation can just be managed in time at speeds of 15000mm/min (step interval 50us), but not at 20000mm/min (step interval 37.5us).
 	// Therefore, where the step interval falls below 60us, we don't calculate on every step.
 	// Note: the above measurements were taken some time ago, before some firmware optimisations.
-#ifdef DUET_NG
+#if SAM4E || SAM4S
 	static constexpr int32_t MinCalcIntervalDelta = (40 * stepClockRate)/1000000; 		// the smallest sensible interval between calculations (40us) in step timer clocks
 	static constexpr int32_t MinCalcIntervalCartesian = (40 * stepClockRate)/1000000;	// same as delta for now, but could be lower
-	static constexpr uint32_t minInterruptInterval = 6;									// about 2us minimum interval between interrupts, in clocks
+	static constexpr uint32_t minInterruptInterval = 6;									// about 6us minimum interval between interrupts, in step clocks
 #else
 	static constexpr int32_t MinCalcIntervalDelta = (60 * stepClockRate)/1000000; 		// the smallest sensible interval between calculations (60us) in step timer clocks
 	static constexpr int32_t MinCalcIntervalCartesian = (60 * stepClockRate)/1000000;	// same as delta for now, but could be lower
-	static constexpr uint32_t minInterruptInterval = 6;									// about 2us minimum interval between interrupts, in clocks
+	static constexpr uint32_t minInterruptInterval = 4;									// about 6us minimum interval between interrupts, in step clocks
 #endif
 
 	static void PrintMoves();										// print saved moves for debugging
@@ -109,14 +115,15 @@ public:
 	static int32_t loggedProbePositions[XYZ_AXES * MaxLoggedProbePositions];
 #endif
 
+	static uint32_t maxReps;
+
 private:
 	void RecalculateMove() __attribute__ ((hot));
 	void CalcNewSpeeds() __attribute__ ((hot));
 	void ReduceHomingSpeed();										// called to reduce homing speed when a near-endstop is triggered
 	void StopDrive(size_t drive);									// stop movement of a drive and recalculate the endpoint
-	void MoveAborted();
 	void InsertDM(DriveMovement *dm) __attribute__ ((hot));
-	DriveMovement *RemoveDM(size_t drive) __attribute__ ((hot));
+	void RemoveDM(size_t drive);
 	void ReleaseDMs();
 	bool IsDecelerationMove() const;								// return true if this move is or have been might have been intended to be a deceleration-only move
 	void DebugPrintVector(const char *name, const float *vec, size_t len) const;
@@ -135,16 +142,25 @@ private:
 	DDA *prev;								// The previous one in the ring
 
 	volatile DDAState state;				// What state this DDA is in
-	uint8_t endCoordinatesValid : 1;		// True if endCoordinates can be relied on
-	uint8_t isDeltaMovement : 1;			// True if this is a delta printer movement
-	uint8_t canPauseAfter : 1;				// True if we can pause at the end of this move
-	uint8_t canPauseBefore : 1;				// True if we can pause just before this move
-	uint8_t isPrintingMove : 1;				// True if this move includes XY movement and extrusion
-	uint8_t usePressureAdvance : 1;			// True if pressure advance should be applied to any forward extrusion
-	uint8_t hadLookaheadUnderrun : 1;		// True if the lookahead queue was not long enough to optimise this move
-	uint8_t xyMoving : 1;					// True if movement along an X axis or the Y axis was requested, even it if's too small to do
-	uint8_t goingSlow : 1;					// True if we have slowed the movement because the Z probe is approaching its threshold
-	uint8_t isLeadscrewAdjustmentMove : 1;	// True if this is a leadscrews adjustment move
+	uint8_t proportionRemaining;			// What proportion of the extrusion in the G1 or G0 move of which this is a part remains to be done after this segment is complete
+											// - we use a uint8_t instead of a float to save space because it only affects the extrusion amount, so ~0.4% error is acceptable
+	union
+	{
+		struct
+		{
+			uint8_t endCoordinatesValid : 1;		// True if endCoordinates can be relied on
+			uint8_t isDeltaMovement : 1;			// True if this is a delta printer movement
+			uint8_t canPauseAfter : 1;				// True if we can pause at the end of this move
+			uint8_t canPauseBefore : 1;				// True if we can pause just before this move
+			uint8_t isPrintingMove : 1;				// True if this move includes XY movement and extrusion
+			uint8_t usePressureAdvance : 1;			// True if pressure advance should be applied to any forward extrusion
+			uint8_t hadLookaheadUnderrun : 1;		// True if the lookahead queue was not long enough to optimise this move
+			uint8_t xyMoving : 1;					// True if movement along an X axis or the Y axis was requested, even it if's too small to do
+			uint8_t goingSlow : 1;					// True if we have slowed the movement because the Z probe is approaching its threshold
+			uint8_t isLeadscrewAdjustmentMove : 1;	// True if this is a leadscrews adjustment move
+		};
+		uint16_t flags;								// so that we can print all the flags at once for debugging
+	};
 
     EndstopChecks endStopsToCheck;			// Which endstops we are checking on this move
     AxesBitmap xAxes;						// Which axes are behaving as X axes
@@ -161,7 +177,6 @@ private:
     float virtualExtruderPosition;			// the virtual extruder position at the end of this move, used for pause/resume
 
     // These are used only in delta calculations
-    float a2plusb2;							// Sum of the squares of the X and Y movement fractions
     int32_t cKc;							// The Z movement fraction multiplied by Kc and converted to integer
 
     // These vary depending on how we connect the move with its predecessor and successor, but remain constant while the move is being executed
diff --git a/src/Movement/DriveMovement.cpp b/src/Movement/DriveMovement.cpp
index a2aa8228..04b98c05 100644
--- a/src/Movement/DriveMovement.cpp
+++ b/src/Movement/DriveMovement.cpp
@@ -10,6 +10,7 @@
 #include "Move.h"
 #include "RepRap.h"
 #include "Libraries/Math/Isqrt.h"
+#include "Kinematics/LinearDeltaKinematics.h"
 
 // Static members
 
@@ -46,16 +47,6 @@ DriveMovement *DriveMovement::Allocate(size_t drive, DMState st)
 	return dm;
 }
 
-void DriveMovement::Release(DriveMovement *item)
-{
-	if (item != nullptr)
-	{
-		item->nextDM = freeList;
-		freeList = item;
-		++numFree;
-	}
-}
-
 // Constructors
 DriveMovement::DriveMovement(DriveMovement *next) : nextDM(next)
 {
@@ -102,6 +93,64 @@ void DriveMovement::PrepareCartesianAxis(const DDA& dda, const PrepParams& param
 void DriveMovement::PrepareDeltaAxis(const DDA& dda, const PrepParams& params)
 {
 	const float stepsPerMm = reprap.GetPlatform().DriveStepsPerUnit(drive);
+	const float A = params.initialX - params.dparams->GetTowerX(drive);
+	const float B = params.initialY - params.dparams->GetTowerY(drive);
+	const float aAplusbB = A * dda.directionVector[X_AXIS] + B * dda.directionVector[Y_AXIS];
+	const float dSquaredMinusAsquaredMinusBsquared = params.diagonalSquared - fsquare(A) - fsquare(B);
+	const float h0MinusZ0 = sqrtf(dSquaredMinusAsquaredMinusBsquared);
+	mp.delta.hmz0sK = (int32_t)(h0MinusZ0 * stepsPerMm * DriveMovement::K2);
+	mp.delta.minusAaPlusBbTimesKs = -(int32_t)(aAplusbB * stepsPerMm * DriveMovement::K2);
+	mp.delta.dSquaredMinusAsquaredMinusBsquaredTimesKsquaredSsquared =
+			(int64_t)(dSquaredMinusAsquaredMinusBsquared * fsquare(stepsPerMm * DriveMovement::K2));
+
+	// Calculate the distance at which we need to reverse direction.
+	if (params.a2plusb2 <= 0.0)
+	{
+		// Pure Z movement. We can't use the main calculation because it divides by a2plusb2.
+		direction = (dda.directionVector[Z_AXIS] >= 0.0);
+		reverseStartStep = totalSteps + 1;
+	}
+	else
+	{
+		// The distance to reversal is the solution to a quadratic equation. One root corresponds to the carriages being below the bed,
+		// the other root corresponds to the carriages being above the bed.
+		const float drev = ((dda.directionVector[Z_AXIS] * sqrtf(params.a2b2D2 - fsquare(A * dda.directionVector[Y_AXIS] - B * dda.directionVector[X_AXIS])))
+							- aAplusbB)/params.a2plusb2;
+		if (drev > 0.0 && drev < dda.totalDistance)		// if the reversal point is within range
+		{
+			// Calculate how many steps we need to move up before reversing
+			const float hrev = dda.directionVector[Z_AXIS] * drev + sqrtf(dSquaredMinusAsquaredMinusBsquared - 2 * drev * aAplusbB - params.a2plusb2 * fsquare(drev));
+			const int32_t numStepsUp = (int32_t)((hrev - h0MinusZ0) * stepsPerMm);
+
+			// We may be almost at the peak height already, in which case we don't really have a reversal.
+			if (numStepsUp < 1 || (direction && (uint32_t)numStepsUp <= totalSteps))
+			{
+				reverseStartStep = totalSteps + 1;
+			}
+			else
+			{
+				reverseStartStep = (uint32_t)numStepsUp + 1;
+
+				// Correct the initial direction and the total number of steps
+				if (direction)
+				{
+					// Net movement is up, so we will go up a bit and then down by a lesser amount
+					totalSteps = (2 * numStepsUp) - totalSteps;
+				}
+				else
+				{
+					// Net movement is down, so we will go up first and then down by a greater amount
+					direction = true;
+					totalSteps = (2 * numStepsUp) + totalSteps;
+				}
+			}
+		}
+		else
+		{
+			reverseStartStep = totalSteps + 1;
+		}
+	}
+
 	mp.delta.twoCsquaredTimesMmPerStepDivAK = (uint32_t)((float)DDA::stepClockRateSquared/(stepsPerMm * dda.acceleration * (K2/2)));
 
 	// Acceleration phase parameters
diff --git a/src/Movement/DriveMovement.h b/src/Movement/DriveMovement.h
index 36b44f95..788f495d 100644
--- a/src/Movement/DriveMovement.h
+++ b/src/Movement/DriveMovement.h
@@ -10,16 +10,29 @@
 
 #include "RepRapFirmware.h"
 
+class LinearDeltaKinematics;
+
 // Struct for passing parameters to the DriveMovement Prepare methods
 struct PrepParams
 {
+	// Parameters used for all types of motion
 	float decelStartDistance;
 	uint32_t startSpeedTimesCdivA;
 	uint32_t topSpeedTimesCdivA;
 	uint32_t decelStartClocks;
 	uint32_t topSpeedTimesCdivAPlusDecelStartClocks;
 	uint32_t accelClocksMinusAccelDistanceTimesCdivTopSpeed;
+
+	// Parameters used only for extruders
 	float compFactor;
+
+	// Parameters used only for delta moves
+	float initialX;
+	float initialY;
+	const LinearDeltaKinematics *dparams;
+	float diagonalSquared;
+	float a2plusb2;								// sum of the squares of the X and Y movement fractions
+	float a2b2D2;
 };
 
 enum class DMState : uint8_t
@@ -35,11 +48,11 @@ class DriveMovement
 public:
 	DriveMovement(DriveMovement *next);
 
-	bool CalcNextStepTimeCartesian(const DDA &dda, bool live);
-	bool CalcNextStepTimeDelta(const DDA &dda, bool live);
-	void PrepareCartesianAxis(const DDA& dda, const PrepParams& params);
-	void PrepareDeltaAxis(const DDA& dda, const PrepParams& params);
-	void PrepareExtruder(const DDA& dda, const PrepParams& params, bool doCompensation);
+	bool CalcNextStepTimeCartesian(const DDA &dda, bool live) __attribute__ ((hot));
+	bool CalcNextStepTimeDelta(const DDA &dda, bool live) __attribute__ ((hot));
+	void PrepareCartesianAxis(const DDA& dda, const PrepParams& params) __attribute__ ((hot));
+	void PrepareDeltaAxis(const DDA& dda, const PrepParams& params) __attribute__ ((hot));
+	void PrepareExtruder(const DDA& dda, const PrepParams& params, bool doCompensation) __attribute__ ((hot));
 	void ReduceSpeed(const DDA& dda, float inverseSpeedFactor);
 	void DebugPrint(char c, bool withDelta) const;
 	int32_t GetNetStepsLeft() const;
@@ -53,8 +66,8 @@ public:
 	static void Release(DriveMovement *item);
 
 private:
-	bool CalcNextStepTimeCartesianFull(const DDA &dda, bool live);
-	bool CalcNextStepTimeDeltaFull(const DDA &dda, bool live);
+	bool CalcNextStepTimeCartesianFull(const DDA &dda, bool live) __attribute__ ((hot));
+	bool CalcNextStepTimeDeltaFull(const DDA &dda, bool live) __attribute__ ((hot));
 
 	static DriveMovement *freeList;
 	static int numFree;
@@ -144,6 +157,7 @@ inline bool DriveMovement::CalcNextStepTimeCartesian(const DDA &dda, bool live)
 
 // Calculate the time since the start of the move when the next step for the specified DriveMovement is due
 // Return true if there are more steps to do. When finished, leave nextStep == totalSteps + 1.
+// We inline this part to speed things up when we are doing double/quad/octal stepping.
 inline bool DriveMovement::CalcNextStepTimeDelta(const DDA &dda, bool live)
 {
 	++nextStep;
@@ -201,4 +215,12 @@ inline int32_t DriveMovement::GetNetStepsTaken() const
 	return (direction) ? netStepsTaken : -netStepsTaken;
 }
 
+// This is inlined because it is only called from one place
+inline void DriveMovement::Release(DriveMovement *item)
+{
+	item->nextDM = freeList;
+	freeList = item;
+	++numFree;
+}
+
 #endif /* DRIVEMOVEMENT_H_ */
diff --git a/src/Movement/Kinematics/CartesianKinematics.cpp b/src/Movement/Kinematics/CartesianKinematics.cpp
index db884171..f9a889b1 100644
--- a/src/Movement/Kinematics/CartesianKinematics.cpp
+++ b/src/Movement/Kinematics/CartesianKinematics.cpp
@@ -19,7 +19,7 @@ const char *CartesianKinematics::GetName(bool forStatusReport) const
 }
 
 // Convert Cartesian coordinates to motor coordinates
-bool CartesianKinematics::CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool allowModeChange) const
+bool CartesianKinematics::CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool isCoordinated) const
 {
 	for (size_t axis = 0; axis < numVisibleAxes; ++axis)
 	{
@@ -50,7 +50,14 @@ bool CartesianKinematics::QueryTerminateHomingMove(size_t axis) const
 void CartesianKinematics::OnHomingSwitchTriggered(size_t axis, bool highEnd, const float stepsPerMm[], DDA& dda) const
 {
 	const float hitPoint = (highEnd) ? reprap.GetPlatform().AxisMaximum(axis) : reprap.GetPlatform().AxisMinimum(axis);
-	dda.SetDriveCoordinate(hitPoint * stepsPerMm[axis], axis);
+	dda.SetDriveCoordinate(lrintf(hitPoint * stepsPerMm[axis]), axis);
+}
+
+// Limit the speed and acceleration of a move to values that the mechanics can handle.
+// The speeds in Cartesian space have already been limited.
+void CartesianKinematics::LimitSpeedAndAcceleration(DDA& dda, const float *normalisedDirectionVector) const
+{
+	// The axes of a Cartesian printer move independently, so there is nothing to do here
 }
 
 // End
diff --git a/src/Movement/Kinematics/CartesianKinematics.h b/src/Movement/Kinematics/CartesianKinematics.h
index 22051f6f..6b578282 100644
--- a/src/Movement/Kinematics/CartesianKinematics.h
+++ b/src/Movement/Kinematics/CartesianKinematics.h
@@ -17,11 +17,12 @@ public:
 
 	// Overridden base class functions. See Kinematics.h for descriptions.
 	const char *GetName(bool forStatusReport) const override;
-    bool CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool allowModeChange) const override;
+    bool CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool isCoordinated) const override;
     void MotorStepsToCartesian(const int32_t motorPos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, float machinePos[]) const override;
 	HomingMode GetHomingMode() const override { return homeCartesianAxes; }
 	bool QueryTerminateHomingMove(size_t axis) const override;
 	void OnHomingSwitchTriggered(size_t axis, bool highEnd, const float stepsPerMm[], DDA& dda) const override;
+	void LimitSpeedAndAcceleration(DDA& dda, const float *normalisedDirectionVector) const override;
 };
 
 #endif /* SRC_MOVEMENT_KINEMATICS_CARTESIANKINEMATICS_H_ */
diff --git a/src/Movement/Kinematics/CoreBaseKinematics.cpp b/src/Movement/Kinematics/CoreBaseKinematics.cpp
index 92bb81b5..f6b74b5e 100644
--- a/src/Movement/Kinematics/CoreBaseKinematics.cpp
+++ b/src/Movement/Kinematics/CoreBaseKinematics.cpp
@@ -74,7 +74,7 @@ void CoreBaseKinematics::OnHomingSwitchTriggered(size_t axis, bool highEnd, cons
 	}
 	else
 	{
-		dda.SetDriveCoordinate(hitPoint * stepsPerMm[axis], axis);
+		dda.SetDriveCoordinate(lrintf(hitPoint * stepsPerMm[axis]), axis);
 	}
 }
 
diff --git a/src/Movement/Kinematics/CoreXYKinematics.cpp b/src/Movement/Kinematics/CoreXYKinematics.cpp
index 6f35e321..86d32a33 100644
--- a/src/Movement/Kinematics/CoreXYKinematics.cpp
+++ b/src/Movement/Kinematics/CoreXYKinematics.cpp
@@ -6,6 +6,7 @@
  */
 
 #include "CoreXYKinematics.h"
+#include "Movement/DDA.h"
 
 CoreXYKinematics::CoreXYKinematics() : CoreBaseKinematics(KinematicsType::coreXY)
 {
@@ -18,7 +19,7 @@ const char *CoreXYKinematics::GetName(bool forStatusReport) const
 }
 
 // Convert Cartesian coordinates to motor coordinates returning true if successful
-bool CoreXYKinematics::CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool allowModeChange) const
+bool CoreXYKinematics::CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool isCoordinated) const
 {
 	motorPos[X_AXIS] = lrintf(((machinePos[X_AXIS] * axisFactors[X_AXIS]) + (machinePos[Y_AXIS] * axisFactors[Y_AXIS])) * stepsPerMm[X_AXIS]);
 	motorPos[Y_AXIS] = lrintf(((machinePos[X_AXIS] * axisFactors[X_AXIS]) - (machinePos[Y_AXIS] * axisFactors[Y_AXIS])) * stepsPerMm[Y_AXIS]);
@@ -55,4 +56,24 @@ bool CoreXYKinematics::DriveIsShared(size_t drive) const
 	return drive == X_AXIS || drive == Y_AXIS;
 }
 
+// Limit the speed and acceleration of a move to values that the mechanics can handle.
+// The speeds along individual Cartesian axes have already been limited before this is called.
+void CoreXYKinematics::LimitSpeedAndAcceleration(DDA& dda, const float *normalisedDirectionVector) const
+{
+	const float vecX = normalisedDirectionVector[0];
+	const float vecY = normalisedDirectionVector[1];
+	const float vecMax = max<float>(fabs(vecX + vecY), fabs(vecX - vecY));		// pick the case for the motor that is working hardest
+	if (vecMax > 0.01)															// avoid division by zero or near-zero
+	{
+		const Platform& platform = reprap.GetPlatform();
+		const float aX = platform.Acceleration(0);
+		const float aY = platform.Acceleration(1);
+		const float vX = platform.MaxFeedrate(0);
+		const float vY = platform.MaxFeedrate(1);
+		const float aMax = (fabs(vecX) + fabs(vecY)) * aX * aY/(vecMax * (fabs(vecX) * aY + fabs(vecY) * aX));
+		const float vMax = (fabs(vecX) + fabs(vecY)) * vX * vY/(vecMax * (fabs(vecX) * vY + fabs(vecY) * vX));
+		dda.LimitSpeedAndAcceleration(vMax, aMax);
+	}
+}
+
 // End
diff --git a/src/Movement/Kinematics/CoreXYKinematics.h b/src/Movement/Kinematics/CoreXYKinematics.h
index 4663acc1..711dd3dc 100644
--- a/src/Movement/Kinematics/CoreXYKinematics.h
+++ b/src/Movement/Kinematics/CoreXYKinematics.h
@@ -17,9 +17,10 @@ public:
 
 	// Overridden base class functions. See Kinematics.h for descriptions.
 	const char *GetName(bool forStatusReport) const override;
-	bool CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool allowModeChange) const override;
+	bool CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool isCoordinated) const override;
 	void MotorStepsToCartesian(const int32_t motorPos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, float machinePos[]) const override;
 	bool DriveIsShared(size_t drive) const override;
+	void LimitSpeedAndAcceleration(DDA& dda, const float *normalisedDirectionVector) const override;
 };
 
 #endif /* SRC_MOVEMENT_KINEMATICS_COREXYKINEMATICS_H_ */
diff --git a/src/Movement/Kinematics/CoreXYUKinematics.cpp b/src/Movement/Kinematics/CoreXYUKinematics.cpp
index 560037aa..ca327652 100644
--- a/src/Movement/Kinematics/CoreXYUKinematics.cpp
+++ b/src/Movement/Kinematics/CoreXYUKinematics.cpp
@@ -7,6 +7,7 @@
 
 #include "CoreXYUKinematics.h"
 #include "GCodes/GCodes.h"
+#include "Movement/DDA.h"
 
 const size_t CoreXYU_AXES = 5;
 const size_t U_AXIS = 3;			// X2
@@ -55,7 +56,7 @@ bool CoreXYUKinematics::Configure(unsigned int mCode, GCodeBuffer& gb, StringRef
 }
 
 // Convert Cartesian coordinates to motor coordinates
-bool CoreXYUKinematics::CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool allowModeChange) const
+bool CoreXYUKinematics::CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool isCoordinated) const
 {
 	motorPos[X_AXIS] = lrintf(((machinePos[X_AXIS] * axisFactors[X_AXIS]) + (machinePos[Y_AXIS] * axisFactors[Y_AXIS])) * stepsPerMm[X_AXIS]);
 	motorPos[Y_AXIS] = lrintf(((machinePos[X_AXIS] * axisFactors[X_AXIS]) - (machinePos[Y_AXIS] * axisFactors[Y_AXIS])) * stepsPerMm[Y_AXIS]);
@@ -102,4 +103,41 @@ bool CoreXYUKinematics::DriveIsShared(size_t drive) const
 			 || drive == V_AXIS;			// V doesn't have endstop switches, but include it here just in case
 }
 
+// Limit the speed and acceleration of a move to values that the mechanics can handle.
+// The speeds along individual Cartesian axes have already been limited before this is called.
+void CoreXYUKinematics::LimitSpeedAndAcceleration(DDA& dda, const float *normalisedDirectionVector) const
+{
+	const float vecX = normalisedDirectionVector[0];
+	const float vecY = normalisedDirectionVector[1];
+
+	// Limit the XY motor accelerations
+	const float vecMaxXY = max<float>(fabs(vecX + vecY), fabs(vecX - vecY));		// pick the case for the motor that is working hardest
+	if (vecMaxXY > 0.01)															// avoid division by zero or near-zero
+	{
+		const Platform& platform = reprap.GetPlatform();
+		const float aX = platform.Acceleration(0);
+		const float aY = platform.Acceleration(1);
+		const float vX = platform.MaxFeedrate(0);
+		const float vY = platform.MaxFeedrate(1);
+		const float aMax = (fabs(vecX) + fabs(vecY)) * aX * aY/(vecMaxXY * (fabs(vecX) * aY + fabs(vecY) * aX));
+		const float vMax = (fabs(vecX) + fabs(vecY)) * vX * vY/(vecMaxXY * (fabs(vecX) * vY + fabs(vecY) * vX));
+		dda.LimitSpeedAndAcceleration(vMax, aMax);
+	}
+
+	// Limit the UV motor accelerations
+	const float vecU = normalisedDirectionVector[3];
+	const float vecMaxUV = max<float>(fabs(vecU + vecY), fabs(vecY - vecY));		// pick the case for the motor that is working hardest
+	if (vecMaxUV > 0.01)															// avoid division by zero or near-zero
+	{
+		const Platform& platform = reprap.GetPlatform();
+		const float aU = platform.Acceleration(3);
+		const float aY = platform.Acceleration(1);
+		const float vU = platform.MaxFeedrate(3);
+		const float vY = platform.MaxFeedrate(1);
+		const float aMax = (fabs(vecX) + fabs(vecY)) * aU * aY/(vecMaxUV * (fabs(vecX) * aY + fabs(vecY) * aU));
+		const float vMax = (fabs(vecX) + fabs(vecY)) * vU * vY/(vecMaxUV * (fabs(vecX) * vY + fabs(vecY) * vU));
+		dda.LimitSpeedAndAcceleration(vMax, aMax);
+	}
+}
+
 // End
diff --git a/src/Movement/Kinematics/CoreXYUKinematics.h b/src/Movement/Kinematics/CoreXYUKinematics.h
index 77db9ae9..47a867b0 100644
--- a/src/Movement/Kinematics/CoreXYUKinematics.h
+++ b/src/Movement/Kinematics/CoreXYUKinematics.h
@@ -18,9 +18,10 @@ public:
 	// Overridden base class functions. See Kinematics.h for descriptions.
 	const char *GetName(bool forStatusReport) const override;
 	bool Configure(unsigned int mCode, GCodeBuffer& gb, StringRef& reply, bool& error) override;
-	bool CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool allowModeChange) const override;
+	bool CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool isCoordinated) const override;
 	void MotorStepsToCartesian(const int32_t motorPos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, float machinePos[]) const override;
 	bool DriveIsShared(size_t drive) const override;
+	void LimitSpeedAndAcceleration(DDA& dda, const float *normalisedDirectionVector) const override;
 };
 
 #endif /* SRC_MOVEMENT_KINEMATICS_COREXYKINEMATICS_H_ */
diff --git a/src/Movement/Kinematics/CoreXZKinematics.cpp b/src/Movement/Kinematics/CoreXZKinematics.cpp
index d81f13ca..4144aa47 100644
--- a/src/Movement/Kinematics/CoreXZKinematics.cpp
+++ b/src/Movement/Kinematics/CoreXZKinematics.cpp
@@ -18,7 +18,7 @@ const char *CoreXZKinematics::GetName(bool forStatusReport) const
 }
 
 // Convert Cartesian coordinates to motor coordinates
-bool CoreXZKinematics::CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool allowModeChange) const
+bool CoreXZKinematics::CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool isCoordinated) const
 {
 	motorPos[X_AXIS] = lrintf(((machinePos[X_AXIS] * axisFactors[X_AXIS]) + (machinePos[Z_AXIS] * axisFactors[Z_AXIS])) * stepsPerMm[X_AXIS]);
 	motorPos[Y_AXIS] = lrintf(machinePos[Y_AXIS] * stepsPerMm[Y_AXIS]);
@@ -56,4 +56,13 @@ bool CoreXZKinematics::DriveIsShared(size_t drive) const
 	return drive == X_AXIS || drive == Z_AXIS;
 }
 
+// Limit the speed and acceleration of a move to values that the mechanics can handle.
+// The speeds in Cartesian space have already been limited.
+void CoreXZKinematics::LimitSpeedAndAcceleration(DDA& dda, const float *normalisedDirectionVector) const
+{
+	// Ideally we would do something here, but for now we don't.
+	// This could mean that a simultaneous X and Z movement with Z at maximum speed up and X at 3x that speed would be under-powered,
+	// but the workaround in that case would be just to lower the maximum Z speed a little, which won't affect printing speed significantly.
+}
+
 // End
diff --git a/src/Movement/Kinematics/CoreXZKinematics.h b/src/Movement/Kinematics/CoreXZKinematics.h
index 3a8d9344..9f9078ef 100644
--- a/src/Movement/Kinematics/CoreXZKinematics.h
+++ b/src/Movement/Kinematics/CoreXZKinematics.h
@@ -17,11 +17,12 @@ public:
 
 	// Overridden base class functions. See Kinematics.h for descriptions.
 	const char *GetName(bool forStatusReport) const override;
-	bool CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool allowModeChange) const override;
+	bool CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool isCoordinated) const override;
 	void MotorStepsToCartesian(const int32_t motorPos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, float machinePos[]) const override;
 	AxesBitmap AxesToHomeBeforeProbing() const override { return MakeBitmap<AxesBitmap>(X_AXIS) | MakeBitmap<AxesBitmap>(Y_AXIS) | MakeBitmap<AxesBitmap>(Z_AXIS); }
 	bool DriveIsShared(size_t drive) const override;
 	bool SupportsAutoCalibration() const override { return false; }
+	void LimitSpeedAndAcceleration(DDA& dda, const float *normalisedDirectionVector) const override;
 };
 
 #endif /* SRC_MOVEMENT_KINEMATICS_COREXZKINEMATICS_H_ */
diff --git a/src/Movement/Kinematics/Kinematics.cpp b/src/Movement/Kinematics/Kinematics.cpp
index 484f7d35..d85f8d24 100644
--- a/src/Movement/Kinematics/Kinematics.cpp
+++ b/src/Movement/Kinematics/Kinematics.cpp
@@ -12,6 +12,7 @@
 #include "CoreXZKinematics.h"
 #include "ScaraKinematics.h"
 #include "CoreXYUKinematics.h"
+#include "PolarKinematics.h"
 #include "RepRap.h"
 #include "Platform.h"
 
@@ -42,15 +43,15 @@ bool Kinematics::Configure(unsigned int mCode, GCodeBuffer& gb, StringRef& reply
 
 // Return true if the specified XY position is reachable by the print head reference point.
 // This default implementation assumes a rectangular reachable area, so it just uses the bed dimensions give in the M208 command.
-bool Kinematics::IsReachable(float x, float y) const
+bool Kinematics::IsReachable(float x, float y, bool isCoordinated) const
 {
 	const Platform& platform = reprap.GetPlatform();
 	return x >= platform.AxisMinimum(X_AXIS) && y >= platform.AxisMinimum(Y_AXIS) && x <= platform.AxisMaximum(X_AXIS) && y <= platform.AxisMaximum(Y_AXIS);
 }
 
-// Limit the Cartesian position that the user wants to move to
+// Limit the Cartesian position that the user wants to move to, returning true if any coordinates were changed
 // This default implementation just applies the rectangular limits set up by M208 to those axes that have been homed.
-bool Kinematics::LimitPosition(float coords[], size_t numVisibleAxes, AxesBitmap axesHomed) const
+bool Kinematics::LimitPosition(float coords[], size_t numVisibleAxes, AxesBitmap axesHomed, bool isCoordinated) const
 {
 	return LimitPositionFromAxis(coords, 0, numVisibleAxes, axesHomed);
 }
@@ -139,6 +140,8 @@ const char* Kinematics::GetHomingFileName(AxesBitmap toBeHomed, AxesBitmap alrea
 		return new ScaraKinematics();
 	case KinematicsType::coreXYU:
 		return new CoreXYUKinematics();
+	case KinematicsType::polar:
+		return new PolarKinematics();
 	}
 }
 
diff --git a/src/Movement/Kinematics/Kinematics.h b/src/Movement/Kinematics/Kinematics.h
index 3d960929..fa9b2e25 100644
--- a/src/Movement/Kinematics/Kinematics.h
+++ b/src/Movement/Kinematics/Kinematics.h
@@ -26,6 +26,8 @@ enum class KinematicsType : uint8_t
 	linearDelta,
 	scara,
 	coreXYU,
+	hangprinter,
+	polar,
 
 	unknown				// this one must be last!
 };
@@ -52,7 +54,7 @@ public:
 
 	// Return the name of the current kinematics.
 	// If 'forStatusReport' is true then the string must be the one for that kinematics expected by DuetWebControl and PanelDue.
-	// Otherwise it should be in a format suitable fore printing.
+	// Otherwise it should be in a format suitable for printing.
 	// For any new kinematics, the same string can be returned regardless of the parameter.
 	virtual const char *GetName(bool forStatusReport = false) const = 0;
 
@@ -71,7 +73,7 @@ public:
 	// 'numAxes' is the number of machine axes to convert, which will always be at least 3
 	// 'motorPos' is the output vector of motor positions
 	// Return true if successful, false if we were unable to convert
-	virtual bool CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool allowModeChange) const = 0;
+	virtual bool CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool isCoordinated) const = 0;
 
 	// Convert motor positions (measured in steps from reference position) to Cartesian coordinates
 	// 'motorPos' is the input vector of motor positions
@@ -102,12 +104,12 @@ public:
 	virtual float GetTiltCorrection(size_t axis) const { return 0.0; }
 
 	// Return true if the specified XY position is reachable by the print head reference point.
-	// The default implementation assumes a rectangular reachable area, so it just used the bed dimensions give in the M208 commands.
-	virtual bool IsReachable(float x, float y) const;
+	// The default implementation assumes a rectangular reachable area, so it just uses the bed dimensions give in the M208 commands.
+	virtual bool IsReachable(float x, float y, bool isCoordinated) const;
 
 	// Limit the Cartesian position that the user wants to move to, returning true if any coordinates were changed
 	// The default implementation just applies the rectangular limits set up by M208 to those axes that have been homed.
-	virtual bool LimitPosition(float coords[], size_t numVisibleAxes, AxesBitmap axesHomed) const;
+	virtual bool LimitPosition(float coords[], size_t numVisibleAxes, AxesBitmap axesHomed, bool isCoordinated) const;
 
 	// Return the set of axes that must have been homed before bed probing is allowed
 	// The default implementation requires just X and Y, but some kinematics require additional axes to be homed (e.g. delta, CoreXZ)
@@ -123,7 +125,7 @@ public:
 	virtual size_t NumHomingButtons(size_t numVisibleAxes) const { return numVisibleAxes; }
 
 	// Override this if the homing buttons are not named after the axes (e.g. SCARA printer)
-	virtual const char* HomingButtonNames() const { return "XYZUVW"; }
+	virtual const char* HomingButtonNames() const { return "XYZUVWABC"; }
 
 	// This function is called when a request is made to home the axes in 'toBeHomed' and the axes in 'alreadyHomed' have already been homed.
 	// If we can proceed with homing some axes, return the name of the homing file to be called. Optionally, update 'alreadyHomed' to indicate
@@ -149,6 +151,10 @@ public:
 	// Write any calibration data that we need to resume a print after power fail, returning true if successful. Override where necessary.
 	virtual bool WriteResumeSettings(FileStore *f) const { return true; }
 
+	// Limit the speed and acceleration of a move to values that the mechanics can handle.
+	// The speeds along individual Cartesian axes have already been limited before this is called.
+	virtual void LimitSpeedAndAcceleration(DDA& dda, const float *normalisedDirectionVector) const = 0;
+
 	// Override this virtual destructor if your constructor allocates any dynamic memory
 	virtual ~Kinematics() { }
 
diff --git a/src/Movement/Kinematics/LinearDeltaKinematics.cpp b/src/Movement/Kinematics/LinearDeltaKinematics.cpp
index 28ed94e4..ecfc093d 100644
--- a/src/Movement/Kinematics/LinearDeltaKinematics.cpp
+++ b/src/Movement/Kinematics/LinearDeltaKinematics.cpp
@@ -134,7 +134,7 @@ void LinearDeltaKinematics::InverseTransform(float Ha, float Hb, float Hc, float
 }
 
 // Convert Cartesian coordinates to motor steps
-bool LinearDeltaKinematics::CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool allowModeChange) const
+bool LinearDeltaKinematics::CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool isCoordinated) const
 {
 	//TODO return false if we can't transform the position
 	for (size_t axis = 0; axis < min<size_t>(numVisibleAxes, DELTA_AXES); ++axis)
@@ -163,20 +163,20 @@ void LinearDeltaKinematics::MotorStepsToCartesian(const int32_t motorPos[], cons
 }
 
 // Return true if the specified XY position is reachable by the print head reference point.
-bool LinearDeltaKinematics::IsReachable(float x, float y) const
+bool LinearDeltaKinematics::IsReachable(float x, float y, bool isCoordinated) const
 {
 	return fsquare(x) + fsquare(y) < printRadiusSquared;
 }
 
 // Limit the Cartesian position that the user wants to move to
-bool LinearDeltaKinematics::LimitPosition(float coords[], size_t numVisibleAxes, AxesBitmap axesHomed) const
+bool LinearDeltaKinematics::LimitPosition(float coords[], size_t numVisibleAxes, AxesBitmap axesHomed, bool isCoordinated) const
 {
 	const AxesBitmap allAxes = MakeBitmap<AxesBitmap>(X_AXIS) | MakeBitmap<AxesBitmap>(Y_AXIS) | MakeBitmap<AxesBitmap>(Z_AXIS);
 	bool limited = false;
 	if ((axesHomed & allAxes) == allAxes)
 	{
 		// If axes have been homed on a delta printer and this isn't a homing move, check for movements outside limits.
-		// Skip this check if axes have not been homed, so that extruder-only moved are allowed before homing
+		// Skip this check if axes have not been homed, so that extruder-only moves are allowed before homing
 		// Constrain the move to be within the build radius
 		const float diagonalSquared = fsquare(coords[X_AXIS]) + fsquare(coords[Y_AXIS]);
 		if (diagonalSquared > printRadiusSquared)
@@ -735,7 +735,22 @@ void LinearDeltaKinematics::OnHomingSwitchTriggered(size_t axis, bool highEnd, c
 	if (highEnd)
 	{
 		const float hitPoint = GetHomedCarriageHeight(axis);
-		dda.SetDriveCoordinate(hitPoint * stepsPerMm[axis], axis);
+		dda.SetDriveCoordinate(lrintf(hitPoint * stepsPerMm[axis]), axis);
+	}
+}
+
+// Limit the speed and acceleration of a move to values that the mechanics can handle.
+// The speeds in Cartesian space have already been limited.
+void LinearDeltaKinematics::LimitSpeedAndAcceleration(DDA& dda, const float *normalisedDirectionVector) const
+{
+	// Limit the speed in the XY plane to the lower of the X and Y maximum speeds, and similarly for the acceleration
+	const float xyFactor = sqrtf(fsquare(normalisedDirectionVector[X_AXIS]) + fsquare(normalisedDirectionVector[Y_AXIS]));
+	if (xyFactor > 0.01)
+	{
+		const Platform& platform = reprap.GetPlatform();
+		const float maxSpeed = min<float>(platform.MaxFeedrate(X_AXIS), platform.MaxFeedrate(Y_AXIS));
+		const float maxAcceleration = min<float>(platform.Acceleration(X_AXIS), platform.Acceleration(Y_AXIS));
+		dda.LimitSpeedAndAcceleration(maxSpeed/xyFactor, maxAcceleration/xyFactor);
 	}
 }
 
diff --git a/src/Movement/Kinematics/LinearDeltaKinematics.h b/src/Movement/Kinematics/LinearDeltaKinematics.h
index 6a3bdc3f..bc09cb1c 100644
--- a/src/Movement/Kinematics/LinearDeltaKinematics.h
+++ b/src/Movement/Kinematics/LinearDeltaKinematics.h
@@ -26,15 +26,15 @@ public:
 	// Overridden base class functions. See Kinematics.h for descriptions.
 	const char *GetName(bool forStatusReport) const override;
 	bool Configure(unsigned int mCode, GCodeBuffer& gb, StringRef& reply, bool& error) override;
-	bool CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool allowModeChange) const override;
+	bool CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool isCoordinated) const override;
 	void MotorStepsToCartesian(const int32_t motorPos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, float machinePos[]) const override;
 	bool SupportsAutoCalibration() const override { return true; }
 	bool DoAutoCalibration(size_t numFactors, const RandomProbePointSet& probePoints, StringRef& reply) override;
 	void SetCalibrationDefaults() override { Init(); }
 	bool WriteCalibrationParameters(FileStore *f) const override;
 	float GetTiltCorrection(size_t axis) const override;
-	bool IsReachable(float x, float y) const override;
-	bool LimitPosition(float coords[], size_t numVisibleAxes, AxesBitmap axesHomed) const override;
+	bool IsReachable(float x, float y, bool isCoordinated) const override;
+	bool LimitPosition(float coords[], size_t numVisibleAxes, AxesBitmap axesHomed, bool isCoordinated) const override;
 	void GetAssumedInitialPosition(size_t numAxes, float positions[]) const override;
 	AxesBitmap AxesToHomeBeforeProbing() const override { return MakeBitmap<AxesBitmap>(X_AXIS) | MakeBitmap<AxesBitmap>(Y_AXIS) | MakeBitmap<AxesBitmap>(Z_AXIS); }
 	MotionType GetMotionType(size_t axis) const override;
@@ -45,6 +45,7 @@ public:
 	bool QueryTerminateHomingMove(size_t axis) const override;
 	void OnHomingSwitchTriggered(size_t axis, bool highEnd, const float stepsPerMm[], DDA& dda) const override;
 	bool WriteResumeSettings(FileStore *f) const override;
+	void LimitSpeedAndAcceleration(DDA& dda, const float *normalisedDirectionVector) const override;
 
     // Public functions specific to this class
 	float GetDiagonalSquared() const { return D2; }
diff --git a/src/Movement/Kinematics/PolarKinematics.cpp b/src/Movement/Kinematics/PolarKinematics.cpp
new file mode 100644
index 00000000..8d1eb8b8
--- /dev/null
+++ b/src/Movement/Kinematics/PolarKinematics.cpp
@@ -0,0 +1,260 @@
+/*
+ * PolarKinematics.cpp
+ *
+ *  Created on: 13 Oct 2017
+ *      Author: David
+ */
+
+#include "PolarKinematics.h"
+
+#include "RepRap.h"
+#include "Platform.h"
+#include "Storage/MassStorage.h"
+#include "GCodes/GCodeBuffer.h"
+#include "Movement/DDA.h"
+
+// Constructor
+PolarKinematics::PolarKinematics()
+	: Kinematics(KinematicsType::polar, DefaultSegmentsPerSecond, DefaultMinSegmentSize, true),
+	  minRadius(0.0), maxRadius(DefaultMaxRadius), homedRadius(0.0),
+	  maxTurntableSpeed(DefaultMaxTurntableSpeed), maxTurntableAcceleration(DefaultMaxTurntableAcceleration)
+{
+	Recalc();
+}
+
+// Return the name of the current kinematics.
+// If 'forStatusReport' is true then the string must be the one for that kinematics expected by DuetWebControl and PanelDue.
+// Otherwise it should be in a format suitable for printing.
+// For any new kinematics, the same string can be returned regardless of the parameter.
+const char *PolarKinematics::GetName(bool forStatusReport) const
+{
+	return "Polar";
+}
+
+// Set or report the parameters from a M665, M666 or M669 command
+// If 'mCode' is an M-code used to set parameters for the current kinematics (which should only ever be 665, 666, 667 or 669)
+// then search for parameters used to configure the current kinematics. If any are found, perform appropriate actions,
+// and return true if the changes affect the geometry.
+// If errors were discovered while processing parameters, put an appropriate error message in 'reply' and set 'error' to true.
+// If no relevant parameters are found, print the existing ones to 'reply' and return false.
+// If 'mCode' does not apply to this kinematics, call the base class version of this function, which will print a suitable error message.
+bool PolarKinematics::Configure(unsigned int mCode, GCodeBuffer& gb, StringRef& reply, bool& error)
+{
+	if (mCode == 669)
+	{
+		bool seenNonGeometry = false;
+		gb.TryGetFValue('S', segmentsPerSecond, seenNonGeometry);
+		gb.TryGetFValue('T', minSegmentLength, seenNonGeometry);
+
+		bool seen = false;
+		if (gb.Seen('R'))
+		{
+			seen = true;
+			float radiusLimits[2];
+			size_t numRadiusLimits = 2;
+			gb.GetFloatArray(radiusLimits, numRadiusLimits, false);
+			if (numRadiusLimits == 2)
+			{
+				minRadius = radiusLimits[0];
+				maxRadius = radiusLimits[1];
+			}
+			else
+			{
+				minRadius = 0.0;
+				maxRadius = radiusLimits[0];
+			}
+			homedRadius = minRadius;		// set up default
+		}
+
+		gb.TryGetFValue('H', homedRadius, seen);
+		gb.TryGetFValue('A', maxTurntableAcceleration, seenNonGeometry);
+		gb.TryGetFValue('F', maxTurntableSpeed, seenNonGeometry);
+
+		if (seen || seenNonGeometry)
+		{
+			Recalc();
+		}
+		else
+		{
+			reply.printf("Printer mode 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);
+		}
+		return seen;
+	}
+	else
+	{
+		return Kinematics::Configure(mCode, gb, reply, error);
+	}
+}
+
+// Convert Cartesian coordinates to motor positions measured in steps from reference position
+// 'machinePos' is a set of axis and extruder positions to convert
+// 'stepsPerMm' is as configured in M92. On a Scara or polar machine this would actually be steps per degree.
+// 'numAxes' is the number of machine axes to convert, which will always be at least 3
+// 'motorPos' is the output vector of motor positions
+// Return true if successful, false if we were unable to convert
+bool PolarKinematics::CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool isCoordinated) const
+{
+	motorPos[0] = lrintf(sqrtf(fsquare(machinePos[0]) + fsquare(machinePos[1])) * stepsPerMm[0]);
+	motorPos[1] = (motorPos[0] == 0.0) ? 0 : lrintf(atan2f(machinePos[1], machinePos[0]) * RadiansToDegrees * stepsPerMm[1]);
+
+	// Transform remaining axes linearly
+	for (size_t axis = Z_AXIS; axis < numVisibleAxes; ++axis)
+	{
+		motorPos[axis] = lrintf(machinePos[axis] * stepsPerMm[axis]);
+	}
+	return true;
+}
+
+// Convert motor positions (measured in steps from reference position) to Cartesian coordinates
+// 'motorPos' is the input vector of motor positions
+// 'stepsPerMm' is as configured in M92. On a Scara or polar machine this would actually be steps per degree.
+// 'numDrives' is the number of machine drives to convert, which will always be at least 3
+// 'machinePos' is the output set of converted axis and extruder positions
+void PolarKinematics::MotorStepsToCartesian(const int32_t motorPos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, float machinePos[]) const
+{
+	const float angle = (motorPos[1] * DegreesToRadians)/stepsPerMm[1];
+	const float radius = (float)motorPos[0]/stepsPerMm[0];
+	machinePos[0] = radius * cosf(angle);
+	machinePos[1] = radius * sinf(angle);
+
+	// Convert any additional axes linearly
+	for (size_t drive = Z_AXIS; drive < numVisibleAxes; ++drive)
+	{
+		machinePos[drive] = motorPos[drive]/stepsPerMm[drive];
+	}
+}
+
+// Return true if the specified XY position is reachable by the print head reference point.
+bool PolarKinematics::IsReachable(float x, float y, bool isCoordinated) const
+{
+	const float r2 = fsquare(x) + fsquare(y);
+	return r2 >= minRadiusSquared && r2 <= maxRadiusSquared;
+}
+
+// Limit the Cartesian position that the user wants to move to, returning true if any coordinates were changed
+bool PolarKinematics::LimitPosition(float position[], size_t numAxes, AxesBitmap axesHomed, bool isCoordinated) const
+{
+	const bool m208Limited = Kinematics::LimitPositionFromAxis(position, Z_AXIS, numAxes, axesHomed);	// call base class function to limit Z and higher axes
+	const float r2 = fsquare(position[0]) + fsquare(position[1]);
+	bool radiusLimited;
+	if (r2 < minRadiusSquared)
+	{
+		radiusLimited = true;
+		const float r = sqrtf(r2);
+		if (r < 0.01)
+		{
+			position[0] = minRadius;
+			position[1] = 0.0;
+		}
+		else
+		{
+			position[0] *= minRadius/r;
+			position[1] *= minRadius/r;
+		}
+	}
+	else if (r2 > maxRadiusSquared)
+	{
+		radiusLimited = true;
+		const float r = sqrtf(r2);
+		position[0] *= maxRadius/r;
+		position[1] *= maxRadius/r;
+	}
+	else
+	{
+		radiusLimited = false;
+	}
+
+	return m208Limited || radiusLimited;
+}
+
+// Return the initial Cartesian coordinates we assume after switching to this kinematics
+void PolarKinematics::GetAssumedInitialPosition(size_t numAxes, float positions[]) const
+{
+	for (size_t i = 0; i < numAxes; ++i)
+	{
+		positions[i] = 0.0;
+	}
+}
+
+// Return the axes that we can assume are homed after executing a G92 command to set the specified axis coordinates
+AxesBitmap PolarKinematics::AxesAssumedHomed(AxesBitmap g92Axes) const
+{
+	// If both X and Y have been specified then we know the positions the radius motor and the turntable, otherwise we don't
+	const AxesBitmap xyAxes = MakeBitmap<AxesBitmap>(X_AXIS) | MakeBitmap<AxesBitmap>(Y_AXIS);
+	if ((g92Axes & xyAxes) != xyAxes)
+	{
+		g92Axes &= ~xyAxes;
+	}
+	return g92Axes;
+}
+
+// This function is called when a request is made to home the axes in 'toBeHomed' and the axes in 'alreadyHomed' have already been homed.
+// If we can proceed with homing some axes, return the name of the homing file to be called. Optionally, update 'alreadyHomed' to indicate
+// that some additional axes should be considered not homed.
+// If we can't proceed because other axes need to be homed first, return nullptr and pass those axes back in 'mustBeHomedFirst'.
+const char* PolarKinematics::GetHomingFileName(AxesBitmap toBeHomed, AxesBitmap alreadyHomed, size_t numVisibleAxes, AxesBitmap& mustHomeFirst) const
+{
+	// Ask the base class which homing file we should call first
+	const char* ret = Kinematics::GetHomingFileName(toBeHomed, alreadyHomed, numVisibleAxes, mustHomeFirst);
+	// Change the returned name if it is X or Y
+	if (ret == StandardHomingFileNames[X_AXIS])
+	{
+		ret = HomeRadiusFileName;
+	}
+	else if (ret == StandardHomingFileNames[Y_AXIS])
+	{
+		ret = HomeBedFileName;
+	}
+	return ret;
+}
+
+// This function is called from the step ISR when an endstop switch is triggered during homing.
+// Return true if the entire homing move should be terminated, false if only the motor associated with the endstop switch should be stopped.
+bool PolarKinematics::QueryTerminateHomingMove(size_t axis) const
+{
+	return false;
+}
+
+// This function is called from the step ISR when an endstop switch is triggered during homing after stopping just one motor or all motors.
+// Take the action needed to define the current position, normally by calling dda.SetDriveCoordinate() and return false.
+void PolarKinematics::OnHomingSwitchTriggered(size_t axis, bool highEnd, const float stepsPerMm[], DDA& dda) const
+{
+	switch(axis)
+	{
+	case X_AXIS:	// radius
+		dda.SetDriveCoordinate(lrintf(homedRadius * stepsPerMm[axis]), axis);
+		break;
+
+	case Y_AXIS:	// bed
+		dda.SetDriveCoordinate(0, axis);
+		break;
+
+	default:
+		const float hitPoint = (highEnd) ? reprap.GetPlatform().AxisMaximum(axis) : reprap.GetPlatform().AxisMinimum(axis);
+		dda.SetDriveCoordinate(lrintf(hitPoint * stepsPerMm[axis]), axis);
+		break;
+	}
+}
+
+// Limit the speed and acceleration of a move to values that the mechanics can handle.
+// The speeds in Cartesian space have already been limited.
+void PolarKinematics::LimitSpeedAndAcceleration(DDA& dda, const float *normalisedDirectionVector) const
+{
+	const int32_t turntableMovement = labs(dda.DriveCoordinates()[1] - dda.GetPrevious()->DriveCoordinates()[1]);
+	if (turntableMovement != 0)
+	{
+		const float stepRatio = dda.GetTotalDistance() * reprap.GetPlatform().DriveStepsPerUnit(1)/turntableMovement;
+		dda.LimitSpeedAndAcceleration(stepRatio * maxTurntableSpeed, stepRatio * maxTurntableAcceleration);
+	}
+}
+
+// Update the derived parameters after the master parameters have been changed
+void PolarKinematics::Recalc()
+{
+	minRadiusSquared = (minRadius <= 0.0) ? 0.0 : fsquare(minRadius);
+	maxRadiusSquared = fsquare(maxRadius);
+}
+
+// End
diff --git a/src/Movement/Kinematics/PolarKinematics.h b/src/Movement/Kinematics/PolarKinematics.h
new file mode 100644
index 00000000..fe473d9b
--- /dev/null
+++ b/src/Movement/Kinematics/PolarKinematics.h
@@ -0,0 +1,51 @@
+/*
+ * PolarKinematics.h
+ *
+ *  Created on: 13 Oct 2017
+ *      Author: David
+ */
+
+#ifndef SRC_MOVEMENT_KINEMATICS_POLARKINEMATICS_H_
+#define SRC_MOVEMENT_KINEMATICS_POLARKINEMATICS_H_
+
+#include "Kinematics.h"
+
+class PolarKinematics : public Kinematics
+{
+public:
+	PolarKinematics();
+
+	// Overridden base class functions. See Kinematics.h for descriptions.
+	const char *GetName(bool forStatusReport) const override;
+	bool Configure(unsigned int mCode, GCodeBuffer& gb, StringRef& reply, bool& error) override;
+	bool CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool isCoordinated) const override;
+	void MotorStepsToCartesian(const int32_t motorPos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, float machinePos[]) const override;
+	bool IsReachable(float x, float y, bool isCoordinated) const override;
+	bool LimitPosition(float position[], size_t numAxes, AxesBitmap axesHomed, bool isCoordinated) const override;
+	void GetAssumedInitialPosition(size_t numAxes, float positions[]) const override;
+	const char* HomingButtonNames() const override { return "RTZUVWABC"; }
+	HomingMode GetHomingMode() const override { return homeIndividualMotors; }
+	AxesBitmap AxesAssumedHomed(AxesBitmap g92Axes) const override;
+	const char* GetHomingFileName(AxesBitmap toBeHomed, AxesBitmap alreadyHomed, size_t numVisibleAxes, AxesBitmap& mustHomeFirst) const override;
+	bool QueryTerminateHomingMove(size_t axis) const override;
+	void OnHomingSwitchTriggered(size_t axis, bool highEnd, const float stepsPerMm[], DDA& dda) const override;
+	void LimitSpeedAndAcceleration(DDA& dda, const float *normalisedDirectionVector) const override;
+
+private:
+	static constexpr float DefaultSegmentsPerSecond = 100.0;
+	static constexpr float DefaultMinSegmentSize = 0.2;
+	static constexpr float DefaultMaxRadius = 150.0;
+	static constexpr float DefaultMaxTurntableSpeed = 30.0;				// degrees per second
+	static constexpr float DefaultMaxTurntableAcceleration = 30.0;		// degrees per second per second
+	static constexpr const char *HomeRadiusFileName = "homeradius.g";
+	static constexpr const char *HomeBedFileName = "homebed.g";
+
+	void Recalc();
+
+	float minRadius, maxRadius, homedRadius;
+	float maxTurntableSpeed, maxTurntableAcceleration;
+
+	float minRadiusSquared, maxRadiusSquared;
+};
+
+#endif /* SRC_MOVEMENT_KINEMATICS_POLARKINEMATICS_H_ */
diff --git a/src/Movement/Kinematics/ScaraKinematics.cpp b/src/Movement/Kinematics/ScaraKinematics.cpp
index a350f42b..9a5580b0 100644
--- a/src/Movement/Kinematics/ScaraKinematics.cpp
+++ b/src/Movement/Kinematics/ScaraKinematics.cpp
@@ -12,14 +12,16 @@
 #include "GCodes/GCodeBuffer.h"
 #include "Movement/DDA.h"
 
+#include <limits>
+
 ScaraKinematics::ScaraKinematics()
 	: ZLeadscrewKinematics(KinematicsType::scara, DefaultSegmentsPerSecond, DefaultMinSegmentSize, true),
 	  proximalArmLength(DefaultProximalArmLength), distalArmLength(DefaultDistalArmLength), xOffset(0.0), yOffset(0.0)
 {
 	thetaLimits[0] = DefaultMinTheta;
 	thetaLimits[1] = DefaultMaxTheta;
-	phiLimits[0] = DefaultMinPhi;
-	phiLimits[1] = DefaultMaxPhi;
+	psiLimits[0] = DefaultMinPsi;
+	psiLimits[1] = DefaultMaxPsi;
 	crosstalk[0] = crosstalk[1] = crosstalk[2] = 0.0;
 	Recalc();
 }
@@ -30,11 +32,11 @@ const char *ScaraKinematics::GetName(bool forStatusReport) const
 	return "Scara";
 }
 
-// Convert Cartesian coordinates to motor coordinates
-// In the following, theta is the proximal arm angle relative to the X axis, psi is the distal arm angle relative to the proximal arm
-bool ScaraKinematics::CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool allowModeChange) const
+// Calculate theta, psi and the new arm mode form a target position.
+// If the position is not reachable because it is out of radius limits, set theta and psi to NaN and return false.
+// Otherwise set theta and psi to the required values and return true if they are in range.
+bool ScaraKinematics::CalculateThetaAndPsi(const float machinePos[], bool isCoordinated, float& theta, float& psi, bool& armMode) const
 {
-	// No need to limit x,y to reachable positions here, we already did that in class GCodes
 	const float x = machinePos[X_AXIS] + xOffset;
 	const float y = machinePos[Y_AXIS] + yOffset;
 	const float cosPsi = (fsquare(x) + fsquare(y) - proximalArmLengthSquared - distalArmLengthSquared) / twoPd;
@@ -43,40 +45,46 @@ bool ScaraKinematics::CartesianToMotorSteps(const float machinePos[], const floa
 	const float square = 1.0f - fsquare(cosPsi);
 	if (square < 0.01f)
 	{
+		theta = psi = std::numeric_limits<float>::quiet_NaN();
 		return false;		// not reachable
 	}
 
+	psi = acosf(cosPsi);
 	const float sinPsi = sqrtf(square);
-	float psi = acosf(cosPsi);
 	const float SCARA_K1 = proximalArmLength + distalArmLength * cosPsi;
 	const float SCARA_K2 = distalArmLength * sinPsi;
-	float theta;
 
 	// Try the current arm mode, then the other one
 	bool switchedMode = false;
 	for (;;)
 	{
-		if (isDefaultArmMode != switchedMode)
+		if (armMode != switchedMode)
 		{
 			// The following equations choose arm mode 0 i.e. distal arm rotated anticlockwise relative to proximal arm
-			theta = atan2f(SCARA_K1 * y - SCARA_K2 * x, SCARA_K1 * x + SCARA_K2 * y);
-			if (theta >= thetaLimits[0] && theta <= thetaLimits[1] && psi >= phiLimits[0] && psi <= phiLimits[1])
+			if (psi >= psiLimits[0] && psi <= psiLimits[1])
 			{
-				break;
+				theta = atan2f(SCARA_K1 * y - SCARA_K2 * x, SCARA_K1 * x + SCARA_K2 * y);
+				if (theta >= thetaLimits[0] && theta <= thetaLimits[1])
+				{
+					break;
+				}
 			}
 		}
 		else
 		{
 			// The following equations choose arm mode 1 i.e. distal arm rotated clockwise relative to proximal arm
-			theta = atan2f(SCARA_K1 * y + SCARA_K2 * x, SCARA_K1 * x - SCARA_K2 * y);
-			if (theta >= thetaLimits[0] && theta <= thetaLimits[1] && psi >= phiLimits[0] && psi <= phiLimits[1])
+			if ((-psi) >= psiLimits[0] && (-psi) <= psiLimits[1])
 			{
-				psi = -psi;
-				break;
+				theta = atan2f(SCARA_K1 * y + SCARA_K2 * x, SCARA_K1 * x - SCARA_K2 * y);
+				if (theta >= thetaLimits[0] && theta <= thetaLimits[1])
+				{
+					psi = -psi;
+					break;
+				}
 			}
 		}
 
-		if (switchedMode)
+		if (isCoordinated || switchedMode)
 		{
 			return false;		// not reachable
 		}
@@ -86,7 +94,31 @@ bool ScaraKinematics::CartesianToMotorSteps(const float machinePos[], const floa
 	// Now that we know we are going to do the move, update the arm mode
 	if (switchedMode)
 	{
-		isDefaultArmMode = !isDefaultArmMode;
+		armMode = !armMode;
+	}
+
+	return true;
+}
+
+// Convert Cartesian coordinates to motor coordinates, returning true if successful
+// In the following, theta is the proximal arm angle relative to the X axis, psi is the distal arm angle relative to the proximal arm
+bool ScaraKinematics::CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool isCoordinated) const
+{
+	float theta, psi;
+	if (machinePos[0] == cachedX && machinePos[1] == cachedY)
+	{
+		theta = cachedTheta;
+		psi = cachedPsi;
+		currentArmMode = cachedArmMode;
+	}
+	else
+	{
+		bool armMode = currentArmMode;
+		if (!CalculateThetaAndPsi(machinePos, isCoordinated, theta, psi, armMode))
+		{
+			return false;
+		}
+		currentArmMode = armMode;
 	}
 
 //debugPrintf("psi = %.2f, theta = %.2f\n", psi * RadiansToDegrees, theta * RadiansToDegrees);
@@ -107,11 +139,11 @@ bool ScaraKinematics::CartesianToMotorSteps(const float machinePos[], const floa
 // For Scara, the X and Y components of stepsPerMm are actually steps per degree angle.
 void ScaraKinematics::MotorStepsToCartesian(const int32_t motorPos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, float machinePos[]) const
 {
-	const float arm1Angle = ((float)motorPos[X_AXIS]/stepsPerMm[X_AXIS]) * DegreesToRadians;
-    const float arm2Angle = (((float)motorPos[Y_AXIS] + ((float)motorPos[X_AXIS] * crosstalk[0]))/stepsPerMm[Y_AXIS]) * DegreesToRadians;
+	const float psi = ((float)motorPos[X_AXIS]/stepsPerMm[X_AXIS]) * DegreesToRadians;
+    const float theta = (((float)motorPos[Y_AXIS] + ((float)motorPos[X_AXIS] * crosstalk[0]))/stepsPerMm[Y_AXIS]) * DegreesToRadians;
 
-    machinePos[X_AXIS] = (cosf(arm1Angle) * proximalArmLength + cosf(arm1Angle + arm2Angle) * distalArmLength) - xOffset;
-    machinePos[Y_AXIS] = (sinf(arm1Angle) * proximalArmLength + sinf(arm1Angle + arm2Angle) * distalArmLength) - yOffset;
+    machinePos[X_AXIS] = (cosf(psi) * proximalArmLength + cosf(psi + theta) * distalArmLength) - xOffset;
+    machinePos[Y_AXIS] = (sinf(psi) * proximalArmLength + sinf(psi + theta) * distalArmLength) - yOffset;
 
     // On some machines (e.g. Helios), the X and/or Y arm motors also affect the Z height
     machinePos[Z_AXIS] = ((float)motorPos[Z_AXIS] + ((float)motorPos[X_AXIS] * crosstalk[1]) + ((float)motorPos[Y_AXIS] * crosstalk[2]))/stepsPerMm[Z_AXIS];
@@ -139,14 +171,17 @@ bool ScaraKinematics::Configure(unsigned int mCode, GCodeBuffer& gb, StringRef&
 		gb.TryGetFValue('Y', yOffset, seen);
 		if (gb.TryGetFloatArray('A', 2, thetaLimits, reply, seen))
 		{
+			error = true;
 			return true;
 		}
-		if (gb.TryGetFloatArray('B', 2, phiLimits, reply, seen))
+		if (gb.TryGetFloatArray('B', 2, psiLimits, reply, seen))
 		{
+			error = true;
 			return true;
 		}
 		if (gb.TryGetFloatArray('C', 3, crosstalk, reply, seen))
 		{
+			error = true;
 			return true;
 		}
 
@@ -159,7 +194,7 @@ bool ScaraKinematics::Configure(unsigned int mCode, GCodeBuffer& gb, StringRef&
 			reply.printf("Printer mode is Scara with proximal arm %.2fmm range %.1f to %.1f" DEGREE_SYMBOL
 							", distal arm %.2fmm range %.1f to %.1f" DEGREE_SYMBOL ", crosstalk %.1f:%.1f:%.1f, bed origin (%.1f, %.1f), segments/sec %d, min. segment length %.2f",
 							(double)proximalArmLength, (double)thetaLimits[0], (double)thetaLimits[1],
-							(double)distalArmLength, (double)phiLimits[0], (double)phiLimits[1],
+							(double)distalArmLength, (double)psiLimits[0], (double)psiLimits[1],
 							(double)crosstalk[0], (double)crosstalk[1], (double)crosstalk[2],
 							(double)xOffset, (double)yOffset,
 							(int)segmentsPerSecond, (double)minSegmentLength);
@@ -173,57 +208,95 @@ bool ScaraKinematics::Configure(unsigned int mCode, GCodeBuffer& gb, StringRef&
 }
 
 // Return true if the specified XY position is reachable by the print head reference point.
-// TODO add an arm mode parameter?
-bool ScaraKinematics::IsReachable(float x, float y) const
+bool ScaraKinematics::IsReachable(float x, float y, bool isCoordinated) const
 {
-	// TODO The following isn't quite right, in particular it doesn't take account of the maximum arm travel
-    const float r = sqrtf(fsquare(x + xOffset) + fsquare(y + yOffset));
-    return r >= minRadius && r <= maxRadius && (x + xOffset) > 0.0;
+	// Check the M208 limits first
+	float coords[2] = {x, y};
+	if (Kinematics::LimitPosition(coords, 2, LowestNBits<AxesBitmap>(2), isCoordinated))
+	{
+		return false;
+	}
+
+	// See if we can transform the position
+	float theta, psi;
+	bool armMode = currentArmMode;
+	const bool reachable = CalculateThetaAndPsi(coords, isCoordinated, theta, psi, armMode);
+	if (reachable)
+	{
+		// Save the original and transformed coordinates so that we don't need to calculate them again if we are commanded to move to this position
+		cachedX = x;
+		cachedY = y;
+		cachedTheta = theta;
+		cachedPsi = psi;
+		cachedArmMode = armMode;
+	}
+
+    return reachable;
 }
 
-// Limit the Cartesian position that the user wants to move to
-// TODO take account of arm angle limits
-bool ScaraKinematics::LimitPosition(float coords[], size_t numVisibleAxes, AxesBitmap axesHomed) const
+// Limit the Cartesian position that the user wants to move to, returning true if any coordinates were changed
+bool ScaraKinematics::LimitPosition(float coords[], size_t numVisibleAxes, AxesBitmap axesHomed, bool isCoordinated) const
 {
 	// First limit all axes according to M208
-	const bool m208Limited = Kinematics::LimitPosition(coords, numVisibleAxes, axesHomed);
-
-	// Now check that the XY position is within radius limits, in case the M208 limits are too generous
-	bool radiusLimited = false;
-	float x = coords[X_AXIS] + xOffset;
-	float y = coords[Y_AXIS] + yOffset;
-	const float r2 = fsquare(x) + fsquare(y);
-	if (r2 < minRadiusSquared)
+	const bool m208Limited = Kinematics::LimitPosition(coords, numVisibleAxes, axesHomed, isCoordinated);
+
+	float theta, psi;
+	bool armMode = currentArmMode;
+	if (CalculateThetaAndPsi(coords, isCoordinated, theta, psi, armMode))
 	{
-		const float r = sqrtf(r2);
-		// The user may have specified x=0 y=0 so allow for this
-		if (r < 1.0)
+		// Save the original and transformed coordinates so that we don't need to calculate them again if we are commanded to move to this position
+		cachedX = coords[0];
+		cachedY = coords[1];
+		cachedTheta = theta;
+		cachedPsi = psi;
+		cachedArmMode = armMode;
+		return m208Limited;
+	}
+
+	// The requested position was not reachable
+	if (isnan(theta))
+	{
+		// We are radius-limited
+		float x = coords[X_AXIS] + xOffset;
+		float y = coords[Y_AXIS] + yOffset;
+		const float r = sqrtf(fsquare(x) + fsquare(y));
+		if (r < minRadius)
 		{
-			x = minRadius;
-			y = 0.0;
+			// Radius is too small. The user may have specified x=0 y=0 so allow for this.
+			if (r < 1.0)
+			{
+				x = minRadius;
+				y = 0.0;
+			}
+			else
+			{
+				x *= minRadius/r;
+				y *= minRadius/r;
+			}
 		}
 		else
 		{
-			x *= minRadius/r;
-			y *= minRadius/r;
+			// Radius must be too large
+			x *= maxRadius/r;
+			y *= maxRadius/r;
 		}
-		radiusLimited = true;
-	}
-	else if (r2 > maxRadiusSquared)
-	{
-		const float r = sqrtf(r2);
-		x *= maxRadius/r;
-		y *= maxRadius/r;
-		radiusLimited = true;
-	}
 
-	if (radiusLimited)
-	{
 		coords[X_AXIS] = x - xOffset;
 		coords[Y_AXIS] = y - yOffset;
 	}
 
-	return m208Limited || radiusLimited;
+	// Recalculate theta and psi, but don't allow arm mode changes this time
+	if (!CalculateThetaAndPsi(coords, true, theta, psi, armMode) && !isnan(theta))
+	{
+		// Radius is in range but at least one arm angle isn't
+		cachedTheta = theta = constrain<float>(theta, thetaLimits[0], thetaLimits[1]);
+		cachedPsi = psi = constrain<float>(psi, psiLimits[0], psiLimits[1]);
+		cachedX = coords[X_AXIS] = (cosf(psi) * proximalArmLength + cosf(psi + theta) * distalArmLength) - xOffset;
+		cachedY = coords[Y_AXIS] = (sinf(psi) * proximalArmLength + sinf(psi + theta) * distalArmLength) - yOffset;
+		cachedArmMode = currentArmMode;
+	}
+
+	return true;
 }
 
 // Return the initial Cartesian coordinates we assume after switching to this kinematics
@@ -302,16 +375,33 @@ bool ScaraKinematics::QueryTerminateHomingMove(size_t axis) const
 
 // This function is called from the step ISR when an endstop switch is triggered during homing after stopping just one motor or all motors.
 // Take the action needed to define the current position, normally by calling dda.SetDriveCoordinate() and return false.
+//TODO this doesn't appear to take account of the crosstalk factors yet
 void ScaraKinematics::OnHomingSwitchTriggered(size_t axis, bool highEnd, const float stepsPerMm[], DDA& dda) const
 {
 	const float hitPoint = (axis == 0)
 							? ((highEnd) ? thetaLimits[1] : thetaLimits[0])						// proximal joint homing switch
 							  : (axis == 1)
-								? ((highEnd) ? phiLimits[1] : phiLimits[0])	// distal joint homing switch
+								? ((highEnd) ? psiLimits[1] : psiLimits[0])	// distal joint homing switch
 								  : (highEnd)
 									? reprap.GetPlatform().AxisMaximum(axis)					// other axis (e.g. Z) high end homing switch
 									  : reprap.GetPlatform().AxisMinimum(axis);					// other axis (e.g. Z) low end homing switch
-	dda.SetDriveCoordinate(hitPoint * stepsPerMm[axis], axis);
+	dda.SetDriveCoordinate(lrintf(hitPoint * stepsPerMm[axis]), axis);
+}
+
+// Limit the speed and acceleration of a move to values that the mechanics can handle.
+// The speeds in Cartesian space have already been limited.
+void ScaraKinematics::LimitSpeedAndAcceleration(DDA& dda, const float *normalisedDirectionVector) const
+{
+	// For now we limit the speed in the XY plane to the lower of the X and Y maximum speeds, and similarly for the acceleration.
+	// Limiting the angular rates of the arms would be better.
+	const float xyFactor = sqrtf(fsquare(normalisedDirectionVector[X_AXIS]) + fsquare(normalisedDirectionVector[Y_AXIS]));
+	if (xyFactor > 0.01)
+	{
+		const Platform& platform = reprap.GetPlatform();
+		const float maxSpeed = min<float>(platform.MaxFeedrate(X_AXIS), platform.MaxFeedrate(Y_AXIS));
+		const float maxAcceleration = min<float>(platform.Acceleration(X_AXIS), platform.Acceleration(Y_AXIS));
+		dda.LimitSpeedAndAcceleration(maxSpeed/xyFactor, maxAcceleration/xyFactor);
+	}
 }
 
 // Recalculate the derived parameters
@@ -321,20 +411,22 @@ void ScaraKinematics::Recalc()
 	distalArmLengthSquared = fsquare(distalArmLength);
 	twoPd = proximalArmLength * distalArmLength * 2.0f;
 
-	minRadius = (proximalArmLength + distalArmLength * min<float>(cosf(phiLimits[0] * DegreesToRadians), cosf(phiLimits[1] * DegreesToRadians))) * 1.005;
-	if (phiLimits[0] < 0.0 && phiLimits[1] > 0.0)
+	minRadius = (proximalArmLength + distalArmLength * min<float>(cosf(psiLimits[0] * DegreesToRadians), cosf(psiLimits[1] * DegreesToRadians))) * 1.005;
+	if (psiLimits[0] < 0.0 && psiLimits[1] > 0.0)
 	{
 		// Zero distal arm angle is reachable
 		maxRadius = proximalArmLength + distalArmLength;
 	}
 	else
 	{
-		const float minAngle = min<float>(fabs(phiLimits[0]), fabs(phiLimits[1])) * DegreesToRadians;
+		const float minAngle = min<float>(fabs(psiLimits[0]), fabs(psiLimits[1])) * DegreesToRadians;
 		maxRadius = sqrtf(proximalArmLengthSquared + distalArmLengthSquared + (twoPd * cosf(minAngle)));
 	}
 	maxRadius *= 0.995;
 	minRadiusSquared = fsquare(minRadius);
 	maxRadiusSquared = fsquare(maxRadius);
+
+	cachedX = cachedY = std::numeric_limits<float>::quiet_NaN();		// make sure that the cached values won't match any coordinates
 }
 
 // End
diff --git a/src/Movement/Kinematics/ScaraKinematics.h b/src/Movement/Kinematics/ScaraKinematics.h
index e229468d..43e1807b 100644
--- a/src/Movement/Kinematics/ScaraKinematics.h
+++ b/src/Movement/Kinematics/ScaraKinematics.h
@@ -28,18 +28,19 @@ public:
 	// Overridden base class functions. See Kinematics.h for descriptions.
 	const char *GetName(bool forStatusReport) const override;
 	bool Configure(unsigned int mCode, GCodeBuffer& gb, StringRef& reply, bool& error) override;
-	bool CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool allowModeChange) const override;
+	bool CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool isCoordinated) const override;
 	void MotorStepsToCartesian(const int32_t motorPos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, float machinePos[]) const override;
-	bool IsReachable(float x, float y) const override;
-	bool LimitPosition(float position[], size_t numAxes, AxesBitmap axesHomed) const override;
+	bool IsReachable(float x, float y, bool isCoordinated) const override;
+	bool LimitPosition(float position[], size_t numAxes, AxesBitmap axesHomed, bool isCoordinated) const override;
 	void GetAssumedInitialPosition(size_t numAxes, float positions[]) const override;
 	size_t NumHomingButtons(size_t numVisibleAxes) const override;
-	const char* HomingButtonNames() const override { return "PDZUVW"; }
+	const char* HomingButtonNames() const override { return "PDZUVWABC"; }
 	HomingMode GetHomingMode() const override { return homeIndividualMotors; }
 	AxesBitmap AxesAssumedHomed(AxesBitmap g92Axes) const override;
 	const char* GetHomingFileName(AxesBitmap toBeHomed, AxesBitmap alreadyHomed, size_t numVisibleAxes, AxesBitmap& mustHomeFirst) const override;
 	bool QueryTerminateHomingMove(size_t axis) const override;
 	void OnHomingSwitchTriggered(size_t axis, bool highEnd, const float stepsPerMm[], DDA& dda) const override;
+	void LimitSpeedAndAcceleration(DDA& dda, const float *normalisedDirectionVector) const override;
 
 private:
 	static constexpr float DefaultSegmentsPerSecond = 100.0;
@@ -48,19 +49,20 @@ private:
 	static constexpr float DefaultDistalArmLength = 100.0;
 	static constexpr float DefaultMinTheta = -90.0;					// minimum proximal joint angle
 	static constexpr float DefaultMaxTheta = 90.0;					// maximum proximal joint angle
-	static constexpr float DefaultMinPhi = -135.0;					// minimum distal joint angle
-	static constexpr float DefaultMaxPhi = 135.0;					// maximum distal joint angle
+	static constexpr float DefaultMinPsi = -135.0;					// minimum distal joint angle
+	static constexpr float DefaultMaxPsi = 135.0;					// maximum distal joint angle
 
 	static constexpr const char *HomeProximalFileName = "homeproximal.g";
 	static constexpr const char *HomeDistalFileName = "homedistal.g";
 
 	void Recalc();
+	bool CalculateThetaAndPsi(const float machinePos[], bool isCoordinated, float& theta, float& psi, bool& armMode) const;
 
 	// Primary parameters
 	float proximalArmLength;
 	float distalArmLength;
 	float thetaLimits[2];							// minimum proximal joint angle
-	float phiLimits[2];								// minimum distal joint angle
+	float psiLimits[2];								// minimum distal joint angle
 	float crosstalk[3];								// if we rotate the distal arm motor, for each full rotation the Z height goes up by this amount
 	float xOffset;									// where bed X=0 is relative to the proximal joint
 	float yOffset;									// where bed Y=0 is relative to the proximal joint
@@ -73,7 +75,8 @@ private:
 	float twoPd;
 
 	// State variables
-	mutable bool isDefaultArmMode;					// this should be moved into class Move when it knows about different arm modes
+	mutable float cachedX, cachedY, cachedTheta, cachedPsi;
+	mutable bool currentArmMode, cachedArmMode;
 };
 
 #endif /* SRC_MOVEMENT_KINEMATICS_SCARAKINEMATICS_H_ */
diff --git a/src/Movement/Move.cpp b/src/Movement/Move.cpp
index 8096ae79..4304db5d 100644
--- a/src/Movement/Move.cpp
+++ b/src/Movement/Move.cpp
@@ -7,13 +7,12 @@
 
 #include "Move.h"
 #include "Platform.h"
-#include "RepRap.h"
-#include "Kinematics/LinearDeltaKinematics.h"	// temporary
 
-Move::Move() : currentDda(nullptr), scheduledMoves(0), completedMoves(0)
-{
-	active = false;
+static constexpr uint32_t UsualMinimumPreparedTime = DDA::stepClockRate/10;			// 100ms
+static constexpr uint32_t AbsoluteMinimumPreparedTime = DDA::stepClockRate/20;		// 50ms
 
+Move::Move() : currentDda(nullptr), active(false), scheduledMoves(0), completedMoves(0)
+{
 	kinematics = Kinematics::Create(KinematicsType::cartesian);			// default to Cartesian
 
 	// Build the DDA ring
@@ -21,7 +20,7 @@ Move::Move() : currentDda(nullptr), scheduledMoves(0), completedMoves(0)
 	ddaRingGetPointer = ddaRingAddPointer = dda;
 	for (size_t i = 1; i < DdaRingLength; i++)
 	{
-		DDA *oldDda = dda;
+		DDA * const oldDda = dda;
 		dda = new DDA(dda);
 		oldDda->SetPrevious(dda);
 	}
@@ -74,13 +73,13 @@ void Move::Init()
 
 	longWait = millis();
 	idleTimeout = DefaultIdleTimeout;
-	iState = IdleState::idle;
+	moveState = MoveState::idle;
 	idleCount = 0;
 
 	simulationMode = 0;
 	simulationTime = 0.0;
 	longestGcodeWaitInterval = 0;
-	waitingForMove = specialMoveAvailable = false;
+	specialMoveAvailable = false;
 
 	active = true;
 }
@@ -119,9 +118,10 @@ void Move::Spin()
 		++idleCount;
 	}
 
-	// Check for DDA errors to print if Move debug is enabled
+	// Recycle the DDAs for completed moves, checking for DDA errors to print if Move debug is enabled
 	while (ddaRingCheckPointer->GetState() == DDA::completed)
 	{
+		// Check for step errors and record/print them if we have any, before we lose the DMs
 		if (ddaRingCheckPointer->HasStepError())
 		{
 			if (reprap.Debug(moduleMove))
@@ -131,6 +131,8 @@ void Move::Spin()
 			++stepErrors;
 			reprap.GetPlatform().LogError(ErrorCode::BadMove);
 		}
+
+		// Now release the DMs and check for underrun
 		if (ddaRingCheckPointer->Free())
 		{
 			++numLookaheadUnderruns;
@@ -139,21 +141,21 @@ void Move::Spin()
 	}
 
 	// See if we can add another move to the ring
-	if (
+	bool canAddMove = (
 #if SUPPORT_ROLAND
-		   !reprap.GetRoland()->Active() &&
+						  !reprap.GetRoland()->Active() &&
 #endif
-		   ddaRingAddPointer->GetState() == DDA::empty
-		&& ddaRingAddPointer->GetNext()->GetState() != DDA::provisional		// function Prepare needs to access the endpoints in the previous move, so don't change them
-		&& DriveMovement::NumFree() >= (int)DRIVES							// check that we won't run out of DMs
-	   )
+						  ddaRingAddPointer->GetState() == DDA::empty
+					   && ddaRingAddPointer->GetNext()->GetState() != DDA::provisional		// function Prepare needs to access the endpoints in the previous move, so don't change them
+					   && DriveMovement::NumFree() >= (int)DRIVES							// check that we won't run out of DMs
+					  );
+	if (canAddMove)
 	{
 		// In order to react faster to speed and extrusion rate changes, only add more moves if the total duration of
-		// all un-frozen moves is less than 2 seconds, or the total duration of all but the first un-frozen move is
-		// less than 0.5 seconds.
+		// all un-frozen moves is less than 2 seconds, or the total duration of all but the first un-frozen move is less than 0.5 seconds.
 		const DDA *dda = ddaRingAddPointer;
-		float unPreparedTime = 0.0;
-		float prevMoveTime = 0.0;
+		uint32_t unPreparedTime = 0;
+		uint32_t prevMoveTime = 0;
 		for(;;)
 		{
 			dda = dda->GetPrevious();
@@ -162,119 +164,124 @@ void Move::Spin()
 				break;
 			}
 			unPreparedTime += prevMoveTime;
-			prevMoveTime = dda->CalcTime();
+			prevMoveTime = dda->GetClocksNeeded();
 		}
 
-		if (unPreparedTime < 0.5 || unPreparedTime + prevMoveTime < 2.0)
+		canAddMove = (unPreparedTime < DDA::stepClockRate/2 || unPreparedTime + prevMoveTime < 2 * DDA::stepClockRate);
+	}
+
+	if (canAddMove)
+	{
+		// OK to add another move. First check if a special move is available.
+		if (specialMoveAvailable)
 		{
-			// First check for a special move
-			if (specialMoveAvailable)
+			if (simulationMode < 2)
 			{
-				if (simulationMode < 2)
+				if (ddaRingAddPointer->Init(specialMoveCoords))
 				{
-					if (ddaRingAddPointer->Init(specialMoveCoords))
+					ddaRingAddPointer = ddaRingAddPointer->GetNext();
+					if (moveState == MoveState::idle || moveState == MoveState::timing)
 					{
-						ddaRingAddPointer = ddaRingAddPointer->GetNext();
-						idleCount = 0;
+						// We were previously idle, so we have a state change
+						moveState = MoveState::collecting;
+						const uint32_t now = millis();
+						const uint32_t timeWaiting = now - lastStateChangeTime;
+						if (timeWaiting > longestGcodeWaitInterval)
+						{
+							longestGcodeWaitInterval = timeWaiting;
+						}
+						lastStateChangeTime = now;
 					}
 				}
-				specialMoveAvailable = false;
 			}
-			else
+			specialMoveAvailable = false;
+		}
+		else
+		{
+			// If there's a G Code move available, add it to the DDA ring for processing.
+			GCodes::RawMove nextMove;
+			if (reprap.GetGCodes().ReadMove(nextMove))		// if we have a new move
 			{
-				// If there's a G Code move available, add it to the DDA ring for processing.
-				GCodes::RawMove nextMove;
-				if (reprap.GetGCodes().ReadMove(nextMove))		// if we have a new move
+				if (simulationMode < 2)		// in simulation mode 2 and higher, we don't process incoming moves beyond this point
 				{
-					if (waitingForMove)
+#if 0	// disabled this because it causes jerky movements on the SCARA printer
+					// Add on the extrusion left over from last time.
+					const size_t numAxes = reprap.GetGCodes().GetTotalAxes();
+					for (size_t drive = numAxes; drive < DRIVES; ++drive)
 					{
-						waitingForMove = false;
-						const uint32_t timeWaiting = millis() - gcodeWaitStartTime;
-						if (timeWaiting > longestGcodeWaitInterval)
-						{
-							longestGcodeWaitInterval = timeWaiting;
-						}
+						nextMove.coords[drive] += extrusionPending[drive - numAxes];
 					}
-					if (simulationMode < 2)		// in simulation mode 2 and higher, we don't process incoming moves beyond this point
-					{
-#if 0	// disabled this because it causes jerky movements on the SCARA printer
-						// Add on the extrusion left over from last time.
-						const size_t numAxes = reprap.GetGCodes().GetTotalAxes();
-						for (size_t drive = numAxes; drive < DRIVES; ++drive)
-						{
-							nextMove.coords[drive] += extrusionPending[drive - numAxes];
-						}
 #endif
-						if (nextMove.moveType == 0)
-						{
-							AxisAndBedTransform(nextMove.coords, nextMove.xAxes, nextMove.yAxes, true);
-						}
-						if (ddaRingAddPointer->Init(nextMove, !IsRawMotorMove(nextMove.moveType)))
-						{
-							ddaRingAddPointer = ddaRingAddPointer->GetNext();
-							idleCount = 0;
-							scheduledMoves++;
-						}
-#if 0	// see above
-						// Save the amount of extrusion not done
-						for (size_t drive = numAxes; drive < DRIVES; ++drive)
+					if (nextMove.moveType == 0)
+					{
+						AxisAndBedTransform(nextMove.coords, nextMove.xAxes, nextMove.yAxes, true);
+					}
+					if (ddaRingAddPointer->Init(nextMove, !IsRawMotorMove(nextMove.moveType)))
+					{
+						ddaRingAddPointer = ddaRingAddPointer->GetNext();
+						idleCount = 0;
+						scheduledMoves++;
+						if (moveState == MoveState::idle || moveState == MoveState::timing)
 						{
-							extrusionPending[drive - numAxes] = nextMove.coords[drive];
+							moveState = MoveState::collecting;
+							const uint32_t now = millis();
+							const uint32_t timeWaiting = now - lastStateChangeTime;
+							if (timeWaiting > longestGcodeWaitInterval)
+							{
+								longestGcodeWaitInterval = timeWaiting;
+							}
+							lastStateChangeTime = now;
 						}
-#endif
 					}
-				}
-				else
-				{
-					// We wanted another move, but none was available
-					if (currentDda != nullptr && !waitingForMove)
+#if 0	// see above
+					// Save the amount of extrusion not done
+					for (size_t drive = numAxes; drive < DRIVES; ++drive)
 					{
-						gcodeWaitStartTime = millis();
-						waitingForMove = true;
+						extrusionPending[drive - numAxes] = nextMove.coords[drive];
 					}
+#endif
 				}
 			}
 		}
 	}
 
+
 	// See whether we need to kick off a move
 	if (currentDda == nullptr)
 	{
 		// No DDA is executing, so start executing a new one if possible
-		if (idleCount > 10)											// better to have a few moves in the queue so that we can do lookahead
+		if (!canAddMove || idleCount > 10)							// better to have a few moves in the queue so that we can do lookahead
 		{
-			DDA *dda = ddaRingGetPointer;
+			// Prepare one move and execute it. We assume that we will enter the next if-block before it completes, giving us time to prepare more moves.
+			Platform::DisableStepInterrupt();						// should be disabled already because we weren't executing a move, but make sure
+			DDA * const dda = ddaRingGetPointer;					// capture volatile variable
 			if (dda->GetState() == DDA::provisional)
 			{
 				dda->Prepare(simulationMode);
-			}
-			if (dda->GetState() == DDA::frozen)
-			{
 				if (simulationMode != 0)
 				{
 					currentDda = dda;								// pretend we are executing this move
 				}
 				else
 				{
-					Platform::DisableStepInterrupt();				// should be disabled already because we weren't executing a move, but make sure
 					if (StartNextMove(Platform::GetInterruptClocks()))	// start the next move
 					{
 						Interrupt();
 					}
 				}
-				iState = IdleState::busy;
+				moveState = MoveState::executing;
 			}
 			else if (simulationMode != 0)
 			{
-				if (iState == IdleState::busy && !reprap.GetGCodes().IsPaused())
+				if (moveState == MoveState::executing && !reprap.GetGCodes().IsPaused())
 				{
-					lastMoveTime = millis();						// record when we first noticed that the machine was idle
-					iState = IdleState::timing;
+					lastStateChangeTime = millis();					// record when we first noticed that the machine was idle
+					moveState = MoveState::timing;
 				}
-				else if (iState == IdleState::timing && millis() - lastMoveTime >= idleTimeout)
+				else if (moveState == MoveState::timing && millis() - lastStateChangeTime >= idleTimeout)
 				{
 					reprap.GetPlatform().SetDriversIdle();			// put all drives in idle hold
-					iState = IdleState::idle;
+					moveState = MoveState::idle;
 				}
 			}
 		}
@@ -283,7 +290,7 @@ void Move::Spin()
 	DDA *cdda = currentDda;											// currentDda is volatile, so copy it
 	if (cdda != nullptr)
 	{
-		// See whether we need to prepare any moves
+		// See whether we need to prepare any moves. First count how many prepared or executing moves we have and how long they will take.
 		int32_t preparedTime = 0;
 		uint32_t preparedCount = 0;
 		DDA::DDAState st;
@@ -298,13 +305,17 @@ void Move::Spin()
 			}
 		}
 
-		// If the number of prepared moves will execute in less than the minimum time, prepare another move
+		// If the number of prepared moves will execute in less than the minimum time, prepare another move.
+		// Try to avoid preparing deceleration-only moves
 		while (st == DDA::provisional
-				&& preparedTime < (int32_t)(DDA::stepClockRate/8)	// prepare moves one eighth of a second ahead of when they will be needed
-				&& preparedCount < DdaRingLength/2					// but don't prepare more than half the ring
+				&& preparedTime < (int32_t)UsualMinimumPreparedTime		// prepare moves one eighth of a second ahead of when they will be needed
+				&& preparedCount < DdaRingLength/2 - 1					// but don't prepare as much as half the ring
 			  )
 		{
-			cdda->Prepare(simulationMode);
+			if (cdda->IsGoodToPrepare() || preparedTime < (int32_t)AbsoluteMinimumPreparedTime)
+			{
+				cdda->Prepare(simulationMode);
+			}
 			preparedTime += cdda->GetTimeLeft();
 			++preparedCount;
 			cdda = cdda->GetNext();
@@ -317,7 +328,7 @@ void Move::Spin()
 			// Simulate completion of the current move
 //DEBUG
 //currentDda->DebugPrint();
-			simulationTime += currentDda->CalcTime();
+			simulationTime += (float)currentDda->GetClocksNeeded()/DDA::stepClockRate;
 			currentDda->Complete();
 			CurrentMoveCompleted();
 		}
@@ -361,13 +372,10 @@ bool Move::IsRawMotorMove(uint8_t moveType) const
 bool Move::IsAccessibleProbePoint(float x, float y) const
 {
 	const ZProbeParameters& params = reprap.GetPlatform().GetCurrentZProbeParameters();
-	return kinematics->IsReachable(x - params.xOffset, y - params.yOffset);
+	return kinematics->IsReachable(x - params.xOffset, y - params.yOffset, false);
 }
 
-// Pause the print as soon as we can, returning true if we are able to.
-// Returns the file position of the first queue move we are going to skip, or noFilePosition we we are not skipping any moves.
-// We update 'positions' to the positions and feed rate expected for the next move, and the amount of extrusion in the moves we skipped.
-// If we are not skipping any moves then the feed rate, virtual extruder position and iobits are left alone, therefore the caller should set them up first.
+// Pause the print as soon as we can, returning true if we are able to skip any moves and updating 'rp' to the first move we skipped.
 bool Move::PausePrint(RestorePoint& rp)
 {
 	// Find a move we can pause after.
@@ -393,7 +401,7 @@ bool Move::PausePrint(RestorePoint& rp)
 	// In general, we can pause after a move if it is the last segment and its end speed is slow enough.
 	// We can pause before a move if it is the first segment in that move.
 
-	const DDA *savedDdaRingAddPointer = ddaRingAddPointer;
+	const DDA * const savedDdaRingAddPointer = ddaRingAddPointer;
 	bool pauseOkHere;
 
 	cpu_irq_disable();
@@ -414,7 +422,6 @@ bool Move::PausePrint(RestorePoint& rp)
 		if (pauseOkHere && dda->CanPauseBefore())
 		{
 			// We can pause before executing this move
-			(void)dda->Free();							// free the DDA we are going to pause before it so that it won't get executed after we enable interrupts
 			ddaRingAddPointer = dda;
 			break;
 		}
@@ -460,7 +467,79 @@ bool Move::PausePrint(RestorePoint& rp)
 	return true;
 }
 
-uint32_t maxReps = 0;
+// Pause the print immediately, returning true if we were able to skip or abort any moves and setting up to the move we aborted
+bool Move::LowPowerPause(RestorePoint& rp)
+{
+	const DDA * const savedDdaRingAddPointer = ddaRingAddPointer;
+	bool abortedMove = false;
+
+	cpu_irq_disable();
+	DDA *dda = currentDda;
+	if (dda != nullptr && dda->GetFilePosition() != noFilePosition)
+	{
+		// We are executing a move that has a file address, so we can interrupt it
+		Platform::DisableStepInterrupt();
+		dda->MoveAborted();
+		CurrentMoveCompleted();							// updates live endpoints, extrusion, ddaRingGetPointer, currentDda etc.
+		--completedMoves;								// this move wasn't really completed
+		abortedMove = true;
+	}
+	else
+	{
+		if (dda == nullptr)
+		{
+			// No move is being executed
+			dda = ddaRingGetPointer;
+		}
+		while (dda != savedDdaRingAddPointer)
+		{
+			if (dda->GetFilePosition() != noFilePosition)
+			{
+				break;									// we can pause before executing this move
+			}
+			dda = dda->GetNext();
+		}
+	}
+
+	cpu_irq_enable();
+
+	if (dda == savedDdaRingAddPointer)
+	{
+		return false;									// we can't skip any moves
+	}
+
+	// We are going to skip some moves, or part of a move.
+	// Store the parameters of the first move we are going to execute when we resume
+	rp.feedRate = dda->GetRequestedSpeed();
+	rp.virtualExtruderPosition = dda->GetVirtualExtruderPosition();
+	rp.filePos = dda->GetFilePosition();
+	rp.proportionDone = dda->GetProportionDone();		// store how much of the complete multi-segment move's extrusion has been done
+
+#if SUPPORT_IOBITS
+	rp.ioBits = dda->GetIoBits();
+#endif
+
+	ddaRingAddPointer = (abortedMove) ? dda->GetNext() : dda;
+
+	// Get the end coordinates of the last move that was or will be completed, or the coordinates of the current move when we aborted it.
+	DDA * const prevDda = ddaRingAddPointer->GetPrevious();
+	const size_t numVisibleAxes = reprap.GetGCodes().GetVisibleAxes();
+	for (size_t axis = 0; axis < numVisibleAxes; ++axis)
+	{
+		rp.moveCoords[axis] = prevDda->GetEndCoordinate(axis, false);
+	}
+
+	InverseAxisAndBedTransform(rp.moveCoords, prevDda->GetXAxes(), prevDda->GetYAxes());	// we assume that xAxes and yAxes have't changed between the moves
+
+	// Free the DDAs for the moves we are going to skip
+	for (dda = ddaRingAddPointer; dda != savedDdaRingAddPointer; dda = dda->GetNext())
+	{
+		(void)dda->Free();
+		scheduledMoves--;
+	}
+
+	return true;
+}
 
 #if 0
 // For debugging
@@ -473,8 +552,8 @@ void Move::Diagnostics(MessageType mtype)
 	Platform& p = reprap.GetPlatform();
 	p.Message(mtype, "=== Move ===\n");
 	p.MessageF(mtype, "MaxReps: %" PRIu32 ", StepErrors: %u, FreeDm: %d, MinFreeDm %d, MaxWait: %" PRIu32 "ms, Underruns: %u, %u\n",
-						maxReps, stepErrors, DriveMovement::NumFree(), DriveMovement::MinFree(), longestGcodeWaitInterval, numLookaheadUnderruns, numPrepareUnderruns);
-	maxReps = 0;
+						DDA::maxReps, stepErrors, DriveMovement::NumFree(), DriveMovement::MinFree(), longestGcodeWaitInterval, numLookaheadUnderruns, numPrepareUnderruns);
+	DDA::maxReps = 0;
 	numLookaheadUnderruns = numPrepareUnderruns = 0;
 	longestGcodeWaitInterval = 0;
 	DriveMovement::ResetMinFree();
@@ -584,10 +663,10 @@ void Move::MotorStepsToCartesian(const int32_t motorPos[], size_t numVisibleAxes
 
 // Convert Cartesian coordinates to motor steps, axes only, returning true if successful.
 // Used to perform movement and G92 commands.
-bool Move::CartesianToMotorSteps(const float machinePos[MaxAxes], int32_t motorPos[MaxAxes], bool allowModeChange) const
+bool Move::CartesianToMotorSteps(const float machinePos[MaxAxes], int32_t motorPos[MaxAxes], bool isCoordinated) const
 {
 	const bool b = kinematics->CartesianToMotorSteps(machinePos, reprap.GetPlatform().GetDriveStepsPerUnit(),
-														reprap.GetGCodes().GetVisibleAxes(), reprap.GetGCodes().GetTotalAxes(), motorPos, allowModeChange);
+														reprap.GetGCodes().GetVisibleAxes(), reprap.GetGCodes().GetTotalAxes(), motorPos, isCoordinated);
 	if (reprap.Debug(moduleMove) && reprap.Debug(moduleDda))
 	{
 		if (b)
@@ -861,9 +940,9 @@ void Move::AdjustMotorPositions(const float_t adjustment[], size_t numMotors)
 	const int32_t * const endCoordinates = lastQueuedMove->DriveCoordinates();
 	const float * const driveStepsPerUnit = reprap.GetPlatform().GetDriveStepsPerUnit();
 
-	for (size_t drive = 0; drive < DELTA_AXES; ++drive)
+	for (size_t drive = 0; drive < numMotors; ++drive)
 	{
-		const int32_t ep = endCoordinates[drive] + (int32_t)(adjustment[drive] * driveStepsPerUnit[drive]);
+		const int32_t ep = endCoordinates[drive] + lrintf(adjustment[drive] * driveStepsPerUnit[drive]);
 		lastQueuedMove->SetDriveCoordinate(ep, drive);
 		liveEndPoints[drive] = ep;
 	}
diff --git a/src/Movement/Move.h b/src/Movement/Move.h
index ac145c41..38e55e5e 100644
--- a/src/Movement/Move.h
+++ b/src/Movement/Move.h
@@ -21,7 +21,7 @@
 // Each DDA needs one DM per drive that it moves.
 // However, DM's are large, so we provide fewer than DRIVES * DdaRingLength of them. The planner checks that enough DMs are available before filling in a new DDA.
 
-#ifdef DUET_NG
+#if SAM4E || SAM4S
 const unsigned int DdaRingLength = 30;
 const unsigned int NumDms = DdaRingLength * 8;						// suitable for e.g. a delta + 5 input hot end
 #else
@@ -46,10 +46,9 @@ public:
 																	// Return the position (after all queued moves have been executed) in transformed coords
 	int32_t GetEndPoint(size_t drive) const { return liveEndPoints[drive]; } 	// Get the current position of a motor
 	void LiveCoordinates(float m[DRIVES], AxesBitmap xAxes, AxesBitmap yAxes);	// Gives the last point at the end of the last complete DDA transformed to user coords
-	void Interrupt();												// The hardware's (i.e. platform's)  interrupt should call this.
-	void InterruptTime();											// Test function - not used
+	void Interrupt() __attribute__ ((hot));							// The hardware's (i.e. platform's)  interrupt should call this.
 	bool AllMovesAreFinished();										// Is the look-ahead ring empty?  Stops more moves being added as well.
-	void DoLookAhead();												// Run the look-ahead procedure
+	void DoLookAhead() __attribute__ ((hot));						// Run the look-ahead procedure
 	void SetNewPosition(const float positionNow[DRIVES], bool doBedCompensation); // Set the current position to be this
 	void SetLiveCoordinates(const float coords[DRIVES]);			// Force the live coordinates (see above) to be these
 	void ResetExtruderPositions();									// Resets the extrusion amounts of the live coordinates
@@ -75,7 +74,7 @@ public:
 	// Kinematics and related functions
 	Kinematics& GetKinematics() const { return *kinematics; }
 	bool SetKinematics(KinematicsType k);											// Set kinematics, return true if successful
-	bool CartesianToMotorSteps(const float machinePos[MaxAxes], int32_t motorPos[MaxAxes], bool allowModeChange) const;
+	bool CartesianToMotorSteps(const float machinePos[MaxAxes], int32_t motorPos[MaxAxes], bool isCoordinated) const;
 																					// Convert Cartesian coordinates to delta motor coordinates, return true if successful
 	void MotorStepsToCartesian(const int32_t motorPos[], size_t numVisibleAxes, size_t numTotalAxes, float machinePos[]) const;
 																					// Convert motor coordinates to machine coordinates
@@ -90,8 +89,8 @@ public:
 
 	bool IsRawMotorMove(uint8_t moveType) const;									// Return true if this is a raw motor move
 
-	void CurrentMoveCompleted();													// Signal that the current move has just been completed
-	bool TryStartNextMove(uint32_t startTime);										// Try to start another move, returning true if Step() needs to be called immediately
+	void CurrentMoveCompleted() __attribute__ ((hot));								// Signal that the current move has just been completed
+	bool TryStartNextMove(uint32_t startTime) __attribute__ ((hot));				// Try to start another move, returning true if Step() needs to be called immediately
 	float IdleTimeout() const;														// Returns the idle timeout in seconds
 	void SetIdleTimeout(float timeout);												// Set the idle timeout in seconds
 
@@ -100,6 +99,8 @@ public:
 	void PrintCurrentDda() const;													// For debugging
 
 	bool PausePrint(RestorePoint& rp);												// Pause the print as soon as we can, returning true if we were able to
+	bool LowPowerPause(RestorePoint& rp);											// Pause the print immediately, returning true if we were able to
+
 	bool NoLiveMovement() const;													// Is a move running, or are there any queued?
 
 	bool IsExtruding() const;														// Is filament being extruded?
@@ -122,9 +123,15 @@ public:
 	static float MotorEndpointToPosition(int32_t endpoint, size_t drive);			// Convert number of motor steps to motor position
 
 private:
-	enum class IdleState : uint8_t { idle, busy, timing };
+	enum class MoveState : uint8_t
+	{
+		idle,			// no moves being executed or in queue, motors are at idle hold
+		collecting,		// no moves currently being executed but we are collecting moves ready to execute them
+		executing,		// we are executing moves
+		timing			// no moves being executed or in queue, motors are at full current
+	};
 
-	bool StartNextMove(uint32_t startTime);														// Start the next move, returning true if Step() needs to be called immediately
+	bool StartNextMove(uint32_t startTime) __attribute__ ((hot));								// Start the next move, returning true if Step() needs to be called immediately
 	void BedTransform(float move[MaxAxes], AxesBitmap xAxes, AxesBitmap yAxes) const;			// Take a position and apply the bed compensations
 	void InverseBedTransform(float move[MaxAxes], AxesBitmap xAxes, AxesBitmap yAxes) const;	// Go from a bed-transformed point back to user coordinates
 	void AxisTransform(float move[MaxAxes], AxesBitmap xAxes, AxesBitmap yAxes) const;			// Take a position and apply the axis-angle compensations
@@ -142,12 +149,12 @@ private:
 
 	bool active;										// Are we live and running?
 	uint8_t simulationMode;								// Are we simulating, or really printing?
-	bool waitingForMove;								// True if we are waiting for a new move
+	MoveState moveState;								// whether the idle timer is active
+
 	unsigned int numLookaheadUnderruns;					// How many times we have run out of moves to adjust during lookahead
 	unsigned int numPrepareUnderruns;					// How many times we wanted a new move but there were only un-prepared moves in the queue
 	unsigned int idleCount;								// The number of times Spin was called and had no new moves to process
-	uint32_t longestGcodeWaitInterval;					// the longest we had to wait for a new gcode
-	uint32_t gcodeWaitStartTime;						// When we last asked for a gcode and didn't get one
+	uint32_t longestGcodeWaitInterval;					// the longest we had to wait for a new GCode
 	float simulationTime;								// Print time since we started simulating
 
 	float extrusionPending[MaxExtruders];				// Extrusion not done due to rounding to nearest step
@@ -170,9 +177,8 @@ private:
 	float taperHeight;									// Height over which we taper
 
 	uint32_t idleTimeout;								// How long we wait with no activity before we reduce motor currents to idle, in milliseconds
-	uint32_t lastMoveTime;								// The approximate time at which the last move was completed
+	uint32_t lastStateChangeTime;						// The approximate time at which the state last changed, except we don't record timing->idle
 	uint32_t longWait;									// A long time for things that need to be done occasionally
-	IdleState iState;									// whether the idle timer is active
 
 	Kinematics *kinematics;								// What kinematics we are using
 
diff --git a/src/Platform.cpp b/src/Platform.cpp
index d1b72425..b9a79ad4 100644
--- a/src/Platform.cpp
+++ b/src/Platform.cpp
@@ -31,14 +31,18 @@
 #include "Scanner.h"
 #include "Version.h"
 #include "SoftTimer.h"
+#include "Logger.h"
 #include "Libraries/Math/Isqrt.h"
 
 #include "sam/drivers/tc/tc.h"
 #include "sam/drivers/hsmci/hsmci.h"
 #include "sd_mmc.h"
 
-#ifdef DUET_NG
+#if HAS_SMART_DRIVERS
 # include "TMC2660.h"
+#endif
+
+#ifdef DUET_NG
 # include "FirmwareUpdater.h"
 #endif
 
@@ -52,7 +56,7 @@ extern "C" char *sbrk(int i);
 # error Missing feature definition
 #endif
 
-#ifdef DUET_NG
+#if HAS_VOLTAGE_MONITOR
 
 inline constexpr float AdcReadingToPowerVoltage(uint16_t adcVal)
 {
@@ -254,10 +258,12 @@ bool ZProbeParameters::WriteParameters(FileStore *f, unsigned int probeType) con
 //*************************************************************************************************
 // Platform class
 
+uint8_t Platform::softwareResetDebugInfo = 0;			// extra info for debugging
+
 Platform::Platform() :
-		logger(nullptr), lastLogFlushTime(0), board(DEFAULT_BOARD_TYPE), active(false), errorCodeBits(0), lastFanCheckTime(0),
+		logger(nullptr), board(DEFAULT_BOARD_TYPE), active(false), errorCodeBits(0), lastFanCheckTime(0),
 		auxGCodeReply(nullptr), fileStructureInitialised(false), tickState(0), debugCode(0)
-#ifdef DUET_NG
+#if HAS_SMART_DRIVERS
 		, lastWarningMillis(0), nextDriveToPoll(0),
 		onBoardDriversFanRunning(false), offBoardDriversFanRunning(false), onBoardDriversFanStartMillis(0), offBoardDriversFanStartMillis(0)
 #endif
@@ -411,6 +417,7 @@ void Platform::Init()
 	// AXES
 	ARRAY_INIT(axisMaxima, AXIS_MAXIMA);
 	ARRAY_INIT(axisMinima, AXIS_MINIMA);
+	axisMaximaProbed = axisMinimaProbed = 0;
 
 	idleCurrentFactor = DefaultIdleCurrentFactor;
 
@@ -436,7 +443,7 @@ void Platform::Init()
 
 	for (size_t drive = 0; drive < DRIVES; drive++)
 	{
-		enableValues[drive] = false;				// assume active low enable signal
+		enableValues[drive] = 0;					// assume active low enable signal
 		directions[drive] = true;					// drive moves forwards by default
 
 		// Map axes and extruders straight through
@@ -501,22 +508,23 @@ void Platform::Init()
 	switch (expansionBoard)
 	{
 	case ExpansionBoardType::DueX2:
-		numTMC2660Drivers = 7;
+		numSmartDrivers = 7;
 		break;
 	case ExpansionBoardType::DueX5:
-		numTMC2660Drivers = 10;
+		numSmartDrivers = 10;
 		break;
 	case ExpansionBoardType::none:
 	case ExpansionBoardType::DueX0:
 	default:
-		numTMC2660Drivers = 5;									// assume that additional drivers are dumb enable/step/dir ones
+		numSmartDrivers = 5;									// assume that any additional drivers are dumb enable/step/dir ones
 		break;
 	}
 	DuetExpansion::AdditionalOutputInit();
 
 	// Initialise TMC2660 driver module
 	driversPowered = false;
-	TMC2660::Init(ENABLE_PINS, numTMC2660Drivers);
+	SmartDrivers::Init(ENABLE_PINS, numSmartDrivers);
+	pauseOnStallDrivers = rehomeOnStallDrivers = 0;
 
 	// Set up the VSSA sense pin. Older Duet WiFis don't have it connected, so we enable the pulldown resistor to keep it inactive.
 	{
@@ -559,7 +567,13 @@ void Platform::Init()
 	{
 		if (heatOnPins[heater] != NoPin)
 		{
-			pinMode(heatOnPins[heater], (HEAT_ON) ? OUTPUT_LOW : OUTPUT_HIGH);
+			pinMode(heatOnPins[heater],
+#if ACTIVE_LOW_HEAT_ON
+				OUTPUT_LOW
+#else
+				OUTPUT_HIGH
+#endif
+			);
 		}
 		AnalogChannelNumber chan = PinToAdcChannel(tempSensePins[heater]);	// translate the Arduino Due Analog pin number to the SAM ADC channel number
 		pinMode(tempSensePins[heater], AIN);
@@ -611,10 +625,10 @@ void Platform::Init()
 	AnalogInEnableChannel(temperatureAdcChannel, true);
 	highestMcuTemperature = 0;									// the highest output we have seen from the ADC filter
 	lowestMcuTemperature = 4095 * ThermistorAverageReadings;	// the lowest output we have seen from the ADC filter
-#endif
 	mcuTemperatureAdjust = 0.0;
+#endif
 
-#ifdef DUET_NG
+#if HAS_VOLTAGE_MONITOR
 	// Power monitoring
 	vInMonitorAdcChannel = PinToAdcChannel(PowerMonitorVinDetectPin);
 	pinMode(PowerMonitorVinDetectPin, AIN);
@@ -1329,7 +1343,7 @@ void Platform::Spin()
 		for (;;) {}
 	}
 
-#ifdef DUET_NG
+#if HAS_SMART_DRIVERS
 	// Check whether the TMC drivers need to be initialised.
 	// The tick ISR also looks for over-voltage events, but it just disables the driver without changing driversPowerd or numOverVoltageEvents
 	if (driversPowered)
@@ -1347,44 +1361,46 @@ void Platform::Spin()
 		else
 		{
 			// Poll one TMC2660 for temperature warning or temperature shutdown
-			const uint32_t stat = TMC2660::GetStatus(nextDriveToPoll);
-			const uint16_t mask = 1 << nextDriveToPoll;
-			if (stat & TMC_RR_OT)
-			{
-				temperatureShutdownDrivers |= mask;
-				temperatureWarningDrivers &= ~mask;			// no point in setting warning as well as error
-			}
-			else
+			if (enableValues[nextDriveToPoll] >= 0)				// don't poll driver if it is flagged "no poll"
 			{
-				temperatureShutdownDrivers &= ~mask;
-				if (stat & TMC_RR_OTPW)
+				const uint32_t stat = SmartDrivers::GetStatus(nextDriveToPoll);
+				const DriversBitmap mask = MakeBitmap<DriversBitmap>(nextDriveToPoll);
+				if (stat & TMC_RR_OT)
 				{
-					temperatureWarningDrivers |= mask;
+					temperatureShutdownDrivers |= mask;
+					temperatureWarningDrivers &= ~mask;			// no point in setting warning as well as error
 				}
 				else
 				{
-					temperatureWarningDrivers &= ~mask;
+					temperatureShutdownDrivers &= ~mask;
+					if (stat & TMC_RR_OTPW)
+					{
+						temperatureWarningDrivers |= mask;
+					}
+					else
+					{
+						temperatureWarningDrivers &= ~mask;
+					}
+				}
+				if (stat & TMC_RR_S2G)
+				{
+					shortToGroundDrivers |= mask;
+				}
+				else
+				{
+					shortToGroundDrivers &= ~mask;
+				}
+				if ((stat & (TMC_RR_OLA | TMC_RR_OLB)) != 0 && (stat & TMC_RR_STST) == 0)
+				{
+					openLoadDrivers |= mask;
+				}
+				else
+				{
+					openLoadDrivers &= ~mask;
 				}
 			}
-			if (stat & TMC_RR_S2G)
-			{
-				shortToGroundDrivers |= mask;
-			}
-			else
-			{
-				shortToGroundDrivers &= ~mask;
-			}
-			if ((stat & (TMC_RR_OLA | TMC_RR_OLB)) != 0 && (stat & TMC_RR_STST) == 0)
-			{
-				openLoadDrivers |= mask;
-			}
-			else
-			{
-				openLoadDrivers &= ~mask;
-			}
-
 			++nextDriveToPoll;
-			if (nextDriveToPoll > numTMC2660Drivers)
+			if (nextDriveToPoll > numSmartDrivers)
 			{
 				nextDriveToPoll = 0;
 			}
@@ -1394,7 +1410,7 @@ void Platform::Spin()
 	{
 		driversPowered = true;
 	}
-	TMC2660::SetDriversPowered(driversPowered);
+	SmartDrivers::SetDriversPowered(driversPowered);
 #endif
 
 	const uint32_t now = millis();
@@ -1418,7 +1434,7 @@ void Platform::Spin()
 			fans[fan].Check();
 		}
 
-#ifdef DUET_NG
+#if HAS_SMART_DRIVERS
 		// Check whether it is time to report any faults (do this after checking fans in case driver cooling fans are turned on)
 		if (now - lastWarningMillis > MinimumWarningInterval)
 		{
@@ -1457,7 +1473,7 @@ void Platform::Spin()
 #endif
 	}
 
-#ifdef DUET_NG
+#if HAS_VOLTAGE_MONITOR
 	// Check for auto-pause, shutdown or resume
 	if (autoSaveEnabled)
 	{
@@ -1473,7 +1489,7 @@ void Platform::Spin()
 		case AutoSaveState::normal:
 			if (currentVin < autoPauseReading)
 			{
-				if (reprap.GetGCodes().AutoPause())
+				if (reprap.GetGCodes().LowVoltagePause())
 				{
 					autoSaveState = AutoSaveState::autoPaused;
 				}
@@ -1481,26 +1497,9 @@ void Platform::Spin()
 			break;
 
 		case AutoSaveState::autoPaused:
-			if (currentVin < autoShutdownReading)
-			{
-				if (reprap.GetGCodes().AutoShutdown())
-				{
-					autoSaveState = AutoSaveState::autoShutdown;
-				}
-			}
-			else if (currentVin >= autoResumeReading)
-			{
-				if (reprap.GetGCodes().AutoResume())
-				{
-					autoSaveState = AutoSaveState::normal;
-				}
-			}
-			break;
-
-		case AutoSaveState::autoShutdown:
 			if (currentVin >= autoResumeReading)
 			{
-				if (reprap.GetGCodes().AutoResumeAfterShutdown())
+				if (reprap.GetGCodes().LowVoltageResume())
 				{
 					autoSaveState = AutoSaveState::normal;
 				}
@@ -1516,21 +1515,17 @@ void Platform::Spin()
 	// Flush the log file it it is time. This may take some time, so do it last.
 	if (logger != nullptr)
 	{
-		if (now - lastLogFlushTime >= LogFlushInterval)
-		{
-			logger->Flush();
-			lastLogFlushTime = now;
-		}
+		logger->Flush();
 	}
 
 	ClassReport(longWait);
 }
 
-#ifdef DUET_NG
+#if HAS_SMART_DRIVERS
 
 // Report driver status conditions that require attention.
 // Sets 'reported' if we reported anything, else leaves 'reported' alone.
-void Platform::ReportDrivers(uint16_t whichDrivers, const char* text, bool& reported)
+void Platform::ReportDrivers(DriversBitmap whichDrivers, const char* text, bool& reported)
 {
 	if (whichDrivers != 0)
 	{
@@ -1548,36 +1543,35 @@ void Platform::ReportDrivers(uint16_t whichDrivers, const char* text, bool& repo
 	}
 }
 
-// Configure auto save on power fail
-bool Platform::ConfigureAutoSave(GCodeBuffer& gb, StringRef& reply)
+#endif
+
+#if HAS_VOLTAGE_MONITOR
+
+void Platform::DisableAutoSave()
 {
-	bool seen = false;
-	float autoSaveVoltages[3];
-	if (gb.TryGetFloatArray('S', 3, autoSaveVoltages, reply, seen))
-	{
-		return true;
-	}
+	autoSaveEnabled = false;
+}
 
-	if (seen)
-	{
-		autoSaveEnabled = (autoSaveVoltages[0] >= 5.0 && autoSaveVoltages[1] > autoSaveVoltages[0] && autoSaveVoltages[2] > autoSaveVoltages[1]);
-		if (autoSaveEnabled)
-		{
-			autoShutdownReading = PowerVoltageToAdcReading(autoSaveVoltages[0]);
-			autoPauseReading = PowerVoltageToAdcReading(autoSaveVoltages[1]);
-			autoResumeReading = PowerVoltageToAdcReading(autoSaveVoltages[2]);
-		}
-	}
-	else if (!autoSaveEnabled)
-	{
-		reply.copy("Auto save is disabled");
-	}
-	else
+bool Platform::IsPowerOk() const
+{
+	return currentVin > autoPauseReading;
+}
+
+void Platform::EnableAutoSave(float saveVoltage, float resumeVoltage)
+{
+	autoPauseReading = PowerVoltageToAdcReading(saveVoltage);
+	autoResumeReading = PowerVoltageToAdcReading(resumeVoltage);
+	autoSaveEnabled = true;
+}
+
+bool Platform::GetAutoSaveSettings(float& saveVoltage, float&resumeVoltage)
+{
+	if (autoSaveEnabled)
 	{
-		reply.printf(" Auto shutdown at %.1fV, save/pause at %.1fV, resume at %.1fV",
-			(double)AdcReadingToPowerVoltage(autoShutdownReading), (double)AdcReadingToPowerVoltage(autoPauseReading), (double)AdcReadingToPowerVoltage(autoResumeReading));
+		saveVoltage = AdcReadingToPowerVoltage(autoPauseReading);
+		resumeVoltage = AdcReadingToPowerVoltage(autoResumeReading);
 	}
-	return false;
+	return autoSaveEnabled;
 }
 
 #endif
@@ -1593,16 +1587,22 @@ bool Platform::WriteFanSettings(FileStore *f) const
 	return ok;
 }
 
+#if HAS_CPU_TEMP_SENSOR
+
 float Platform::AdcReadingToCpuTemperature(uint32_t adcVal) const
 {
-	float voltage = (float)adcVal * (3.3/(float)(4096 * ThermistorAverageReadings));
-#if SAM4E
+	const float voltage = (float)adcVal * (3.3/(float)(4096 * ThermistorAverageReadings));
+#if SAM4E || SAM4S
 	return (voltage - 1.44) * (1000.0/4.7) + 27.0 + mcuTemperatureAdjust;			// accuracy at 27C is +/-13C
 #elif SAM3XA
 	return (voltage - 0.8) * (1000.0/2.65) + 27.0 + mcuTemperatureAdjust;			// accuracy at 27C is +/-45C
+#else
+# error undefined CPU temp conversion
 #endif
 }
 
+#endif
+
 // Perform a software reset. 'stk' points to the program counter on the stack if the cause is an exception, otherwise it is nullptr.
 void Platform::SoftwareReset(uint16_t reason, const uint32_t *stk)
 {
@@ -1635,17 +1635,20 @@ void Platform::SoftwareReset(uint16_t reason, const uint32_t *stk)
 				reason |= (uint16_t)SoftwareResetReason::inAuxOutput;	// if we are resetting because we are stuck in a Spin function, record whether we are trying to send to aux
 			}
 		}
-		reason |= (uint16_t)reprap.GetSpinningModule();
+		reason |= (uint8_t)reprap.GetSpinningModule();
+		reason |= (softwareResetDebugInfo & 0x07) << 5;
 
 		// Record the reason for the software reset
 		// First find a free slot (wear levelling)
 		size_t slot = SoftwareResetData::numberOfSlots;
 		SoftwareResetData srdBuf[SoftwareResetData::numberOfSlots];
 
-#if SAM4E
+#if SAM4E || SAM4S
 		if (flash_read_user_signature(reinterpret_cast<uint32_t*>(srdBuf), sizeof(srdBuf)/sizeof(uint32_t)) == FLASH_RC_OK)
-#else
+#elif SAM3XA
 		DueFlashStorage::read(SoftwareResetData::nvAddress, srdBuf, sizeof(srdBuf));
+#else
+# error
 #endif
 		{
 			while (slot != 0 && srdBuf[slot - 1].isVacant())
@@ -1657,7 +1660,7 @@ void Platform::SoftwareReset(uint16_t reason, const uint32_t *stk)
 		if (slot == SoftwareResetData::numberOfSlots)
 		{
 			// No free slots, so erase the area
-#if SAM4E
+#if SAM4E || SAM4S
 			flash_erase_user_signature();
 #endif
 			memset(srdBuf, 0xFF, sizeof(srdBuf));
@@ -1680,7 +1683,7 @@ void Platform::SoftwareReset(uint16_t reason, const uint32_t *stk)
 		}
 
 		// Save diagnostics data to Flash
-#if SAM4E
+#if SAM4E || SAM4S
 		flash_write_user_signature(srdBuf, sizeof(srdBuf)/sizeof(uint32_t));
 #else
 		DueFlashStorage::write(SoftwareResetData::nvAddress, srdBuf, sizeof(srdBuf));
@@ -1696,11 +1699,13 @@ void Platform::SoftwareReset(uint16_t reason, const uint32_t *stk)
 // Interrupts
 
 #if HAS_LWIP_NETWORKING
+
 void NETWORK_TC_HANDLER()
 {
 	tc_get_status(NETWORK_TC, NETWORK_TC_CHAN);
 	reprap.GetNetwork().Interrupt();
 }
+
 #endif
 
 static void FanInterrupt(void*)
@@ -1720,7 +1725,7 @@ void Platform::InitialiseInterrupts()
 	// Set the tick interrupt to the highest priority. We need to to monitor the heaters and kick the watchdog.
 	NVIC_SetPriority(SysTick_IRQn, NvicPrioritySystick);	// set priority for tick interrupts
 
-#if SAM4E
+#if SAM4E || SAM4S
 	NVIC_SetPriority(UART0_IRQn, NvicPriorityUart);			// set priority for UART interrupt - must be higher than step interrupt
 #else
 	NVIC_SetPriority(UART_IRQn, NvicPriorityUart);			// set priority for UART interrupt - must be higher than step interrupt
@@ -1763,11 +1768,12 @@ void Platform::InitialiseInterrupts()
 	NVIC_SetPriority(PIOE_IRQn, NvicPriorityPins);
 #endif
 
-#if SAM3XA
-	NVIC_SetPriority(UOTGHS_IRQn, NvicPriorityUSB);
-#endif
-#if SAM4E
+#if SAM4E || SAM4S
 	NVIC_SetPriority(UDP_IRQn, NvicPriorityUSB);
+#elif SAM3XA
+	NVIC_SetPriority(UOTGHS_IRQn, NvicPriorityUSB);
+#else
+# error
 #endif
 	NVIC_SetPriority(TWI1_IRQn, NvicPriorityTwi);
 
@@ -1804,7 +1810,7 @@ void Platform::Diagnostics(MessageType mtype)
 	MessageF(mtype, "%s version %s running on %s", FIRMWARE_NAME, VERSION, GetElectronicsString());
 
 #ifdef DUET_NG
-	const char* expansionName = DuetExpansion::GetExpansionBoardName();
+	const char* const expansionName = DuetExpansion::GetExpansionBoardName();
 	if (expansionName != nullptr)
 	{
 		MessageF(mtype, " + %s", expansionName);
@@ -1813,7 +1819,7 @@ void Platform::Diagnostics(MessageType mtype)
 
 	Message(mtype, "\n");
 
-#ifdef DUET_NG
+#if SAM4E || SAM4S
 	// Print the unique ID
 	{
 		uint32_t idBuf[5];
@@ -1883,7 +1889,7 @@ void Platform::Diagnostics(MessageType mtype)
 
 	// Print memory stats and error codes to USB and copy them to the current webserver reply
 	const char *ramstart =
-#if SAM4E
+#if SAM4E || SAM4S
 			(char *) 0x20000000;
 #elif SAM3XA
 			(char *) 0x20070000;
@@ -1902,7 +1908,7 @@ void Platform::Diagnostics(MessageType mtype)
 	// Show the up time and reason for the last reset
 	const uint32_t now = (uint32_t)(millis64()/1000u);		// get up time in seconds
 	const char* resetReasons[8] = { "power up", "backup", "watchdog", "software",
-#if SAM4E
+#if SAM4E || SAM4S
 	// On the SAM4E a watchdog reset may be reported as a user reset because of the capacitor on the NRST pin
 									"reset button or watchdog",
 #else
@@ -1919,7 +1925,7 @@ void Platform::Diagnostics(MessageType mtype)
 		memset(srdBuf, 0, sizeof(srdBuf));
 		int slot = -1;
 
-#if SAM4E
+#if SAM4E || SAM4S
 		// Work around bug in ASF flash library: flash_read_user_signature calls a RAMFUNC without disabling interrupts first.
 		// This caused a crash (watchdog timeout) sometimes if we run M122 while a print is in progress
 		const irqflags_t flags = cpu_irq_save();
@@ -2002,7 +2008,7 @@ void Platform::Diagnostics(MessageType mtype)
 	lowestMcuTemperature = highestMcuTemperature = currentMcuTemperature;
 #endif
 
-#ifdef DUET_NG
+#if HAS_VOLTAGE_MONITOR
 	// Show the supply voltage
 	MessageF(mtype, "Supply voltage: min %.1f, current %.1f, max %.1f, under voltage events: %" PRIu32 ", over voltage events: %" PRIu32 "\n",
 		(double)AdcReadingToPowerVoltage(lowestVin), (double)AdcReadingToPowerVoltage(currentVin), (double)AdcReadingToPowerVoltage(highestVin),
@@ -2016,9 +2022,9 @@ void Platform::Diagnostics(MessageType mtype)
 		MessageF(mtype, "Expansion motor(s) stall indication: %s\n", (stalled) ? "yes" : "no");
 	}
 
-	for (size_t drive = 0; drive < numTMC2660Drivers; ++drive)
+	for (size_t drive = 0; drive < numSmartDrivers; ++drive)
 	{
-		const uint32_t stat = TMC2660::GetStatus(drive);
+		const uint32_t stat = SmartDrivers::GetStatus(drive);
 		MessageF(mtype, "Driver %d:%s%s%s%s%s%s\n", drive,
 						(stat & TMC_RR_SG) ? " stalled" : "",
 						(stat & TMC_RR_OT) ? " temperature-shutdown!"
@@ -2097,10 +2103,12 @@ void Platform::DiagnosticTest(int d)
 
 	case (int)DiagnosticTestType::BusFault:
 		// Read from the "Undefined (Abort)" area
-#ifdef DUET_NG
+#if SAM4E || SAM4S
 		(void)RepRap::ReadDword(reinterpret_cast<const char*>(0x20800000));
-#else
+#elif SAM3XA
 		(void)RepRap::ReadDword(reinterpret_cast<const char*>(0x20200000));
+#else
+# error
 #endif
 		break;
 
@@ -2168,7 +2176,8 @@ void Platform::ClassReport(uint32_t &lastTime)
 	}
 }
 
-#ifdef DUET_NG
+#if HAS_SMART_DRIVERS
+
 // This is called when a fan that monitors driver temperatures is turned on when it was off
 void Platform::DriverCoolingFansOn(uint32_t driverChannelsMonitored)
 {
@@ -2183,6 +2192,7 @@ void Platform::DriverCoolingFansOn(uint32_t driverChannelsMonitored)
 		offBoardDriversFanRunning = true;
 	}
 }
+
 #endif
 
 // Power is a fraction in [0,1]
@@ -2191,7 +2201,13 @@ void Platform::SetHeater(size_t heater, float power)
 	if (heatOnPins[heater] != NoPin)
 	{
 		const uint16_t freq = (reprap.GetHeat().UseSlowPwm(heater)) ? SlowHeaterPwmFreq : NormalHeaterPwmFreq;
-		IoPort::WriteAnalog(heatOnPins[heater], (HEAT_ON) ? power : 1.0 - power, freq);
+		const float pwm =
+#if ACTIVE_LOW_HEAT_ON
+			1.0 - power;
+#else
+			power;
+#endif
+		IoPort::WriteAnalog(heatOnPins[heater], pwm, freq);
 	}
 }
 
@@ -2277,10 +2293,20 @@ EndStopHit Platform::GetZProbeResult() const
 				: EndStopHit::noStop;
 }
 
-// Write the Z probe parameters to file
-bool Platform::WriteZProbeParameters(FileStore *f) const
+// Write the platform parameters to file
+bool Platform::WritePlatformParameters(FileStore *f) const
 {
-	bool ok = f->Write("; Z probe parameters\n");
+	bool ok = WriteAxisLimits(f, axisMinimaProbed, axisMinima, 1);
+	if (ok)
+	{
+		ok = WriteAxisLimits(f, axisMaximaProbed, axisMaxima, 0);
+	}
+
+#if 0	// From version 1.20 we no longer write the Z probe parameters, but keep the code for now in case too many users complain
+	if (ok)
+	{
+		ok = f->Write("; Z probe parameters\n");
+	}
 	if (ok)
 	{
 		ok = irZProbeParameters.WriteParameters(f, 1);
@@ -2293,9 +2319,27 @@ bool Platform::WriteZProbeParameters(FileStore *f) const
 	{
 		ok = switchZProbeParameters.WriteParameters(f, 4);
 	}
+#endif
+
 	return ok;
 }
 
+bool Platform::WriteAxisLimits(FileStore *f, AxesBitmap axesProbed, const float limits[MaxAxes], int sParam)
+{
+	if (axesProbed == 0)
+	{
+		return true;
+	}
+
+	scratchString.printf("M208 S%d", sParam);
+	for (size_t axis = 0; axis < MaxAxes && IsBitSet(axesProbed, axis); ++axis)
+	{
+		scratchString.catf(" %c%.2f", GCodes::axisLetters[axis], (double)limits[axis]);
+	}
+	scratchString.cat('\n');
+	return f->Write(scratchString.Pointer());
+}
+
 // This is called from the step ISR as well as other places, so keep it fast
 // If drive >= DRIVES then we are setting an individual motor direction
 void Platform::SetDirection(size_t drive, bool direction)
@@ -2326,16 +2370,16 @@ void Platform::EnableDriver(size_t driver)
 		driverState[driver] = DriverStatus::enabled;
 		UpdateMotorCurrent(driver);						// the current may have been reduced by the idle timeout
 
-#if defined(DUET_NG)
-		if (driver < numTMC2660Drivers)
+#if HAS_SMART_DRIVERS
+		if (driver < numSmartDrivers)
 		{
-			TMC2660::EnableDrive(driver, true);
+			SmartDrivers::EnableDrive(driver, true);
 		}
 		else
 		{
 #endif
-			digitalWrite(ENABLE_PINS[driver], enableValues[driver]);
-#if defined(DUET_NG)
+			digitalWrite(ENABLE_PINS[driver], enableValues[driver] > 0);
+#if HAS_SMART_DRIVERS
 		}
 #endif
 	}
@@ -2346,16 +2390,16 @@ void Platform::DisableDriver(size_t driver)
 {
 	if (driver < DRIVES)
 	{
-#if defined(DUET_NG)
-		if (driver < numTMC2660Drivers)
+#if HAS_SMART_DRIVERS
+		if (driver < numSmartDrivers)
 		{
-			TMC2660::EnableDrive(driver, false);
+			SmartDrivers::EnableDrive(driver, false);
 		}
 		else
 		{
 #endif
-			digitalWrite(ENABLE_PINS[driver], !enableValues[driver]);
-#if defined(DUET_NG)
+			digitalWrite(ENABLE_PINS[driver], enableValues[driver] <= 0);
+#if HAS_SMART_DRIVERS
 		}
 #endif
 		driverState[driver] = DriverStatus::disabled;
@@ -2470,13 +2514,13 @@ void Platform::UpdateMotorCurrent(size_t driver)
 			current *= motorCurrentFraction[driver];
 		}
 
-#if defined(DUET_NG)
-		if (driver < numTMC2660Drivers)
+#if HAS_SMART_DRIVERS
+		if (driver < numSmartDrivers)
 		{
-			TMC2660::SetCurrent(driver, current);
+			SmartDrivers::SetCurrent(driver, current);
 		}
 #elif defined (DUET_06_085)
-		unsigned short pot = (unsigned short)((0.256*current*8.0*senseResistor + maxStepperDigipotVoltage/2)/maxStepperDigipotVoltage);
+		const uint16_t pot = (unsigned short)((0.256*current*8.0*senseResistor + maxStepperDigipotVoltage/2)/maxStepperDigipotVoltage);
 		if (driver < 4)
 		{
 			mcpDuet.setNonVolatileWiper(potWipes[driver], pot);
@@ -2555,10 +2599,10 @@ bool Platform::SetDriverMicrostepping(size_t driver, int microsteps, int mode)
 {
 	if (driver < DRIVES)
 	{
-#if defined(DUET_NG)
-		if (driver < numTMC2660Drivers)
+#if HAS_SMART_DRIVERS
+		if (driver < numSmartDrivers)
 		{
-			return TMC2660::SetMicrostepping(driver, microsteps, mode);
+			return SmartDrivers::SetMicrostepping(driver, microsteps, mode);
 		}
 		else
 		{
@@ -2599,10 +2643,10 @@ bool Platform::SetMicrostepping(size_t drive, int microsteps, int mode)
 // Get the microstepping for a driver
 unsigned int Platform::GetDriverMicrostepping(size_t driver, int mode, bool& interpolation) const
 {
-#if defined(DUET_NG)
-	if (driver < numTMC2660Drivers)
+#if HAS_SMART_DRIVERS
+	if (driver < numSmartDrivers)
 	{
-		return TMC2660::GetMicrostepping(driver, mode, interpolation);
+		return SmartDrivers::GetMicrostepping(driver, mode, interpolation);
 	}
 	// On-board drivers only support x16 microstepping without interpolation
 	interpolation = false;
@@ -2635,6 +2679,23 @@ unsigned int Platform::GetMicrostepping(size_t drive, int mode, bool& interpolat
 	}
 }
 
+void Platform::SetEnableValue(size_t driver, int8_t eVal)
+{
+	enableValues[driver] = eVal;
+	DisableDriver(driver);				// disable the drive, because the enable polarity may have been wrong before
+#if HAS_SMART_DRIVERS
+	if (eVal == -1)
+	{
+		// User has asked to disable status monitoring for this driver, so clear its error bits
+		DriversBitmap mask = ~MakeBitmap<DriversBitmap>(driver);
+		temperatureShutdownDrivers &= mask;
+		temperatureWarningDrivers &= mask;
+		shortToGroundDrivers &= mask;
+		openLoadDrivers &= mask;
+	}
+#endif
+}
+
 void Platform::SetAxisDriversConfig(size_t drive, const AxisDriversConfig& config)
 {
 	axisDrivers[drive] = config;
@@ -2914,9 +2975,16 @@ void Platform::RawMessage(MessageType type, const char *message)
 
 	if ((type & BlockingUsbMessage) != 0)
 	{
-		// Debug messages in blocking mode - potentially DANGEROUS, use with care!
-		SERIAL_MAIN_DEVICE.write(message);
-		SERIAL_MAIN_DEVICE.flush();
+		// Debug output sends messages in blocking mode. We now give up sending if we are close to software watchdog timeout.
+		const char *p = message;
+		size_t len = strlen(p);
+		while (SERIAL_MAIN_DEVICE && len != 0 && !reprap.SpinTimeoutImminent())
+		{
+			const size_t written = SERIAL_MAIN_DEVICE.write(p, len);
+			len -= written;
+			p += written;
+		}
+		// We no longer flush afterwards
 	}
 	else if ((type & UsbMessage) != 0)
 	{
@@ -3153,7 +3221,7 @@ bool Platform::ConfigureLogging(GCodeBuffer& gb, StringRef& reply)
 	return false;
 }
 
-// This is called form EmergencyStop. It closes the log file and stops logging.
+// This is called from EmergencyStop. It closes the log file and stops logging.
 void Platform::StopLogging()
 {
 	if (logger != nullptr)
@@ -3352,7 +3420,11 @@ bool Platform::GetFirmwarePin(LogicalPin logicalPin, PinAccess access, Pin& firm
 		if (!reprap.GetHeat().IsHeaterEnabled(logicalPin - Heater0LogicalPin))
 		{
 			firmwarePin = heatOnPins[logicalPin - Heater0LogicalPin];
-			invert = !HEAT_ON;
+#if ACTIVE_LOW_HEAT_ON
+			invert = true;
+#else
+			invert = false;
+#endif
 		}
 	}
 	else if (logicalPin >= Fan0LogicalPin && logicalPin < Fan0LogicalPin + (int)NUM_FANS)		// pins 20- correspond to fan channels
@@ -3497,6 +3569,25 @@ Pin Platform::GetEndstopPin(int endstop) const
 	return (endstop >= 0 && endstop < (int)ARRAY_SIZE(endStopPins)) ? endStopPins[endstop] : NoPin;
 }
 
+// Axis limits
+void Platform::SetAxisMaximum(size_t axis, float value, bool byProbing)
+{
+	axisMaxima[axis] = value;
+	if (byProbing)
+	{
+		SetBit(axisMaximaProbed, axis);
+	}
+}
+
+void Platform::SetAxisMinimum(size_t axis, float value, bool byProbing)
+{
+	axisMinima[axis] = value;
+	if (byProbing)
+	{
+		SetBit(axisMinimaProbed, axis);
+	}
+}
+
 #if SUPPORT_INKJET
 
 // Fire the inkjet (if any) in the given pattern
@@ -3552,7 +3643,7 @@ void Platform::GetMcuTemperatures(float& minT, float& currT, float& maxT) const
 }
 #endif
 
-#ifdef DUET_NG
+#if HAS_VOLTAGE_MONITOR
 
 // Power in voltage
 void Platform::GetPowerVoltages(float& minV, float& currV, float& maxV) const
@@ -3562,6 +3653,10 @@ void Platform::GetPowerVoltages(float& minV, float& currV, float& maxV) const
 	maxV = AdcReadingToPowerVoltage(highestVin);
 }
 
+#endif
+
+#if HAS_SMART_DRIVERS
+
 // TMC driver temperatures
 float Platform::GetTmcDriversTemperature(unsigned int board) const
 {
@@ -3571,6 +3666,127 @@ float Platform::GetTmcDriversTemperature(unsigned int board) const
 				: 0.0;
 }
 
+// Configure the motor stall detection, returning true if an error was encountered
+bool Platform::ConfigureStallDetection(GCodeBuffer& gb, StringRef& reply)
+{
+	// Build a bitmap of all the drivers referenced
+	// First looks for explicit driver numbers
+	DriversBitmap drivers = 0;
+	if (gb.Seen('P'))
+	{
+		long int drives[DRIVES];
+		size_t dCount = DRIVES;
+		gb.GetLongArray(drives, dCount);
+		for (size_t i = 0; i < dCount; i++)
+		{
+			if (drives[i] < 0 || (size_t)drives[i] >= DRIVES)
+			{
+				reply.printf("Invalid drive number '%ld'", drives[i]);
+				return true;
+			}
+			SetBit(drivers, i);
+		}
+	}
+
+	// Now look for axes
+	for (size_t i = 0; i < reprap.GetGCodes().GetTotalAxes(); ++i)
+	{
+		if (gb.Seen(GCodes::axisLetters[i]))
+		{
+			for (size_t j = 0; j < axisDrivers[i].numDrivers; ++j)
+			{
+				SetBit(drivers, axisDrivers[i].driverNumbers[j]);
+			}
+		}
+	}
+
+	// Now check for values to change
+	bool seen = false;
+	if (gb.Seen('S'))
+	{
+		seen = true;
+		const int sgThreshold = gb.GetIValue();
+		for (size_t drive = 0; drive < numSmartDrivers; ++drive)
+		{
+			if (IsBitSet(drivers, drive))
+			{
+				SmartDrivers::SetStallThreshold(drive, sgThreshold);
+			}
+		}
+	}
+	if (gb.Seen('F'))
+	{
+		seen = true;
+		const int sgFilter = (gb.GetIValue() == 1);
+		for (size_t drive = 0; drive < numSmartDrivers; ++drive)
+		{
+			if (IsBitSet(drivers, drive))
+			{
+				SmartDrivers::SetStallFilter(drive, sgFilter);
+			}
+		}
+	}
+	if (gb.Seen('T'))
+	{
+		seen = true;
+		const uint16_t coolStepConfig = (uint16_t)gb.GetUIValue();
+		for (size_t drive = 0; drive < numSmartDrivers; ++drive)
+		{
+			if (IsBitSet(drivers, drive))
+			{
+				SmartDrivers::SetCoolStep(drive, coolStepConfig);
+			}
+		}
+	}
+	if (gb.Seen('R'))
+	{
+		seen = true;
+		const int action = gb.GetIValue();
+		switch (action)
+		{
+		case 0:
+		default:
+			pauseOnStallDrivers &= ~drivers;
+			rehomeOnStallDrivers &= ~drivers;
+			break;
+
+		case 1:
+			rehomeOnStallDrivers &= ~drivers;
+			pauseOnStallDrivers |= drivers;
+			break;
+
+		case 2:
+			pauseOnStallDrivers &= ~drivers;
+			rehomeOnStallDrivers |= drivers;
+			break;
+		}
+	}
+
+	if (!seen)
+	{
+		if (drivers == 0)
+		{
+			drivers = LowestNBits<DriversBitmap>(numSmartDrivers);
+		}
+		bool printed = false;
+		for (size_t drive = 0; drive < numSmartDrivers; ++drive)
+		{
+			if (IsBitSet(drivers, drive))
+			{
+				if (printed)
+				{
+					reply.cat('\n');
+				}
+				reply.catf("Driver %u: ", drive);
+				SmartDrivers::AppendStallConfig(drive, reply);
+				printed = true;
+			}
+		}
+
+	}
+	return false;
+}
+
 #endif
 
 // Real-time clock
@@ -3591,8 +3807,11 @@ bool Platform::SetDateTime(time_t time)
 	const bool ok = (gmtime_r(&time, &brokenDateTime) != nullptr);
 	if (ok)
 	{
-		Message(LogMessage, "Time and date set\n");
-		realTime = time;
+		realTime = time;			// set the date and time
+
+		// Write a log message, giving the time since power up in same format as the logger does
+		const uint32_t timeSincePowerUp = (uint32_t)(millis64()/1000u);
+		MessageF(LogMessage, "Date and time set at power up + %02" PRIu32 ":%02" PRIu32 ":%02" PRIu32 "\n", timeSincePowerUp/3600u, (timeSincePowerUp % 3600u)/60u, timeSincePowerUp % 60u);
 		timeLastUpdatedMillis = millis();
 	}
 	return ok;
@@ -3628,7 +3847,7 @@ void STEP_TC_HANDLER()
 {
 	const irqflags_t flags = cpu_irq_save();
 	const int32_t diff = (int32_t)(tim - GetInterruptClocks());		// see how long we have to go
-	if (diff < (int32_t)DDA::minInterruptInterval)					// if less than about 2us or already passed
+	if (diff < (int32_t)DDA::minInterruptInterval)					// if less than about 6us or already passed
 	{
 		cpu_irq_restore(flags);
 		return true;												// tell the caller to simulate an interrupt instead
@@ -3661,7 +3880,7 @@ void STEP_TC_HANDLER()
 {
 	const irqflags_t flags = cpu_irq_save();
 	const int32_t diff = (int32_t)(tim - GetInterruptClocks());		// see how long we have to go
-	if (diff < (int32_t)DDA::minInterruptInterval)					// if less than about 2us or already passed
+	if (diff < (int32_t)DDA::minInterruptInterval)					// if less than about 6us or already passed
 	{
 		cpu_irq_restore(flags);
 		return true;												// tell the caller to simulate an interrupt instead
@@ -3700,7 +3919,7 @@ void Platform::Tick()
 {
 	if (tickState != 0)
 	{
-#ifdef DUET_NG
+#if HAS_VOLTAGE_MONITOR
 		// Read the power input voltage
 		currentVin = AnalogInReadChannel(vInMonitorAdcChannel);
 		if (currentVin > highestVin)
@@ -3713,7 +3932,7 @@ void Platform::Tick()
 		}
 		if (driversPowered && currentVin > driverOverVoltageAdcReading)
 		{
-			TMC2660::SetDriversPowered(false);
+			SmartDrivers::SetDriversPowered(false);
 			// We deliberately do not clear driversPowered here or increase the over voltage event count - we let the spin loop handle that
 		}
 #endif
diff --git a/src/Platform.h b/src/Platform.h
index a5e4425b..773ec33f 100644
--- a/src/Platform.h
+++ b/src/Platform.h
@@ -49,7 +49,6 @@ Licence: GPL
 #include "Storage/MassStorage.h"	// must be after Pins.h because it needs NumSdCards defined
 #include "MessageType.h"
 #include "ZProbeProgrammer.h"
-#include "Logger.h"
 
 #if defined(DUET_NG)
 # include "DueXn.h"
@@ -107,7 +106,7 @@ const uint32_t Z_PROBE_AXES = (1 << Z_AXIS);					// Axes for which the Z-probe i
 
 // HEATERS - The bed is assumed to be the at index 0
 
-// Define the number of temperature readings we average for each thermistor. This should be a power of 2 and at least 4 ** AD_OVERSAMPLE_BITS.
+// Define the number of temperature readings we average for each thermistor. This should be a power of 2 and at least 4 ^ AD_OVERSAMPLE_BITS.
 // Keep THERMISTOR_AVERAGE_READINGS * NUM_HEATERS * 2ms no greater than HEAT_SAMPLE_TIME or the PIDs won't work well.
 const unsigned int ThermistorAverageReadings = 32;
 
@@ -394,7 +393,7 @@ public:
 	void SetDirection(size_t drive, bool direction);
 	void SetDirectionValue(size_t driver, bool dVal);
 	bool GetDirectionValue(size_t driver) const;
-	void SetEnableValue(size_t driver, bool eVal);
+	void SetEnableValue(size_t driver, int8_t eVal);
 	bool GetEnableValue(size_t driver) const;
 	void EnableDriver(size_t driver);
 	void DisableDriver(size_t driver);
@@ -436,9 +435,9 @@ public:
 	void SetInstantDv(size_t drive, float value);
 	EndStopHit Stopped(size_t drive) const;
 	float AxisMaximum(size_t axis) const;
-	void SetAxisMaximum(size_t axis, float value);
+	void SetAxisMaximum(size_t axis, float value, bool byProbing);
 	float AxisMinimum(size_t axis) const;
-	void SetAxisMinimum(size_t axis, float value);
+	void SetAxisMinimum(size_t axis, float value, bool byProbing);
 	float AxisTotalLength(size_t axis) const;
 	float GetPressureAdvance(size_t drive) const;
 	void SetPressureAdvance(size_t extruder, float factor);
@@ -481,7 +480,7 @@ public:
 	const ZProbeParameters& GetCurrentZProbeParameters() const { return GetZProbeParameters(zProbeType); }
 	void SetZProbeParameters(int32_t probeType, const struct ZProbeParameters& params);
 	bool HomingZWithProbe() const;
-	bool WriteZProbeParameters(FileStore *f) const;
+	bool WritePlatformParameters(FileStore *f) const;
 	void SetProbing(bool isProbing);
 	bool ProgramZProbe(GCodeBuffer& gb, StringRef& reply);
 	void SetZProbeModState(bool b) const;
@@ -537,16 +536,23 @@ public:
 	// MCU temperature
 #if HAS_CPU_TEMP_SENSOR
 	void GetMcuTemperatures(float& minT, float& currT, float& maxT) const;
-#endif
 	void SetMcuTemperatureAdjust(float v) { mcuTemperatureAdjust = v; }
 	float GetMcuTemperatureAdjust() const { return mcuTemperatureAdjust; }
+#endif
 
-#ifdef DUET_NG
+#if HAS_VOLTAGE_MONITOR
 	// Power in voltage
 	void GetPowerVoltages(float& minV, float& currV, float& maxV) const;
+	bool IsPowerOk() const;
+	void DisableAutoSave();
+	void EnableAutoSave(float saveVoltage, float resumeVoltage);
+	bool GetAutoSaveSettings(float& saveVoltage, float&resumeVoltage);
+#endif
+
+#if HAS_SMART_DRIVERS
 	float GetTmcDriversTemperature(unsigned int board) const;
 	void DriverCoolingFansOn(uint32_t driverChannelsMonitored);
-	bool ConfigureAutoSave(GCodeBuffer& gb, StringRef& reply);
+	bool ConfigureStallDetection(GCodeBuffer& gb, StringRef& reply);
 #endif
 
 	// User I/O and servo support
@@ -582,6 +588,8 @@ public:
 	void SetLaserPwmFrequency(float freq);
 	float GetLaserPwmFrequency() const { return laserPort.GetFrequency(); }
 
+	static uint8_t softwareResetDebugInfo;			// extra info for debugging
+
 //-------------------------------------------------------------------------------------------------------
   
 private:
@@ -592,8 +600,8 @@ private:
 	void ResetChannel(size_t chan);					// re-initialise a serial channel
 	float AdcReadingToCpuTemperature(uint32_t reading) const;
 
-#ifdef DUET_NG
-	void ReportDrivers(uint16_t whichDrivers, const char* text, bool& reported);
+#if HAS_SMART_DRIVERS
+	void ReportDrivers(DriversBitmap whichDrivers, const char* text, bool& reported);
 #endif
 
 	// These are the structures used to hold our non-volatile data.
@@ -640,7 +648,7 @@ private:
 		}
 	};
 
-#if SAM4E
+#if SAM4E || SAM4S
 	static_assert(SoftwareResetData::numberOfSlots * sizeof(SoftwareResetData) <= 512, "Can't fit software reset data in SAM4E user signature area");
 #else
 	static_assert(SoftwareResetData::numberOfSlots * sizeof(SoftwareResetData) <= FLASH_DATA_LENGTH, "NVData too large");
@@ -648,7 +656,6 @@ private:
 
 	// Logging
 	Logger *logger;
-	uint32_t lastLogFlushTime;
 
 	// Z probes
 	ZProbeParameters switchZProbeParameters;		// Z probe values for the switch Z-probe
@@ -687,7 +694,7 @@ private:
 
 	volatile DriverStatus driverState[DRIVES];
 	bool directions[DRIVES];
-	bool enableValues[DRIVES];
+	int8_t enableValues[DRIVES];
 	Pin endStopPins[DRIVES];
 	float maxFeedrates[DRIVES];
 	float accelerations[DRIVES];
@@ -705,9 +712,12 @@ private:
 	uint32_t slowDrivers;							// bitmap of driver port bits that need extended step pulse timing
 	float idleCurrentFactor;
 
-#if defined(DUET_NG)
-	size_t numTMC2660Drivers;						// the number of TMC2660 drivers we have, the remaining are simple enable/step/dir drivers
-#elif defined(DUET_06_085)
+#if HAS_SMART_DRIVERS
+	size_t numSmartDrivers;						// the number of TMC2660 drivers we have, the remaining are simple enable/step/dir drivers
+	DriversBitmap pauseOnStallDrivers, rehomeOnStallDrivers;
+#endif
+
+#if defined(DUET_06_085)
 	// Digipots
 	MCP4461 mcpDuet;
 	MCP4461 mcpExpansion;
@@ -743,9 +753,12 @@ private:
 
 	float axisMaxima[MaxAxes];
 	float axisMinima[MaxAxes];
+	AxesBitmap axisMinimaProbed, axisMaximaProbed;
 	EndStopType endStopType[MaxAxes];
 	bool endStopLogicLevel[MaxAxes];
-  
+
+	static bool WriteAxisLimits(FileStore *f, AxesBitmap axesProbed, const float limits[MaxAxes], int sParam);
+
 	// Heaters - bed is assumed to be the first
 
 	Pin tempSensePins[Heaters];
@@ -828,36 +841,38 @@ private:
 #if HAS_CPU_TEMP_SENSOR		// reading temperature on the RADDS messes up one of the heater pins, so don't do it
 	AnalogChannelNumber temperatureAdcChannel;
 	uint32_t highestMcuTemperature, lowestMcuTemperature;
-#endif
 	float mcuTemperatureAdjust;
+#endif
 
-#ifdef DUET_NG
+#if HAS_VOLTAGE_MONITOR
 	AnalogChannelNumber vInMonitorAdcChannel;
 	volatile uint16_t currentVin, highestVin, lowestVin;
 	uint32_t numUnderVoltageEvents;
 	volatile uint32_t numOverVoltageEvents;
-	uint32_t lastWarningMillis;							// When we last sent a warning message about a Vssa short
-	uint16_t temperatureShutdownDrivers, temperatureWarningDrivers, shortToGroundDrivers, openLoadDrivers;
-	uint8_t nextDriveToPoll;
-	bool driversPowered;
-	bool vssaSenseWorking;
-	bool onBoardDriversFanRunning;						// true if a fan is running to cool the on-board drivers
-	bool offBoardDriversFanRunning;						// true if a fan is running to cool the drivers on the DueX
-	uint32_t onBoardDriversFanStartMillis;				// how many times we have suppressed a temperature warning
-	uint32_t offBoardDriversFanStartMillis;				// how many times we have suppressed a temperature warning
-	uint16_t autoShutdownReading, autoPauseReading, autoResumeReading;
 	bool autoSaveEnabled;
 
 	enum class AutoSaveState : uint8_t
 	{
 		starting = 0,
 		normal,
-		autoPaused,
-		autoShutdown
+		autoPaused
 	};
 	AutoSaveState autoSaveState;
 #endif
 
+#if HAS_SMART_DRIVERS
+	uint32_t lastWarningMillis;							// When we last sent a warning message about a Vssa short
+	DriversBitmap temperatureShutdownDrivers, temperatureWarningDrivers, shortToGroundDrivers, openLoadDrivers;
+	uint8_t nextDriveToPoll;
+	bool driversPowered;
+	bool vssaSenseWorking;
+	bool onBoardDriversFanRunning;						// true if a fan is running to cool the on-board drivers
+	bool offBoardDriversFanRunning;						// true if a fan is running to cool the drivers on the DueX
+	uint32_t onBoardDriversFanStartMillis;				// how many times we have suppressed a temperature warning
+	uint32_t offBoardDriversFanStartMillis;				// how many times we have suppressed a temperature warning
+	uint16_t autoPauseReading, autoResumeReading;
+#endif
+
 	// RTC
 	time_t realTime;									// the current date/time, or zero if never set
 	uint32_t timeLastUpdatedMillis;						// the milliseconds counter when we last incremented the time
@@ -977,12 +992,6 @@ inline void Platform::SetDriverDirection(uint8_t driver, bool direction)
 	digitalWrite(DIRECTION_PINS[driver], d);
 }
 
-inline void Platform::SetEnableValue(size_t driver, bool eVal)
-{
-	enableValues[driver] = eVal;
-	DisableDriver(driver);				// disable the drive, because the enable polarity may have been wrong before
-}
-
 inline bool Platform::GetEnableValue(size_t driver) const
 {
 	return enableValues[driver];
@@ -993,21 +1002,11 @@ inline float Platform::AxisMaximum(size_t axis) const
 	return axisMaxima[axis];
 }
 
-inline void Platform::SetAxisMaximum(size_t axis, float value)
-{
-	axisMaxima[axis] = value;
-}
-
 inline float Platform::AxisMinimum(size_t axis) const
 {
 	return axisMinima[axis];
 }
 
-inline void Platform::SetAxisMinimum(size_t axis, float value)
-{
-	axisMinima[axis] = value;
-}
-
 inline float Platform::AxisTotalLength(size_t axis) const
 {
 	return axisMaxima[axis] - axisMinima[axis];
@@ -1177,7 +1176,7 @@ inline OutputBuffer *Platform::GetAuxGCodeReply()
 	return temp;
 }
 
-// *** These next two functions must use the same bit assignments in the drivers bitmap ***
+// *** These next three functions must use the same bit assignments in the drivers bitmap ***
 // The bitmaps are organised like this:
 // Duet WiFi:
 //	All step pins are on port D, so the bitmap is just the map of bits in port D.
@@ -1188,6 +1187,8 @@ inline OutputBuffer *Platform::GetAuxGCodeReply()
 // RADDS:
 //  Step pins are PA2,9,12,15 PB16,19 PC3,12 PD6
 //	PC12 clashes with PA12 so we shift PC3,12 left one bit
+// Alligator:
+//  Pins on ports B,C,D are used but the bit numbers are all different, so we use their actual positions
 
 // Calculate the step bit for a driver. This doesn't need to be fast.
 /*static*/ inline uint32_t Platform::CalcDriverBitmap(size_t driver)
diff --git a/src/PrintMonitor.cpp b/src/PrintMonitor.cpp
index b88b8917..3093a6df 100644
--- a/src/PrintMonitor.cpp
+++ b/src/PrintMonitor.cpp
@@ -144,7 +144,10 @@ void PrintMonitor::Spin()
 				LayerComplete();
 				currentLayer++;
 
-				lastLayerZ = liveCoordinates[Z_AXIS];
+				// If we know the layer height, compute what the current layer height should be. This is to handle slicers that use a different layer height for support.
+				lastLayerZ = (printingFileInfo.layerHeight > 0.0)
+								? printingFileInfo.firstLayerHeight + (currentLayer - 1) * printingFileInfo.layerHeight
+									: liveCoordinates[Z_AXIS];
 				lastLayerChangeTime = GetPrintDuration();
 			}
 		}
diff --git a/src/PrintMonitor.h b/src/PrintMonitor.h
index 7cc6783e..72df1e51 100644
--- a/src/PrintMonitor.h
+++ b/src/PrintMonitor.h
@@ -25,7 +25,7 @@ Licence: GPL
 const FilePosition GCODE_HEADER_SIZE = 20000uL;		// How many bytes to read from the header - I (DC) have a Kisslicer file with a layer height comment 14Kb from the start
 const FilePosition GCODE_FOOTER_SIZE = 400000uL;	// How many bytes to read from the footer
 
-#ifdef DUET_NG
+#if SAM4E || SAM4S
 const size_t GCODE_READ_SIZE = 4096;				// How many bytes to read in one go in GetFileInfo() (should be a multiple of 512 for read efficiency)
 #else
 const size_t GCODE_READ_SIZE = 1024;				// How many bytes to read in one go in GetFileInfo() (should be a multiple of 512 for read efficiency)
diff --git a/src/RADDS/Pins_RADDS.h b/src/RADDS/Pins_RADDS.h
index 6f4b26cb..eb56525c 100644
--- a/src/RADDS/Pins_RADDS.h
+++ b/src/RADDS/Pins_RADDS.h
@@ -7,6 +7,9 @@
 #define HAS_LWIP_NETWORKING		0
 #define HAS_CPU_TEMP_SENSOR		0				// enabling the CPU temperature sensor disables Due pin 13 due to bug in SAM3X
 #define HAS_HIGH_SPEED_SD		0
+#define HAS_SMART_DRIVERS		0
+#define HAS_VOLTAGE_MONITOR		0
+#define ACTIVE_LOW_HEAT_ON		0
 
 const size_t NumFirmwareUpdateModules = 1;
 #define IAP_UPDATE_FILE "iapradds.bin"
@@ -76,10 +79,6 @@ const Pin END_STOP_PINS[DRIVES] = { 28, 30, 32, 39, NoPin, NoPin, NoPin, NoPin }
 
 // HEATERS - The bed is assumed to be the at index 0
 
-// 0 for inverted heater (e.g. Duet v0.6)
-// 1 for not (e.g. Duet v0.4; RADDS)
-const bool HEAT_ON = true;
-
 // Analogue pin numbers
 const Pin TEMP_SENSE_PINS[Heaters] = HEATERS_(4, 0, 1, 2, e, f, g, h);
 
diff --git a/src/RepRap.cpp b/src/RepRap.cpp
index 34fda25a..25ebfd99 100644
--- a/src/RepRap.cpp
+++ b/src/RepRap.cpp
@@ -170,63 +170,63 @@ void RepRap::Spin()
 	if(!active)
 		return;
 
-	spinningModule = modulePlatform;
 	ticksInSpinState = 0;
+	spinningModule = modulePlatform;
 	platform->Spin();
 
-	spinningModule = moduleNetwork;
 	ticksInSpinState = 0;
+	spinningModule = moduleNetwork;
 	network->Spin(true);
 
-	spinningModule = moduleWebserver;
 	ticksInSpinState = 0;
+	spinningModule = moduleWebserver;
 
-	spinningModule = moduleGcodes;
 	ticksInSpinState = 0;
+	spinningModule = moduleGcodes;
 	gCodes->Spin();
 
-	spinningModule = moduleMove;
 	ticksInSpinState = 0;
+	spinningModule = moduleMove;
 	move->Spin();
 
-	spinningModule = moduleHeat;
 	ticksInSpinState = 0;
+	spinningModule = moduleHeat;
 	heat->Spin();
 
 #if SUPPORT_ROLAND
-	spinningModule = moduleRoland;
 	ticksInSpinState = 0;
+	spinningModule = moduleRoland;
 	roland->Spin();
 #endif
 
 #if SUPPORT_SCANNER
-	spinningModule = moduleScanner;
 	ticksInSpinState = 0;
+	spinningModule = moduleScanner;
 	scanner->Spin();
 #endif
 
 #if SUPPORT_IOBITS
-	spinningModule = modulePortControl;
 	ticksInSpinState = 0;
+	spinningModule = modulePortControl;
 	portControl->Spin(true);
 #endif
 
-	spinningModule = modulePrintMonitor;
 	ticksInSpinState = 0;
+	spinningModule = modulePrintMonitor;
 	printMonitor->Spin();
 
 #ifdef DUET_NG
-	spinningModule = moduleDuetExpansion;
 	ticksInSpinState = 0;
+	spinningModule = moduleDuetExpansion;
 	DuetExpansion::Spin(true);
 #endif
 
-	spinningModule = moduleFilamentSensors;
 	ticksInSpinState = 0;
+	spinningModule = moduleFilamentSensors;
 	FilamentSensor::Spin(true);
 
-	spinningModule = noModule;
 	ticksInSpinState = 0;
+	spinningModule = noModule;
 
 	// Check if we need to display a cold extrusion warning
 	const uint32_t now = millis();
@@ -534,7 +534,7 @@ void RepRap::Tick()
 	{
 		platform->Tick();
 		++ticksInSpinState;
-		if (ticksInSpinState >= 20000)	// if we stall for 20 seconds, save diagnostic data and reset
+		if (ticksInSpinState >= MaxTicksInSpinState)	// if we stall for 20 seconds, save diagnostic data and reset
 		{
 			resetting = true;
 			for(size_t i = 0; i < Heaters; i++)
@@ -554,6 +554,12 @@ void RepRap::Tick()
 	}
 }
 
+// Return true if we are close to timeout
+bool RepRap::SpinTimeoutImminent() const
+{
+	return ticksInSpinState >= HighTicksInSpinState;
+}
+
 // Get the JSON status response for the web server (or later for the M105 command).
 // Type 1 is the ordinary JSON status response.
 // Type 2 is the same except that static parameters are also included.
@@ -1046,7 +1052,7 @@ OutputBuffer *RepRap::GetStatusResponse(uint8_t type, ResponseSource source)
 		}
 #endif
 
-#ifdef DUET_NG
+#if HAS_VOLTAGE_MONITOR
 		// Power in voltages
 		{
 			float minV, currV, maxV;
@@ -1748,6 +1754,30 @@ bool RepRap::WriteToolSettings(FileStore *f) const
 	return ok;
 }
 
+// Save some information in config-override.g
+bool RepRap::WriteToolParameters(FileStore *f) const
+{
+	bool ok = true;
+	for (const Tool *t = toolList; ok && t != nullptr; t = t->Next())
+	{
+		const AxesBitmap axesProbed = t->GetAxisOffsetsProbed();
+		if (axesProbed != 0)
+		{
+			scratchString.printf("G10 P%d", t->Number());
+			for (size_t axis = 0; axis < MaxAxes; ++axis)
+			{
+				if (IsBitSet(axesProbed, axis))
+				{
+					scratchString.catf(" %c%.2f", GCodes::axisLetters[axis], (double)(t->GetOffsets()[axis]));
+				}
+			}
+		}
+		scratchString.cat('\n');
+		ok = f->Write(scratchString.Pointer());
+	}
+	return ok;
+}
+
 // Helper function for diagnostic tests in Platform.cpp, to cause a deliberate divide-by-zero
 /*static*/ uint32_t RepRap::DoDivide(uint32_t a, uint32_t b)
 {
@@ -1760,4 +1790,10 @@ bool RepRap::WriteToolSettings(FileStore *f) const
 	return *reinterpret_cast<const uint32_t*>(p);
 }
 
+// Report an internal error
+void RepRap::ReportInternalError(const char *file, const char *func, int line) const
+{
+	platform->MessageF(ErrorMessage, "Internal Error in %s at %s(%d)\n", func, file, line);
+}
+
 // End
diff --git a/src/RepRap.h b/src/RepRap.h
index 2c3ceeeb..c84d1416 100644
--- a/src/RepRap.h
+++ b/src/RepRap.h
@@ -87,7 +87,7 @@ public:
 #endif
 
 	void Tick();
-	uint16_t GetTicksInSpinState() const;
+	bool SpinTimeoutImminent() const;
 	bool IsStopped() const;
 
 	uint16_t GetExtrudersInUse() const;
@@ -104,7 +104,10 @@ public:
 	void SetAlert(const char *msg, const char *title, int mode, float timeout, AxesBitmap controls);
 	void ClearAlert();
 
-	bool WriteToolSettings(FileStore *f) const;				// Save some resume information
+	bool WriteToolSettings(FileStore *f) const;				// save some information for the resume file
+	bool WriteToolParameters(FileStore *f) const;			// save some information in config-override.g
+
+	void ReportInternalError(const char *file, const char *func, int line) const;	// Report an internal error
 
 	static uint32_t DoDivide(uint32_t a, uint32_t b);		// helper function for diagnostic tests
 	static uint32_t ReadDword(const char* p);				// helper function for diagnostic tests
@@ -114,6 +117,9 @@ private:
 
 	char GetStatusCharacter() const;
 
+	static constexpr uint32_t MaxTicksInSpinState = 20000;	// timeout before we reset the processor
+	static constexpr uint32_t HighTicksInSpinState = 16000;	// how long before we warn that timeout is approaching
+
 	Platform* platform;
 	Network* network;
 	Move* move;
@@ -178,7 +184,8 @@ inline Tool* RepRap::GetCurrentTool() const { return currentTool; }
 inline uint16_t RepRap::GetExtrudersInUse() const { return activeExtruders; }
 inline uint16_t RepRap::GetToolHeatersInUse() const { return activeToolHeaters; }
 inline bool RepRap::IsStopped() const { return stopped; }
-inline uint16_t RepRap::GetTicksInSpinState() const { return ticksInSpinState; }
+
+#define INTERNAL_ERROR do { reprap.ReportInternalError((__FILE__), (__func__), (__LINE__)); } while(0)
 
 #endif
 
diff --git a/src/RepRapFirmware.h b/src/RepRapFirmware.h
index e7a0fcd9..679e2099 100644
--- a/src/RepRapFirmware.h
+++ b/src/RepRapFirmware.h
@@ -62,7 +62,10 @@ class Platform;
 class GCodes;
 class Move;
 class DDA;
+class Kinematics;
 class Heat;
+class PID;
+class TemperatureSensor;
 class Tool;
 class Roland;
 class Scanner;
@@ -75,13 +78,14 @@ class GCodeBuffer;
 class GCodeQueue;
 class FilamentSensor;
 class RandomProbePointSet;
+class Logger;
 
 #if SUPPORT_IOBITS
 class PortControl;
 #endif
 
 // Define floating point type to use for calculations where we would like high precision in matrix calculations
-#ifdef DUET_NG
+#if SAM4E || SAM4S
 typedef double floatc_t;					// type of matrix element used for calibration
 #else
 // We are more memory-constrained on the SAM3X
@@ -89,6 +93,7 @@ typedef float floatc_t;						// type of matrix element used for calibration
 #endif
 
 typedef uint32_t AxesBitmap;				// Type of a bitmap representing a set of axes
+typedef uint32_t DriversBitmap;				// Type of a bitmap representing a set of driver numbers
 typedef uint32_t FansBitmap;				// Type of a bitmap representing a set of fan numbers
 
 // A single instance of the RepRap class contains all the others
diff --git a/src/Storage/FileWriteBuffer.h b/src/Storage/FileWriteBuffer.h
index 1e7130c4..9cbc3b8a 100644
--- a/src/Storage/FileWriteBuffer.h
+++ b/src/Storage/FileWriteBuffer.h
@@ -11,7 +11,7 @@
 #include "RepRapFirmware.h"
 
 
-#ifdef DUET_NG
+#if SAM4E || SAM4S
 const size_t NumFileWriteBuffers = 2;					// Number of write buffers
 const size_t FileWriteBufLen = 8192;					// Size of each write buffer
 #else
diff --git a/src/Storage/MassStorage.cpp b/src/Storage/MassStorage.cpp
index aab6f287..19f9d9a0 100644
--- a/src/Storage/MassStorage.cpp
+++ b/src/Storage/MassStorage.cpp
@@ -71,7 +71,7 @@ void MassStorage::Init()
 	// Try to mount the first SD card only
 	char replyBuffer[100];
 	StringRef reply(replyBuffer, ARRAY_SIZE(replyBuffer));
-	do { } while (!Mount(0, reply, false));
+	do { } while (Mount(0, reply, false) == GCodeResult::notFinished);
 	if (reply.strlen() != 0)
 	{
 		delay(3000);		// Wait a few seconds so users have a chance to see this
@@ -342,19 +342,19 @@ bool MassStorage::SetLastModifiedTime(const char* directory, const char *fileNam
 // Mount the specified SD card, returning true if done, false if needs to be called again.
 // If an error occurs, return true with the error message in 'reply'.
 // This may only be called to mount one card at a time.
-bool MassStorage::Mount(size_t card, StringRef& reply, bool reportSuccess)
+GCodeResult MassStorage::Mount(size_t card, StringRef& reply, bool reportSuccess)
 {
 	if (card >= NumSdCards)
 	{
 		reply.copy("SD card number out of range");
-		return true;
+		return GCodeResult::error;
 	}
 
 	if (isMounted[card] && platform->AnyFileOpen(&fileSystems[card]))
 	{
 		// Don't re-mount the card if any files are open on it
 		reply.copy("SD card has open file(s)");
-		return true;
+		return GCodeResult::error;
 	}
 
 	static bool mounting = false;
@@ -374,58 +374,57 @@ bool MassStorage::Mount(size_t card, StringRef& reply, bool reportSuccess)
 	if (err != SD_MMC_OK && millis() - startTime < 5000)
 	{
 		delay(2);
-		return false;
+		return GCodeResult::notFinished;
 	}
 
 	mounting = false;
 	if (err != SD_MMC_OK)
 	{
 		reply.printf("Cannot initialise SD card %u: %s", card, TranslateCardError(err));
+		return GCodeResult::error;
 	}
-	else
+
+	// Mount the file systems
+	memset(&fileSystems[card], 0, sizeof(FATFS));					// f_mount doesn't initialise the file structure, we must do it ourselves
+	FRESULT mounted = f_mount(card, &fileSystems[card]);
+	if (mounted != FR_OK)
+	{
+		reply.printf("Cannot mount SD card %u: code %d", card, mounted);
+		return GCodeResult::error;
+	}
+
+	isMounted[card] = true;
+	if (reportSuccess)
 	{
-		// Mount the file systems
-		memset(&fileSystems[card], 0, sizeof(FATFS));					// f_mount doesn't initialise the file structure, we must do it ourselves
-		FRESULT mounted = f_mount(card, &fileSystems[card]);
-		if (mounted != FR_OK)
+		float capacity = sd_mmc_get_capacity(card)/1024;		// get capacity and convert to Mbytes
+		const char* capUnits;
+		if (capacity >= 1024.0)
 		{
-			reply.printf("Cannot mount SD card %u: code %d", card, mounted);
+			capacity /= 1024.0;
+			capUnits = "Gb";
 		}
 		else
 		{
-			isMounted[card] = true;
-			if (reportSuccess)
-			{
-				float capacity = sd_mmc_get_capacity(card)/1024;		// get capacity and convert to Mbytes
-				const char* capUnits;
-				if (capacity >= 1024.0)
-				{
-					capacity /= 1024.0;
-					capUnits = "Gb";
-				}
-				else
-				{
-					capUnits = "Mb";
-				}
-				reply.printf("%s card mounted in slot %u, capacity %.2f%s", TranslateCardType(sd_mmc_get_type(card)), card, (double)capacity, capUnits);
-			}
-			else
-			{
-				reply.Clear();
-			}
+			capUnits = "Mb";
 		}
+		reply.printf("%s card mounted in slot %u, capacity %.2f%s", TranslateCardType(sd_mmc_get_type(card)), card, (double)capacity, capUnits);
 	}
-	return true;
+	else
+	{
+		reply.Clear();
+	}
+
+	return GCodeResult::ok;
 }
 
 // Unmount the specified SD card, returning true if done, false if needs to be called again.
 // If an error occurs, return true with the error message in 'reply'.
-bool MassStorage::Unmount(size_t card, StringRef& reply)
+GCodeResult MassStorage::Unmount(size_t card, StringRef& reply)
 {
 	if (card >= NumSdCards)
 	{
 		reply.copy("SD card number out of range");
-		return true;
+		return GCodeResult::error;
 	}
 
 	platform->InvalidateFiles(&fileSystems[card]);
@@ -433,7 +432,7 @@ bool MassStorage::Unmount(size_t card, StringRef& reply)
 	sd_mmc_unmount(card);
 	isMounted[card] = false;
 	reply.Clear();
-	return true;
+	return GCodeResult::ok;
 }
 
 // Check if the drive referenced in the specified path is mounted. Return true if it is.
diff --git a/src/Storage/MassStorage.h b/src/Storage/MassStorage.h
index d2f2ed4f..b6e87c05 100644
--- a/src/Storage/MassStorage.h
+++ b/src/Storage/MassStorage.h
@@ -5,6 +5,7 @@
 #include "Pins.h"
 #include "FileWriteBuffer.h"
 #include "Libraries/Fatfs/ff.h"
+#include "GCodes/GCodeResult.h"
 #include <ctime>
 
 // Info returned by FindFirst/FindNext calls
@@ -34,8 +35,8 @@ public:
 	bool DirectoryExists(const char* directory, const char* subDirectory);
 	time_t GetLastModifiedTime(const char* directory, const char *fileName) const;
 	bool SetLastModifiedTime(const char* directory, const char *file, time_t time);
-	bool Mount(size_t card, StringRef& reply, bool reportSuccess);
-	bool Unmount(size_t card, StringRef& reply);
+	GCodeResult Mount(size_t card, StringRef& reply, bool reportSuccess);
+	GCodeResult Unmount(size_t card, StringRef& reply);
 	bool IsDriveMounted(size_t drive) const { return drive < NumSdCards && isMounted[drive]; }
 	bool CheckDriveMounted(const char* path);
 
diff --git a/src/Tools/Tool.cpp b/src/Tools/Tool.cpp
index 4453e2c7..8f69ba33 100644
--- a/src/Tools/Tool.cpp
+++ b/src/Tools/Tool.cpp
@@ -100,6 +100,7 @@ Tool * Tool::freelist = nullptr;
 	t->yMapping = yMap;
 	t->fanMapping = fanMap;
 	t->heaterFault = false;
+	t->axisOffsetsProbed = 0;
 	t->displayColdExtrudeWarning = false;
 
 	for (size_t axis = 0; axis < MaxAxes; axis++)
@@ -463,10 +464,19 @@ bool Tool::WriteSettings(FileStore *f) const
 
 void Tool::SetOffsets(const float offs[MaxAxes])
 {
-	for(size_t i = 0; i < MaxAxes; ++i)
+	for (size_t i = 0; i < MaxAxes; ++i)
 	{
 		offset[i] = offs[i];
 	}
 }
 
+void Tool::SetOffset(size_t axis, float offs, bool byProbing)
+{
+	offset[axis] = offs;
+	if (byProbing)
+	{
+		SetBit(axisOffsetsProbed, axis);
+	}
+}
+
 // End
diff --git a/src/Tools/Tool.h b/src/Tools/Tool.h
index 9af30d4a..4b0d2909 100644
--- a/src/Tools/Tool.h
+++ b/src/Tools/Tool.h
@@ -49,7 +49,8 @@ public:
 
 	const float *GetOffsets() const;
 	void SetOffsets(const float offs[MaxAxes]);
-	void SetOffset(size_t axis, float offs) pre(axis < MaxAxes);
+	void SetOffset(size_t axis, float offs, bool byProbing) pre(axis < MaxAxes);
+	AxesBitmap GetAxisOffsetsProbed() const { return axisOffsetsProbed; }
 	size_t DriveCount() const;
 	int Drive(size_t driveNumber) const;
 	bool ToolCanDrive(bool extrude);
@@ -99,6 +100,7 @@ private:
 	float standbyTemperatures[Heaters];
 	size_t heaterCount;
 	float offset[MaxAxes];
+	AxesBitmap axisOffsetsProbed;
 	AxesBitmap xMapping, yMapping;
 	FansBitmap fanMapping;
 	Filament *filament;
@@ -149,9 +151,4 @@ inline const float *Tool::GetOffsets() const
 	return offset;
 }
 
-inline void Tool::SetOffset(size_t axis, float offs)
-{
-	offset[axis] = offs;
-}
-
 #endif /* TOOL_H_ */
diff --git a/src/Version.h b/src/Version.h
index f7d87e17..d4e6ab47 100644
--- a/src/Version.h
+++ b/src/Version.h
@@ -9,11 +9,11 @@
 #define SRC_VERSION_H_
 
 #ifndef VERSION
-# define VERSION "1.20beta1"
+# define VERSION "1.20beta2"
 #endif
 
 #ifndef DATE
-# define DATE "2017-10-02"
+# define DATE "2017-10-25"
 #endif
 
 #define AUTHORS "reprappro, dc42, chrishamm, t3p3, dnewman"
author	David Crocker <dcrocker@eschertech.com>	2017-10-26 00:36:48 +0300
committer	David Crocker <dcrocker@eschertech.com>	2017-10-26 00:37:05 +0300
commit	ad73549a01954de291cea135cde2527d1883596e (patch)
tree	0fcd0f7b05c9e40e986851c5f6455c0f1f589108
parent	dff15aadac70efe87d3ec952af8b6a3d9acbcf66 (diff)