Version 2.01/1.22 beta 22.01beta2

Bug fixes:
- Non-movement commands were not correctly synchronised with movement commands when there were more than 8 non-movement commands interspersed with more than 2 seconds of movement
- If M28/M29 was used in a macro file to then the commands between M28 and M29 were executed as well as being written to the target file
- If you used the jog buttons in DWC before homing the axes, and you didn't use M564 H0 in config.g, then as well as the "insufficient axes homed" error message, a stack underflow error message was produced
- Neither M561 nor G29 S2 adjusted the user Z coordinate if bed compensation was previously active (G29 S2 did if you ran it twice)
- Duet 06/085 only: fixed a buffer overflow in the Netbios responder code
- Duet 2 series: when an under-voltage event occurred, spurious driver status warnings/errors were sometimes reported
- Duet 2 series: when an under- or over-voltage event occurred, the VIN voltage reported was the current voltage, not the voltage when the event was recorded
- When an axis was made visible and later hidden, subsequent move commands sometimes sent step commands incorrectly to the driver(s) associated with that axis. This could cause unwanted movement if the axis was still mapped to a real driver. If it was mapped to a dummy driver, it could still cause step errors to be recorded and/or some movements to be slowed down.
- The longest loop time reported by M122 was distorted by the fact that M122 itself takes a long time to execute, due to the volume of output it produces and the need to synchronise with the Network task

New features/changed behaviour:
- On the Duet 2 Maestro, the 2 optional add-on drivers are now assumed to be TMC2224 with UART interface
- When the Z probe type is set to 9 for BLTouch, the probe output is no longer filtered, for faster response
- If an error occurs while reading or writing the SD card, the operation is retried. The M122 report includes the maximum number of retries that were done before a successful outcome.
- On the Duet WiFi/Ethernet, at startup the firmware does additional retries when checking for the presence of a DueX2 or DueX5 and/or additional I/O expansion board
- When a resurrect.g file is generated, it now includes G92 commands just before it invokes resurrect-prologue.g, to set the assumed head position to the point at which power was lost or the print was paused. This is to better handle printers for which homing Z is not possible when a print is already on the bed. Caution: this doesn't allow for any Z lift or other movement in the power fail script.
- RTOS builds only: added a separate software watchdog to monitor the Heat task
- RTOS builds only: in the M122 report, the software reset data now includes which task was active, and only owned mutexes are listed
- Upgraded compiler to 2018-q2-update

33 files changed, 558 insertions, 453 deletions

diff --git a/.cproject b/.cproject
index c8e57e44..a08d73f1 100644
--- a/.cproject
+++ b/.cproject
@@ -7,7 +7,7 @@
 					<stringMacro name="LinkFlags2" type="VALUE_TEXT" value="-Wl,--end-group -lm"/>
 					<stringMacro name="LinkFlags1" type="VALUE_TEXT" value="-mthumb -Wl,--cref -Wl,--check-sections -Wl,--gc-sections -Wl,--entry=Reset_Handler -Wl,--unresolved-symbols=report-all -Wl,--warn-common -Wl,--warn-section-align -Wl,--warn-unresolved-symbols -Wl,--start-group"/>
 					<stringMacro name="CoreName" type="VALUE_TEXT" value="CoreNG"/>
-					<stringMacro name="GccPath" type="VALUE_TEXT" value="C:\Program Files (x86)\GNU Tools ARM Embedded\6 2017-q2-update\bin"/>
+					<stringMacro name="GccPath" type="VALUE_TEXT" value="C:\Program Files (x86)\GNU Tools ARM Embedded\7 2018-q2-update\bin"/>
 				</macros>
 				<externalSettings/>
 				<extensions>
@@ -122,7 +122,7 @@
 					<stringMacro name="LinkFlags2" type="VALUE_TEXT" value="-Wl,--end-group -lm"/>
 					<stringMacro name="LinkFlags1" type="VALUE_TEXT" value="-mthumb -Wl,--cref -Wl,--check-sections -Wl,--gc-sections -Wl,--entry=Reset_Handler -Wl,--unresolved-symbols=report-all -Wl,--warn-common -Wl,--warn-section-align -Wl,--warn-unresolved-symbols -Wl,--start-group"/>
 					<stringMacro name="CoreName" type="VALUE_TEXT" value="CoreNG"/>
-					<stringMacro name="GccPath" type="VALUE_TEXT" value="C:\Program Files (x86)\GNU Tools ARM Embedded\6 2017-q2-update\bin"/>
+					<stringMacro name="GccPath" type="VALUE_TEXT" value="C:\Program Files (x86)\GNU Tools ARM Embedded\7 2018-q2-update\bin"/>
 				</macros>
 				<externalSettings/>
 				<extensions>
@@ -232,7 +232,7 @@
 					<stringMacro name="LinkFlags2" type="VALUE_TEXT" value="-Wl,--end-group -lm"/>
 					<stringMacro name="LinkFlags1" type="VALUE_TEXT" value="-mthumb -Wl,--cref -Wl,--check-sections -Wl,--gc-sections -Wl,--entry=Reset_Handler -Wl,--unresolved-symbols=report-all -Wl,--warn-common -Wl,--warn-section-align -Wl,--warn-unresolved-symbols -Wl,--start-group"/>
 					<stringMacro name="CoreName" type="VALUE_TEXT" value="CoreNG"/>
-					<stringMacro name="GccPath" type="VALUE_TEXT" value="C:\Program Files (x86)\GNU Tools ARM Embedded\6 2017-q2-update\bin"/>
+					<stringMacro name="GccPath" type="VALUE_TEXT" value="C:\Program Files (x86)\GNU Tools ARM Embedded\7 2018-q2-update\bin"/>
 				</macros>
 				<externalSettings/>
 				<extensions>
@@ -353,7 +353,7 @@
 					<stringMacro name="LinkFlags2" type="VALUE_TEXT" value="-Wl,--end-group -lm"/>
 					<stringMacro name="LinkFlags1" type="VALUE_TEXT" value="-mthumb -Wl,--cref -Wl,--check-sections -Wl,--gc-sections -Wl,--entry=Reset_Handler -Wl,--unresolved-symbols=report-all -Wl,--warn-common -Wl,--warn-section-align -Wl,--warn-unresolved-symbols -Wl,--start-group"/>
 					<stringMacro name="CoreName" type="VALUE_TEXT" value="CoreNG"/>
-					<stringMacro name="GccPath" type="VALUE_TEXT" value="C:\Program Files (x86)\GNU Tools ARM Embedded\6 2017-q2-update\bin"/>
+					<stringMacro name="GccPath" type="VALUE_TEXT" value="C:\Program Files (x86)\GNU Tools ARM Embedded\7 2018-q2-update\bin"/>
 				</macros>
 				<externalSettings/>
 				<extensions>
@@ -473,7 +473,7 @@
 					<stringMacro name="LinkFlags2" type="VALUE_TEXT" value="-Wl,--end-group -lm"/>
 					<stringMacro name="LinkFlags1" type="VALUE_TEXT" value="-mthumb -Wl,--cref -Wl,--check-sections -Wl,--gc-sections -Wl,--entry=Reset_Handler -Wl,--unresolved-symbols=report-all -Wl,--warn-common -Wl,--warn-section-align -Wl,--warn-unresolved-symbols -Wl,--start-group"/>
 					<stringMacro name="CoreName" type="VALUE_TEXT" value="CoreNG"/>
-					<stringMacro name="GccPath" type="VALUE_TEXT" value="C:\Program Files (x86)\GNU Tools ARM Embedded\6 2017-q2-update\bin"/>
+					<stringMacro name="GccPath" type="VALUE_TEXT" value="C:\Program Files (x86)\GNU Tools ARM Embedded\7 2018-q2-update\bin"/>
 				</macros>
 				<externalSettings/>
 				<extensions>
@@ -593,7 +593,7 @@
 					<stringMacro name="LinkFlags2" type="VALUE_TEXT" value="-Wl,--end-group -lm"/>
 					<stringMacro name="LinkFlags1" type="VALUE_TEXT" value="-mthumb -Wl,--cref -Wl,--check-sections -Wl,--gc-sections -Wl,--entry=Reset_Handler -Wl,--unresolved-symbols=report-all -Wl,--warn-common -Wl,--warn-section-align -Wl,--warn-unresolved-symbols -Wl,--start-group"/>
 					<stringMacro name="CoreName" type="VALUE_TEXT" value="CoreNG"/>
-					<stringMacro name="GccPath" type="VALUE_TEXT" value="C:\Program Files (x86)\GNU Tools ARM Embedded\6 2017-q2-update\bin"/>
+					<stringMacro name="GccPath" type="VALUE_TEXT" value="C:\Program Files (x86)\GNU Tools ARM Embedded\7 2018-q2-update\bin"/>
 				</macros>
 				<externalSettings/>
 				<extensions>
diff --git a/src/BugList.txt b/src/BugList.txt
index e920e51f..a72885b3 100644
--- a/src/BugList.txt
+++ b/src/BugList.txt
@@ -195,7 +195,7 @@ WiFi:
 - [resolve library incompatibility] New ESP8266 SDK
 - [done] check what's new in Arduino ESP 2.4.1
 
-Bug fixes/investigations:
+Done in 2.01beta1:
 - [done, test] sqrt(2) error factor in extrusion-only moves, see Michael's email
 - [done] Homing wrong if axes not labeled sequentially, see Christian's email of 18/6/18
 - [done, test] Fix "2dtstc2diva=u" in debug printout, https://forum.duet3d.com/topic/3139/high-level-of-steperrors-what-can-cause-them/5
@@ -212,30 +212,42 @@ Bug fixes/investigations:
 - [done] Add SW_ENC pin on CONN_SD to available GPIO ports
 - [done] Increased M999 delay to 1 second
 - [done] If M911 resume threshold is set too high, save-on-power-off never primes itself
+- [done] display mutex owners in task diagnostics
+
+Bug fixes/investigations for 2.02beta2:
+- [done, test] GCode queue overflow, see https://forum.duet3d.com/topic/5821/major-performance-problem. Suspend processing until queue space is available.
+- [done, test M28/M29 and M559/M560 file uploads] M28 and M29 in a macro, https://forum.duet3d.com/topic/5642/filament-load-macro-writing-to-file-in-sys. Move fileBeingWritten to GCodeBuffer, get rid of writingFileDirectory.
+- [done, test] we get a Pop underflow message when you send DWC jog commands and axes are not homed
+- [done, test] clear bed compensation not working properly, https://forum.duet3d.com/topic/5978/dwc-ui-position-updates-are-behind-causing-leveling-problems/6
+- [done] Duet Maestro default to TMC2224 drivers on expansion
+- [done] bltouch now uses unfiltered digital probe mode
+- [done] Reset/retry after SD card error - we have confirmation that fatfs is reporting a low-level error (error 1)
+- [increased retries, test detection] Doesn't find DueX5 after M999 reset? https://forum.duet3d.com/topic/5713/duet2-with-2-0-rtos-looses-duex5-on-reset
+- [made some changes] Test Duet 06/085 DHCP problem again, see https://forum.duet3d.com/topic/5572/it-s-out-reprapfirmware-2-0-and-1-21-1-released/16 for config.g files
+- [done, test] resurrect.g to include G92 commands to set head position to the power fail position
+- [done] Software watchdog to watch Heat task
+- [done, test] When an under-voltage event occurs, spurious driver status warnings/error were sometimes reported
+- [done, test] When an under- or over-voltage event occurred, the VIN voltage reported was the current voltage, not the voltage when the event was recorded
+- [done, test] Software reset data now includes which task was active
+- [done, test] Step errors on a drive corresponding to an invisible axis, https://forum.duet3d.com/topic/3139/high-level-of-steperrors-what-can-cause-them/5
+
+- [awaiting reply] Step errors when usimng Numbe and pressure advance, https://forum.duet3d.com/topic/3139/high-level-of-steperrors-what-can-cause-them/5
+- [awaiting reply] GCode file that pauses between parts, https://forum.duet3d.com/topic/5702/printer-keeps-pausing-during-print/15
+- [looks like it was a temperature sensing error] Is there a kink during auto tuning? https://forum.duet3d.com/topic/5582/hot-end-auto-tuning-failed-due. Could explain oddly low dead times from auto tuning.
+- Pressure advance, see https://www.duet3d.com/forum/thread.php?pid=38036#p38036 and https://forum.duet3d.com/topic/1935/more-strange-pressure-advance-behaviour/95
+- Stuck in spin loop, https://forum.duet3d.com/topic/5674/damned-crash-stuck-in-spin-loop
+- Report that if you use FileZilla to upload several short files, some of them are written blank, https://forum.duet3d.com/topic/5992/files-uploaded-over-ftp-occasionally-blank
 
-- Test gcode file that pauses between parts, https://forum.duet3d.com/topic/5702/printer-keeps-pausing-during-print/15
-- Doesn't find DueX5 after M999 reset? https://forum.duet3d.com/topic/5713/duet2-with-2-0-rtos-looses-duex5-on-reset
-- stuck in spin loop, https://forum.duet3d.com/topic/5674/damned-crash-stuck-in-spin-loop
-- step errors, see https://forum.duet3d.com/topic/3139/high-level-of-steperrors-what-can-cause-them/5
 - Investigate failed .BMP conversion, https://forum.duet3d.com/topic/5623/boot-logo-corruption-in-1-21/2
-- Duet Maestro default to TMC2224 drivers on expansion? How to detect them?
-- M28 and M29 in a macro, https://forum.duet3d.com/topic/5642/filament-load-macro-writing-to-file-in-sys. Move fileBeingWritten to GCodeBuffer, get rid of writingFileDirectory.
-- Is there a kink during auto tuning? https://forum.duet3d.com/topic/5582/hot-end-auto-tuning-failed-due. Could explain oddly low dead times from auto tuning.
-- Does bltouch need a default recovery time to allow it to deploy? (probably not)
-- Should bltouch use digital probe mode? Some users having problems with P25 in G31 command.
-- Test Duet 06/085 DHCP problem again, see https://forum.duet3d.com/topic/5572/it-s-out-reprapfirmware-2-0-and-1-21-1-released/16 for config.g files
-- Retry after SD card error?
 
 Other:
-- @wilriker PR for M106 PWM limit
-- [done, test] display mutex owners in task diagnostics
 
 Planned for 2.02:
-- WiFi continuous auto reconnect, or extra M552 parameter? See https://forum.duet3d.com/topic/5765/wifi-module-auto-reconnect
+- @wilriker PR for M106 PWM limit
+- WiFi continuous auto reconnect in client mode, or extra M552 parameter? See https://forum.duet3d.com/topic/5765/wifi-module-auto-reconnect
 - Better dead time measurement during auto tuning. Measure both turn-on and turn-off?
 - Bezier speed curves or other S-curve acceleration, e.g. look at https://github.com/MarlinFirmware/Marlin/pull/10373/files
 - Option to send M280 servo commands just a few times instead of continuously, for E3D
-- Recovery from SD card errors during a print (Dan)
 - Danny's modified SCARA kinematics (workpiece is on a plate, extruder is fixed)
 - Laser support via G1 S parameter, see https://forum.duet3d.com/topic/4702/laser-cnc-support-in-rrf-gcode-semantics/4
 - Look at pushover notification support, https://forum.duet3d.com/topic/169/notification-via-pushover-or-other-service/45
@@ -244,11 +256,10 @@ Planned for 2.02:
 - M584 when assigning a drive, unmap any existing assignment. Also allow an axis to be mapped to driver -1.
 - Add S4 option to G1 command, like S1 but no endstop checks (needed for CoreXY, CoreXZ)
 - CNC: look at G17/18/19, see https://forum.duet3d.com/topic/4669/ooznest-workbee-screw-driven
-- M569 command to allow selection of smart/dumb driver, also allow all 12 drivers to be smart
+- M569 command to allow selection of smart/dumb driver (including on Duet M), also allow all 12 drivers to be smart
 - M569 TOFF parameter, https://forum.duet3d.com/topic/5392/does-m906-set-rms-or-peak-current/28
 - Apostrophe in quoted filename: can we make apostrophe special in SSIDs/passwords but not filenames?
 - If wifi module gets stuck in starting or changing mode state, reset it again
-- If wifi disconnects when in client mode, keep retrying the connection
 - M140 command to set default bed heaters for M140 S commands (e.g. M140 P0:1:2)
 
 Other (some of these may be in 2.02):
@@ -260,7 +271,6 @@ Other (some of these may be in 2.02):
 - support M205 for setting jerk
 - support G12 clean tool?
 - Add fan PWM limit, https://forum.duet3d.com/topic/5370/m106-feature-request-limit-max-pwm-parameter/4
-- M116 extra "close enough" parameter
 - after homing, warn if outside M208 movement limits on SCARA, polar etc.
 - Unexpected heaters off/tool selection behaviour, https://www.duet3d.com/forum/thread.php?pid=43059#p43059
 - warn when using : where ; was probably meant
@@ -271,7 +281,6 @@ Other (some of these may be in 2.02):
 - Auto mount main SD card when inserted
 - Workplace offsets are supposed to be persistant (check NIST), https://www.duet3d.com/forum/thread.php?pid=43755#p43755
 - At the end of a simulation, restore the original workplace coordinate selection
-- looks like we get a Pop underflow message when you send DWC jog commands and axes are not homed
 - Add warning message when print exceeds bounds
 - When uploading while a file is being printed, don't allow the currently-printing file to be replaced
 
@@ -288,7 +297,6 @@ Other (some of these may be in 2.02):
 - laser control, https://www.duet3d.com/forum/thread.php?pid=37891#p37891
 - slow DWC but fast FTP, https://www.duet3d.com/forum/thread.php?pid=38345#p38345
 - no stall detect on Z axis, https://www.duet3d.com/forum/thread.php?pid=38504#p38504, https://www.duet3d.com/forum/thread.php?pid=39484#p39484
-- Pressure advance, see https://www.duet3d.com/forum/thread.php?pid=38036#p38036
 - check that we never enable the drivers before we set vsense
 
 - [don't do] Don't report the motor current for a non-existent extruder
diff --git a/src/Configuration.h b/src/Configuration.h
index 13f5c9d8..d1f60232 100644
--- a/src/Configuration.h
+++ b/src/Configuration.h
@@ -154,6 +154,8 @@ static_assert(MaxCalibrationPoints <= MaxProbePoints, "MaxCalibrationPoints must
 
 // SD card
 constexpr uint32_t SdCardDetectDebounceMillis = 200;	// How long we give the SD card to settle in the socket
+constexpr unsigned int MaxSdCardTries = 3;				// Number of read or write attempts before giving up
+constexpr uint32_t SdCardRetryDelay = 10;				// Number of milliseconds delay between SD transfer retries
 
 // Z probing
 constexpr float DefaultZDive = 5.0;						// Millimetres
@@ -177,8 +179,10 @@ constexpr size_t PASSWORD_LENGTH = 20;
 #if SAM4E || SAM4S || SAME70
 // Increased GCODE_LENGTH on the SAM4 because M587 and M589 commands on the Duet WiFi can get very long
 constexpr size_t GCODE_LENGTH = 161;					// maximum number of non-comment characters in a line of GCode including the null terminator
+constexpr size_t SHORT_GCODE_LENGTH = 61;				// maximum length of a GCode that we can queue to synchronise it to a move
 #else
 constexpr size_t GCODE_LENGTH = 101;					// maximum number of non-comment characters in a line of GCode including the null terminator
+constexpr size_t SHORT_GCODE_LENGTH = 61;				// maximum length of a GCode that we can queue to synchronise it to a move
 #endif
 
 constexpr size_t MaxMessageLength = 256;
@@ -207,6 +211,8 @@ constexpr size_t RESERVED_OUTPUT_BUFFERS = 2;			// Number of reserved output buf
 # error
 #endif
 
+const size_t maxQueuedCodes = 16;						// How many codes can be queued?
+
 // Move system
 constexpr float DefaultFeedRate = 3000.0;				// The initial requested feed rate after resetting the printer, in mm/min
 constexpr float DefaultG0FeedRate = 18000;				// The initial feed rate for G0 commands after resetting the printer, in mm/min
diff --git a/src/Duet/Lwip/contrib/apps/mdns/mdns_responder.c b/src/Duet/Lwip/contrib/apps/mdns/mdns_responder.c
index 5353b367..167b38e7 100644
--- a/src/Duet/Lwip/contrib/apps/mdns/mdns_responder.c
+++ b/src/Duet/Lwip/contrib/apps/mdns/mdns_responder.c
@@ -544,7 +544,11 @@ static void setup_hostnames(struct mdns_state *ms, struct netif *netif)
     sprintf(ms->hostnames[1], "%c%s-%02X%s", hostlen+3, netif->hostname,
             netif->hwaddr[5], dotlocal);
 
+#if 1	// DC42
+    char macaddr[13];
+#else
     char macaddr[12];
+#endif
     sprintf(macaddr, "%02X%02X%02X%02X%02X%02X",
             netif->hwaddr[0], netif->hwaddr[1], netif->hwaddr[2],
             netif->hwaddr[3], netif->hwaddr[4], netif->hwaddr[5]);
diff --git a/src/Duet/Network.cpp b/src/Duet/Network.cpp
index a0ddfaa5..7713c2d8 100644
--- a/src/Duet/Network.cpp
+++ b/src/Duet/Network.cpp
@@ -160,8 +160,11 @@ static void ethernet_rx_callback(uint32_t ul_status)
 
 static void conn_err(void *arg, err_t err)
 {
-	// Report the error to the monitor
-	reprap.GetPlatform().MessageF(UsbMessage, "Network: Connection error, code %d\n", err);
+	if (reprap.Debug(moduleNetwork) && !inInterrupt())
+	{
+		// Report the error to the monitor
+		reprap.GetPlatform().MessageF(UsbMessage, "Network: Connection error, code %d\n", err);
+	}
 
 	// Tell the higher levels about the error
 	ConnectionState *cs = (ConnectionState*)arg;
@@ -179,7 +182,10 @@ static err_t conn_recv(void *arg, tcp_pcb *pcb, pbuf *p, err_t err)
 	{
 		if (cs->pcb != pcb)
 		{
-			reprap.GetPlatform().Message(UsbMessage, "Network: Mismatched pcb in conn_recv!\n");
+			if (reprap.Debug(moduleNetwork) && !inInterrupt())
+			{
+				reprap.GetPlatform().Message(UsbMessage, "Network: Mismatched pcb in conn_recv!\n");
+			}
 			tcp_abort(pcb);
 			return ERR_ABRT;
 		}
diff --git a/src/DuetM/TMC22xx.cpp b/src/DuetM/TMC22xx.cpp
index 5f373c26..4a71f2d4 100644
--- a/src/DuetM/TMC22xx.cpp
+++ b/src/DuetM/TMC22xx.cpp
@@ -268,6 +268,7 @@ public:
 	void AppendDriverStatus(const StringRef& reply);
 	uint8_t GetDriverNumber() const { return driverNumber; }
 	bool UpdatePending() const { return registersToUpdate != 0; }
+	bool UsesGlobalEnable() const { return enablePin == NoPin; }
 
 	float GetStandstillCurrentPercent() const;
 	void SetStandstillCurrentPercent(float percent);
@@ -640,24 +641,27 @@ inline void TmcDriverState::TransferDone()
 		}
 		lastIfCount = currentIfCount;
 	}
-	else if (sendData[2] == ReadRegNumbers[registerToRead] && ReadRegNumbers[registerToRead] == receiveData[6] && receiveData[4] == 0x05 && receiveData[5] == 0xFF)
+	else if (driversState != DriversState::noPower)		// we don't check the CRC, so only accept the result if power is still good
 	{
-		// We asked to read the scheduled read register, and the sync byte, slave address and register number in the received message match
-		//TODO here we could check the CRC of the received message, but for now we assume that we won't get any corruption in the 32-bit received data
-		const uint32_t regVal = ((uint32_t)receiveData[7] << 24) | ((uint32_t)receiveData[8] << 16) | ((uint32_t)receiveData[9] << 8) | receiveData[10];
-		readRegisters[registerToRead] = regVal;
-		accumulatedReadRegisters[registerToRead] |= regVal;
-
-		++registerToRead;
-		if (registerToRead == NumReadRegisters)
+		if (sendData[2] == ReadRegNumbers[registerToRead] && ReadRegNumbers[registerToRead] == receiveData[6] && receiveData[4] == 0x05 && receiveData[5] == 0xFF)
 		{
-			registerToRead = 0;
+			// We asked to read the scheduled read register, and the sync byte, slave address and register number in the received message match
+			//TODO here we could check the CRC of the received message, but for now we assume that we won't get any corruption in the 32-bit received data
+			const uint32_t regVal = ((uint32_t)receiveData[7] << 24) | ((uint32_t)receiveData[8] << 16) | ((uint32_t)receiveData[9] << 8) | receiveData[10];
+			readRegisters[registerToRead] = regVal;
+			accumulatedReadRegisters[registerToRead] |= regVal;
+
+			++registerToRead;
+			if (registerToRead == NumReadRegisters)
+			{
+				registerToRead = 0;
+			}
+			++numReads;
+		}
+		else
+		{
+			++readErrors;
 		}
-		++numReads;
-	}
-	else
-	{
-		++readErrors;
 	}
 }
 
@@ -903,7 +907,6 @@ namespace SmartDrivers
 			if (powered)
 			{
 				// Power to the drivers has been provided or restored, so we need to enable and re-initialise them
-
 				for (size_t drive = 0; drive < numTmc22xxDrivers; ++drive)
 				{
 					driverStates[drive].WriteAll();
@@ -938,11 +941,11 @@ namespace SmartDrivers
 
 			if (driversState == DriversState::initialising)
 			{
-				// If all drivers have been initialised, set the global enable
+				// If all drivers that share the global enable have been initialised, set the global enable
 				bool allInitialised = true;
 				for (size_t i = 0; i < numTmc22xxDrivers; ++i)
 				{
-					if (driverStates[i].UpdatePending())
+					if (driverStates[i].UsesGlobalEnable() && driverStates[i].UpdatePending())
 					{
 						allInitialised = false;
 						break;
diff --git a/src/DuetNG/DueXn.cpp b/src/DuetNG/DueXn.cpp
index 1e3418b2..3a642559 100644
--- a/src/DuetNG/DueXn.cpp
+++ b/src/DuetNG/DueXn.cpp
@@ -59,12 +59,15 @@ namespace DuetExpansion
 	{
 		reprap.GetPlatform().InitI2c();					// initialise I2C
 
-		bool ret = dueXnExpander.begin(DueXnAddress);
-		if (!ret)
+		// DC 2018-07-12: occasionally the SX1509B isn't found after doing a software reset, so try a few more attempts
+		bool ret;
+		unsigned int attempts = 0;
+		do
 		{
-			delay(100);									// wait a little while
-			ret = dueXnExpander.begin(DueXnAddress);	// do 1 retry
-		}
+			++attempts;
+			delay(50);
+			ret = dueXnExpander.begin(DueXnAddress);
+		} while (!ret && attempts < 5);
 
 		if (ret)
 		{
@@ -102,12 +105,15 @@ namespace DuetExpansion
 	void AdditionalOutputInit()
 	{
 		reprap.GetPlatform().InitI2c();										// initialise I2C
-		bool ret = additionalIoExpander.begin(AdditionalIoExpanderAddress);
-		if (!ret)
+
+		bool ret;
+		unsigned int attempts = 0;
+		do
 		{
-			delay(100);														// wait a little while
-			ret = additionalIoExpander.begin(AdditionalIoExpanderAddress);	// do 1 retry
-		}
+			++attempts;
+			delay(50);
+			ret = additionalIoExpander.begin(AdditionalIoExpanderAddress);
+		} while (!ret && attempts < 5);
 
 		if (ret)
 		{
diff --git a/src/DuetNG/SX1509.cpp b/src/DuetNG/SX1509.cpp
index 9b7a54d7..3c601e28 100644
--- a/src/DuetNG/SX1509.cpp
+++ b/src/DuetNG/SX1509.cpp
@@ -21,11 +21,10 @@ local, and you've found our code helpful, please buy us a round!
 Distributed as-is; no warranty is given.
 ******************************************************************************/
 
-#include "Core.h"
+#include "RepRapFirmware.h"
 #include "Wire.h"
 #include "SX1509.h"
 #include "SX1509Registers.h"
-#include "Pins.h"
 
 SX1509::SX1509() : _clkX(0), errorCount(0)
 {
@@ -38,6 +37,7 @@ bool SX1509::begin(uint8_t address)
 	deviceAddress =  address;
 	
 	reset();
+	delay(2);														// not sure this is needed, but it may help
 
 	pwmPins = 0;
 
diff --git a/src/DuetNG/TMC2660.cpp b/src/DuetNG/TMC2660.cpp
index c601fcf0..afaf625e 100644
--- a/src/DuetNG/TMC2660.cpp
+++ b/src/DuetNG/TMC2660.cpp
@@ -501,27 +501,30 @@ uint32_t TmcDriverState::GetChopConf() const
 // This is called by the ISR when the SPI transfer has completed
 inline void TmcDriverState::TransferDone()
 {
-	fastDigitalWriteHigh(pin);								// set the CS pin high for the driver we just polled
-	uint32_t status = be32_to_cpu(spiDataIn) >> 12;			// get the status
-	const uint32_t interval = reprap.GetMove().GetStepInterval(axisNumber, microstepShiftFactor);		// get the full step interval
-	if (interval == 0 || interval > maxStallStepInterval)	// if the motor speed is too low to get reliable stall indication
+	fastDigitalWriteHigh(pin);									// set the CS pin high for the driver we just polled
+	if (driversPowered)											// if the power is still good, update the status
 	{
-		status &= ~TMC_RR_SG;								// remove the stall status bit
-	}
-	else
-	{
-		const uint32_t sgLoad = (status >> TMC_RR_SG_LOAD_SHIFT) & 1023;	// get the StallGuard load register
-		if (sgLoad < minSgLoadRegister)
+		uint32_t status = be32_to_cpu(spiDataIn) >> 12;			// get the status
+		const uint32_t interval = reprap.GetMove().GetStepInterval(axisNumber, microstepShiftFactor);		// get the full step interval
+		if (interval == 0 || interval > maxStallStepInterval)	// if the motor speed is too low to get reliable stall indication
 		{
-			minSgLoadRegister = sgLoad;
+			status &= ~TMC_RR_SG;								// remove the stall status bit
 		}
-		if (sgLoad > maxSgLoadRegister)
+		else
 		{
-			maxSgLoadRegister = sgLoad;
+			const uint32_t sgLoad = (status >> TMC_RR_SG_LOAD_SHIFT) & 1023;	// get the StallGuard load register
+			if (sgLoad < minSgLoadRegister)
+			{
+				minSgLoadRegister = sgLoad;
+			}
+			if (sgLoad > maxSgLoadRegister)
+			{
+				maxSgLoadRegister = sgLoad;
+			}
 		}
+		lastReadStatus = status;
+		accumulatedStatus |= status;
 	}
-	lastReadStatus = status;
-	accumulatedStatus |= status;
 }
 
 // This is called from the ISR or elsewhere to start a new SPI transfer. Inlined for ISR speed.
diff --git a/src/GCodes/GCodeBuffer.cpp b/src/GCodes/GCodeBuffer.cpp
index 3c190852..de098702 100644
--- a/src/GCodes/GCodeBuffer.cpp
+++ b/src/GCodes/GCodeBuffer.cpp
@@ -8,14 +8,16 @@
 //*************************************************************************************
 
 #include "GCodeBuffer.h"
-
+#include "GCodes.h"
 #include "GCodeInput.h"
 #include "Platform.h"
 #include "RepRap.h"
 
+static constexpr char eofString[] = EOF_STRING;		// What's at the end of an HTML file?
+
 // Create a default GCodeBuffer
 GCodeBuffer::GCodeBuffer(const char* id, MessageType mt, bool usesCodeQueue)
-	: machineState(new GCodeMachineState()), identity(id), writingFileDirectory(nullptr),
+	: machineState(new GCodeMachineState()), identity(id), fileBeingWritten(nullptr), writingFileSize(0), eofStringCounter(0),
 	  toolNumberAdjust(0), responseMessageType(mt), checksumRequired(false), queueCodes(usesCodeQueue), binaryWriting(false)
 {
 	Init();
@@ -253,7 +255,7 @@ bool GCodeBuffer::LineFinished()
 	gcodeBuffer[gcodeLineEnd] = 0;
 	const bool badChecksum = (hadChecksum && computedChecksum != declaredChecksum);
 	const bool missingChecksum = (checksumRequired && !hadChecksum && machineState->previous == nullptr);
-	if (reprap.Debug(moduleGcodes) && !writingFileDirectory)
+	if (reprap.Debug(moduleGcodes) && fileBeingWritten == nullptr)
 	{
 		reprap.GetPlatform().MessageF(DebugMessage, "%s%s: %s\n", identity, ((badChecksum) ? "(bad-csum)" : (missingChecksum) ? "(no-csum)" : ""), gcodeBuffer);
 	}
@@ -760,6 +762,28 @@ void GCodeBuffer::TryGetBValue(char c, bool& val, bool& seen)
 	}
 }
 
+// Try to get an int array exactly 'numVals' long after parameter letter 'c'.
+// If the wrong number of values is provided, generate an error message and return true.
+// Else set 'seen' if we saw the letter and value, and return false.
+bool GCodeBuffer::TryGetUIArray(char c, size_t numVals, uint32_t vals[], const StringRef& reply, bool& seen, bool doPad)
+{
+	if (Seen(c))
+	{
+		size_t count = numVals;
+		GetUnsignedArray(vals, count, doPad);
+		if (count == numVals)
+		{
+			seen = true;
+		}
+		else
+		{
+			reply.printf("Wrong number of values after '\''%c'\'', expected %d", c, numVals);
+			return true;
+		}
+	}
+	return false;
+}
+
 // Try to get a float array exactly 'numVals' long after parameter letter 'c'.
 // If the wrong number of values is provided, generate an error message and return true.
 // Else set 'seen' if we saw the letter and value, and return false.
@@ -962,16 +986,20 @@ bool GCodeBuffer::PopState()
 }
 
 // Abort execution of any files or macros being executed, returning true if any files were closed
+// We now avoid popping the state if we were not executing from a file, so that if DWC or PanelDue is used to jog the axes before they are homed, we don't report stack underflow.
 void GCodeBuffer::AbortFile(FileGCodeInput* fileInput)
 {
-	do
+	if (machineState->fileState.IsLive())
 	{
-		if (machineState->fileState.IsLive())
+		do
 		{
-			fileInput->Reset(machineState->fileState);
-			machineState->fileState.Close();
-		}
-	} while (PopState());							// abandon any macros
+			if (machineState->fileState.IsLive())
+			{
+				fileInput->Reset(machineState->fileState);
+				machineState->fileState.Close();
+			}
+		} while (PopState());							// abandon any macros
+	}
 }
 
 // Return true if this source is executing a file macro
@@ -1011,4 +1039,95 @@ void GCodeBuffer::PrintCommand(const StringRef& s) const
 	}
 }
 
+// Open a file to write to
+bool GCodeBuffer::OpenFileToWrite(const char* directory, const char* fileName, const FilePosition size, const bool binaryWrite, const uint32_t fileCRC32)
+{
+	fileBeingWritten = reprap.GetPlatform().OpenFile(directory, fileName, OpenMode::write);
+	eofStringCounter = 0;
+	writingFileSize = size;
+	if (fileBeingWritten == nullptr)
+	{
+		return false;
+	}
+
+	crc32 = fileCRC32;
+	binaryWriting = binaryWrite;
+	return true;
+}
+
+// Write the current GCode to file
+void GCodeBuffer::WriteToFile()
+{
+	if (GetCommandLetter() == 'M')
+	{
+		if (GetCommandNumber() == 29)						// end of file?
+		{
+			fileBeingWritten->Close();
+			fileBeingWritten = nullptr;
+			const char* const r = (reprap.GetPlatform().Emulating() == Compatibility::marlin) ? "Done saving file." : "";
+			reprap.GetGCodes().HandleReply(*this, GCodeResult::ok, r);
+			SetFinished(true);
+			return;
+		}
+	}
+	else if (GetCommandLetter() == 'G' && GetCommandNumber() == 998)						// resend request?
+	{
+		if (Seen('P'))
+		{
+			String<ShortScratchStringLength> scratchString;
+			scratchString.printf("%" PRIi32 "\n", GetIValue());
+			reprap.GetGCodes().HandleReply(*this, GCodeResult::ok, scratchString.c_str());
+			SetFinished(true);
+			return;
+		}
+	}
+
+	fileBeingWritten->Write(Buffer());
+	fileBeingWritten->Write('\n');
+	SetFinished(true);
+}
+
+void GCodeBuffer::WriteBinaryToFile(char b)
+{
+	if (b == eofString[eofStringCounter] && writingFileSize == 0)
+	{
+		eofStringCounter++;
+		if (eofStringCounter < ARRAY_SIZE(eofString) - 1)
+		{
+			return;			// not reached end of input yet
+		}
+	}
+	else
+	{
+		if (eofStringCounter != 0)
+		{
+			for (uint8_t i = 0; i < eofStringCounter; i++)
+			{
+				fileBeingWritten->Write(eofString[i]);
+			}
+			eofStringCounter = 0;
+		}
+		fileBeingWritten->Write(b);		// writing one character at a time isn't very efficient, but uploading HTML files via USB is rarely done these days
+		if (writingFileSize == 0 || fileBeingWritten->Length() < writingFileSize)
+		{
+			return;			// not reached end of input yet
+		}
+	}
+
+	// If we get here then we have come to the end of the data
+	fileBeingWritten->Close();
+	const bool crcOk = (crc32 == fileBeingWritten->GetCRC32() || crc32 == 0);
+	fileBeingWritten = nullptr;
+	binaryWriting = false;
+	if (crcOk)
+	{
+		const char* const r = (reprap.GetPlatform().Emulating() == Compatibility::marlin) ? "Done saving file." : "";
+		reprap.GetGCodes().HandleReply(*this, GCodeResult::ok, r);
+	}
+	else
+	{
+		reprap.GetGCodes().HandleReply(*this, GCodeResult::error, "CRC32 checksum doesn't match");
+	}
+}
+
 // End
diff --git a/src/GCodes/GCodeBuffer.h b/src/GCodes/GCodeBuffer.h
index 6675cf2a..bd34a354 100644
--- a/src/GCodes/GCodeBuffer.h
+++ b/src/GCodes/GCodeBuffer.h
@@ -48,7 +48,8 @@ public:
 	void TryGetUIValue(char c, uint32_t& val, bool& seen);
 	void TryGetBValue(char c, bool& val, bool& seen);
 	bool TryGetFloatArray(char c, size_t numVals, float vals[], const StringRef& reply, bool& seen, bool doPad = false);
-	bool TryGetQuotedString(char c, const StringRef& str, bool& seen);
+	bool TryGetUIArray(char c, size_t numVals, uint32_t vals[], const StringRef& reply, bool& seen, bool doPad = false);
+ 	bool TryGetQuotedString(char c, const StringRef& str, bool& seen);
 	bool TryGetPossiblyQuotedString(char c, const StringRef& str, bool& seen);
 
 	const char* Buffer() const;
@@ -57,8 +58,6 @@ public:
 	bool IsReady() const;								// Return true if a gcode is ready but hasn't been started yet
 	bool IsExecuting() const;							// Return true if a gcode has been started and is not paused
 	void SetFinished(bool f);							// Set the G Code executed (or not)
-	const char* WritingFileDirectory() const;			// If we are writing the G Code to a file, where that file is
-	void SetWritingFileDirectory(const char* wfd);		// Set the directory for the file to write the GCode in
 	int GetToolNumberAdjust() const { return toolNumberAdjust; }
 	void SetToolNumberAdjust(int arg) { toolNumberAdjust = arg; }
 	void SetCommsProperties(uint32_t arg) { checksumRequired = (arg & 1); }
@@ -79,10 +78,16 @@ public:
 	bool CanQueueCodes() const;
 	void MessageAcknowledged(bool cancelled);
 	FilePosition GetFilePosition(size_t bytesCached) const;	// Get the file position at the start of the current command
-	bool IsWritingBinary() const;						// returns true if writing binary
-	void SetBinaryWriting(bool state);					// set true if writing binary
-	uint32_t GetCRC32() const;
-	void SetCRC32(uint32_t newCRC32);
+
+	bool OpenFileToWrite(const char* directory, const char* fileName, const FilePosition size, const bool binaryWrite, const uint32_t fileCRC32);	// open a file to write to
+	bool IsWritingFile() const { return fileBeingWritten != nullptr; }		// returns true if writing a file
+	void WriteToFile();														// write the current GCode to file
+
+	bool IsWritingBinary() const { return IsWritingFile() && binaryWriting; }	// returns true if writing binary
+	void WriteBinaryToFile(char b);											// write a byte to the file
+
+	size_t CommandLength() const { return commandEnd - commandStart; }		// get the length of the current command
+	const char* CommandStart() const { return gcodeBuffer + commandStart; }	// get the start of the current command
 
 	void PrintCommand(const StringRef& s) const;
 
@@ -123,7 +128,11 @@ private:
 	unsigned int gcodeLineEnd;							// Number of characters in the entire line of gcode
 	int readPointer;									// Where in the buffer to read next
 	GCodeBufferState bufferState;						// Idle, executing or paused
-	const char* writingFileDirectory;					// If the G Code is going into a file, where that is
+
+	FileStore *fileBeingWritten;						// If we are copying GCodes to a file, which file it is
+	FilePosition writingFileSize;						// Size of the file being written, or zero if not known
+	uint8_t eofStringCounter;							// Check the...
+
 	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;
@@ -145,26 +154,6 @@ private:
 	bool binaryWriting;									// Executing gcode or writing binary file?
 };
 
-inline uint32_t GCodeBuffer::GetCRC32() const
-{
-	return crc32;
-}
-
-inline void GCodeBuffer::SetCRC32(uint32_t newCRC32)
-{
-	crc32 = newCRC32;
-}
-
-inline bool GCodeBuffer::IsWritingBinary() const
-{
-	return binaryWriting;
-}
-
-inline void GCodeBuffer::SetBinaryWriting(bool state)
-{
-	binaryWriting = state;
-}
-
 inline const char* GCodeBuffer::Buffer() const
 {
 	return gcodeBuffer;
@@ -190,16 +179,6 @@ inline bool GCodeBuffer::IsExecuting() const
 	return bufferState == GCodeBufferState::executing;
 }
 
-inline const char* GCodeBuffer::WritingFileDirectory() const
-{
-	return writingFileDirectory;
-}
-
-inline void GCodeBuffer::SetWritingFileDirectory(const char* wfd)
-{
-	writingFileDirectory = wfd;
-}
-
 inline GCodeState GCodeBuffer::GetState() const
 {
 	return machineState->state;
diff --git a/src/GCodes/GCodeInput.cpp b/src/GCodes/GCodeInput.cpp
index 2e7383db..155e71e0 100644
--- a/src/GCodes/GCodeInput.cpp
+++ b/src/GCodes/GCodeInput.cpp
@@ -21,15 +21,13 @@ bool GCodeInput::FillBuffer(GCodeBuffer *gb)
 		if (gb->IsWritingBinary())
 		{
 			// HTML uploads are handled by the GCodes class
-			reprap.GetGCodes().WriteHTMLToFile(*gb, c);
+			gb->WriteBinaryToFile(c);
 		}
 		else if (gb->Put(c))
 		{
-			// Check if we can finish a file upload
-			if (gb->WritingFileDirectory() != nullptr)
+			if (gb->IsWritingFile())
 			{
-				reprap.GetGCodes().WriteGCodeToFile(*gb);
-				gb->SetFinished(true);
+				gb->WriteToFile();
 			}
 
 			// Code is complete, stop here
@@ -116,7 +114,7 @@ void NetworkGCodeInput::Put(MessageType mtype, char c)
 				return;
 			}
 
-			state = (c == 'M') ? GCodeInputState::doingMCode : GCodeInputState::doingCode;
+			state = (c == 'M' || c == 'm') ? GCodeInputState::doingMCode : GCodeInputState::doingCode;
 			break;
 
 		case GCodeInputState::doingCode:
diff --git a/src/GCodes/GCodeQueue.cpp b/src/GCodes/GCodeQueue.cpp
index 207bfd17..97240d36 100644
--- a/src/GCodes/GCodeQueue.cpp
+++ b/src/GCodes/GCodeQueue.cpp
@@ -32,118 +32,94 @@ GCodeQueue::GCodeQueue() : freeItems(nullptr), queuedItems(nullptr)
 	}
 #endif
 
-	switch (gb.GetCommandLetter())
+	// Don't queue anything if no moves are being performed
+	const uint32_t scheduledMoves = reprap.GetMove().GetScheduledMoves();
+	if (scheduledMoves != reprap.GetMove().GetCompletedMoves())
 	{
-	case 'G':
+		switch (gb.GetCommandLetter())
 		{
-			const int code = gb.GetCommandNumber();
-			return code == 10 && gb.Seen('P');			// Set active/standby temperatures
-		}
+		case 'G':
+			{
+				const int code = gb.GetCommandNumber();
+				return code == 10 && gb.Seen('P');			// Set active/standby temperatures
+			}
 
-	case 'M':
-		{
-			switch (gb.GetCommandNumber())
+		case 'M':
 			{
-			case 3:		// spindle control
-			case 4:		// spindle control
-			case 5:		// spindle control
-			case 42:	// set IO pin
-			case 106:	// fan control
-			case 107:	// fan off
-			case 104:	// set temperatures and return immediately
-			case 140:	// set bed temperature and return immediately
-			case 141:	// set chamber temperature and return immediately
-			case 144:	// bed standby
-			case 117:	// display message
-			case 280:	// set servo
-			case 300:	// beep
-			case 420:	// set RGB colour
-				return true;
-
-			case 291:
+				switch (gb.GetCommandNumber())
 				{
-					bool seen = false;
-					int32_t sParam = 1;
-					gb.TryGetIValue('S', sParam, seen);
-					return sParam < 2;					// queue non-blocking messages only
+				case 3:		// spindle control
+				case 4:		// spindle control
+				case 5:		// spindle control
+				case 42:	// set IO pin
+				case 106:	// fan control
+				case 107:	// fan off
+				case 104:	// set temperatures and return immediately
+				case 140:	// set bed temperature and return immediately
+				case 141:	// set chamber temperature and return immediately
+				case 144:	// bed standby
+				case 117:	// display message
+				case 280:	// set servo
+				case 300:	// beep
+				case 420:	// set RGB colour
+					return true;
+
+				case 291:
+					{
+						bool seen = false;
+						int32_t sParam = 1;
+						gb.TryGetIValue('S', sParam, seen);
+						return sParam < 2;					// queue non-blocking messages only
+					}
+
+				default:
+					break;
 				}
-
-			default:
-				break;
 			}
-		}
-		break;
+			break;
 
-	default:
-		break;
+		default:
+			break;
+		}
 	}
 
 	return false;
 }
 
-// If moves are scheduled and a command can be queued, try to queue the command in the passed GCodeBuffer.
-// If successful, return true to indicate it has been queued and the caller should not execute it.
-// If the queue is full, free up the oldest queued entry by copying its command to our own gcode buffer so that we have room to queue the original command.
+// Try to queue the command in the passed GCodeBuffer.
+// If successful, return true to indicate it has been queued.
+// If the queue is full or the command is too long to be queued, return false.
 bool GCodeQueue::QueueCode(GCodeBuffer &gb)
 {
-	// Don't queue anything if no moves are being performed
-	const uint32_t scheduledMoves = reprap.GetMove().GetScheduledMoves();
-	bool queueCode = (scheduledMoves != reprap.GetMove().GetCompletedMoves());
-
-	if (queueCode)
+	// Can we queue this code somewhere?
+	if (freeItems == nullptr || gb.CommandLength() > SHORT_GCODE_LENGTH - 1)
 	{
-		char codeToRun[GCODE_LENGTH];
-		size_t codeToRunLength;
-
-		// Can we queue this code somewhere?
-		if (freeItems == nullptr)
-		{
-			// No - we've run out of free items. Run the first outstanding code
-			queueCode = false;
-			codeToRunLength = strlen(queuedItems->code) + 1;
-			SafeStrncpy(codeToRun, queuedItems->code, ARRAY_SIZE(codeToRun));
-
-			// Release the first queued item so that it can be reused later
-			QueuedCode * const item = queuedItems;
-			queuedItems = item->next;
-			item->next = nullptr;
-			freeItems = item;
-		}
-
-		// Unlink a free element and assign gb's code to it
-		QueuedCode * const code = freeItems;
-		freeItems = code->next;
-		code->AssignFrom(gb);
-		code->executeAtMove = scheduledMoves;
-		code->next = nullptr;
+		return false;
+	}
 
-		// Append it to the list of queued codes
-		if (queuedItems == nullptr)
-		{
-			queuedItems = code;
-		}
-		else
-		{
-			QueuedCode *last = queuedItems;
-			while (last->Next() != nullptr)
-			{
-				last = last->Next();
-			}
-			last->next = code;
-		}
+	// Unlink a free element and assign gb's code to it
+	QueuedCode * const code = freeItems;
+	freeItems = code->next;
+	code->AssignFrom(gb);
+	code->executeAtMove = reprap.GetMove().GetScheduledMoves();
+	code->next = nullptr;
 
-		// Overwrite the passed gb's content if we could not store its original code
-		if (!queueCode)
+	// Append it to the list of queued codes
+	if (queuedItems == nullptr)
+	{
+		queuedItems = code;
+	}
+	else
+	{
+		QueuedCode *last = queuedItems;
+		while (last->Next() != nullptr)
 		{
-			if (reprap.Debug(moduleGcodes))
-			{
-				reprap.GetPlatform().Message(DebugMessage, "(swap) ");
-			}
-			gb.Put(codeToRun, codeToRunLength);
+			last = last->Next();
 		}
+		last->next = code;
 	}
 
-	return queueCode;
+	return true;
 }
 
 bool GCodeQueue::FillBuffer(GCodeBuffer *gb)
@@ -237,7 +213,9 @@ void GCodeQueue::Diagnostics(MessageType mtype)
 void QueuedCode::AssignFrom(GCodeBuffer &gb)
 {
 	toolNumberAdjust = gb.GetToolNumberAdjust();
-	SafeStrncpy(code, gb.Buffer(), ARRAY_SIZE(code));
+	const size_t length = min<size_t>(gb.CommandLength(), ARRAY_SIZE(code) - 1);
+	memcpy(code, gb.CommandStart(), length);
+	code[length] = 0;
 }
 
 void QueuedCode::AssignTo(GCodeBuffer *gb)
diff --git a/src/GCodes/GCodeQueue.h b/src/GCodes/GCodeQueue.h
index 26f6e065..a366218f 100644
--- a/src/GCodes/GCodeQueue.h
+++ b/src/GCodes/GCodeQueue.h
@@ -10,8 +10,6 @@
 #include "RepRapFirmware.h"
 #include "GCodeBuffer.h"
 
-const size_t maxQueuedCodes = 8;				// How many codes can be queued?
-
 class QueuedCode;
 
 class GCodeQueue
@@ -44,7 +42,7 @@ public:
 private:
 	QueuedCode *next;
 
-	char code[GCODE_LENGTH];
+	char code[SHORT_GCODE_LENGTH];
 	uint32_t executeAtMove;
 	int toolNumberAdjust;
 
diff --git a/src/GCodes/GCodes.cpp b/src/GCodes/GCodes.cpp
index 0fd79e68..8724e9e7 100644
--- a/src/GCodes/GCodes.cpp
+++ b/src/GCodes/GCodes.cpp
@@ -105,16 +105,14 @@ void GCodes::Init()
 	{
 		rawExtruderTotalByDrive[extruder] = 0.0;
 	}
-	eofString = EOF_STRING;
-	eofStringCounter = 0;
-	eofStringLength = strlen(eofString);
+
 	runningConfigFile = false;
 	m501SeenInConfigFile = false;
 	doingToolChange = false;
 	active = true;
-	fileSize = 0;
 	limitAxes = noMovesBeforeHoming = true;
 	SetAllAxesNotHomed();
+
 	for (size_t i = 0; i < NUM_FANS; ++i)
 	{
 		pausedFanSpeeds[i] = 0.0;
@@ -160,7 +158,6 @@ void GCodes::Reset()
 	nextGcodeSource = 0;
 
 	fileToPrint.Close();
-	fileBeingWritten = nullptr;
 	speedFactor = SecondsToMinutes;						// default is just to convert from mm/minute to mm/second
 
 	for (size_t i = 0; i < MaxExtruders; ++i)
@@ -1441,7 +1438,11 @@ void GCodes::DoFilePrint(GCodeBuffer& gb, const StringRef& reply)
 		// Yes - fill up the GCodeBuffer and run the next code
 		if (fileInput->FillBuffer(&gb))
 		{
-			gb.SetFinished(ActOnCode(gb, reply));
+			// We read some data, but we don't necessarily have a command available because we may be executing M28 within a file
+			if (gb.IsReady())
+			{
+				gb.SetFinished(ActOnCode(gb, reply));
+			}
 		}
 		break;
 
@@ -1959,6 +1960,17 @@ void GCodes::SaveResumeInfo(bool wasPowerFailure)
 					&& reprap.GetMove().WriteResumeSettings(f);				// load grid, if we are using one
 			if (ok)
 			{
+				// Write a G92 command to say where the head is. This is useful if we can't Z-home the printer with a print on the bed and the Z steps/mm is high.
+				buf.copy("G92");
+				for (size_t axis = 0; axis < numVisibleAxes; ++axis)
+				{
+					buf.catf(" %c%.3f", axisLetters[axis], (double)pauseRestorePoint.moveCoords[axis]);
+				}
+				buf.cat('\n');
+				ok = f->Write(buf.c_str());
+			}
+			if (ok)
+			{
 				buf.printf("M98 P%s\n", RESUME_PROLOGUE_G);					// call the prologue - must contain at least M116
 				ok = f->Write(buf.c_str())
 					&& platform.WriteFanSettings(f);						// set the speeds of non-thermostatic fans
@@ -2007,7 +2019,7 @@ void GCodes::SaveResumeInfo(bool wasPowerFailure)
 				{
 					if (axis != Z_AXIS)
 					{
-						buf.catf(" %c%.2f", axisLetters[axis], (double)pauseRestorePoint.moveCoords[axis]);
+						buf.catf(" %c%.3f", axisLetters[axis], (double)pauseRestorePoint.moveCoords[axis]);
 					}
 				}
 
@@ -3058,6 +3070,7 @@ bool GCodes::SaveHeightMap(GCodeBuffer& gb, const StringRef& reply) const
 void GCodes::ClearBedMapping()
 {
 	reprap.GetMove().SetIdentityTransform();
+	reprap.GetMove().GetCurrentUserPosition(moveBuffer.coords, 0, reprap.GetCurrentXAxes(), reprap.GetCurrentYAxes());
 	ToolOffsetInverseTransform(moveBuffer.coords, currentUserPosition);		// update user coordinates to remove any height map offset there was at the current position
 }
 
@@ -3103,114 +3116,6 @@ void GCodes::GetCurrentCoordinates(const StringRef& s) const
 	}
 }
 
-bool GCodes::OpenFileToWrite(GCodeBuffer& gb, const char* directory, const char* fileName, const FilePosition size, const bool binaryWrite, const uint32_t fileCRC32)
-{
-	fileBeingWritten = platform.OpenFile(directory, fileName, OpenMode::write);
-	eofStringCounter = 0;
-	fileSize = size;
-	if (fileBeingWritten == nullptr)
-	{
-		platform.MessageF(ErrorMessage, "Failed to open GCode file \"%s\" for writing.\n", fileName);
-		return false;
-	}
-	else
-	{
-		gb.SetCRC32(fileCRC32);
-		gb.SetBinaryWriting(binaryWrite);
-		gb.SetWritingFileDirectory(directory);
-		return true;
-	}
-}
-
-void GCodes::WriteHTMLToFile(GCodeBuffer& gb, char b)
-{
-	if (fileBeingWritten == nullptr)
-	{
-		platform.Message(ErrorMessage, "Attempt to write to a null file.\n");
-		return;
-	}
-
-	if ((b == eofString[eofStringCounter]) && (fileSize == 0))
-	{
-		eofStringCounter++;
-		if (eofStringCounter >= eofStringLength)
-		{
-			FinishWrite(gb);
-		}
-	}
-	else
-	{
-		if (eofStringCounter != 0)
-		{
-			for (uint8_t i = 0; i < eofStringCounter; i++)
-			{
-				fileBeingWritten->Write(eofString[i]);
-			}
-			eofStringCounter = 0;
-		}
-		fileBeingWritten->Write(b);		// writing one character at a time isn't very efficient, but uploading HTML files via USB is rarely done these days
-		if (fileSize > 0 && fileBeingWritten->Length() >= fileSize)
-		{
-			FinishWrite(gb);
-		}
-	}
-}
-
-void GCodes::FinishWrite(GCodeBuffer& gb)
-{
-	const char* r;
-	fileBeingWritten->Close();
-	if ((gb.GetCRC32() != fileBeingWritten->GetCRC32()) && (gb.GetCRC32() != 0))
-	{
-		r = "Error: CRC32 checksum doesn't match";
-	}
-	else
-	{
-		r = (platform.Emulating() == Compatibility::marlin) ? "Done saving file." : "";
-	}
-	fileBeingWritten = nullptr;
-	gb.SetBinaryWriting(false);
-	gb.SetWritingFileDirectory(nullptr);
-
-	HandleReply(gb, GCodeResult::ok, r);
-}
-
-void GCodes::WriteGCodeToFile(GCodeBuffer& gb)
-{
-	if (fileBeingWritten == nullptr)
-	{
-		platform.Message(ErrorMessage, "Attempt to write to a null file.\n");
-		return;
-	}
-
-	if (gb.GetCommandLetter() == 'M')
-	{
-		if (gb.GetCommandNumber() == 29)						// end of file?
-		{
-			fileBeingWritten->Close();
-			fileBeingWritten = nullptr;
-			gb.SetWritingFileDirectory(nullptr);
-			const char* r = (platform.Emulating() == Compatibility::marlin) ? "Done saving file." : "";
-			HandleReply(gb, GCodeResult::ok, r);
-			return;
-		}
-	}
-	else if (gb.GetCommandLetter() == 'G' && gb.GetCommandNumber() == 998)						// resend request?
-	{
-		if (gb.Seen('P'))
-		{
-			String<ShortScratchStringLength> scratchString;
-			scratchString.printf("%" PRIi32 "\n", gb.GetIValue());
-			HandleReply(gb, GCodeResult::ok, scratchString.c_str());
-			return;
-		}
-	}
-
-	fileBeingWritten->Write(gb.Buffer());
-	fileBeingWritten->Write('\n');
-	HandleReply(gb, GCodeResult::ok, "");
-}
-
 // Set up a file to print, but don't print it yet.
 // If successful return true, else write an error message to reply and return false
 bool GCodes::QueueFileToPrint(const char* fileName, const StringRef& reply)
@@ -4911,7 +4816,7 @@ 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 from the Heat module.
-// The Heat module will generate an appropriate error message, so on need to do that here.
+// The Heat module will generate an appropriate error message, so no need to do that here.
 void GCodes::HandleHeaterFault(int heater)
 {
 	if (heaterFaultState == HeaterFaultState::noFault && fileGCode->OriginalMachineState().fileState.IsLive())
@@ -4953,11 +4858,12 @@ void GCodes::CheckHeaterFault()
 			reprap.GetHeat().SwitchOffAll(true);
 			platform.MessageF(ErrorMessage, "Shutting down due to un-cleared heater fault after %lu seconds\n", heaterFaultTimeout/1000);
 			heaterFaultState = HeaterFaultState::stopping;
+			heaterFaultTime = millis();
 		}
 		break;
 
 	case HeaterFaultState::stopping:
-		if (millis() - heaterFaultTime >= 2000)			// wait 2 seconds for the message to be picked up by DWC and PanelDue
+		if (millis() - heaterFaultTime >= 1000)			// wait 1 second for the message to be picked up by DWC and PanelDue
 		{
 			platform.AtxPowerOff(false);
 			heaterFaultState = HeaterFaultState::stopped;
diff --git a/src/GCodes/GCodes.h b/src/GCodes/GCodes.h
index 0c0de8ce..74f86ba9 100644
--- a/src/GCodes/GCodes.h
+++ b/src/GCodes/GCodes.h
@@ -171,9 +171,6 @@ public:
 	NetworkGCodeInput *GetHTTPInput() const { return httpInput; }
 	NetworkGCodeInput *GetTelnetInput() const { return telnetInput; }
 
-	void WriteGCodeToFile(GCodeBuffer& gb);								// Write this GCode into a file
-	void WriteHTMLToFile(GCodeBuffer& gb, char b);						// Save an HTML file (usually to upload a new web interface)
-
 	bool IsFlashing() const { return isFlashing; }						// Is a new firmware binary going to be flashed?
 
 	bool IsPaused() const;
@@ -222,6 +219,7 @@ public:
 #endif
 
 	void SetMappedFanSpeed(float f);									// Set the mapped fan speed
+	void HandleReply(GCodeBuffer& gb, GCodeResult rslt, const char *reply);	// Handle G-Code replies
 
 private:
 	GCodes(const GCodes&);												// private copy constructor to prevent copying
@@ -261,7 +259,6 @@ private:
 	bool HandleMcode(GCodeBuffer& gb, const StringRef& reply);			// Do an M code
 	bool HandleTcode(GCodeBuffer& gb, const StringRef& reply);			// Do a T code
 	bool HandleResult(GCodeBuffer& gb, GCodeResult rslt, const StringRef& reply);
-	void HandleReply(GCodeBuffer& gb, GCodeResult rslt, const char *reply);		// Handle G-Code replies
 	void HandleReply(GCodeBuffer& gb, bool error, OutputBuffer *reply);
 
 	const char* DoStraightMove(GCodeBuffer& gb, bool isCoordinated) __attribute__((hot));	// Execute a straight move returning any error message
@@ -290,9 +287,7 @@ private:
 	bool Push(GCodeBuffer& gb);													// Push feedrate etc on the stack
 	void Pop(GCodeBuffer& gb);													// Pop feedrate etc
 	void DisableDrives();														// Turn the motors off
-	bool OpenFileToWrite(GCodeBuffer& gb, const char* directory, const char* fileName, const FilePosition size, const bool binaryWrite, const uint32_t fileCRC32);
 																				// Start saving GCodes in a file
-	void FinishWrite(GCodeBuffer& gb);											// Finish writing to the file and respond
 	bool SendConfigToLine();													// Deal with M503
 
 	GCodeResult OffsetAxes(GCodeBuffer& gb, const StringRef& reply);			// Set/report offsets
@@ -478,13 +473,6 @@ private:
 	FileData fileToPrint;						// The next file to print
 	FilePosition fileOffsetToPrint;				// The offset to print from
 
-	FileStore* fileBeingWritten;				// A file to write G Codes (or sometimes HTML) to
-	FilePosition fileSize;						// Size of the file being written
-
-	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.
-
 	char axisLetters[MaxAxes + 1];				// The names of the axes, with a null terminator
 	bool limitAxes;								// Don't think outside the box
 	bool noMovesBeforeHoming;					// Don't allow movement prior to homing the associates axes
diff --git a/src/GCodes/GCodes2.cpp b/src/GCodes/GCodes2.cpp
index a87c8d7a..2e0f04fe 100644
--- a/src/GCodes/GCodes2.cpp
+++ b/src/GCodes/GCodes2.cpp
@@ -61,6 +61,8 @@ bool GCodes::ActOnCode(GCodeBuffer& gb, const StringRef& reply)
 			HandleReply(gb, GCodeResult::ok, "");
 			return true;
 		}
+
+		return false;		// we should queue this code but we can't, so wait until we can either execute it or queue it
 	}
 
 	switch (gb.GetCommandLetter())
@@ -230,9 +232,13 @@ bool GCodes::HandleGcode(GCodeBuffer& gb, const StringRef& reply)
 				result = LoadHeightMap(gb, reply);
 				break;
 
-			default:	// clear height map
+			case 2:		// clear height map
 				ClearBedMapping();
 				break;
+
+			default:
+				result = GCodeResult::badOrMissingParameter;
+				break;
 			}
 		}
 		break;
@@ -850,7 +856,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply)
 			String<MaxFilenameLength> filename;
 			if (gb.GetUnprecedentedString(filename.GetRef()))
 			{
-				const bool ok = OpenFileToWrite(gb, platform.GetGCodeDir(), filename.c_str(), 0, false, 0);
+				const bool ok = gb.OpenFileToWrite(platform.GetGCodeDir(), filename.c_str(), 0, false, 0);
 				if (ok)
 				{
 					reply.printf("Writing to file: %s", filename.c_str());
@@ -2960,7 +2966,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply)
 		}
 		const FilePosition size = (gb.Seen('S') ? (FilePosition)gb.GetIValue() : 0);
 		const uint32_t crc32 = (gb.Seen('C') ? gb.GetUIValue() : 0);
-		const bool ok = OpenFileToWrite(gb, folder, filename.c_str(), size, true, crc32);
+		const bool ok = gb.OpenFileToWrite(folder, filename.c_str(), size, true, crc32);
 		if (ok)
 		{
 			reply.printf("Writing to file: %s", filename.c_str());
@@ -2973,7 +2979,11 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply)
 	}
 	break;
 
-	case 561: // Set identity transform (also clears bed probe grid)
+	case 561: // Set identity transform and disable height map
+		if (!LockMovementAndWaitForStandstill(gb))
+		{
+			return false;
+		}
 		ClearBedMapping();
 		break;
 
diff --git a/src/Heating/Heat.cpp b/src/Heating/Heat.cpp
index d56fbb83..ffd44acc 100644
--- a/src/Heating/Heat.cpp
+++ b/src/Heating/Heat.cpp
@@ -185,6 +185,8 @@ void Heat::Task()
 			heaterBeingTuned = -1;
 		}
 
+		reprap.KickHeatTaskWatchdog();
+
 		// Delay until it is time again
 		vTaskDelayUntil(&lastWakeTime, platform.HeatSampleInterval());
 	}
diff --git a/src/Libraries/Fatfs/diskio.cpp b/src/Libraries/Fatfs/diskio.cpp
index 6ee696eb..70656778 100644
--- a/src/Libraries/Fatfs/diskio.cpp
+++ b/src/Libraries/Fatfs/diskio.cpp
@@ -52,6 +52,15 @@
 
 #include <cstring>
 
+static unsigned int highestSdRetriesDone = 0;
+
+unsigned int DiskioGetAndClearMaxRetryCount()
+{
+	const unsigned int ret = highestSdRetriesDone;
+	highestSdRetriesDone = 0;
+	return ret;
+}
+
 //void debugPrintf(const char*, ...);
 
 //#if (SAM3S || SAM3U || SAM3N || SAM3XA_SERIES || SAM4S)
@@ -161,22 +170,35 @@ DRESULT disk_read(BYTE drv, BYTE *buff, DWORD sector, BYTE count)
 #if ACCESS_MEM_TO_RAM
 	MutexLocker lock((drv >= SD_MMC_HSMCI_MEM_CNT) ? Tasks::GetSpiMutex() : nullptr);
 
-	uint8_t uc_sector_size = mem_sector_size(drv);
-	uint32_t ul_last_sector_num;
-
-	if (uc_sector_size == 0) {
+	const uint8_t uc_sector_size = mem_sector_size(drv);
+	if (uc_sector_size == 0)
+	{
 		return RES_ERROR;
 	}
 
 	/* Check valid address */
+	uint32_t ul_last_sector_num;
 	mem_read_capacity(drv, &ul_last_sector_num);
-	if ((sector + count * uc_sector_size) > (ul_last_sector_num + 1) * uc_sector_size) {
+	if ((sector + count * uc_sector_size) > (ul_last_sector_num + 1) * uc_sector_size)
+	{
 		return RES_PARERR;
 	}
 
 	/* Read the data */
-	if (memory_2_ram(drv, sector, buff, count) != CTRL_GOOD) {
-		return RES_ERROR;
+	unsigned int retryNumber = 0;
+	while (memory_2_ram(drv, sector, buff, count) != CTRL_GOOD)
+	{
+		++retryNumber;
+		if (retryNumber == MaxSdCardTries)
+		{
+			return RES_ERROR;
+		}
+		delay(SdCardRetryDelay);
+	}
+
+	if (retryNumber > highestSdRetriesDone)
+	{
+		highestSdRetriesDone = retryNumber;
 	}
 
 	return RES_OK;
@@ -208,23 +230,36 @@ DRESULT disk_write(BYTE drv, BYTE const *buff, DWORD sector, BYTE count)
 #if ACCESS_MEM_TO_RAM
 	MutexLocker lock((drv >= SD_MMC_HSMCI_MEM_CNT) ? Tasks::GetSpiMutex() : nullptr);
 
-	uint8_t uc_sector_size = mem_sector_size(drv);
-	uint32_t ul_last_sector_num;
+	const uint8_t uc_sector_size = mem_sector_size(drv);
 
-	if (uc_sector_size == 0) {
+	if (uc_sector_size == 0)
+	{
 		return RES_ERROR;
 	}
 
 	/* Check valid address */
+	uint32_t ul_last_sector_num;
 	mem_read_capacity(drv, &ul_last_sector_num);
-	if ((sector + count * uc_sector_size) >
-			(ul_last_sector_num + 1) * uc_sector_size) {
+	if ((sector + count * uc_sector_size) > (ul_last_sector_num + 1) * uc_sector_size)
+	{
 		return RES_PARERR;
 	}
 
 	/* Write the data */
-	if (ram_2_memory(drv, sector, buff, count) != CTRL_GOOD) {
-		return RES_ERROR;
+	unsigned int retryNumber = 0;
+	while (ram_2_memory(drv, sector, buff, count) != CTRL_GOOD)
+	{
+		++retryNumber;
+		if (retryNumber == MaxSdCardTries)
+		{
+			return RES_ERROR;
+		}
+		delay(SdCardRetryDelay);
+	}
+
+	if (retryNumber > highestSdRetriesDone)
+	{
+		highestSdRetriesDone = retryNumber;
 	}
 
 	return RES_OK;
diff --git a/src/Libraries/Fatfs/diskio.h b/src/Libraries/Fatfs/diskio.h
index 40817bc5..43e5966c 100644
--- a/src/Libraries/Fatfs/diskio.h
+++ b/src/Libraries/Fatfs/diskio.h
@@ -5,6 +5,7 @@
 #ifndef _DISKIO
 
 #ifdef __cplusplus
+unsigned int DiskioGetAndClearMaxRetryCount();
 extern "C" {
 #endif
 
diff --git a/src/Libraries/Fatfs/ff.h b/src/Libraries/Fatfs/ff.h
index 7f0d91ab..9e89aa4f 100644
--- a/src/Libraries/Fatfs/ff.h
+++ b/src/Libraries/Fatfs/ff.h
@@ -193,7 +193,7 @@ typedef enum {
 	FR_NO_FILESYSTEM,		/* (13) There is no valid FAT volume */
 	FR_MKFS_ABORTED,		/* (14) The f_mkfs() aborted due to any parameter error */
 	FR_TIMEOUT,				/* (15) Could not get a grant to access the volume within defined period */
-	FR_LOCKED,				/* (16) The operation is rejected according to the file shareing policy */
+	FR_LOCKED,				/* (16) The operation is rejected according to the file sharing policy */
 	FR_NOT_ENOUGH_CORE,		/* (17) LFN working buffer could not be allocated */
 	FR_TOO_MANY_OPEN_FILES,	/* (18) Number of open files > _FS_SHARE */
 	FR_INVALID_PARAMETER	/* (19) Given parameter is invalid */
diff --git a/src/Movement/DDA.cpp b/src/Movement/DDA.cpp
index dfa7ae75..398a41e0 100644
--- a/src/Movement/DDA.cpp
+++ b/src/Movement/DDA.cpp
@@ -248,8 +248,17 @@ void DDA::Init()
 // Either way, return the amount of extrusion we didn't do in the extruder coordinates of nextMove
 bool DDA::Init(GCodes::RawMove &nextMove, bool doMotorMapping)
 {
-	// 1. Compute the new endpoints and the movement vector
+	// 0. If there are more total axes than visible axes, then we must ignore any movement data in nextMove for the invisible axes.
+	// The call to CartesianToMotorSteps may adjust the invisible axis endpoints for architectures such as CoreXYU, so set them up here.
+	const size_t numTotalAxes = reprap.GetGCodes().GetTotalAxes();
+	const size_t numVisibleAxes = reprap.GetGCodes().GetVisibleAxes();
 	const int32_t * const positionNow = prev->DriveCoordinates();
+	for (size_t axis = numVisibleAxes; axis < numTotalAxes; ++axis)
+	{
+		endPoint[axis] = positionNow[axis];
+	}
+
+	// 1. Compute the new endpoints and the movement vector
 	const Move& move = reprap.GetMove();
 	if (doMotorMapping)
 	{
@@ -271,20 +280,19 @@ bool DDA::Init(GCodes::RawMove &nextMove, bool doMotorMapping)
 	bool realMove = false;
 	float accelerations[DRIVES];
 	const float * const normalAccelerations = reprap.GetPlatform().Accelerations();
-	const size_t numAxes = reprap.GetGCodes().GetTotalAxes();
 	const Kinematics& k = move.GetKinematics();
 
 	for (size_t drive = 0; drive < DRIVES; drive++)
 	{
 		accelerations[drive] = normalAccelerations[drive];
-		if (drive >= numAxes || !doMotorMapping)
+		if (drive >= numTotalAxes || (!doMotorMapping && drive < numVisibleAxes))
 		{
 			endPoint[drive] = Move::MotorEndPointToMachine(drive, nextMove.coords[drive]);
 		}
 
 		endCoordinates[drive] = nextMove.coords[drive];
 		int32_t delta;
-		if (drive < numAxes)
+		if (drive < numTotalAxes)
 		{
 			delta = endPoint[drive] - positionNow[drive];
 			if (k.IsContinuousRotationAxis(drive) && nextMove.moveType != 1 && nextMove.moveType != 2)
@@ -305,7 +313,7 @@ bool DDA::Init(GCodes::RawMove &nextMove, bool doMotorMapping)
 			delta = endPoint[drive];
 		}
 
-		if (drive < numAxes && doMotorMapping)
+		if (drive < numTotalAxes && doMotorMapping)
 		{
 			const float positionDelta = nextMove.coords[drive] - prev->GetEndCoordinate(drive, false);
 			directionVector[drive] = positionDelta;
@@ -317,7 +325,7 @@ bool DDA::Init(GCodes::RawMove &nextMove, bool doMotorMapping)
 		else
 		{
 			directionVector[drive] = (float)delta/reprap.GetPlatform().DriveStepsPerUnit(drive);
-			if (drive >= numAxes && nextMove.coords[drive] > 0.0)
+			if (drive >= numTotalAxes && nextMove.coords[drive] > 0.0)
 			{
 				extruding = true;
 			}
@@ -331,14 +339,14 @@ bool DDA::Init(GCodes::RawMove &nextMove, bool doMotorMapping)
 			pdm->totalSteps = labs(delta);				// for now this is the number of net steps, but gets adjusted later if there is a reverse in direction
 			pdm->direction = (delta >= 0);				// for now this is the direction of net movement, but gets adjusted later if it is a delta movement
 
-			if (drive >= numAxes)
+			if (drive >= numTotalAxes)
 			{
 				// It's an extruder movement
 				nextMove.coords[drive] -= directionVector[drive];
 														// subtract the amount of extrusion we actually did to leave the residue outstanding
 				if (xyMoving && nextMove.usePressureAdvance)
 				{
-					const float compensationTime = reprap.GetPlatform().GetPressureAdvance(drive - numAxes);
+					const float compensationTime = reprap.GetPlatform().GetPressureAdvance(drive - numTotalAxes);
 					if (compensationTime > 0.0)
 					{
 						// Compensation causes instant velocity changes equal to acceleration * k, so we may need to limit the acceleration
@@ -359,7 +367,7 @@ bool DDA::Init(GCodes::RawMove &nextMove, bool doMotorMapping)
 		// Update the end position in the previous move, so that on the next move we don't think there is XY movement when the user didn't ask for any
 		if (doMotorMapping)
 		{
-			for (size_t drive = 0; drive < numAxes; ++drive)
+			for (size_t drive = 0; drive < numTotalAxes; ++drive)
 			{
 				prev->endCoordinates[drive] = nextMove.coords[drive];
 			}
@@ -423,7 +431,7 @@ bool DDA::Init(GCodes::RawMove &nextMove, bool doMotorMapping)
 	else if (axesMoving)
 	{
 		// Some axes are moving, but not axes that X or Y are mapped to. Normalise the movement to the vector sum of the axes that are moving.
-		totalDistance = Normalise(directionVector, DRIVES, numAxes);
+		totalDistance = Normalise(directionVector, DRIVES, numTotalAxes);
 	}
 	else
 	{
diff --git a/src/Movement/Kinematics/HangprinterKinematics.cpp b/src/Movement/Kinematics/HangprinterKinematics.cpp
index a77d377c..f7c066a0 100644
--- a/src/Movement/Kinematics/HangprinterKinematics.cpp
+++ b/src/Movement/Kinematics/HangprinterKinematics.cpp
@@ -12,19 +12,14 @@
 #include "Movement/Move.h"
 //#include "Movement/BedProbing/RandomProbePointSet.h"
 
-// Default anchor coordinates, copied from https://github.com/tobbelobb/hangprinter/blob/fabf19bf4653d1d1daf72d53217ba5962c9aca6e/firmware/HangprinterMarlin/Configuration.h
-constexpr float DefaultAnchorA[3] = {     0.0, -2163.0, -75.5};
-constexpr float DefaultAnchorB[3] = {-1841.0,    741.0, -75.5};
-constexpr float DefaultAnchorC[3] = { 1639.0,   1404.0, -75.5};
-constexpr float DefaultAnchorDz = 3250.5;
+// Default anchor coordinates
+// These are only placeholders. Each machine must have these values calibrated in order to work correctly.
+constexpr float DefaultAnchorA[3] = {    0.0, -2000.0, -100.0};
+constexpr float DefaultAnchorB[3] = { 2000.0,  1000.0, -100.0};
+constexpr float DefaultAnchorC[3] = {-2000.0,  1000.0, -100.0};
+constexpr float DefaultAnchorDz = 3000.0;
 constexpr float DefaultPrintRadius = 1500.0;
 
-constexpr size_t HANGPRINTER_AXES = 4;
-constexpr size_t A_AXIS = 0;
-constexpr size_t B_AXIS = 1;
-constexpr size_t C_AXIS = 2;
-constexpr size_t D_AXIS = 3;
-
 // Constructor
 HangprinterKinematics::HangprinterKinematics()
 	: Kinematics(KinematicsType::scara, DefaultSegmentsPerSecond, DefaultMinSegmentSize, true)
@@ -39,7 +34,7 @@ void HangprinterKinematics::Init()
 	ARRAY_INIT(anchorA, DefaultAnchorA);
 	ARRAY_INIT(anchorB, DefaultAnchorB);
 	ARRAY_INIT(anchorC, DefaultAnchorC);
-    doneAutoCalibration = false;
+	doneAutoCalibration = false;
 	Recalc();
 }
 
@@ -140,7 +135,7 @@ bool HangprinterKinematics::Configure(unsigned int mCode, GCodeBuffer& gb, const
 }
 
 // Calculate the square of the line length from a spool from a Cartesian coordinate
-inline float HangprinterKinematics::LineLengthASquared(const float machinePos[3], const float anchor[3]) const
+inline float HangprinterKinematics::LineLengthSquared(const float machinePos[3], const float anchor[3]) const
 {
 	return fsquare(anchor[Z_AXIS] - machinePos[Z_AXIS]) + fsquare(anchor[Y_AXIS] - machinePos[Y_AXIS]) + fsquare(anchor[X_AXIS] - machinePos[X_AXIS]);
 }
@@ -148,15 +143,13 @@ inline float HangprinterKinematics::LineLengthASquared(const float machinePos[3]
 // Convert Cartesian coordinates to motor coordinates, returning true if successful
 bool HangprinterKinematics::CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool isCoordinated) const
 {
-    // Geometry of hangprinter makes fsquare(anchorABC[Z_AXIS] - machinePos[Z_AXIS]) the smallest term in the sum.
-    // Starting sum with smallest number gives smallest roundoff error.
-	const float aSquared = LineLengthASquared(machinePos, anchorA);
-	const float bSquared = LineLengthASquared(machinePos, anchorB);
-	const float cSquared = LineLengthASquared(machinePos, anchorC);
+	const float aSquared = LineLengthSquared(machinePos, anchorA);
+	const float bSquared = LineLengthSquared(machinePos, anchorB);
+	const float cSquared = LineLengthSquared(machinePos, anchorC);
 	const float dSquared =    fsquare(machinePos[X_AXIS])
 							+ fsquare(machinePos[Y_AXIS])
 							+ fsquare(anchorDz - machinePos[Z_AXIS]);
-	if (aSquared < 0.0 && bSquared < 0.0 && cSquared < 0.0 && dSquared < 0.0)
+	if (aSquared > 0.0 && bSquared > 0.0 && cSquared > 0.0 && dSquared > 0.0)
 	{
 		motorPos[A_AXIS] = lrintf(sqrtf(aSquared) * stepsPerMm[A_AXIS]);
 		motorPos[B_AXIS] = lrintf(sqrtf(bSquared) * stepsPerMm[B_AXIS]);
@@ -372,9 +365,9 @@ bool HangprinterKinematics::DoAutoCalibration(size_t numFactors, const RandomPro
 		const floatc_t zp = reprap.GetMove().GetProbeCoordinates(i, machinePos[X_AXIS], machinePos[Y_AXIS], probePoints.PointWasCorrected(i));
 		machinePos[Z_AXIS] = 0.0;
 
-		probeMotorPositions(i, A_AXIS) = sqrtf(LineLengthASquared(machinePos, anchorA));
-		probeMotorPositions(i, B_AXIS) = sqrtf(LineLengthASquared(machinePos, anchorB));
-		probeMotorPositions(i, C_AXIS) = sqrtf(LineLengthASquared(machinePos, anchorC));
+		probeMotorPositions(i, A_AXIS) = sqrtf(LineLengthSquared(machinePos, anchorA));
+		probeMotorPositions(i, B_AXIS) = sqrtf(LineLengthSquared(machinePos, anchorB));
+		probeMotorPositions(i, C_AXIS) = sqrtf(LineLengthSquared(machinePos, anchorC));
 		initialSumOfSquares += fcsquare(zp);
 	}
 
diff --git a/src/Movement/Kinematics/HangprinterKinematics.h b/src/Movement/Kinematics/HangprinterKinematics.h
index 60a342d9..96ceb9ef 100644
--- a/src/Movement/Kinematics/HangprinterKinematics.h
+++ b/src/Movement/Kinematics/HangprinterKinematics.h
@@ -43,9 +43,16 @@ private:
 	static constexpr float DefaultSegmentsPerSecond = 100.0;
 	static constexpr float DefaultMinSegmentSize = 0.2;
 
+	// Basic facts about movement system
+	static constexpr size_t HANGPRINTER_AXES = 4;
+	static constexpr size_t A_AXIS = 0;
+	static constexpr size_t B_AXIS = 1;
+	static constexpr size_t C_AXIS = 2;
+	static constexpr size_t D_AXIS = 3;
+
 	void Init();
 	void Recalc();
-    float LineLengthASquared(const float machinePos[3], const float anchor[3]) const;	// Calculate the square of the line length from a spool from a Cartesian coordinate
+	float LineLengthSquared(const float machinePos[3], const float anchor[3]) const;	// Calculate the square of the line length from a spool from a Cartesian coordinate
 	void InverseTransform(float La, float Lb, float Lc, float machinePos[3]) const;
 
 	floatc_t ComputeDerivative(unsigned int deriv, float La, float Lb, float Lc) const;	// Compute the derivative of height with respect to a parameter at a set of motor endpoints
diff --git a/src/Movement/Move.cpp b/src/Movement/Move.cpp
index 1765ac71..8b7b3ba9 100644
--- a/src/Movement/Move.cpp
+++ b/src/Movement/Move.cpp
@@ -104,6 +104,7 @@ void Move::Init()
 
 	idleTimeout = DefaultIdleTimeout;
 	moveState = MoveState::idle;
+	lastStateChangeTime = millis();
 	idleCount = 0;
 
 	simulationMode = 0;
@@ -589,7 +590,7 @@ void Move::Diagnostics(MessageType mtype)
 {
 	Platform& p = reprap.GetPlatform();
 	p.Message(mtype, "=== Move ===\n");
-	p.MessageF(mtype, "Hiccups: %" PRIu32 ", StepErrors: %u, LaErrors: %u, FreeDm: %d, MinFreeDm %d, MaxWait: %" PRIu32 "ms, Underruns: %u, %u\n",
+	p.MessageF(mtype, "Hiccups: %" PRIu32 ", StepErrors: %u, LaErrors: %u, FreeDm: %d, MinFreeDm: %d, MaxWait: %" PRIu32 "ms, Underruns: %u, %u\n",
 						DDA::numHiccups, stepErrors, numLookaheadErrors, DriveMovement::NumFree(), DriveMovement::MinFree(), longestGcodeWaitInterval, numLookaheadUnderruns, numPrepareUnderruns);
 	DDA::numHiccups = 0;
 	numLookaheadUnderruns = numPrepareUnderruns = numLookaheadErrors = 0;
diff --git a/src/Platform.cpp b/src/Platform.cpp
index 2e74416e..87321ac7 100644
--- a/src/Platform.cpp
+++ b/src/Platform.cpp
@@ -438,7 +438,7 @@ void Platform::Init()
 	DuetExpansion::AdditionalOutputInit();
 
 #elif defined(DUET_M)
-	numSmartDrivers = 5;										// TODO for now we assume that additional drivers are dumb
+	numSmartDrivers = 7;										// TODO for now we assume that additional drivers are smart
 #endif
 
 #if HAS_SMART_DRIVERS
@@ -653,7 +653,7 @@ void Platform::InitZProbe()
 int Platform::GetZProbeReading() const
 {
 	int zProbeVal = 0;			// initialised to avoid spurious compiler warning
-	if (zProbeType == ZProbeType::unfilteredDigital || (zProbeOnFilter.IsValid() && zProbeOffFilter.IsValid()))
+	if (zProbeType == ZProbeType::unfilteredDigital || zProbeType == ZProbeType::blTouch || (zProbeOnFilter.IsValid() && zProbeOffFilter.IsValid()))
 	{
 		switch (zProbeType)
 		{
@@ -663,7 +663,6 @@ int Platform::GetZProbeReading() const
 		case ZProbeType::digital:				// Switch connected to Z probe input
 		case ZProbeType::e1Switch:				// Switch connected to E1 endstop input
 		case ZProbeType::zSwitch:				// Switch connected to Z endstop input
-		case ZProbeType::blTouch:
 			zProbeVal = (int) ((zProbeOnFilter.GetSum() + zProbeOffFilter.GetSum()) / (8 * Z_PROBE_AVERAGE_READINGS));
 			break;
 
@@ -674,6 +673,7 @@ int Platform::GetZProbeReading() const
 			break;
 
 		case ZProbeType::unfilteredDigital:		// Switch connected to Z probe input, no filtering
+		case ZProbeType::blTouch:				// blTouch is now unfiltered too
 			zProbeVal = GetRawZProbeReading()/4;
 			break;
 
@@ -1330,13 +1330,15 @@ void Platform::Spin()
 	{
 		if (currentVin < driverPowerOffAdcReading)
 		{
-			++numUnderVoltageEvents;
 			driversPowered = false;
+			++numUnderVoltageEvents;
+			lastUnderVoltageValue = currentVin;					// save this because the voltage may have changed by the time we report it
 		}
 		else if (currentVin > driverOverVoltageAdcReading)
 		{
 			driversPowered = false;
 			++numOverVoltageEvents;
+			lastOverVoltageValue = currentVin;					// save this because the voltage may have changed by the time we report it
 		}
 		else
 		{
@@ -1510,13 +1512,13 @@ void Platform::Spin()
 #if HAS_VOLTAGE_MONITOR
 			if (numOverVoltageEvents != previousOverVoltageEvents)
 			{
-				MessageF(WarningMessage, "VIN over-voltage event (%.1fV)", (double)GetCurrentPowerVoltage());
+				MessageF(WarningMessage, "VIN over-voltage event (%.1fV)", (double)AdcReadingToPowerVoltage(lastOverVoltageValue));
 				previousOverVoltageEvents = numOverVoltageEvents;
 				reported = true;
 			}
 			if (numUnderVoltageEvents != previousUnderVoltageEvents)
 			{
-				MessageF(WarningMessage, "VIN under-voltage event (%.1fV)", (double)GetCurrentPowerVoltage());
+				MessageF(WarningMessage, "VIN under-voltage event (%.1fV)", (double)AdcReadingToPowerVoltage(lastUnderVoltageValue));
 				previousUnderVoltageEvents = numUnderVoltageEvents;
 				reported = true;
 			}
@@ -1792,6 +1794,10 @@ void Platform::SoftwareReset(uint16_t reason, const uint32_t *stk)
 		srdBuf[slot].cfsr = SCB->CFSR;
 		srdBuf[slot].icsr = SCB->ICSR;
 		srdBuf[slot].bfar = SCB->BFAR;
+#ifdef RTOS
+		srdBuf[slot].taskName = *reinterpret_cast<const uint32_t*>(pcTaskGetName(nullptr));
+#endif
+
 		if (stk != nullptr)
 		{
 			srdBuf[slot].sp = reinterpret_cast<uint32_t>(stk);
@@ -2093,7 +2099,8 @@ void Platform::Diagnostics(MessageType mtype)
 															: (reason == (uint32_t)SoftwareResetReason::otherFault) ? "Other fault"
 																: (reason == (uint32_t)SoftwareResetReason::stackOverflow) ? "Stack overflow"
 																	: (reason == (uint32_t)SoftwareResetReason::assertCalled) ? "Assertion failed"
-																		: "Unknown";
+																		: (reason == (uint32_t)SoftwareResetReason::heaterWatchdog) ? "Heat task stuck"
+																			: "Unknown";
 			String<ScratchStringLength> scratchString;
 			if (srdBuf[slot].when != 0)
 			{
@@ -2112,8 +2119,15 @@ void Platform::Diagnostics(MessageType mtype)
 								(srdBuf[slot].resetReason & (uint32_t)SoftwareResetReason::deliberate) ? "deliberate " : "",
 								reasonText, moduleName[srdBuf[slot].resetReason & 0x0F], srdBuf[slot].neverUsedRam, slot);
 			// Our format buffer is only 256 characters long, so the next 2 lines must be written separately
-			MessageF(mtype, "Software reset code 0x%04x HFSR 0x%08" PRIx32 ", CFSR 0x%08" PRIx32 ", ICSR 0x%08" PRIx32 ", BFAR 0x%08" PRIx32 ", SP 0x%08" PRIx32 "\n",
-				srdBuf[slot].resetReason, srdBuf[slot].hfsr, srdBuf[slot].cfsr, srdBuf[slot].icsr, srdBuf[slot].bfar, srdBuf[slot].sp);
+			MessageF(mtype,
+#ifdef RTOS
+					"Software reset code 0x%04x HFSR 0x%08" PRIx32 " CFSR 0x%08" PRIx32 " ICSR 0x%08" PRIx32 " BFAR 0x%08" PRIx32 " SP 0x%08" PRIx32 " Task 0x%08" PRIx32 "\n",
+					srdBuf[slot].resetReason, srdBuf[slot].hfsr, srdBuf[slot].cfsr, srdBuf[slot].icsr, srdBuf[slot].bfar, srdBuf[slot].sp, srdBuf[slot].taskName
+#else
+					"Software reset code 0x%04x HFSR 0x%08" PRIx32 " CFSR 0x%08" PRIx32 " ICSR 0x%08" PRIx32 " BFAR 0x%08" PRIx32 " SP 0x%08" PRIx32 "\n",
+					srdBuf[slot].resetReason, srdBuf[slot].hfsr, srdBuf[slot].cfsr, srdBuf[slot].icsr, srdBuf[slot].bfar, srdBuf[slot].sp
+#endif
+				);
 			if (srdBuf[slot].sp != 0xFFFFFFFF)
 			{
 				// We saved a stack dump, so print it
@@ -2145,7 +2159,7 @@ void Platform::Diagnostics(MessageType mtype)
 #endif
 
 	// Show the longest SD card write time
-	MessageF(mtype, "SD card longest block write time: %.1fms\n", (double)FileStore::GetAndClearLongestWriteTime());
+	MessageF(mtype, "SD card longest block write time: %.1fms, max retries %u\n", (double)FileStore::GetAndClearLongestWriteTime(), FileStore::GetAndClearMaxRetryCount());
 
 #if HAS_CPU_TEMP_SENSOR
 	// Show the MCU temperatures
@@ -2807,12 +2821,15 @@ void Platform::DisableDrive(size_t drive)
 	}
 }
 
-// Disable all drives
+// Disable all drives. Called from emergency stop and the tick ISR.
 void Platform::DisableAllDrives()
 {
 	for (size_t drive = 0; drive < DRIVES; drive++)
 	{
-		SetDriverCurrent(drive, 0.0, false);
+		if (!inInterrupt())		// on the Duet 06/085 we need interrupts running to send the I2C commands to set motor currents
+		{
+			SetDriverCurrent(drive, 0.0, false);
+		}
 		DisableDriver(drive);
 	}
 }
@@ -3663,12 +3680,11 @@ void Platform::AtxPowerOff(bool defer)
 	deferredPowerDown = defer;
 	if (!defer)
 	{
-		deferredPowerDown = false;
 		if (logger != nullptr)
 		{
 			logger->LogMessage(realTime, "Power off commanded");
 			logger->Flush(true);
-			// We don't call logger->Stop() here because we don't now whether turning off the power will work
+			// We don't call logger->Stop() here because we don't know whether turning off the power will work
 		}
 		IoPort::WriteDigital(ATX_POWER_PIN, false);
 	}
diff --git a/src/Platform.h b/src/Platform.h
index 9d040849..b58e2295 100644
--- a/src/Platform.h
+++ b/src/Platform.h
@@ -180,12 +180,13 @@ enum class SoftwareResetReason : uint16_t
 	erase = 0x10,					// special M999 command to erase firmware and reset
 	NMI = 0x20,
 	hardFault = 0x30,				// most exceptions get escalated to a hard fault
-	stuckInSpin = 0x40,				// we got stuck in a Spin() function for too long
+	stuckInSpin = 0x40,				// we got stuck in a Spin() function in the Main task for too long
 	wdtFault = 0x50,				// secondary watchdog
 	usageFault = 0x60,
 	otherFault = 0x70,
-	stackOverflow = 0x80,
-	assertCalled = 0x90,
+	stackOverflow = 0x80,			// FreeRTOS detected stack overflow
+	assertCalled = 0x90,			// FreeRTOS assertion failure
+	heaterWatchdog = 0xA0,			// the Heat task didn't kick the watchdog often enough
 
 	// Bits that are or'ed in
 	inAuxOutput = 0x0800,			// this bit is or'ed in if we were in aux output at the time
@@ -294,7 +295,7 @@ struct AxisDriversConfig
 
 // The main class that defines the RepRap machine for the benefit of the other classes
 class Platform
-{   
+{
 public:
 	// Enumeration to describe the status of a drive
 	enum class DriverStatus : uint8_t { disabled, idle, enabled };
@@ -660,7 +661,12 @@ private:
 		uint32_t bfar;								// bus fault address register
 		uint32_t sp;								// stack pointer
 		uint32_t when;								// value of the RTC when the software reset occurred
+#ifdef RTOS
+		uint32_t taskName;							// first 4 bytes of the task name
+		uint32_t stack[23];							// stack when the exception occurred, with the program counter at the bottom
+#else
 		uint32_t stack[24];							// stack when the exception occurred, with the program counter at the bottom
+#endif
 
 		bool isVacant() const						// return true if this struct can be written without erasing it first
 		{
@@ -885,6 +891,7 @@ private:
 #if HAS_VOLTAGE_MONITOR
 	AnalogChannelNumber vInMonitorAdcChannel;
 	volatile uint16_t currentVin, highestVin, lowestVin;
+	uint16_t lastUnderVoltageValue, lastOverVoltageValue;
 	uint16_t autoPauseReading, autoResumeReading;
 	uint32_t numUnderVoltageEvents, previousUnderVoltageEvents;
 	volatile uint32_t numOverVoltageEvents, previousOverVoltageEvents;
diff --git a/src/RepRap.cpp b/src/RepRap.cpp
index e83bd6e7..0315f061 100644
--- a/src/RepRap.cpp
+++ b/src/RepRap.cpp
@@ -121,10 +121,14 @@ extern "C" void hsmciIdle(uint32_t stBits, uint32_t dmaBits)
 // Do nothing more in the constructor; put what you want in RepRap:Init()
 
 RepRap::RepRap() : toolList(nullptr), currentTool(nullptr), lastWarningMillis(0), activeExtruders(0),
-	activeToolHeaters(0), ticksInSpinState(0), spinningModule(noModule), debug(0), stopped(false),
+	activeToolHeaters(0), ticksInSpinState(0),
+#ifdef RTOS
+	heatTaskIdleTicks(0),
+#endif
+	spinningModule(noModule), debug(0), stopped(false),
 	active(false), resetting(false), processingConfig(true), beepFrequency(0), beepDuration(0),
 	displayMessageBox(false), boxSeq(0),
-	diagnosticsDestination(MessageType::NoDestinationMessage)
+	diagnosticsDestination(MessageType::NoDestinationMessage), justSentDiagnostics(false)
 {
 	OutputBuffer::Init();
 	platform = new Platform();
@@ -355,14 +359,22 @@ void RepRap::Spin()
 	}
 
 	// Keep track of the loop time
-	const uint32_t dt = Platform::GetInterruptClocks() - lastTime;
-	if (dt < fastLoop)
+	if (justSentDiagnostics)
 	{
-		fastLoop = dt;
+		// Sending diagnostics increases the loop time, so don't count it
+		justSentDiagnostics = false;
 	}
-	if (dt > slowLoop)
+	else
 	{
-		slowLoop = dt;
+		const uint32_t dt = Platform::GetInterruptClocks() - lastTime;
+		if (dt < fastLoop)
+		{
+			fastLoop = dt;
+		}
+		if (dt > slowLoop)
+		{
+			slowLoop = dt;
+		}
 	}
 
 	RTOSIface::Yield();
@@ -407,6 +419,7 @@ void RepRap::Diagnostics(MessageType mtype)
 #ifdef DUET_NG
 	DuetExpansion::Diagnostics(mtype);
 #endif
+	justSentDiagnostics = true;
 }
 
 // Turn off the heaters, disable the motors, and deactivate the Heat and Move classes. Leave everything else working.
@@ -672,26 +685,29 @@ void RepRap::Tick()
 	{
 		platform->Tick();
 		++ticksInSpinState;
-		if (ticksInSpinState >= MaxTicksInSpinState)	// if we stall for 20 seconds, save diagnostic data and reset
+#ifdef RTOS
+		++heatTaskIdleTicks;
+		const bool heatTaskStuck = (heatTaskIdleTicks >= MaxTicksInSpinState);
+		if (heatTaskStuck || ticksInSpinState >= MaxTicksInSpinState)		// if we stall for 20 seconds, save diagnostic data and reset
+#else
+		if (ticksInSpinState >= MaxTicksInSpinState)						// if we stall for 20 seconds, save diagnostic data and reset
+#endif
 		{
 			resetting = true;
 			for (size_t i = 0; i < Heaters; i++)
 			{
 				platform->SetHeater(i, 0.0);
 			}
-			for (size_t i = 0; i < DRIVES; i++)
-			{
-				platform->DisableDrive(i);
-				// We can't set motor currents to 0 here because that requires interrupts to be working, and we are in an ISR
-			}
+			platform->DisableAllDrives();
 
 			// We now save the stack when we get stuck in a spin loop
 			register const uint32_t * stackPtr asm ("sp");
-
-			platform->SoftwareReset((uint16_t)SoftwareResetReason::stuckInSpin,
+			platform->SoftwareReset(
 #ifdef RTOS
+				(heatTaskStuck) ? (uint16_t)SoftwareResetReason::heaterWatchdog : (uint16_t)SoftwareResetReason::stuckInSpin,
 				stackPtr + 5 + 15			// discard the stack used by the FreeRTOS stack handler and our tick handler
 #else
+				(uint16_t)SoftwareResetReason::stuckInSpin,
 				stackPtr + 5				// discard the stack used by our tick handler
 #endif
 				);
diff --git a/src/RepRap.h b/src/RepRap.h
index 64d2d459..0f741126 100644
--- a/src/RepRap.h
+++ b/src/RepRap.h
@@ -117,6 +117,10 @@ public:
 
 	static uint32_t DoDivide(uint32_t a, uint32_t b);		// helper function for diagnostic tests
 
+#ifdef RTOS
+	void KickHeatTaskWatchdog() { heatTaskIdleTicks = 0; }
+#endif
+
 private:
 	static void EncodeString(StringRef& response, const char* src, size_t spaceToLeave, bool allowControlChars = false, char prefix = 0);
 
@@ -151,6 +155,9 @@ private:
 	uint16_t activeToolHeaters;
 
 	uint16_t ticksInSpinState;
+#ifdef RTOS
+	uint16_t heatTaskIdleTicks;
+#endif
 	Module spinningModule;
 	uint32_t fastLoop, slowLoop;
 
@@ -176,6 +183,7 @@ private:
 
 	// Deferred diagnostics
 	MessageType diagnosticsDestination;
+	bool justSentDiagnostics;
 };
 
 inline Platform& RepRap::GetPlatform() const { return *platform; }
diff --git a/src/Storage/FileStore.cpp b/src/Storage/FileStore.cpp
index 07322cb9..002c31f6 100644
--- a/src/Storage/FileStore.cpp
+++ b/src/Storage/FileStore.cpp
@@ -5,6 +5,7 @@
 #include "MassStorage.h"
 #include "Platform.h"
 #include "RepRap.h"
+#include "Libraries/Fatfs/diskio.h"
 
 uint32_t FileStore::longestWriteTime = 0;
 
@@ -476,6 +477,12 @@ float FileStore::GetAndClearLongestWriteTime()
 	return ret;
 }
 
+// Return the highest SD card retry count that resulted in a successful transfer
+unsigned int FileStore::GetAndClearMaxRetryCount()
+{
+	return DiskioGetAndClearMaxRetryCount();
+}
+
 #if 0	// not currently used
 
 // Provide a cluster map for fast seeking. Needs _USE_FASTSEEK defined as 1 in conf_fatfs to make any difference.
diff --git a/src/Storage/FileStore.h b/src/Storage/FileStore.h
index 9f674a4b..8626974d 100644
--- a/src/Storage/FileStore.h
+++ b/src/Storage/FileStore.h
@@ -60,7 +60,7 @@ public:
 	bool SetClusterMap(uint32_t[]);					// Provide a cluster map for fast seeking
 #endif
 	static float GetAndClearLongestWriteTime();		// Return the longest time it took to write a block to a file, in milliseconds
-
+	static unsigned int GetAndClearMaxRetryCount();	// Return the highest SD card retry count that resulted in a successful transfer
 	friend class MassStorage;
 
 private:
diff --git a/src/Tasks.cpp b/src/Tasks.cpp
index a3a4c782..26567b43 100644
--- a/src/Tasks.cpp
+++ b/src/Tasks.cpp
@@ -201,23 +201,15 @@ namespace Tasks
 			p.MessageF(mtype, " %s(%s,%u)",
 				taskDetails.pcTaskName, stateText, (unsigned int)(taskDetails.usStackHighWaterMark * sizeof(StackType_t)));
 		}
-		p.Message(mtype, "\nMutexes:");
+		p.Message(mtype, "\nOwned mutexes:");
 
 		for (const Mutex *m = Mutex::GetMutexList(); m != nullptr; m = m->GetNext())
 		{
 			const TaskHandle holder = m->GetHolder();
-			TaskStatus_t taskDetails;
-			const char *holderText;
-			if (holder == nullptr)
-			{
-				holderText = "null";
-			}
-			else
+			if (holder != nullptr)
 			{
-				vTaskGetInfo(holder, &taskDetails, pdTRUE, eInvalid);
-				holderText = taskDetails.pcTaskName;
+				p.MessageF(mtype, " %s(%s)", m->GetName(), pcTaskGetName(holder));
 			}
-			p.MessageF(mtype, " %s(%s)", m->GetName(), holderText);
 		}
 		p.MessageF(mtype, "\n");
 #endif
diff --git a/src/Version.h b/src/Version.h
index b2e0b33c..f033a04d 100644
--- a/src/Version.h
+++ b/src/Version.h
@@ -12,9 +12,9 @@
 #ifndef VERSION
 #ifdef RTOS
 # define RTOSVER		"(RTOS)"
-# define MAIN_VERSION	"2.01beta1"
+# define MAIN_VERSION	"2.01beta2"
 #else
-# define MAIN_VERSION	"1.21.2beta1"
+# define MAIN_VERSION	"1.22beta2"
 # define RTOSVER
 #endif
 
@@ -22,7 +22,7 @@
 #endif
 
 #ifndef DATE
-# define DATE "2018-06-23b1"
+# define DATE "2018-07-14b5"
 #endif
 
 #define AUTHORS "reprappro, dc42, chrishamm, t3p3, dnewman"