diff --git a/src/Configuration.h b/src/Configuration.h
index db91e66..0e0df02 100644
--- a/src/Configuration.h
+++ b/src/Configuration.h
@@ -26,11 +26,11 @@ Licence: GPL
 // Firmware name is now defined in the Pins file
 
 #ifndef VERSION
-# define VERSION "1.15e"
+# define VERSION "1.16beta1"
 #endif
 
 #ifndef DATE
-# define DATE "2016-10-02"
+# define DATE "2016-10-16"
 #endif
 
 #define AUTHORS "reprappro, dc42, zpl, t3p3, dnewman"
@@ -193,17 +193,6 @@ const float FILAMENT_WIDTH = 1.75;					// Millimetres
 
 #define CONFIG_FILE "config.g"
 #define DEFAULT_FILE "default.g"
-#define HOME_X_G "homex.g"
-#define HOME_Y_G "homey.g"
-#define HOME_Z_G "homez.g"
-#define HOME_ALL_G "homeall.g"
-#define HOME_DELTA_G "homedelta.g"
-#define BED_EQUATION_G "bed.g"
-#define PAUSE_G "pause.g"
-#define RESUME_G "resume.g"
-#define CANCEL_G "cancel.g"
-#define STOP_G "stop.g"
-#define SLEEP_G "sleep.g"
 
 #define EOF_STRING "<!-- **EoF** -->"
 
diff --git a/src/DuetNG/Pins_DuetNG.h b/src/DuetNG/Pins_DuetNG.h
index aa2e637..1cfabc3 100644
--- a/src/DuetNG/Pins_DuetNG.h
+++ b/src/DuetNG/Pins_DuetNG.h
@@ -29,7 +29,11 @@ const int8_t HEATERS = 8;						// The number of heaters in the machine; 0 is the
 #define HEATERS_(a,b,c,d,e,f,g,h) { a,b,c,d,e,f,g,h }
 const size_t MaxDriversPerAxis = 4;				// The maximum number of stepper drivers assigned to one axis
 
-const size_t AXES = 3;							// The number of movement axes in the machine, usually just X, Y and Z. <= DRIVES
+const size_t MAX_AXES = 6;						// The maximum number of movement axes in the machine, usually just X, Y and Z, <= DRIVES
+const size_t MIN_AXES = 3;						// The minimum and default number of axes
+const size_t DELTA_AXES = 3;					// The number of axis involved in delta movement
+const size_t CART_AXES = 3;						// The number of Cartesian axes
+
 const size_t NUM_SERIAL_CHANNELS = 2;			// The number of serial IO channels (USB and one auxiliary UART)
 #define SERIAL_MAIN_DEVICE SerialUSB
 #define SERIAL_AUX_DEVICE Serial
diff --git a/src/DuetNG/TMC2660.cpp b/src/DuetNG/TMC2660.cpp
index 8a3e0c1..84c69e9 100644
--- a/src/DuetNG/TMC2660.cpp
+++ b/src/DuetNG/TMC2660.cpp
@@ -13,34 +13,6 @@ static size_t numTmc2660Drivers;
 
 static bool driversPowered = false;
 
-// Connections between Duet 0.6 and TMC2660-EVAL board:
-
-// Driver signal name  	Eval board pin	Our signal name   	Duet 0.6 expansion connector pin #
-// SDI                 	29				MOSI              	11 (TXD1)
-// SDO                  28 				MISO               	12 (RXD1)
-// SCK                  27 				SCLK               	33 (AD7/PA16)
-// /CS                  24				/CS                	17 (PC5_PWMH1/E1_EN)
-// GND					2,3,43,44		GND					2  (GND)
-// INT_STEP				17				E1_STEP				15 (PC9_PWMH3)
-// INT_DIR				18				E1_DIR				16 (PC3_PWMH0)
-// ENN					8				connect to ground	2  (GND)
-// CLK					23				connect to ground	2  (GND
-// 5V_USB				5				+3.3V				3  (+3.3V)
-
-// Connections between DuetNG 0.6 and TMC2660-EVAL board (now using USART0):
-
-// Driver signal name  	Eval board pin	Our signal name   	DuetNG 0.6 expansion connector pin #
-// SDI                 	29				SPI1_MOSI           13 (SPI0_MOSI) was 29
-// SDO                  28 				SPI1_MISO           14 (SPI0_MISO) was 30
-// SCK                  27 				SPI1_SCLK           12 (SPI0_SCLK) was 28
-// /CS                  24				/CS                	24 (E2_EN)
-// GND					2,3,43,44		GND					2  (GND)
-// INT_STEP				19				E2_STEP				19 (E2_STEP)
-// INT_DIR				20				E2_DIR				20 (E2_DIR)
-// ENN					8				connect to ground	2  (GND)
-// CLK					23				connect to ground	2  (GND
-// 5V_USB				5				+3.3V				3  (+3.3V)
-
 // Pin assignments for the second prototype, using USART1 SPI
 const Pin DriversClockPin = 15;								// PB15/TIOA1
 const Pin DriversMosiPin = 22;								// PA13
diff --git a/src/GCodes/GCodes.cpp b/src/GCodes/GCodes.cpp
index d2e2209..54122ed 100644
--- a/src/GCodes/GCodes.cpp
+++ b/src/GCodes/GCodes.cpp
@@ -31,12 +31,22 @@
 
 #define DEGREE_SYMBOL	"\xC2\xB0"				// degree-symbol encoding in UTF8
 
-const char GCodes::axisLetters[AXES] = { 'X', 'Y', 'Z' };
+const char GCodes::axisLetters[MAX_AXES] = { 'X', 'Y', 'Z', 'U', 'V', 'W' };
+
+const char* BED_EQUATION_G = "bed.g";
+const char* PAUSE_G = "pause.g";
+const char* RESUME_G = "resume.g";
+const char* CANCEL_G = "cancel.g";
+const char* STOP_G = "stop.g";
+const char* SLEEP_G = "sleep.g";
+const char* homingFileNames[MAX_AXES] = { "homex.g", "homey.g", "homez.g", "homeu.g", "homev.g", "homew.g" };
+const char* HOME_ALL_G = "homeall.g";
+const char* HOME_DELTA_G = "homedelta.g";
 
 const size_t gcodeReplyLength = 2048;			// long enough to pass back a reasonable number of files in response to M20
 
 GCodes::GCodes(Platform* p, Webserver* w) :
-	platform(p), webserver(w), active(false), stackPointer(0), auxGCodeReply(nullptr), isFlashing(false),
+	platform(p), webserver(w), active(false), stackPointer(0), isFlashing(false),
 	fileBeingHashed(nullptr)
 {
 	httpGCode = new GCodeBuffer(platform, "http", HTTP_MESSAGE);
@@ -56,9 +66,10 @@ void GCodes::Exit()
 void GCodes::Init()
 {
 	Reset();
+	numAxes = MIN_AXES;
 	distanceScale = 1.0;
 	rawExtruderTotal = 0.0;
-	for (size_t extruder = 0; extruder < DRIVES - AXES; extruder++)
+	for (size_t extruder = 0; extruder < DRIVES - MIN_AXES; extruder++)
 	{
 		lastRawExtruderPosition[extruder] = 0.0;
 		rawExtruderTotalByDrive[extruder] = 0.0;
@@ -72,7 +83,7 @@ void GCodes::Init()
 	longWait = platform->Time();
 	dwellTime = longWait;
 	limitAxes = true;
-	for(size_t axis = 0; axis < AXES; axis++)
+	for(size_t axis = 0; axis < MAX_AXES; axis++)
 	{
 		axisScaleFactors[axis] = 1.0;
 	}
@@ -109,7 +120,7 @@ void GCodes::Reset()
 	cannedCycleMoveCount = 0;
 	cannedCycleMoveQueued = false;
 	speedFactor = 1.0 / minutesToSeconds;				// default is just to convert from mm/minute to mm/second
-	for (size_t i = 0; i < DRIVES - AXES; ++i)
+	for (size_t i = 0; i < DRIVES - MIN_AXES; ++i)
 	{
 		extrusionFactors[i] = 1.0;
 	}
@@ -127,12 +138,6 @@ void GCodes::Reset()
 	}
 	triggersPending = 0;
 
-	auxDetected = false;
-	while (auxGCodeReply != nullptr)
-	{
-		auxGCodeReply = OutputBuffer::Release(auxGCodeReply);
-	}
-	auxSeq = 0;
 	simulationMode = 0;
 	simulationTime = 0.0;
 	isPaused = false;
@@ -221,7 +226,7 @@ void GCodes::Spin()
 	StringRef reply(replyBuffer, ARRAY_SIZE(replyBuffer));
 	reply.Clear();
 
-	// Check for M105 poll requests from Pronterface and PanelDue so that the status is kept up to date during execution of file macros etc.
+	// Check for poll requests from Pronterface and PanelDue so that the status is kept up to date during execution of file macros etc.
 	if (serialGCode->IsReady() && serialGCode->IsPollRequest())
 	{
 		serialGCode->SetFinished(ActOnCode(serialGCode, reply));
@@ -249,26 +254,24 @@ void GCodes::Spin()
 			break;
 
 		case GCodeState::homing:
-			if (toBeHomed & (1 << X_AXIS))
-			{
-				toBeHomed &= ~(1 << X_AXIS);
-				DoFileMacro(HOME_X_G);
-			}
-			else if (toBeHomed & (1 << Y_AXIS))
-			{
-				toBeHomed &= ~(1 << Y_AXIS);
-				DoFileMacro(HOME_Y_G);
-			}
-			else if (toBeHomed & (1 << Z_AXIS))
-			{
-				toBeHomed &= ~(1 << Z_AXIS);
-				DoFileMacro(HOME_Z_G);
-			}
-			else
+			if (toBeHomed == 0)
 			{
 				HandleReply(gbCurrent, false, "");
 				state = GCodeState::normal;
 			}
+			else
+			{
+				for (size_t axis = 0; axis < numAxes; ++axis)
+				{
+					// Leave the Z axis until all other axes are done
+					if ((toBeHomed & (1u << axis)) != 0 && (axis != Z_AXIS || toBeHomed == (1u << Z_AXIS)))
+					{
+						toBeHomed &= ~(1u << axis);
+						DoFileMacro(homingFileNames[axis]);
+						break;
+					}
+				}
+			}
 			break;
 
 		case GCodeState::setBed1:
@@ -345,11 +348,11 @@ void GCodes::Spin()
 			if (AllMovesAreFinishedAndMoveBufferIsLoaded())
 			{
 				float currentZ = moveBuffer.coords[Z_AXIS];
-				for (size_t drive = 0; drive < AXES; ++drive)
+				for (size_t drive = 0; drive < numAxes; ++drive)
 				{
 					moveBuffer.coords[drive] =  pausedMoveBuffer[drive];
 				}
-				for (size_t drive = AXES; drive < DRIVES; ++drive)
+				for (size_t drive = numAxes; drive < DRIVES; ++drive)
 				{
 					moveBuffer.coords[drive] = 0.0;
 				}
@@ -380,9 +383,9 @@ void GCodes::Spin()
 				{
 					platform->SetFanValue(i, pausedFanValues[i]);
 				}
-				for (size_t drive = AXES; drive < DRIVES; ++drive)
+				for (size_t drive = numAxes; drive < DRIVES; ++drive)
 				{
-					lastRawExtruderPosition[drive - AXES] = pausedMoveBuffer[drive];	// reset the extruder position in case we are receiving absolute extruder moves
+					lastRawExtruderPosition[drive - numAxes] = pausedMoveBuffer[drive];	// reset the extruder position in case we are receiving absolute extruder moves
 				}
 				feedRate = pausedFeedRate;
 				isPaused = false;
@@ -399,9 +402,9 @@ void GCodes::Spin()
 				bool updating = false;
 				for (unsigned int module = 1; module < NumFirmwareUpdateModules; ++module)
 				{
-					if ((firmwareUpdateModuleMap & (1 << module)) != 0)
+					if ((firmwareUpdateModuleMap & (1u << module)) != 0)
 					{
-						firmwareUpdateModuleMap &= ~(1 << module);
+						firmwareUpdateModuleMap &= ~(1u << module);
 						FirmwareUpdater::UpdateModule(module);
 						updating = true;
 						break;
@@ -531,7 +534,7 @@ void GCodes::FillGCodeBuffers()
 			char b = platform->ReadFromSource(SerialSource::AUX);
 			if (auxGCode->Put(b))	// add char to buffer and test whether the gcode is complete
 			{
-				auxDetected = true;
+				platform->SetAuxDetected();
 				break;
 			}
 		}
@@ -631,9 +634,9 @@ bool GCodes::CheckTriggers()
 			&& (ct.condition == 0 || (ct.condition == 1 && reprap.GetPrintMonitor()->IsPrinting()))
 		   )
 		{
-			triggersPending |= (1 << triggerNumber);
+			triggersPending |= (1u << triggerNumber);
 		}
-		if (triggerNumber < lowestTriggerPending && (triggersPending & (1 << triggerNumber)) != 0)
+		if (triggerNumber < lowestTriggerPending && (triggersPending & (1u << triggerNumber)) != 0)
 		{
 			lowestTriggerPending = triggerNumber;
 		}
@@ -643,7 +646,7 @@ bool GCodes::CheckTriggers()
 	if (lowestTriggerPending < MaxTriggers)
 	{
 		gbCurrent = nullptr;
-		triggersPending &= ~(1 << lowestTriggerPending);		// clear the trigger
+		triggersPending &= ~(1u << lowestTriggerPending);		// clear the trigger
 
 		// Execute the trigger
 		switch(lowestTriggerPending)
@@ -699,16 +702,16 @@ void GCodes::DoPause(bool externalToFile)
 		fileGCode->Init();
 		if (moveAvailable)
 		{
-			for (size_t drive = AXES; drive < DRIVES; ++drive)
+			for (size_t drive = numAxes; drive < DRIVES; ++drive)
 			{
 				pausedMoveBuffer[drive] += moveBuffer.coords[drive];	// add on the extrusion in the move not yet taken
 			}
 			ClearMove();
 		}
 
-		for (size_t drive = AXES; drive < DRIVES; ++drive)
+		for (size_t drive = numAxes; drive < DRIVES; ++drive)
 		{
-			pausedMoveBuffer[drive] = lastRawExtruderPosition[drive - AXES] - pausedMoveBuffer[drive];
+			pausedMoveBuffer[drive] = lastRawExtruderPosition[drive - numAxes] - pausedMoveBuffer[drive];
 		}
 
 		if (reprap.Debug(moduleGcodes))
@@ -719,13 +722,13 @@ void GCodes::DoPause(bool externalToFile)
 	else
 	{
 		// Pausing a file print because of a command in the file itself
-		for (size_t drive = 0; drive < AXES; ++drive)
+		for (size_t drive = 0; drive < numAxes; ++drive)
 		{
 			pausedMoveBuffer[drive] = moveBuffer.coords[drive];
 		}
-		for (size_t drive = AXES; drive < DRIVES; ++drive)
+		for (size_t drive = numAxes; drive < DRIVES; ++drive)
 		{
-			pausedMoveBuffer[drive] = lastRawExtruderPosition[drive - AXES];	// get current extruder positions into pausedMoveBuffer
+			pausedMoveBuffer[drive] = lastRawExtruderPosition[drive - numAxes];	// get current extruder positions into pausedMoveBuffer
 		}
 		pausedFeedRate = feedRate;
 	}
@@ -816,7 +819,7 @@ void GCodes::Pop()
 bool GCodes::LoadMoveBufferFromGCode(GCodeBuffer *gb, bool doingG92, bool applyLimits)
 {
 	// Zero every extruder drive as some drives may not be changed
-	for (size_t drive = AXES; drive < DRIVES; drive++)
+	for (size_t drive = numAxes; drive < DRIVES; drive++)
 	{
 		moveBuffer.coords[drive] = 0.0;
 	}
@@ -840,7 +843,7 @@ bool GCodes::LoadMoveBufferFromGCode(GCodeBuffer *gb, bool doingG92, bool applyL
 		size_t eMoveCount = tool->DriveCount();
 		if (eMoveCount > 0)
 		{
-			float eMovement[DRIVES - AXES];
+			float eMovement[DRIVES - MIN_AXES];
 			if (tool->GetMixing())
 			{
 				float length = gb->GetFValue();
@@ -868,7 +871,7 @@ bool GCodes::LoadMoveBufferFromGCode(GCodeBuffer *gb, bool doingG92, bool applyL
 				float moveArg = eMovement[eDrive] * distanceScale;
 				if (doingG92)
 				{
-					moveBuffer.coords[drive + AXES] = moveArg;
+					moveBuffer.coords[drive + numAxes] = moveArg;
 					lastRawExtruderPosition[drive] = moveArg;
 				}
 				else
@@ -879,7 +882,7 @@ bool GCodes::LoadMoveBufferFromGCode(GCodeBuffer *gb, bool doingG92, bool applyL
 					lastRawExtruderPosition[drive] += extrusionAmount;
 					rawExtruderTotalByDrive[drive] += extrusionAmount;
 					rawExtruderTotal += extrusionAmount;
-					moveBuffer.coords[drive + AXES] = extrusionAmount * extrusionFactors[drive];
+					moveBuffer.coords[drive + numAxes] = extrusionAmount * extrusionFactors[drive];
 				}
 			}
 		}
@@ -887,14 +890,14 @@ bool GCodes::LoadMoveBufferFromGCode(GCodeBuffer *gb, bool doingG92, bool applyL
 
 	// Now the movement axes
 	const Tool *currentTool = reprap.GetCurrentTool();
-	for (size_t axis = 0; axis < AXES; axis++)
+	for (size_t axis = 0; axis < numAxes; axis++)
 	{
 		if (gb->Seen(axisLetters[axis]))
 		{
 			float moveArg = gb->GetFValue() * distanceScale * axisScaleFactors[axis];
 			if (doingG92)
 			{
-				axisIsHomed[axis] = true;		// doing a G92 defines the absolute axis position
+				SetAxisIsHomed(axis);				// doing a G92 defines the absolute axis position
 			}
 			else
 			{
@@ -904,11 +907,11 @@ bool GCodes::LoadMoveBufferFromGCode(GCodeBuffer *gb, bool doingG92, bool applyL
 				}
 				else if (currentTool != NULL)
 				{
-					moveArg -= currentTool->GetOffset()[axis];// adjust requested position to compensate for tool offset
+					moveArg -= currentTool->GetOffset()[axis];	// adjust requested position to compensate for tool offset
 				}
 
 				// If on a Cartesian printer and applying limits, limit all axes
-				if (applyLimits && axisIsHomed[axis] && !reprap.GetMove()->IsDeltaMode()
+				if (applyLimits && GetAxisIsHomed(axis) && !reprap.GetMove()->IsDeltaMode()
 #if SUPPORT_ROLAND
 						&& !reprap.GetRoland()->Active()
 #endif
@@ -975,11 +978,11 @@ int GCodes::SetUpMove(GCodeBuffer *gb, StringRef& reply)
 
 		if (ival == 1)
 		{
-			for (size_t i = 0; i < AXES; ++i)
+			for (size_t i = 0; i < numAxes; ++i)
 			{
 				if (gb->Seen(axisLetters[i]))
 				{
-					moveBuffer.endStopsToCheck |= (1 << i);
+					moveBuffer.endStopsToCheck |= (1u << i);
 				}
 			}
 		}
@@ -1111,12 +1114,19 @@ bool GCodes::DoCannedCycleMove(EndstopChecks ce)
 
 		for (size_t drive = 0; drive < DRIVES; drive++)
 		{
-			if (activeDrive[drive])
+			switch(cannedMoveType[drive])
 			{
-				moveBuffer.coords[drive] = moveToDo[drive];
+			case CannedMoveType::none:
+				break;
+			case CannedMoveType::relative:
+				moveBuffer.coords[drive] += cannedMoveCoords[drive];
+				break;
+			case CannedMoveType::absolute:
+				moveBuffer.coords[drive] = cannedMoveCoords[drive];
+				break;
 			}
 		}
-		moveBuffer.feedRate = moveToDo[DRIVES];
+		moveBuffer.feedRate = cannedFeedRate;
 		moveBuffer.endStopsToCheck = ce;
 		moveBuffer.filePos = noFilePosition;
 		moveBuffer.usePressureAdvance = false;
@@ -1132,7 +1142,7 @@ bool GCodes::SetPositions(GCodeBuffer *gb)
 	// Don't pause the machine if only extruder drives are being reset (DC, 2015-09-06).
 	// This avoids blobs and seams when the gcode uses absolute E coordinates and periodically includes G92 E0.
 	bool includingAxes = false;
-	for (size_t drive = 0; drive < AXES; ++drive)
+	for (size_t drive = 0; drive < numAxes; ++drive)
 	{
 		if (gb->Seen(axisLetters[drive]))
 		{
@@ -1188,35 +1198,29 @@ bool GCodes::OffsetAxes(GCodeBuffer* gb)
 		}
 		for (size_t drive = 0; drive < DRIVES; drive++)
 		{
-			if (drive < AXES)
+			if (drive < numAxes)
 			{
 				record[drive] = moveBuffer.coords[drive];
-				moveToDo[drive] = moveBuffer.coords[drive];
+				if (gb->Seen(axisLetters[drive]))
+				{
+					cannedMoveCoords[drive] = gb->GetFValue();
+					cannedMoveType[drive] = CannedMoveType::relative;
+				}
 			}
 			else
 			{
 				record[drive] = 0.0;
-				moveToDo[drive] = 0.0;
-			}
-			activeDrive[drive] = false;
-		}
-
-		for (size_t axis = 0; axis < AXES; axis++)
-		{
-			if (gb->Seen(axisLetters[axis]))
-			{
-				moveToDo[axis] += gb->GetFValue();
-				activeDrive[axis] = true;
 			}
+			cannedMoveType[drive] = CannedMoveType::none;
 		}
 
 		if (gb->Seen(feedrateLetter)) // Has the user specified a feedrate?
 		{
-			moveToDo[DRIVES] = gb->GetFValue() * distanceScale * SECONDS_TO_MINUTES;
+			cannedFeedRate = gb->GetFValue() * distanceScale * SECONDS_TO_MINUTES;
 		}
 		else
 		{
-			moveToDo[DRIVES] = feedRate;
+			cannedFeedRate = DEFAULT_FEEDRATE;
 		}
 
 		offSetSet = true;
@@ -1274,30 +1278,28 @@ bool GCodes::DoHome(GCodeBuffer *gb, StringRef& reply, bool& error)
 	else
 	{
 		toBeHomed = 0;
-		for (size_t axis = 0; axis < AXES; ++axis)
+		for (size_t axis = 0; axis < numAxes; ++axis)
 		{
 			if (gb->Seen(axisLetters[axis]))
 			{
-				toBeHomed |= (1 << axis);
-				axisIsHomed[axis] = false;
+				toBeHomed |= (1u << axis);
+				SetAxisNotHomed(axis);
 			}
 		}
 
-		if (toBeHomed == 0 || toBeHomed == ((1 << X_AXIS) | (1 << Y_AXIS) | (1 << Z_AXIS)))
+		if (toBeHomed == 0 || toBeHomed == ((1u << numAxes) - 1))
 		{
 			// Homing everything
 			SetAllAxesNotHomed();
 			DoFileMacro(HOME_ALL_G);
 		}
 		else if (   platform->MustHomeXYBeforeZ()
-				 && ((toBeHomed & (1 << Z_AXIS)) != 0)
-				 && (   (((toBeHomed & (1 << X_AXIS)) == 0) && !axisIsHomed[X_AXIS])
-					 || (((toBeHomed & (1 << Y_AXIS)) == 0) && !axisIsHomed[Y_AXIS])
-					)
+				 && ((toBeHomed & (1u << Z_AXIS)) != 0)
+				 && ((toBeHomed | axesHomed | (1u << Z_AXIS)) != ((1u << numAxes) - 1))
 				)
 		{
 			// We can only home Z if both X and Y have already been homed or are being homed
-			reply.copy("Must home X and Y before homing Z");
+			reply.copy("Must home all other axes before homing Z");
 			error = true;
 		}
 		else
@@ -1318,15 +1320,15 @@ bool GCodes::DoSingleZProbeAtPoint(int probePointIndex, float heightAdjust)
 
 	for (size_t drive = 0; drive <= DRIVES; drive++)
 	{
-		activeDrive[drive] = false;
+		cannedMoveType[drive] = CannedMoveType::none;
 	}
 
 	switch (cannedCycleMoveCount)
 	{
 	case 0: // Move Z to the dive height. This only does anything on the first move; on all the others Z is already there
-		moveToDo[Z_AXIS] = platform->GetZProbeDiveHeight() + max<float>(platform->ZProbeStopHeight(), 0.0);
-		activeDrive[Z_AXIS] = true;
-		moveToDo[DRIVES] = platform->GetZProbeTravelSpeed();
+		cannedMoveCoords[Z_AXIS] = platform->GetZProbeDiveHeight() + max<float>(platform->ZProbeStopHeight(), 0.0);
+		cannedMoveType[Z_AXIS] = CannedMoveType::absolute;
+		cannedFeedRate = platform->GetZProbeTravelSpeed();
 		if (DoCannedCycleMove(0))
 		{
 			cannedCycleMoveCount++;
@@ -1334,11 +1336,11 @@ bool GCodes::DoSingleZProbeAtPoint(int probePointIndex, float heightAdjust)
 		return false;
 
 	case 1:	// Move to the correct XY coordinates
-		GetProbeCoordinates(probePointIndex, moveToDo[X_AXIS], moveToDo[Y_AXIS], moveToDo[Z_AXIS]);
-		activeDrive[X_AXIS] = true;
-		activeDrive[Y_AXIS] = true;
+		GetProbeCoordinates(probePointIndex, cannedMoveCoords[X_AXIS], cannedMoveCoords[Y_AXIS], cannedMoveCoords[Z_AXIS]);
+		cannedMoveType[X_AXIS] = CannedMoveType::absolute;
+		cannedMoveType[Y_AXIS] = CannedMoveType::absolute;
 		// NB - we don't use the Z value
-		moveToDo[DRIVES] = platform->GetZProbeTravelSpeed();
+		cannedFeedRate = platform->GetZProbeTravelSpeed();
 		if (DoCannedCycleMove(0))
 		{
 			cannedCycleMoveCount++;
@@ -1347,20 +1349,28 @@ bool GCodes::DoSingleZProbeAtPoint(int probePointIndex, float heightAdjust)
 
 	case 2:	// Probe the bed
 		{
-			const float height = (axisIsHomed[Z_AXIS])
+			const float height = (GetAxisIsHomed(Z_AXIS))
 									? 2 * 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
 			switch(DoZProbe(height))
 			{
 			case 0:
 				// Z probe is already triggered at the start of the move, so abandon the probe and record an error
-				platform->Message(GENERIC_MESSAGE, "Z probe warning: probe already triggered at start of probing move\n");
+				platform->Message(GENERIC_MESSAGE, "Error: Z probe already triggered at start of probing move\n");
 				cannedCycleMoveCount++;
 				reprap.GetMove()->SetZBedProbePoint(probePointIndex, platform->GetZProbeDiveHeight(), true, true);
 				break;
 
 			case 1:
-				if (axisIsHomed[Z_AXIS])
+				// Z probe did not trigger
+				platform->Message(GENERIC_MESSAGE, "Error: Z probe was not triggered during probing move\n");
+				cannedCycleMoveCount++;
+				reprap.GetMove()->SetZBedProbePoint(probePointIndex, -(platform->GetZProbeDiveHeight()), true, true);
+				break;
+
+			case 2:
+				// Successful probing
+				if (GetAxisIsHomed(Z_AXIS))
 				{
 					lastProbedZ = moveBuffer.coords[Z_AXIS] - (platform->ZProbeStopHeight() + heightAdjust);
 				}
@@ -1369,7 +1379,7 @@ bool GCodes::DoSingleZProbeAtPoint(int probePointIndex, float heightAdjust)
 					// The Z axis has not yet been homed, so treat this probe as a homing move.
 					moveBuffer.coords[Z_AXIS] = platform->ZProbeStopHeight() + heightAdjust;
 					SetPositions(moveBuffer.coords);
-					axisIsHomed[Z_AXIS] = true;
+					SetAxisIsHomed(Z_AXIS);
 					lastProbedZ = 0.0;
 				}
 				reprap.GetMove()->SetZBedProbePoint(probePointIndex, lastProbedZ, true, false);
@@ -1383,9 +1393,9 @@ bool GCodes::DoSingleZProbeAtPoint(int probePointIndex, float heightAdjust)
 		return false;
 
 	case 3:	// Raise the head back up to the dive height
-		moveToDo[Z_AXIS] = platform->GetZProbeDiveHeight() + max<float>(platform->ZProbeStopHeight(), 0.0);
-		activeDrive[Z_AXIS] = true;
-		moveToDo[DRIVES] = platform->GetZProbeTravelSpeed();
+		cannedMoveCoords[Z_AXIS] = platform->GetZProbeDiveHeight() + max<float>(platform->ZProbeStopHeight(), 0.0);
+		cannedMoveType[Z_AXIS] = CannedMoveType::absolute;
+		cannedFeedRate = platform->GetZProbeTravelSpeed();
 		if (DoCannedCycleMove(0))
 		{
 			cannedCycleMoveCount = 0;
@@ -1406,14 +1416,15 @@ bool GCodes::DoSingleZProbe(bool reportOnly, float heightAdjust)
 	switch (DoZProbe(1.1 * platform->AxisTotalLength(Z_AXIS)))
 	{
 	case 0:		// failed
+	case 1:
 		return true;
 
-	case 1:		// success
+	case 2:		// success
 		if (!reportOnly)
 		{
 			moveBuffer.coords[Z_AXIS] = platform->ZProbeStopHeight() + heightAdjust;
 			SetPositions(moveBuffer.coords);
-			axisIsHomed[Z_AXIS] = true;
+			SetAxisIsHomed(Z_AXIS);
 			lastProbedZ = 0.0;
 		}
 		return true;
@@ -1425,8 +1436,9 @@ bool GCodes::DoSingleZProbe(bool reportOnly, float heightAdjust)
 
 // Do a Z probe cycle up to the maximum specified distance.
 // Returns -1 if not complete yet
-// Returns 0 if failed
-// Returns 1 if success, with lastProbedZ set to the height we stopped at and the current position in moveBuffer
+// Returns 0 if Z probe already triggered at start of probing
+// Returns 1 if Z probe didn't trigger
+// Returns 2 if success, with the current position in moveBuffer
 int GCodes::DoZProbe(float distance)
 {
 	if (platform->GetZProbeType() == ZProbeTypeDelta)
@@ -1436,24 +1448,29 @@ int GCodes::DoZProbe(float distance)
 	}
 	else
 	{
-		if (!cannedCycleMoveQueued && reprap.GetPlatform()->GetZProbeResult() == EndStopHit::lowHit)
+		// Check for probe already triggered at start
+		if (!cannedCycleMoveQueued)
 		{
-			return 0;
+			if (reprap.GetPlatform()->GetZProbeResult() == EndStopHit::lowHit)
+			{
+				return 0;
+			}
+			zProbeTriggered = false;
 		}
 
 		// Do a normal canned cycle Z movement with Z probe enabled
 		for (size_t drive = 0; drive <= DRIVES; drive++)
 		{
-			activeDrive[drive] = false;
+			cannedMoveType[drive] = CannedMoveType::none;
 		}
 
-		moveToDo[Z_AXIS] = -distance;
-		activeDrive[Z_AXIS] = true;
-		moveToDo[DRIVES] = platform->GetZProbeParameters().probeSpeed;
+		cannedMoveCoords[Z_AXIS] = -distance;
+		cannedMoveType[Z_AXIS] = CannedMoveType::relative;
+		cannedFeedRate = platform->GetZProbeParameters().probeSpeed;
 
 		if (DoCannedCycleMove(ZProbeActive))
 		{
-			return 1;
+			return (zProbeTriggered) ? 2 : 1;
 		}
 		return -1;
 	}
@@ -1621,9 +1638,6 @@ bool GCodes::SetPrintZProbe(GCodeBuffer* gb, StringRef& reply)
 // Return the current coordinates as a printable string.  Coordinates
 // are updated at the end of each movement, so this won't tell you
 // where you are mid-movement.
-
-//Fixed to deal with multiple extruders
-
 void GCodes::GetCurrentCoordinates(StringRef& s) const
 {
 	float liveCoordinates[DRIVES];
@@ -1632,22 +1646,26 @@ void GCodes::GetCurrentCoordinates(StringRef& s) const
 	if (currentTool != NULL)
 	{
 		const float *offset = currentTool->GetOffset();
-		for (size_t i = 0; i < AXES; ++i)
+		for (size_t i = 0; i < numAxes; ++i)
 		{
 			liveCoordinates[i] += offset[i];
 		}
 	}
 
-	s.printf("X:%.2f Y:%.2f Z:%.2f ", liveCoordinates[X_AXIS], liveCoordinates[Y_AXIS], liveCoordinates[Z_AXIS]);
-	for (size_t i = AXES; i < DRIVES; i++)
+	s.Clear();
+	for (size_t axis = 0; axis < numAxes; ++axis)
 	{
-		s.catf("E%u:%.1f ", i - AXES, liveCoordinates[i]);
+		s.catf("%c: %.2f ", axisLetters[axis], liveCoordinates[axis]);
+	}
+	for (size_t i = numAxes; i < DRIVES; i++)
+	{
+		s.catf("E%u: %.1f ", i - numAxes, liveCoordinates[i]);
 	}
 
 	// Print the axis stepper motor positions as Marlin does, as an aid to debugging.
 	// Don't bother with the extruder endpoints, they are zero after any non-extruding move.
 	s.cat(" Count");
-	for (size_t i = 0; i < AXES; ++i)
+	for (size_t i = 0; i < numAxes; ++i)
 	{
 		s.catf(" %d", reprap.GetMove()->GetEndPoint(i));
 	}
@@ -1761,10 +1779,10 @@ void GCodes::QueueFileToPrint(const char* fileName)
 		fileGCode->SetToolNumberAdjust(0);	// clear tool number adjustment
 
 		// Reset all extruder positions when starting a new print
-		for (size_t extruder = AXES; extruder < DRIVES; extruder++)
+		for (size_t extruder = MIN_AXES; extruder < DRIVES; extruder++)
 		{
-			lastRawExtruderPosition[extruder - AXES] = 0.0;
-			rawExtruderTotalByDrive[extruder - AXES] = 0.0;
+			lastRawExtruderPosition[extruder - MIN_AXES] = 0.0;
+			rawExtruderTotalByDrive[extruder - MIN_AXES] = 0.0;
 		}
 		rawExtruderTotal = 0.0;
 		reprap.GetMove()->ResetExtruderPositions();
@@ -1788,7 +1806,16 @@ void GCodes::DeleteFile(const char* fileName)
 // Function to handle dwell delays.  Return true for dwell finished, false otherwise.
 bool GCodes::DoDwell(GCodeBuffer *gb)
 {
-	if (!gb->Seen('P'))
+	float dwell;
+	if (gb->Seen('S'))
+	{
+		dwell = gb->GetFValue();
+	}
+	else if (gb->Seen('P'))
+	{
+		dwell = 0.001 * (float) gb->GetLValue(); // P values are in milliseconds; we need seconds
+	}
+	else
 	{
 		return true;  // No time given - throw it away
 	}
@@ -1807,8 +1834,6 @@ bool GCodes::DoDwell(GCodeBuffer *gb)
 		return false;
 	}
 
-	float dwell = 0.001 * (float) gb->GetLValue(); // P values are in milliseconds; we need seconds
-
 	if (simulationMode != 0)
 	{
 		simulationTime += dwell;
@@ -1858,27 +1883,20 @@ void GCodes::SetOrReportOffsets(StringRef& reply, GCodeBuffer *gb)
 		}
 
 		// Deal with setting offsets
-		float offset[AXES];
-		for (size_t i = 0; i < AXES; ++i)
+		float offset[MAX_AXES];
+		for (size_t i = 0; i < MAX_AXES; ++i)
 		{
 			offset[i] = tool->GetOffset()[i];
 		}
 
 		bool settingOffset = false;
-		if (gb->Seen('X'))
+		for (size_t axis = 0; axis < numAxes; ++axis)
 		{
-			offset[X_AXIS] = gb->GetFValue();
-			settingOffset = true;
-		}
-		if (gb->Seen('Y'))
-		{
-			offset[Y_AXIS] = gb->GetFValue();
-			settingOffset = true;
-		}
-		if (gb->Seen('Z'))
-		{
-			offset[Z_AXIS] = gb->GetFValue();
-			settingOffset = true;
+			if (gb->Seen(axisLetters[axis]))
+			{
+				offset[axis] = gb->GetFValue();
+				settingOffset = true;
+			}
 		}
 		if (settingOffset)
 		{
@@ -1950,8 +1968,8 @@ void GCodes::ManageTool(GCodeBuffer *gb, StringRef& reply)
 	}
 
 	// Check drives
-	long drives[DRIVES - AXES];  // There can never be more than we have...
-	size_t dCount = DRIVES - AXES;  // Sets the limit and returns the count
+	long drives[DRIVES - MIN_AXES];  	// There can never be more than we have...
+	size_t dCount = DRIVES - numAxes;	// Sets the limit and returns the count
 	if (gb->Seen('D'))
 	{
 		gb->GetLongArray(drives, dCount);
@@ -2131,20 +2149,7 @@ void GCodes::HandleReply(GCodeBuffer *gb, bool error, const char* reply)
 	// Second UART device, e.g. dc42's PanelDue. Do NOT use emulation for this one!
 	if (gb == auxGCode || (stackPointer != 0 && stack[0].gb == auxGCode))
 	{
-		// Discard this response if either no aux device is attached or if the response is empty
-		if (reply[0] == 0 || !HaveAux())
-		{
-			return;
-		}
-
-		// Regular text-based responses for AUX are always stored and processed by M105/M408
-		if (auxGCodeReply == nullptr && !OutputBuffer::Allocate(auxGCodeReply))
-		{
-			// No more space to buffer this response. Should never happen
-			return;
-		}
-		auxSeq++;
-		auxGCodeReply->cat(reply);
+		platform->AppendAuxReply(reply);
 		return;
 	}
 
@@ -2247,30 +2252,7 @@ void GCodes::HandleReply(GCodeBuffer *gb, bool error, OutputBuffer *reply)
 	// Second UART device, e.g. dc42's PanelDue. Do NOT use emulation for this one!
 	if (gb == auxGCode || (stackPointer != 0 && stack[0].gb == auxGCode))
 	{
-		// Discard this response if either no aux device is attached or if the response is empty
-		if (reply->Length() == 0 || !HaveAux())
-		{
-			OutputBuffer::ReleaseAll(reply);
-			return;
-		}
-
-		// JSON responses are always sent directly to the AUX device
-		if ((*reply)[0] == '{')
-		{
-			platform->Message(AUX_MESSAGE, reply);
-			return;
-		}
-
-		// Other responses are stored for M105/M408
-		auxSeq++;
-		if (auxGCodeReply == nullptr)
-		{
-			auxGCodeReply = reply;
-		}
-		else
-		{
-			auxGCodeReply->Append(reply);
-		}
+		platform->AppendAuxReply(reply);
 		return;
 	}
 
@@ -2541,7 +2523,7 @@ bool GCodes::RetractFilament(bool retract)
 				}
 
 				reprap.GetMove()->GetCurrentUserPosition(moveBuffer.coords, 0);
-				for (size_t i = AXES; i < DRIVES; ++i)
+				for (size_t i = numAxes; i < DRIVES; ++i)
 				{
 					moveBuffer.coords[i] = 0.0;
 				}
@@ -2623,12 +2605,12 @@ bool GCodes::HandleGcode(GCodeBuffer* gb, StringRef& reply)
 			{
 				return false;
 			}
-			for (size_t axis = 0; axis < AXES; ++axis)
+			for (size_t axis = 0; axis < numAxes; ++axis)
 			{
 				float offset = gb->Seen(axisLetters[axis]) ? gb->GetFValue() * distanceScale : 0.0;
 				moveBuffer.coords[axis] = pausedMoveBuffer[axis] + offset;
 			}
-			for (size_t drive = AXES; drive < DRIVES; ++drive)
+			for (size_t drive = numAxes; drive < DRIVES; ++drive)
 			{
 				moveBuffer.coords[drive] = 0.0;
 			}
@@ -2716,7 +2698,7 @@ bool GCodes::HandleGcode(GCodeBuffer* gb, StringRef& reply)
 		if (!DoFileMacro(BED_EQUATION_G, reprap.GetMove()->IsDeltaMode()))
 		{
 			// If we get here then we are not on a delta printer and there is no bed.g file
-			if (axisIsHomed[X_AXIS] && axisIsHomed[Y_AXIS])
+			if (GetAxisIsHomed(X_AXIS) && GetAxisIsHomed(Y_AXIS))
 			{
 				state = GCodeState::setBed1;		// no bed.g file, so use the coordinates specified by M557
 			}
@@ -2816,11 +2798,11 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 		}
 		{
 			bool seen = false;
-			for (size_t axis = 0; axis < AXES; axis++)
+			for (size_t axis = 0; axis < numAxes; axis++)
 			{
 				if (gb->Seen(axisLetters[axis]))
 				{
-					axisIsHomed[axis] = false;
+					SetAxisNotHomed(axis);
 					platform->DisableDrive(axis);
 					seen = true;
 				}
@@ -2828,19 +2810,19 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 
 			if (gb->Seen(extrudeLetter))
 			{
-				long int eDrive[DRIVES - AXES];
-				size_t eCount = DRIVES - AXES;
+				long int eDrive[DRIVES - MIN_AXES];
+				size_t eCount = DRIVES - numAxes;
 				gb->GetLongArray(eDrive, eCount);
 				for (size_t i = 0; i < eCount; i++)
 				{
 					seen = true;
-					if (eDrive[i] < 0 || (size_t)eDrive[i] >= DRIVES - AXES)
+					if (eDrive[i] < 0 || (size_t)eDrive[i] >= DRIVES - numAxes)
 					{
 						reply.printf("Invalid extruder number specified: %ld", eDrive[i]);
 						error = true;
 						break;
 					}
-					platform->DisableDrive(AXES + eDrive[i]);
+					platform->DisableDrive(numAxes + eDrive[i]);
 				}
 			}
 
@@ -2876,6 +2858,7 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 			if (sparam == 2)
 			{
 				fileResponse = reprap.GetFilesResponse(dir, true);		// Send the file list in JSON format
+				fileResponse->cat('\n');
 			}
 			else
 			{
@@ -3227,9 +3210,9 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 	case 82:	// Use absolute extruder positioning
 		if (drivesRelative)		// don't reset the absolute extruder position if it was already absolute
 		{
-			for (size_t extruder = AXES; extruder < DRIVES; extruder++)
+			for (size_t extruder = MIN_AXES; extruder < DRIVES; extruder++)
 			{
-				lastRawExtruderPosition[extruder - AXES] = 0.0;
+				lastRawExtruderPosition[extruder - MIN_AXES] = 0.0;
 			}
 			drivesRelative = false;
 		}
@@ -3238,9 +3221,9 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 	case 83:	// Use relative extruder positioning
 		if (!drivesRelative)	// don't reset the absolute extruder position if it was already relative
 		{
-			for (size_t extruder = AXES; extruder < DRIVES; extruder++)
+			for (size_t extruder = MIN_AXES; extruder < DRIVES; extruder++)
 			{
-				lastRawExtruderPosition[extruder - AXES] = 0.0;
+				lastRawExtruderPosition[extruder - MIN_AXES] = 0.0;
 			}
 			drivesRelative = true;
 		}
@@ -3263,7 +3246,7 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 			move->GetCurrentUserPosition(positionNow, 0);
 
 			bool seen = false;
-			for (size_t axis = 0; axis < AXES; axis++)
+			for (size_t axis = 0; axis < numAxes; axis++)
 			{
 				if (gb->Seen(axisLetters[axis]))
 				{
@@ -3275,14 +3258,14 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 			if (gb->Seen(extrudeLetter))
 			{
 				seen = true;
-				float eVals[DRIVES - AXES];
-				size_t eCount = DRIVES - AXES;
+				float eVals[DRIVES - MIN_AXES];
+				size_t eCount = DRIVES - numAxes;
 				gb->GetFloatArray(eVals, eCount, true);
 
 				// The user may not have as many extruders as we allow for, so just set the ones for which a value is provided
 				for (size_t e = 0; e < eCount; e++)
 				{
-					platform->SetDriveStepsPerUnit(AXES + e, eVals[e]);
+					platform->SetDriveStepsPerUnit(numAxes + e, eVals[e]);
 				}
 			}
 
@@ -3293,9 +3276,13 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 			}
 			else
 			{
-				reply.printf("Steps/mm: X: %.3f, Y: %.3f, Z: %.3f, E: ", platform->DriveStepsPerUnit(X_AXIS),
-						platform->DriveStepsPerUnit(Y_AXIS), platform->DriveStepsPerUnit(Z_AXIS));
-				for (size_t drive = AXES; drive < DRIVES; drive++)
+				reply.copy("Steps/mm: ");
+				for (size_t axis = 0; axis < numAxes; ++axis)
+				{
+					reply.catf("%c: %.3f, ", platform->DriveStepsPerUnit(axis));
+				}
+				reply.catf("E: ");
+				for (size_t drive = numAxes; drive < DRIVES; drive++)
 				{
 					reply.catf("%.3f", platform->DriveStepsPerUnit(drive));
 					if (drive < DRIVES - 1)
@@ -3435,7 +3422,7 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 					const int hnum = heaters[h];
 					if (hnum >= 0 && hnum < HEATERS)
 					{
-						hh |= (1 << (unsigned int)hnum);
+						hh |= (1u << (unsigned int)hnum);
 					}
 				}
 				if (hh != 0)
@@ -3472,7 +3459,7 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 					reply.catf(", trigger: %dC, heaters:", (int)platform->GetTriggerTemperature(fanNum));
 					for (unsigned int i = 0; i < HEATERS; ++i)
 					{
-						if ((hh & (1 << i)) != 0)
+						if ((hh & (1u << i)) != 0)
 						{
 							reply.catf(" %u", i);
 						}
@@ -3622,7 +3609,7 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 
 	case 119:
 		reply.copy("Endstops - ");
-		for (size_t axis = 0; axis < AXES; axis++)
+		for (size_t axis = 0; axis < numAxes; axis++)
 		{
 			reply.catf("%c: %s, ", axisLetters[axis], TranslateEndStopResult(platform->Stopped(axis)));
 		}
@@ -3659,14 +3646,6 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 		}
 		break;
 
-	case 126: // Valve open
-		reply.copy("M126 - valves not yet implemented");
-		break;
-
-	case 127: // Valve closed
-		reply.copy("M127 - valves not yet implemented");
-		break;
-
 	case 135: // Set PID sample interval
 		if (gb->Seen('S'))
 		{
@@ -3856,7 +3835,7 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 	case 201: // Set/print axis accelerations
 	{
 		bool seen = false;
-		for (size_t axis = 0; axis < AXES; axis++)
+		for (size_t axis = 0; axis < numAxes; axis++)
 		{
 			if (gb->Seen(axisLetters[axis]))
 			{
@@ -3868,12 +3847,12 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 		if (gb->Seen(extrudeLetter))
 		{
 			seen = true;
-			float eVals[DRIVES - AXES];
-			size_t eCount = DRIVES - AXES;
+			float eVals[DRIVES - MIN_AXES];
+			size_t eCount = DRIVES - numAxes;
 			gb->GetFloatArray(eVals, eCount, true);
 			for (size_t e = 0; e < eCount; e++)
 			{
-				platform->SetAcceleration(AXES + e, eVals[e] * distanceScale);
+				platform->SetAcceleration(numAxes + e, eVals[e] * distanceScale);
 			}
 		}
 
@@ -3886,10 +3865,12 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 
 		if (!seen)
 		{
-			reply.printf("Accelerations: X: %.1f, Y: %.1f, Z: %.1f, E: ",
-					platform->Acceleration(X_AXIS) / distanceScale, platform->Acceleration(Y_AXIS) / distanceScale,
-					platform->Acceleration(Z_AXIS) / distanceScale);
-			for (size_t drive = AXES; drive < DRIVES; drive++)
+			reply.printf("Accelerations: ");
+			for (size_t axis = 0; axis < numAxes; ++axis)
+			{
+				reply.catf("%c: %.1f, ", platform->Acceleration(axis) / distanceScale);
+			}
+			for (size_t drive = numAxes; drive < DRIVES; drive++)
 			{
 				reply.catf("%.1f", platform->Acceleration(drive) / distanceScale);
 				if (drive < DRIVES - 1)
@@ -3905,7 +3886,7 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 	case 203: // Set/print maximum feedrates
 		{
 			bool seen = false;
-			for (size_t axis = 0; axis < AXES; axis++)
+			for (size_t axis = 0; axis < numAxes; axis++)
 			{
 				if (gb->Seen(axisLetters[axis]))
 				{
@@ -3917,22 +3898,24 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 			if (gb->Seen(extrudeLetter))
 			{
 				seen = true;
-				float eVals[DRIVES - AXES];
-				size_t eCount = DRIVES - AXES;
+				float eVals[DRIVES - MIN_AXES];
+				size_t eCount = DRIVES - numAxes;
 				gb->GetFloatArray(eVals, eCount, true);
 				for (size_t e = 0; e < eCount; e++)
 				{
-					platform->SetMaxFeedrate(AXES + e, eVals[e] * distanceScale * secondsToMinutes);
+					platform->SetMaxFeedrate(numAxes + e, eVals[e] * distanceScale * secondsToMinutes);
 				}
 			}
 
 			if (!seen)
 			{
-				reply.printf("Maximum feedrates: X: %.1f, Y: %.1f, Z: %.1f, E: ",
-						platform->MaxFeedrate(X_AXIS) / (distanceScale * secondsToMinutes),
-						platform->MaxFeedrate(Y_AXIS) / (distanceScale * secondsToMinutes),
-						platform->MaxFeedrate(Z_AXIS) / (distanceScale * secondsToMinutes));
-				for (size_t drive = AXES; drive < DRIVES; drive++)
+				reply.copy("Maximum feedrates: ");
+				for (size_t axis = 0; axis < numAxes; axis++)
+				{
+					reply.catf("%c: %.1f, ", platform->MaxFeedrate(axis) / (distanceScale * secondsToMinutes));
+				}
+				reply.cat("E: ");
+				for (size_t drive = numAxes; drive < DRIVES; drive++)
 				{
 					reply.catf("%.1f", platform->MaxFeedrate(drive) / (distanceScale * secondsToMinutes));
 					if (drive < DRIVES - 1)
@@ -3987,7 +3970,7 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 		{
 			bool setMin = (gb->Seen('S') ? (gb->GetIValue() == 1) : false);
 			bool seen = false;
-			for (size_t axis = 0; axis < AXES; axis++)
+			for (size_t axis = 0; axis < numAxes; axis++)
 			{
 				if (gb->Seen(axisLetters[axis]))
 				{
@@ -4008,7 +3991,7 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 			{
 				reply.copy("Axis limits ");
 				char sep = '-';
-				for (size_t axis = 0; axis < AXES; axis++)
+				for (size_t axis = 0; axis < numAxes; axis++)
 				{
 					reply.catf("%c %c: %.1f min, %.1f max", sep, axisLetters[axis], platform->AxisMinimum(axis),
 							platform->AxisMaximum(axis));
@@ -4059,11 +4042,11 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 			if (gb->Seen('S'))	// S parameter sets the override percentage
 			{
 				float extrusionFactor = gb->GetFValue() / 100.0;
-				if (extruder >= 0 && (size_t)extruder < DRIVES - AXES && extrusionFactor >= 0.0)
+				if (extruder >= 0 && (size_t)extruder < DRIVES - numAxes && extrusionFactor >= 0.0)
 				{
 					if (moveAvailable && !moveBuffer.isFirmwareRetraction)
 					{
-						moveBuffer.coords[extruder + AXES] *= extrusionFactor/extrusionFactors[extruder];	// last move not gone, so update it
+						moveBuffer.coords[extruder + numAxes] *= extrusionFactor/extrusionFactors[extruder];	// last move not gone, so update it
 					}
 					extrusionFactors[extruder] = extrusionFactor;
 				}
@@ -4245,7 +4228,7 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 			}
 
 			bool seen = false;
-			for (size_t axis = 0; axis < AXES; axis++)
+			for (size_t axis = 0; axis < numAxes; axis++)
 			{
 				if (gb->Seen(axisLetters[axis]))
 				{
@@ -4253,7 +4236,7 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 					int microsteps = gb->GetIValue();
 					if (ChangeMicrostepping(axis, microsteps, interp))
 					{
-						axisIsHomed[axis] = false;
+						SetAxisNotHomed(axis);
 					}
 					else
 					{
@@ -4266,12 +4249,12 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 			if (gb->Seen(extrudeLetter))
 			{
 				seen = true;
-				long eVals[DRIVES - AXES];
-				size_t eCount = DRIVES - AXES;
+				long eVals[DRIVES - MIN_AXES];
+				size_t eCount = DRIVES - numAxes;
 				gb->GetLongArray(eVals, eCount);
 				for (size_t e = 0; e < eCount; e++)
 				{
-					if (!ChangeMicrostepping(AXES + e, (int)eVals[e], interp))
+					if (!ChangeMicrostepping(numAxes + e, (int)eVals[e], interp))
 					{
 						platform->MessageF(GENERIC_MESSAGE, "Drive E%u does not support %dx microstepping%s\n",
 												e, (int)eVals[e], (interp) ? " with interpolation" : "");
@@ -4282,14 +4265,14 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 			if (!seen)
 			{
 				reply.copy("Microstepping - ");
-				for (size_t axis = 0; axis < AXES; ++axis)
+				for (size_t axis = 0; axis < numAxes; ++axis)
 				{
 					bool interp;
 					int microsteps = platform->GetMicrostepping(axis, interp);
 					reply.catf("%c:%d%s, ", axisLetters[axis], microsteps, (interp) ? "(on)" : "");
 				}
 				reply.cat("E");
-				for (size_t drive = AXES; drive < DRIVES; drive++)
+				for (size_t drive = numAxes; drive < DRIVES; drive++)
 				{
 					bool interp;
 					int microsteps = platform->GetMicrostepping(drive, interp);
@@ -4554,11 +4537,11 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 		}
 		break;
 
-	case 556: // Axis compensation
+	case 556: // Axis compensation (we support only X, Y, Z)
 		if (gb->Seen('S'))
 		{
 			float value = gb->GetFValue();
-			for (size_t axis = 0; axis < AXES; axis++)
+			for (size_t axis = 0; axis <= Z_AXIS; axis++)
 			{
 				if (gb->Seen(axisLetters[axis]))
 				{
@@ -4608,13 +4591,19 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 	case 558: // Set or report Z probe type and for which axes it is used
 		{
 			bool seenAxes = false, seenType = false, seenParam = false;
-			bool zProbeAxes[AXES];
-			platform->GetZProbeAxes(zProbeAxes);
-			for (size_t axis = 0; axis < AXES; axis++)
+			uint32_t zProbeAxes = platform->GetZProbeAxes();
+			for (size_t axis = 0; axis < numAxes; axis++)
 			{
 				if (gb->Seen(axisLetters[axis]))
 				{
-					zProbeAxes[axis] = (gb->GetIValue() > 0);
+					if (gb->GetIValue() > 0)
+					{
+						zProbeAxes |= (1u << axis);
+					}
+					else
+					{
+						zProbeAxes &= ~(1u << axis);
+					}
 					seenAxes = true;
 				}
 			}
@@ -4677,9 +4666,9 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 					reply.catf(", parameters %.2f %.2f", params.param1, params.param2);
 				}
 				reply.cat(", used for these axes:");
-				for (size_t axis = 0; axis < AXES; axis++)
+				for (size_t axis = 0; axis < numAxes; axis++)
 				{
-					if (zProbeAxes[axis])
+					if ((zProbeAxes & (1u << axis)) != 0)
 					{
 						reply.catf(" %c", axisLetters[axis]);
 					}
@@ -4758,7 +4747,7 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 	case 566: // Set/print maximum jerk speeds
 	{
 		bool seen = false;
-		for (size_t axis = 0; axis < AXES; axis++)
+		for (size_t axis = 0; axis < numAxes; axis++)
 		{
 			if (gb->Seen(axisLetters[axis]))
 			{
@@ -4770,22 +4759,24 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 		if (gb->Seen(extrudeLetter))
 		{
 			seen = true;
-			float eVals[DRIVES - AXES];
-			size_t eCount = DRIVES - AXES;
+			float eVals[DRIVES - MIN_AXES];
+			size_t eCount = DRIVES - numAxes;
 			gb->GetFloatArray(eVals, eCount, true);
 			for (size_t e = 0; e < eCount; e++)
 			{
-				platform->SetInstantDv(AXES + e, eVals[e] * distanceScale * secondsToMinutes);
+				platform->SetInstantDv(numAxes + e, eVals[e] * distanceScale * secondsToMinutes);
 			}
 		}
 		else if (!seen)
 		{
-			reply.printf("Maximum jerk rates: X: %.1f, Y: %.1f, Z: %.1f, E:",
-					platform->ConfiguredInstantDv(X_AXIS) / (distanceScale * secondsToMinutes),
-					platform->ConfiguredInstantDv(Y_AXIS) / (distanceScale * secondsToMinutes),
-					platform->ConfiguredInstantDv(Z_AXIS) / (distanceScale * secondsToMinutes));
+			reply.copy("Maximum jerk rates: ");
+			for (size_t axis = 0; axis < numAxes; ++axis)
+			{
+				reply.catf("%c: %.1f, ", platform->ConfiguredInstantDv(axis) / (distanceScale * secondsToMinutes));
+			}
+			reply.cat("E:");
 			char sep = ' ';
-			for (size_t drive = AXES; drive < DRIVES; drive++)
+			for (size_t drive = numAxes; drive < DRIVES; drive++)
 			{
 				reply.catf("%c%.1f", sep, platform->ConfiguredInstantDv(drive) / (distanceScale * secondsToMinutes));
 				sep = ':';
@@ -4803,7 +4794,7 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 			{
 				if (gb->Seen(extrudeLetter))
 				{
-					float eVals[DRIVES - AXES];
+					float eVals[DRIVES - MIN_AXES];
 					size_t eCount = tool->DriveCount();
 					gb->GetFloatArray(eVals, eCount, false);
 					if (eCount != tool->DriveCount())
@@ -4870,7 +4861,7 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 					seen = true;
 				}
 				bool badParameter = false;
-				for (size_t axis = 0; axis < AXES; ++axis)
+				for (size_t axis = 0; axis < numAxes; ++axis)
 				{
 					if (gb->Seen(axisLetters[axis]))
 					{
@@ -4973,9 +4964,9 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 		{
 			bool seen = false;
 			bool logicLevel = (gb->Seen('S')) ? (gb->GetIValue() != 0) : true;
-			for (size_t axis = 0; axis <= AXES; ++axis)
+			for (size_t axis = 0; axis <= numAxes; ++axis)
 			{
-				const char letter = (axis == AXES) ? extrudeLetter : axisLetters[axis];
+				const char letter = (axis == numAxes) ? extrudeLetter : axisLetters[axis];
 				if (gb->Seen(letter))
 				{
 					int ival = gb->GetIValue();
@@ -4991,7 +4982,7 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 				reply.copy("Endstop configuration:");
 				EndStopType config;
 				bool logic;
-				for (size_t axis = 0; axis < AXES; ++axis)
+				for (size_t axis = 0; axis < numAxes; ++axis)
 				{
 					platform->GetEndStopConfiguration(axis, config, logic);
 					reply.catf(" %c %s (active %s),", axisLetters[axis],
@@ -5065,7 +5056,7 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 			}
 
 			// Axis endstops
-			for(size_t axis=0; axis<AXES; axis++)
+			for (size_t axis=0; axis < numAxes; axis++)
 			{
 				if (gb->Seen(axisLetters[axis]))
 				{
@@ -5078,15 +5069,15 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 			}
 
 			// Extruder drives
-			size_t eDriveCount = DRIVES - AXES;
-			long eDrives[DRIVES - AXES];
+			size_t eDriveCount = DRIVES - numAxes;
+			long eDrives[DRIVES - MIN_AXES];
 			if (gb->Seen(extrudeLetter))
 			{
 				gb->GetLongArray(eDrives, eDriveCount);
-				for(size_t extruder=0; extruder<DRIVES - AXES; extruder++)
+				for(size_t extruder = 0; extruder < DRIVES - numAxes; extruder++)
 				{
-					const size_t eDrive = eDrives[extruder] + AXES;
-					if (eDrive < AXES || eDrive >= DRIVES)
+					const size_t eDrive = eDrives[extruder] + numAxes;
+					if (eDrive < numAxes || eDrive >= DRIVES)
 					{
 						reply.copy("Invalid extruder drive specified!");
 						error = result = true;
@@ -5120,7 +5111,7 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 	case 579: // Scale Cartesian axes (mostly for Delta configurations)
 		{
 			bool seen = false;
-			for(size_t axis = 0; axis < AXES; axis++)
+			for(size_t axis = 0; axis < numAxes; axis++)
 			{
 				if (gb->Seen(axisLetters[axis]))
 				{
@@ -5133,7 +5124,7 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 			{
 				char sep = ':';
 				reply.copy("Axis scale factors");
-				for(size_t axis = 0; axis < AXES; axis++)
+				for(size_t axis = 0; axis < numAxes; axis++)
 				{
 					reply.catf("%c %c: %.3f", sep, axisLetters[axis], axisScaleFactors[axis]);
 					sep = ',';
@@ -5178,7 +5169,7 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 					uint32_t states = platform->GetAllEndstopStates();
 					if ((triggers[triggerNumber].rising & states) != 0 || (triggers[triggerNumber].falling & ~states) != 0)
 					{
-						triggersPending |= (1 << triggerNumber);
+						triggersPending |= (1u << triggerNumber);
 					}
 				}
 				else
@@ -5198,7 +5189,7 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 						seen = true;
 						int sval = gb->GetIValue();
 						TriggerMask triggerMask = 0;
-						for (size_t axis = 0; axis < AXES; ++axis)
+						for (size_t axis = 0; axis < numAxes; ++axis)
 						{
 							if (gb->Seen(axisLetters[axis]))
 							{
@@ -5207,14 +5198,14 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 						}
 						if (gb->Seen(extrudeLetter))
 						{
-							long eStops[DRIVES - AXES];
-							size_t numEntries = DRIVES - AXES;
+							long eStops[DRIVES - MIN_AXES];
+							size_t numEntries = DRIVES - numAxes;
 							gb->GetLongArray(eStops, numEntries);
 							for (size_t i = 0; i < numEntries; ++i)
 							{
-								if (eStops[i] >= 0 && (unsigned long)eStops[i] < DRIVES - AXES)
+								if (eStops[i] >= 0 && (unsigned long)eStops[i] < DRIVES - numAxes)
 								{
-									triggerMask |= (1u << (eStops[i] + AXES));
+									triggerMask |= (1u << (eStops[i] + numAxes));
 								}
 							}
 						}
@@ -5268,7 +5259,7 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 	case 584: // Set axis/extruder to stepper driver(s) mapping
 		{
 			bool seen = false, badDrive = false;
-			for (size_t drive = 0; drive < AXES; ++drive)
+			for (size_t drive = 0; drive < MAX_AXES; ++drive)
 			{
 				if (gb->Seen(axisLetters[drive]))
 				{
@@ -5298,6 +5289,10 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 					}
 					else
 					{
+						if (drive >= numAxes)
+						{
+							numAxes = drive + 1;		// user has defined a new axis
+						}
 						platform->SetAxisDriversConfig(drive, config);
 					}
 				}
@@ -5306,8 +5301,8 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 			if (gb->Seen(extrudeLetter))
 			{
 				seen = true;
-				size_t numValues = DRIVES - AXES;
-				long drivers[DRIVES - AXES];
+				size_t numValues = DRIVES - numAxes;
+				long drivers[DRIVES - MIN_AXES];
 				gb->GetLongArray(drivers, numValues);
 				for (size_t i = 0; i < numValues; ++i)
 				{
@@ -5329,7 +5324,7 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 			else if (!seen)
 			{
 				reply.copy("Driver assignments:");
-				for (size_t drive = 0; drive < AXES; ++ drive)
+				for (size_t drive = 0; drive < numAxes; ++ drive)
 				{
 					reply.cat(' ');
 					const AxisDriversConfig& axisConfig = platform->GetAxisDriversConfig(drive);
@@ -5342,7 +5337,7 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 				}
 				reply.cat(' ');
 				char c = extrudeLetter;
-				for (size_t extruder = 0; extruder < DRIVES - AXES; ++extruder)
+				for (size_t extruder = 0; extruder < DRIVES - numAxes; ++extruder)
 				{
 					reply.catf("%c%u", c, platform->GetExtruderDriver(extruder));
 					c = ':';
@@ -5421,8 +5416,8 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 			{
 				if (params.IsDeltaMode())
 				{
-					reply.printf("Diagonal %.2f, delta radius %.2f, homed height %.2f, bed radius %.1f"
-								 ", X %.2f" DEGREE_SYMBOL ", Y %.2f" DEGREE_SYMBOL ", Z %.2f" DEGREE_SYMBOL,
+					reply.printf("Diagonal %.3f, delta radius %.3f, homed height %.3f, bed radius %.1f"
+								 ", X %.3f" DEGREE_SYMBOL ", Y %.3f" DEGREE_SYMBOL ", Z %.3f" DEGREE_SYMBOL,
 								 	 params.GetDiagonal() / distanceScale, params.GetRadius() / distanceScale,
 								 	 params.GetHomedHeight() / distanceScale, params.GetPrintRadius() / distanceScale,
 								 	 params.GetXCorrection(), params.GetYCorrection(), params.GetZCorrection());
@@ -5486,11 +5481,11 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 				move->SetCoreXYMode(gb->GetIValue());
 				seen = true;
 			}
-			for (size_t axis = 0; axis < AXES; ++axis)
+			for (size_t axis = 0; axis < numAxes; ++axis)
 			{
 				if (gb->Seen(axisLetters[axis]))
 				{
-					move->setCoreAxisFactor(axis, gb->GetFValue());
+					move->SetCoreAxisFactor(axis, gb->GetFValue());
 					seen = true;
 				}
 			}
@@ -5503,7 +5498,7 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 			else
 			{
 				reply.printf("Printer mode is %s with axis factors", move->GetGeometryString());
-				for (size_t axis = 0; axis < AXES; ++axis)
+				for (size_t axis = 0; axis < numAxes; ++axis)
 				{
 					reply.catf(" %c:%f", axisLetters[axis], move->GetCoreAxisFactor(axis));
 				}
@@ -5519,7 +5514,7 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 		}
 		{
 			bool seen = false;
-			for (size_t axis = 0; axis < AXES; axis++)
+			for (size_t axis = 0; axis < numAxes; axis++)
 			{
 				if (gb->Seen(axisLetters[axis]))
 				{
@@ -5530,13 +5525,13 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 
 			if (gb->Seen(extrudeLetter))
 			{
-				float eVals[DRIVES - AXES];
-				size_t eCount = DRIVES - AXES;
+				float eVals[DRIVES - MIN_AXES];
+				size_t eCount = DRIVES - numAxes;
 				gb->GetFloatArray(eVals, eCount, true);
 				// 2014-09-29 DC42: we no longer insist that the user supplies values for all possible extruder drives
 				for (size_t e = 0; e < eCount; e++)
 				{
-					platform->SetMotorCurrent(AXES + e, eVals[e], code == 913);
+					platform->SetMotorCurrent(numAxes + e, eVals[e], code == 913);
 				}
 				seen = true;
 			}
@@ -5554,12 +5549,12 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 			if (!seen)
 			{
 				reply.copy((code == 913) ? "Motor current % of normal - " : "Motor current (mA) - ");
-				for (size_t axis = 0; axis < AXES; ++axis)
+				for (size_t axis = 0; axis < numAxes; ++axis)
 				{
 					reply.catf("%c:%d, ", axisLetters[axis], (int)platform->GetMotorCurrent(axis, code == 913));
 				}
 				reply.cat("E");
-				for (size_t drive = AXES; drive < DRIVES; drive++)
+				for (size_t drive = numAxes; drive < DRIVES; drive++)
 				{
 					reply.catf(":%d", (int)platform->GetMotorCurrent(drive, code == 913));
 				}
@@ -5614,12 +5609,12 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 						firmwareUpdateModuleMap = 0;
 						break;
 					}
-					firmwareUpdateModuleMap |= (1 << (unsigned int)t);
+					firmwareUpdateModuleMap |= (1u << (unsigned int)t);
 				}
 			}
 			else
 			{
-				firmwareUpdateModuleMap = (1 << 0);			// no modules specified, so update module 0 to match old behaviour
+				firmwareUpdateModuleMap = (1u << 0);			// no modules specified, so update module 0 to match old behaviour
 			}
 
 			if (firmwareUpdateModuleMap == 0)
@@ -5655,8 +5650,6 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 	case 998:
 		// The input handling code replaces the gcode by this when it detects a checksum error.
 		// Since we have no way of asking for the line to be re-sent, just report an error.
-		// If the line number is zero, then it probably came from PanelDue and was caused by input buffer overflow
-		// while we were busy doing a macro, so just ignore it.
 		if (gb->Seen('P'))
 		{
 			int val = gb->GetIValue();
@@ -5681,7 +5674,7 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 
 	default:
 		error = true;
-		reply.printf("invalid M Code: %s", gb->Buffer());
+		reply.printf("unsupported command: %s", gb->Buffer());
 	}
 
 	// Note that we send a reply to M105 requests even if the status is not 'normal', because we reply to these requests even when we are in other states
@@ -5722,12 +5715,12 @@ bool GCodes::HandleTcode(GCodeBuffer* gb, StringRef& reply)
 // Return the amount of filament extruded
 float GCodes::GetRawExtruderPosition(size_t extruder) const
 {
-	return (extruder < (DRIVES - AXES)) ? lastRawExtruderPosition[extruder] : 0.0;
+	return (extruder < (DRIVES - numAxes)) ? lastRawExtruderPosition[extruder] : 0.0;
 }
 
 float GCodes::GetRawExtruderTotalByDrive(size_t extruder) const
 {
-	return (extruder < (DRIVES - AXES)) ? rawExtruderTotalByDrive[extruder] : 0.0;
+	return (extruder < (DRIVES - numAxes)) ? rawExtruderTotalByDrive[extruder] : 0.0;
 }
 
 // Cancel the current SD card print.
@@ -5826,19 +5819,19 @@ void GCodes::ListTriggers(StringRef reply, TriggerMask mask)
 		bool printed = false;
 		for (unsigned int i = 0; i < DRIVES; ++i)
 		{
-			if ((mask & (1 << i)) != 0)
+			if ((mask & (1u << i)) != 0)
 			{
 				if (printed)
 				{
 					reply.cat(' ');
 				}
-				if (i < AXES)
+				if (i < numAxes)
 				{
 					reply.cat(axisLetters[i]);
 				}
 				else
 				{
-					reply.catf("E%d", i - AXES);
+					reply.catf("E%d", i - numAxes);
 				}
 				printed = true;
 			}
@@ -5895,4 +5888,15 @@ bool GCodes::AdvanceHash(StringRef &reply)
 	return true;
 }
 
+bool GCodes::AllAxesAreHomed() const
+{
+	const uint32_t allAxes = (1u << numAxes) - 1;
+	return (axesHomed & allAxes) == allAxes;
+}
+
+void GCodes::SetAllAxesNotHomed()
+{
+	axesHomed = 0;
+}
+
 // End
diff --git a/src/GCodes/GCodes.h b/src/GCodes/GCodes.h
index 4103d7c..d81c549 100644
--- a/src/GCodes/GCodes.h
+++ b/src/GCodes/GCodes.h
@@ -127,8 +127,13 @@ public:
     void Diagnostics(MessageType mtype);								// Send helpful information out
     bool HaveIncomingData() const;										// Is there something that we have to do?
 	size_t GetStackPointer() const;										// Returns the current stack pointer
-    bool GetAxisIsHomed(uint8_t axis) const { return axisIsHomed[axis]; } // Is the axis at 0?
-    void SetAxisIsHomed(uint8_t axis) { axisIsHomed[axis] = true; }		// Tell us that the axis is now homed
+
+	bool GetAxisIsHomed(unsigned int axis) const						// Has the axis been homed?
+    	{ return (axesHomed & (1 << axis)) != 0; }
+    void SetAxisIsHomed(unsigned int axis)								// Tell us that the axis is now homed
+    	{ axesHomed |= (1 << axis); }
+    void SetAxisNotHomed(unsigned int axis)								// Tell us that the axis is not homed
+    	{ axesHomed &= ~(1 << axis); }
 
     float GetSpeedFactor() const { return speedFactor * minutesToSeconds; }	// Return the current speed factor
     float GetExtrusionFactor(size_t extruder) { return extrusionFactors[extruder]; } // Return the current extrusion factors
@@ -136,10 +141,7 @@ public:
     float GetRawExtruderTotalByDrive(size_t extruder) const;			// Get the total extrusion since start of print, for one drive
     float GetTotalRawExtrusion() const { return rawExtruderTotal; }		// Get the total extrusion since start of print, all drives
     
-    bool HaveAux() const { return auxDetected; }						// Any device on the AUX line?
 	bool IsFlashing() const { return isFlashing; }						// Is a new firmware binary going to be flashed?
-	OutputBuffer *GetAuxGCodeReply();									// Returns cached G-Code reply for AUX devices and clears its reference
-    uint32_t GetAuxSeq() { return auxSeq; }
 
 	bool IsPaused() const;
 	bool IsPausing() const;
@@ -151,8 +153,15 @@ public:
 
     void CancelPrint();													// Cancel the current print
 
+    void MoveStoppedByZProbe() { zProbeTriggered = true; }				// Called from the step ISR when the Z probe is triggered, causing the move to be aborted
+
+    size_t GetNumAxes() const { return numAxes; }
+
+    static const char axisLetters[MAX_AXES]; 							// 'X', 'Y', 'Z'
+
 private:
-  
+    enum class CannedMoveType : uint8_t { none, relative, absolute };
+
     void FillGCodeBuffers();											// Get new data into the gcode buffers
     void StartNextGCode(StringRef& reply);								// Fetch a new GCode and process it
     void DoFilePrint(GCodeBuffer* gb, StringRef& reply);				// Get G Codes from a file and print them
@@ -216,6 +225,7 @@ private:
     GCodeBuffer* auxGCode;						// this one is for the LCD display on the async serial interface
     GCodeBuffer* fileMacroGCode;				// ...
     GCodeBuffer *gbCurrent;
+
     bool active;								// Live and running?
     bool isPaused;								// true if the print has been paused
     bool dwellWaiting;							// We are in a dwell
@@ -232,19 +242,20 @@ private:
 	bool axesRelative;
     GCodeMachineState stack[StackSize];			// State that we save when calling macro files
     unsigned int stackPointer;					// Push and Pop stack pointer
-    static const char axisLetters[AXES]; 		// 'X', 'Y', 'Z'
-	float axisScaleFactors[AXES];				// Scale XYZ coordinates by this factor (for Delta configurations)
-    float lastRawExtruderPosition[DRIVES - AXES];	// Extruder position of the last move fed into the Move class
-    float rawExtruderTotalByDrive[DRIVES - AXES];	// Total extrusion amount fed to Move class since starting print, before applying extrusion factor, per drive
+    size_t numAxes;								// How many axes we have. DEDFAULT
+	float axisScaleFactors[MAX_AXES];			// Scale XYZ coordinates by this factor (for Delta configurations)
+    float lastRawExtruderPosition[DRIVES - MIN_AXES];	// Extruder position of the last move fed into the Move class
+    float rawExtruderTotalByDrive[DRIVES - MIN_AXES];	// Total extrusion amount fed to Move class since starting print, before applying extrusion factor, per drive
     float rawExtruderTotal;						// Total extrusion amount fed to Move class since starting print, before applying extrusion factor, summed over all drives
 	float record[DRIVES];						// Temporary store for move positions
-	float moveToDo[DRIVES+1];					// Where to go set by G1 etc
-	bool activeDrive[DRIVES];					// Is this drive involved in a move?
+	float cannedMoveCoords[DRIVES];				// Where to go or how much to move by in a canned cycle move, last is feed rate
+	float cannedFeedRate;						// How fast to do it
+	CannedMoveType cannedMoveType[DRIVES];		// Is this drive involved in a canned cycle move?
 	bool offSetSet;								// Are any axis offsets non-zero?
     float distanceScale;						// MM or inches
     FileData fileBeingPrinted;
     FileData fileToPrint;
-    FileStore* fileBeingWritten;				// A file to write G Codes (or sometimes HTML) in
+    FileStore* fileBeingWritten;				// A file to write G Codes (or sometimes HTML) to
     uint16_t toBeHomed;							// Bitmap of axes still to be homed
     bool doingFileMacro;						// Are we executing a macro file?
     int oldToolNumber, newToolNumber;			// Tools being changed
@@ -254,18 +265,18 @@ private:
     int probeCount;								// Counts multiple probe points
     int8_t cannedCycleMoveCount;				// Counts through internal (i.e. not macro) canned cycle moves
     bool cannedCycleMoveQueued;					// True if a canned cycle move has been set
-    bool zProbesSet;							// True if all Z probing is done and we can set the bed equation
     float longWait;								// Timer for things that happen occasionally (seconds)
     bool limitAxes;								// Don't think outside the box.
-    bool axisIsHomed[AXES];						// These record which of the axes have been homed
+    uint32_t axesHomed;							// Bitmap of which axes have been homed
     float pausedFanValues[NUM_FANS];			// Fan speeds when the print was paused
     float speedFactor;							// speed factor, including the conversion from mm/min to mm/sec, normally 1/60
-    float extrusionFactors[DRIVES - AXES];		// extrusion factors (normally 1.0)
-    float lastProbedZ;							// the last height at which the Z probe stopped
+    float extrusionFactors[DRIVES - MIN_AXES];	// extrusion factors (normally 1.0)
+
+    // Z probe
+    float lastProbedZ;							// the last height at which the Z probe stopped
+    bool zProbesSet;							// True if all Z probing is done and we can set the bed equation
+    volatile bool zProbeTriggered;				// Set by the step ISR when a move is aborted because the Z probe is triggered
 
-    bool auxDetected;							// Have we processed at least one G-Code from an AUX device?
-	OutputBuffer *auxGCodeReply;				// G-Code reply for AUX devices (special one because it is actually encapsulated before sending)
-	uint32_t auxSeq;							// Sequence number for AUX devices
     float simulationTime;						// Accumulated simulation time
     uint8_t simulationMode;						// 0 = not simulating, 1 = simulating, >1 are simulation modes for debugging
     FilePosition filePos;						// The position we got up to in the file being printed
@@ -308,26 +319,9 @@ inline bool GCodes::HaveIncomingData() const
 			platform->GCodeAvailable(SerialSource::AUX);
 }
 
-inline bool GCodes::AllAxesAreHomed() const
-{
-	return axisIsHomed[X_AXIS] && axisIsHomed[Y_AXIS] && axisIsHomed[Z_AXIS];
-}
-
-inline void GCodes::SetAllAxesNotHomed()
-{
-	axisIsHomed[X_AXIS] = axisIsHomed[Y_AXIS] = axisIsHomed[Z_AXIS] = false;
-}
-
 inline size_t GCodes::GetStackPointer() const
 {
 	return stackPointer;
 }
 
-inline OutputBuffer *GCodes::GetAuxGCodeReply()
-{
-	OutputBuffer *temp = auxGCodeReply;
-	auxGCodeReply = nullptr;
-	return temp;
-}
-
 #endif
diff --git a/src/Heating/Heat.cpp b/src/Heating/Heat.cpp
index 5351e7e..402b835 100644
--- a/src/Heating/Heat.cpp
+++ b/src/Heating/Heat.cpp
@@ -43,7 +43,7 @@ void Heat::Init()
 			pids[heater]->Init(DefaultHotEndHeaterGain, DefaultHotEndHeaterTimeConstant, DefaultHotEndHeaterDeadTime, true);
 		}
 	}
-	lastTime = millis();
+	lastTime = millis() - platform->HeatSampleInterval();		// flag the PIDS as due for spinning
 	longWait = platform->Time();
 	coldExtrude = false;
 	active = true;
@@ -210,12 +210,12 @@ void Heat::ResetFault(int8_t heater)
 	}
 }
 
-float Heat::GetAveragePWM(int8_t heater) const
+float Heat::GetAveragePWM(size_t heater) const
 {
 	return pids[heater]->GetAveragePWM();
 }
 
-uint32_t Heat::GetLastSampleTime(int8_t heater) const
+uint32_t Heat::GetLastSampleTime(size_t heater) const
 {
 	return pids[heater]->GetLastSampleTime();
 }
diff --git a/src/Heating/Heat.h b/src/Heating/Heat.h
index ab8d8c0..c3e52bf 100644
--- a/src/Heating/Heat.h
+++ b/src/Heating/Heat.h
@@ -42,11 +42,17 @@ public:
 	void AllowColdExtrude();									// Allow cold extrusion
 	void DenyColdExtrude();										// Deny cold extrusion
 
-	int8_t GetBedHeater() const;								// Get hot bed heater number
-	void SetBedHeater(int8_t heater);							// Set hot bed heater number
+	int8_t GetBedHeater() const									// Get hot bed heater number
+	post(-1 <= result; result < HEATERS);
 
-	int8_t GetChamberHeater() const;							// Get chamber heater number
-	void SetChamberHeater(int8_t heater);						// Set chamber heater number
+	void SetBedHeater(int8_t heater)							// Set hot bed heater number
+	pre(-1 <= heater; heater < HEATERS);
+
+	int8_t GetChamberHeater() const								// Get chamber heater number
+	post(-1 <= result; result < HEATERS);
+
+	void SetChamberHeater(int8_t heater)						// Set chamber heater number
+	pre(-1 <= heater; heater < HEATERS);
 
 	void SetActiveTemperature(int8_t heater, float t);
 	float GetActiveTemperature(int8_t heater) const;
@@ -62,41 +68,40 @@ public:
 	bool AllHeatersAtSetTemperatures(bool includingBed) const;	// Is everything at temperature within tolerance?
 	bool HeaterAtSetTemperature(int8_t heater) const;			// Is a specific heater at temperature within tolerance?
 	void Diagnostics(MessageType mtype);						// Output useful information
-	float GetAveragePWM(int8_t heater) const;					// Return the running average PWM to the heater as a fraction in [0, 1].
+
+	float GetAveragePWM(size_t heater) const					// Return the running average PWM to the heater as a fraction in [0, 1].
+	pre(heater < HEATERS);
 
 	bool UseSlowPwm(int8_t heater) const;						// Queried by the Platform class
-	uint32_t GetLastSampleTime(int8_t heater) const;
+
+	uint32_t GetLastSampleTime(size_t heater) const
+	pre(heater < HEATERS);
 
 	void StartAutoTune(size_t heater, float temperature, float maxPwm, StringRef& reply) // Auto tune a PID
-		pre(heater < HEATERS);
+	pre(heater < HEATERS);
 
 	bool IsTuning(size_t heater) const							// Return true if the specified heater is auto tuning
-		pre(heater < HEATERS);
+	pre(heater < HEATERS);
 
 	void GetAutoTuneStatus(StringRef& reply) const;				// Get the status of the current or last auto tune
 
 	const FopDt& GetHeaterModel(size_t heater) const			// Get the process model for the specified heater
-		pre(heater < HEATERS);
+	pre(heater < HEATERS);
 
-	bool SetHeaterModel(size_t heater, float gain, float tc, float td, float maxPwm, bool usePid); // Set the heater process model
+	bool SetHeaterModel(size_t heater, float gain, float tc, float td, float maxPwm, bool usePid) // Set the heater process model
+	pre(heater < HEATERS);
 
 	bool IsModelUsed(size_t heater) const						// Is the heater using the PID parameters calculated form the model?
-		pre(heater < HEATERS);
+	pre(heater < HEATERS);
 
 	void UseModel(size_t heater, bool b)						// Use or don't use the model to provide the PID parameters
-		pre(heater < HEATERS);
+	pre(heater < HEATERS);
 
 	void GetHeaterProtection(size_t heater, float& maxTempExcursion, float& maxFaultTime) const
-	pre(heater < HEATERS)
-	{
-		pids[heater]->GetHeaterProtection(maxTempExcursion, maxFaultTime);
-	}
+	pre(heater < HEATERS);
 
 	void SetHeaterProtection(size_t heater, float maxTempExcursion, float maxFaultTime)
-	pre(heater < HEATERS)
-	{
-		pids[heater]->SetHeaterProtection(maxTempExcursion, maxFaultTime);
-	}
+	pre(heater < HEATERS);
 
 private:
 	Platform* platform;											// The instance of the RepRap hardware class
@@ -174,4 +179,14 @@ inline void Heat::UseModel(size_t heater, bool b)
 	pids[heater]->UseModel(b);
 }
 
+inline void Heat::GetHeaterProtection(size_t heater, float& maxTempExcursion, float& maxFaultTime) const
+{
+	pids[heater]->GetHeaterProtection(maxTempExcursion, maxFaultTime);
+}
+
+inline void Heat::SetHeaterProtection(size_t heater, float maxTempExcursion, float maxFaultTime)
+{
+	pids[heater]->SetHeaterProtection(maxTempExcursion, maxFaultTime);
+}
+
 #endif
diff --git a/src/Heating/Pid.cpp b/src/Heating/Pid.cpp
index 415c7c3..f8a0b39 100644
--- a/src/Heating/Pid.cpp
+++ b/src/Heating/Pid.cpp
@@ -50,6 +50,7 @@ void PID::Init(float pGain, float pTc, float pTd, bool usePid)
 	useModel = true;						// by default we use the model in this first release
 	averagePWM = lastPwm = 0.0;
 	heatingFaultCount = 0;
+	temperature = BAD_ERROR_TEMPERATURE;
 
 	// Time the sensor was last sampled.  During startup, we use the current
 	// time as the initial value so as to not trigger an immediate warning from the Tick ISR.
@@ -59,10 +60,10 @@ void PID::Init(float pGain, float pTc, float pTd, bool usePid)
 // Set the process model
 bool PID::SetModel(float gain, float tc, float td, float maxPwm, bool usePid)
 {
-	bool rslt = model.SetParameters(gain, tc, td, maxPwm, usePid);
+	const bool rslt = model.SetParameters(gain, tc, td, maxPwm, usePid);
 	if (rslt)
 	{
-		float safeGain = (heater == reprap.GetHeat()->GetBedHeater() || heater == reprap.GetHeat()->GetChamberHeater())
+		const float safeGain = (heater == reprap.GetHeat()->GetBedHeater() || heater == reprap.GetHeat()->GetChamberHeater())
 								? 170.0 : 480.0;
 		if (gain > safeGain)
 		{
@@ -105,6 +106,7 @@ void PID::SwitchOn()
 	}
 	else
 	{
+//debugPrintf("Heater %d on temp %.1f\n", heater, temperature);
 		const float target = (active) ? activeTemperature : standbyTemperature;
 		const HeaterMode oldMode = mode;
 		mode = (temperature + TEMPERATURE_CLOSE_ENOUGH < target) ? HeaterMode::heating
diff --git a/src/Movement/DDA.cpp b/src/Movement/DDA.cpp
index f3fa965..c20a2d0 100644
--- a/src/Movement/DDA.cpp
+++ b/src/Movement/DDA.cpp
@@ -82,16 +82,17 @@ void DDA::DebugPrintVector(const char *name, const float *vec, size_t len) const
 
 void DDA::DebugPrint() const
 {
+	const size_t numAxes = reprap.GetGCodes()->GetNumAxes();
 	debugPrintf("DDA:");
 	if (endCoordinatesValid)
 	{
-		float startCoordinates[AXES];
-		for (size_t i = 0; i < AXES; ++i)
+		float startCoordinates[MAX_AXES];
+		for (size_t i = 0; i < numAxes; ++i)
 		{
 			startCoordinates[i] = endCoordinates[i] - (totalDistance * directionVector[i]);
 		}
-		DebugPrintVector(" start", startCoordinates, AXES);
-		DebugPrintVector(" end", endCoordinates, AXES);
+		DebugPrintVector(" start", startCoordinates, numAxes);
+		DebugPrintVector(" end", endCoordinates, numAxes);
 	}
 
 	debugPrintf(" d=%f", totalDistance);
@@ -100,14 +101,15 @@ void DDA::DebugPrint() const
 				"daccel=%f ddecel=%f cks=%u\n",
 				acceleration, requestedSpeed, topSpeed, startSpeed, endSpeed,
 				accelDistance, decelDistance, clocksNeeded);
-	ddm[0].DebugPrint('x', isDeltaMovement);
-	ddm[1].DebugPrint('y', isDeltaMovement);
-	ddm[2].DebugPrint('z', isDeltaMovement);
-	for (size_t i = AXES; i < DRIVES; ++i)
+	for (size_t axis = 0; axis < numAxes; ++axis)
+	{
+		ddm[axis].DebugPrint(reprap.GetGCodes()->axisLetters[axis], isDeltaMovement);
+	}
+	for (size_t i = numAxes; i < DRIVES; ++i)
 	{
 		if (ddm[i].state != DMState::idle)
 		{
-			ddm[i].DebugPrint((char)('0' + (i - AXES)), false);
+			ddm[i].DebugPrint((char)('0' + (i - numAxes)), false);
 		}
 	}
 }
@@ -148,16 +150,17 @@ bool DDA::Init(const GCodes::RawMove &nextMove, bool doMotorMapping)
 	const bool isSpecialDeltaMove = (move->IsDeltaMode() && !doMotorMapping);
 	float accelerations[DRIVES];
 	const float *normalAccelerations = reprap.GetPlatform()->Accelerations();
+	const size_t numAxes = reprap.GetGCodes()->GetNumAxes();
 	for (size_t drive = 0; drive < DRIVES; drive++)
 	{
 		accelerations[drive] = normalAccelerations[drive];
-		if (drive >= AXES || !doMotorMapping)
+		if (drive >= numAxes || !doMotorMapping)
 		{
 			endPoint[drive] = Move::MotorEndPointToMachine(drive, nextMove.coords[drive]);
 		}
 
 		int32_t delta;
-		if (drive < AXES)
+		if (drive < numAxes)
 		{
 			endCoordinates[drive] = nextMove.coords[drive];
 			delta = endPoint[drive] - positionNow[drive];
@@ -168,7 +171,7 @@ bool DDA::Init(const GCodes::RawMove &nextMove, bool doMotorMapping)
 		}
 
 		DriveMovement& dm = ddm[drive];
-		if (drive < AXES && !isSpecialDeltaMove)
+		if (drive < numAxes && !isSpecialDeltaMove)
 		{
 			directionVector[drive] = nextMove.coords[drive] - prev->GetEndCoordinate(drive, false);
 			dm.state = (isDeltaMovement || delta != 0)
@@ -187,12 +190,12 @@ bool DDA::Init(const GCodes::RawMove &nextMove, bool doMotorMapping)
 			dm.direction = (delta >= 0);				// for now this is the direction of net movement, but gets adjusted later if it is a delta movement
 			realMove = true;
 
-			if (drive < Z_AXIS)
+			if (drive < numAxes && drive != Z_AXIS)
 			{
 				xyMoving = true;
 			}
 
-			if (drive >= AXES && xyMoving)
+			if (drive >= numAxes && xyMoving)
 			{
 				if (delta > 0)
 				{
@@ -228,20 +231,27 @@ bool DDA::Init(const GCodes::RawMove &nextMove, bool doMotorMapping)
 	// This means that the user gets the feed rate that he asked for. It also makes the delta calculations simpler.
 	if (xyMoving || ddm[Z_AXIS].state == DMState::moving)
 	{
-		totalDistance = Normalise(directionVector, DRIVES, AXES);
+		if (isDeltaMovement)
+		{
+			// Add on the Z movement needed to compensate for bed tilt
+			const DeltaParameters& dparams = move->GetDeltaParams();
+			directionVector[Z_AXIS] += (directionVector[X_AXIS] * dparams.GetXTilt()) + (directionVector[Y_AXIS] * dparams.GetYTilt());
+		}
+
+		totalDistance = Normalise(directionVector, DRIVES, numAxes);
 		if (isDeltaMovement)
 		{
 			// The following are only needed when doing delta movements. We could defer computing them until Prepare(), which would make simulation faster.
 			a2plusb2 = fsquare(directionVector[X_AXIS]) + fsquare(directionVector[Y_AXIS]);
 			cKc = (int32_t)(directionVector[Z_AXIS] * DriveMovement::Kc);
 
-			const DeltaParameters& dparams = move->GetDeltaParams();
 			const float initialX = prev->GetEndCoordinate(X_AXIS, false);
 			const float initialY = prev->GetEndCoordinate(Y_AXIS, false);
+			const DeltaParameters& dparams = move->GetDeltaParams();
 			const float diagonalSquared = fsquare(dparams.GetDiagonal());
 			const float a2b2D2 = a2plusb2 * diagonalSquared;
 
-			for (size_t drive = 0; drive < AXES; ++drive)
+			for (size_t drive = 0; drive < DELTA_AXES; ++drive)
 			{
 				const float A = initialX - dparams.GetTowerX(drive);
 				const float B = initialY - dparams.GetTowerY(drive);
@@ -271,11 +281,10 @@ bool DDA::Init(const GCodes::RawMove &nextMove, bool doMotorMapping)
 					if (drev > 0.0 && drev < totalDistance)		// if the reversal point is within range
 					{
 						// Calculate how many steps we need to move up before reversing
-						float hrev = directionVector[Z_AXIS] * drev + sqrt(dSquaredMinusAsquaredMinusBsquared - 2 * drev * aAplusbB - a2plusb2 * fsquare(drev));
+						const float hrev = directionVector[Z_AXIS] * drev + sqrt(dSquaredMinusAsquaredMinusBsquared - 2 * drev * aAplusbB - a2plusb2 * fsquare(drev));
 						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.
-						// We must not set reverseStartStep to 1, because then we would set the direction when Prepare() calls CalcStepTime(), before the previous move finishes.
 						if (numStepsUp < 1 || (dm.direction && (uint32_t)numStepsUp <= dm.totalSteps))
 						{
 							dm.mp.delta.reverseStartStep = dm.totalSteps + 1;
@@ -326,7 +335,7 @@ bool DDA::Init(const GCodes::RawMove &nextMove, bool doMotorMapping)
 		// We use the Cartesian motion system to implement these moves, so the feed rate will be interpreted in Cartesian coordinates.
 		// This is wrong, we want the feed rate to apply to the drive that is moving the farthest.
 		float maxDistance = 0.0;
-		for (size_t axis = 0; axis < AXES; ++axis)
+		for (size_t axis = 0; axis < DELTA_AXES; ++axis)
 		{
 			if (normalisedDirectionVector[axis] > maxDistance)
 			{
@@ -616,7 +625,7 @@ void DDA::CalcNewSpeeds()
 }
 
 // This is called by Move::CurrentMoveCompleted to update the live coordinates from the move that has just finished
-bool DDA::FetchEndPosition(volatile int32_t ep[DRIVES], volatile float endCoords[AXES])
+bool DDA::FetchEndPosition(volatile int32_t ep[DRIVES], volatile float endCoords[MAX_AXES])
 {
 	for (size_t drive = 0; drive < DRIVES; ++drive)
 	{
@@ -624,7 +633,7 @@ bool DDA::FetchEndPosition(volatile int32_t ep[DRIVES], volatile float endCoords
 	}
 	if (endCoordinatesValid)
 	{
-		for (size_t axis = 0; axis < AXES; ++axis)
+		for (size_t axis = 0; axis < MAX_AXES; ++axis)
 		{
 			endCoords[axis] = endCoordinates[axis];
 		}
@@ -635,7 +644,8 @@ bool DDA::FetchEndPosition(volatile int32_t ep[DRIVES], volatile float endCoords
 void DDA::SetPositions(const float move[DRIVES], size_t numDrives)
 {
 	reprap.GetMove()->EndPointToMachine(move, endPoint, numDrives);
-	for (size_t axis = 0; axis < AXES; ++axis)
+	const size_t numAxes = reprap.GetGCodes()->GetNumAxes();
+	for (size_t axis = 0; axis < numAxes; ++axis)
 	{
 		endCoordinates[axis] = move[axis];
 	}
@@ -652,9 +662,10 @@ pre(disableDeltaMapping || drive < AXES)
 	}
 	else
 	{
-		if (drive < AXES && !endCoordinatesValid)
+		const size_t numAxes = reprap.GetGCodes()->GetNumAxes();
+		if (drive < numAxes && !endCoordinatesValid)
 		{
-			reprap.GetMove()->MachineToEndPoint(endPoint, endCoordinates, AXES);
+			reprap.GetMove()->MachineToEndPoint(endPoint, endCoordinates, numAxes);
 			endCoordinatesValid = true;
 		}
 		return endCoordinates[drive];
@@ -733,6 +744,7 @@ void DDA::Prepare()
 	goingSlow = false;
 	firstDM = nullptr;
 
+	const size_t numAxes = reprap.GetGCodes()->GetNumAxes();
 	for (size_t drive = 0; drive < DRIVES; ++drive)
 	{
 		DriveMovement& dm = ddm[drive];
@@ -740,7 +752,7 @@ void DDA::Prepare()
 		{
 			dm.drive = drive;
 			reprap.GetPlatform()->EnableDrive(drive);
-			if (drive >= AXES)
+			if (drive >= numAxes)
 			{
 				dm.PrepareExtruder(*this, params, usePressureAdvance);
 
@@ -782,7 +794,7 @@ void DDA::Prepare()
 			dm.nextStepTime = 0;
 			dm.stepInterval = 999999;						// initialise to a large value so that we will calculate the time for just one step
 			dm.stepsTillRecalc = 0;							// so that we don't skip the calculation
-			bool stepsToDo = (isDeltaMovement && drive < AXES)
+			bool stepsToDo = (isDeltaMovement && drive < numAxes)
 							? dm.CalcNextStepTimeDelta(*this, false)
 							: dm.CalcNextStepTimeCartesian(*this, false);
 			if (stepsToDo)
@@ -834,21 +846,22 @@ pre(state == frozen)
 	else
 	{
 		unsigned int extrusions = 0, retractions = 0;		// bitmaps of extruding and retracting drives
+		const size_t numAxes = reprap.GetGCodes()->GetNumAxes();
 		for (size_t i = 0; i < DRIVES; ++i)
 		{
 			DriveMovement& dm = ddm[i];
 			if (dm.state == DMState::moving)
 			{
 				reprap.GetPlatform()->SetDirection(i, dm.direction);
-				if (i >= AXES)
+				if (i >= numAxes)
 				{
 					if (dm.direction == FORWARDS)
 					{
-						extrusions |= (1 << (i - AXES));
+						extrusions |= (1 << (i - numAxes));
 					}
 					else
 					{
-						retractions |= (1 << (i - AXES));
+						retractions |= (1 << (i - numAxes));
 					}
 				}
 			}
@@ -860,8 +873,8 @@ pre(state == frozen)
 			const unsigned int prohibitedMovements = reprap.GetProhibitedExtruderMovements(extrusions, retractions);
 			for (DriveMovement **dmpp = &firstDM; *dmpp != nullptr; )
 			{
-				bool thisDriveExtruding = (*dmpp)->drive >= AXES;
-				if (thisDriveExtruding && (prohibitedMovements & (1 << ((*dmpp)->drive - AXES))) != 0)
+				bool thisDriveExtruding = (*dmpp)->drive >= numAxes;
+				if (thisDriveExtruding && (prohibitedMovements & (1 << ((*dmpp)->drive - numAxes))) != 0)
 				{
 					*dmpp = (*dmpp)->nextDM;
 				}
@@ -932,7 +945,8 @@ bool DDA::Step()
 				}
 			}
 
-			for (size_t drive = 0; drive < AXES; ++drive)
+			const size_t numAxes = reprap.GetGCodes()->GetNumAxes();
+			for (size_t drive = 0; drive < numAxes; ++drive)
 			{
 				if ((endStopsToCheck & (1 << drive)) != 0)
 				{
@@ -1019,7 +1033,7 @@ bool DDA::Step()
 		firstDM = dm;													// remove the chain from the list
 		while (dmToInsert != dm)										// note that both of these may be nullptr
 		{
-			const bool hasMoreSteps = (isDeltaMovement && dmToInsert->drive < AXES)
+			const bool hasMoreSteps = (isDeltaMovement && dmToInsert->drive < DELTA_AXES)
 					? dmToInsert->CalcNextStepTimeDelta(*this, true)
 					: dmToInsert->CalcNextStepTimeCartesian(*this, true);
 			DriveMovement * const nextToInsert = dmToInsert->nextDM;
@@ -1084,7 +1098,7 @@ void DDA::StopDrive(size_t drive)
 			endPoint[drive] += stepsLeft;			// we were going backwards
 		}
 		dm.state = DMState::idle;
-		if (drive < AXES)
+		if (drive < reprap.GetGCodes()->GetNumAxes())
 		{
 			endCoordinatesValid = false;			// the XYZ position is no longer valid
 		}
@@ -1127,6 +1141,14 @@ void DDA::ReduceHomingSpeed()
 				InsertDM(&dm);
 			}
 		}
+
+		// We also need to adjust the total clocks needed, to prevent step errors being recorded
+		const uint32_t clocksSoFar = Platform::GetInterruptClocks() - moveStartTime;
+		if (clocksSoFar < clocksNeeded)
+		{
+			const uint32_t clocksLeft = clocksNeeded - clocksSoFar;
+			clocksNeeded += (uint32_t)(clocksLeft * (factor - 1.0));
+		}
 	}
 }
 
diff --git a/src/Movement/DDA.h b/src/Movement/DDA.h
index c8b749c..e7f297b 100644
--- a/src/Movement/DDA.h
+++ b/src/Movement/DDA.h
@@ -53,7 +53,7 @@ public:
 	void SetDriveCoordinate(int32_t a, size_t drive);				// Force an end point
 	void SetFeedRate(float rate) { requestedSpeed = rate; }
 	float GetEndCoordinate(size_t drive, bool disableDeltaMapping);
-	bool FetchEndPosition(volatile int32_t ep[DRIVES], volatile float endCoords[AXES]);
+	bool FetchEndPosition(volatile int32_t ep[DRIVES], volatile float endCoords[MAX_AXES]);
     void SetPositions(const float move[], size_t numDrives);		// Force the endpoints to be these
     FilePosition GetFilePosition() const { return filePos; }
     float GetRequestedSpeed() const { return requestedSpeed; }
@@ -116,7 +116,7 @@ private:
     FilePosition filePos;					// The position in the SD card file after this move was read, or zero if not read fro SD card
 
 	int32_t endPoint[DRIVES];  				// Machine coordinates of the endpoint
-	float endCoordinates[AXES];				// The Cartesian coordinates at the end of the move
+	float endCoordinates[MAX_AXES];			// The Cartesian coordinates at the end of the move
 	float directionVector[DRIVES];			// The normalised direction vector - first 3 are XYZ Cartesian coordinates even on a delta
     float totalDistance;					// How long is the move in hypercuboid space
 	float acceleration;						// The acceleration to use
diff --git a/src/Movement/DeltaParameters.cpp b/src/Movement/DeltaParameters.cpp
index 1e808cc..34f3a1c 100644
--- a/src/Movement/DeltaParameters.cpp
+++ b/src/Movement/DeltaParameters.cpp
@@ -13,10 +13,11 @@ void DeltaParameters::Init()
 	diagonal = 0.0;
 	radius = 0.0;
 	xCorrection = yCorrection = zCorrection = 0.0;
+	xTilt = yTilt = 0.0;
 	printRadius = defaultPrintRadius;
 	homedHeight = defaultDeltaHomedHeight;
 
-    for (size_t axis = 0; axis < AXES; ++axis)
+    for (size_t axis = 0; axis < DELTA_AXES; ++axis)
     {
     	endstopAdjustments[axis] = 0.0;
     	towerX[axis] = towerY[axis] = 0.0;
@@ -50,7 +51,7 @@ void DeltaParameters::Recalc()
 
 		// Calculate the base carriage height when the printer is homed, i.e. the carriages are at the endstops less the corrections
 		const float tempHeight = diagonal;		// any sensible height will do here
-		float machinePos[AXES];
+		float machinePos[DELTA_AXES];
 		InverseTransform(tempHeight, tempHeight, tempHeight, machinePos);
 		homedCarriageHeight = homedHeight + tempHeight - machinePos[Z_AXIS];
 	}
@@ -67,14 +68,17 @@ void DeltaParameters::NormaliseEndstopAdjustments()
 	homedCarriageHeight += eav;				// no need for a full recalc, this is sufficient
 }
 
-// Calculate the motor position for a single tower from a Cartesian coordinate
-float DeltaParameters::Transform(const float machinePos[AXES], size_t axis) const
+// Calculate the motor position for a single tower from a Cartesian coordinate.
+float DeltaParameters::Transform(const float machinePos[DELTA_AXES], size_t axis) const
 {
-	return machinePos[Z_AXIS]
-	          + sqrt(D2 - fsquare(machinePos[X_AXIS] - towerX[axis]) - fsquare(machinePos[Y_AXIS] - towerY[axis]));
+	return sqrt(D2 - fsquare(machinePos[X_AXIS] - towerX[axis]) - fsquare(machinePos[Y_AXIS] - towerY[axis]))
+		 + machinePos[Z_AXIS]
+		 + (machinePos[X_AXIS] * xTilt)
+		 + (machinePos[Y_AXIS] * yTilt);
 }
 
-void DeltaParameters::InverseTransform(float Ha, float Hb, float Hc, float machinePos[AXES]) const
+// Calculate the Cartesian coordinates from the motor coordinates.
+void DeltaParameters::InverseTransform(float Ha, float Hb, float Hc, float machinePos[DELTA_AXES]) const
 {
 	const float Fa = coreFa + fsquare(Ha);
 	const float Fb = coreFb + fsquare(Hb);
@@ -103,7 +107,7 @@ void DeltaParameters::InverseTransform(float Ha, float Hb, float Hc, float machi
 	float z = (minusHalfB - sqrtf(fsquare(minusHalfB) - A * C)) / A;
 	machinePos[X_AXIS] = (U * z - S) / Q;
 	machinePos[Y_AXIS] = (P - R * z) / Q;
-	machinePos[Z_AXIS] = z;
+	machinePos[Z_AXIS] = z - ((machinePos[X_AXIS] * xTilt) + (machinePos[Y_AXIS] * yTilt));
 }
 
 // Compute the derivative of height with respect to a parameter at the specified motor endpoints.
@@ -113,7 +117,8 @@ void DeltaParameters::InverseTransform(float Ha, float Hb, float Hc, float machi
 // 4 = X tower correction
 // 5 = Y tower correction
 // 6 = diagonal rod length
-float DeltaParameters::ComputeDerivative(unsigned int deriv, float ha, float hb, float hc)
+// 7, 8 = X tilt, Y tilt. We scale these by the printable radius to get sensible values in the range -1..1
+floatc_t DeltaParameters::ComputeDerivative(unsigned int deriv, float ha, float hb, float hc)
 {
 	const float perturb = 0.2;			// perturbation amount in mm or degrees
 	DeltaParameters hiParams(*this), loParams(*this);
@@ -122,50 +127,71 @@ float DeltaParameters::ComputeDerivative(unsigned int deriv, float ha, float hb,
 	case 0:
 	case 1:
 	case 2:
+		// Endstop corrections
 		break;
 
 	case 3:
 		hiParams.radius += perturb;
 		loParams.radius -= perturb;
+		hiParams.Recalc();
+		loParams.Recalc();
 		break;
 
 	case 4:
 		hiParams.xCorrection += perturb;
 		loParams.xCorrection -= perturb;
+		hiParams.Recalc();
+		loParams.Recalc();
 		break;
 
 	case 5:
 		hiParams.yCorrection += perturb;
 		loParams.yCorrection -= perturb;
+		hiParams.Recalc();
+		loParams.Recalc();
 		break;
 
 	case 6:
 		hiParams.diagonal += perturb;
 		loParams.diagonal -= perturb;
+		hiParams.Recalc();
+		loParams.Recalc();
+		break;
+
+	case 7:
+	case 8:
+		// X and Y tilt
 		break;
 	}
 
-	hiParams.Recalc();
-	loParams.Recalc();
-
-	float newPos[AXES];
+	float newPos[DELTA_AXES];
 	hiParams.InverseTransform((deriv == 0) ? ha + perturb : ha, (deriv == 1) ? hb + perturb : hb, (deriv == 2) ? hc + perturb : hc, newPos);
-	float zHi = newPos[Z_AXIS];
-	loParams.InverseTransform((deriv == 0) ? ha - perturb : ha, (deriv == 1) ? hb - perturb : hb, (deriv == 2) ? hc - perturb : hc, newPos);
-	float zLo = newPos[Z_AXIS];
+	if (deriv == 7)
+	{
+		return -newPos[X_AXIS]/printRadius;
+	}
+	if (deriv == 8)
+	{
+		return -newPos[Y_AXIS]/printRadius;
+	}
 
-	return (zHi - zLo)/(2 * perturb);
+	const float zHi = newPos[Z_AXIS];
+	loParams.InverseTransform((deriv == 0) ? ha - perturb : ha, (deriv == 1) ? hb - perturb : hb, (deriv == 2) ? hc - perturb : hc, newPos);
+	const float zLo = newPos[Z_AXIS];
+
+	return ((floatc_t)zHi - (floatc_t)zLo)/(2 * perturb);
 }
 
-// Perform 3, 4, 6 or 7-factor adjustment.
+// Perform 3, 4, 6, 7, 8 or 9-factor adjustment.
 // The input vector contains the following parameters in this order:
 //  X, Y and Z endstop adjustments
-//  If we are doing 4-factor adjustment, the next argument is the delta radius. Otherwise:
-//  X tower X position adjustment
-//  Y tower X position adjustment
-//  Z tower Y position adjustment
-//  Diagonal rod length adjustment
-void DeltaParameters::Adjust(size_t numFactors, const float v[])
+//  Delta radius
+//  X tower position adjustment
+//  Y tower position adjustment
+//  Diagonal rod length adjustment - omitted if doing 8-factor calibration (remainder are moved down)
+//  X tilt adjustment
+//  Y tilt adjustment
+void DeltaParameters::Adjust(size_t numFactors, const floatc_t v[])
 {
 	const float oldCarriageHeightA = GetHomedCarriageHeight(A_AXIS);	// save for later
 
@@ -184,10 +210,21 @@ void DeltaParameters::Adjust(size_t numFactors, const float v[])
 			xCorrection += v[4];
 			yCorrection += v[5];
 
-			if (numFactors == 7)
+			if (numFactors == 7 || numFactors == 9)
 			{
 				diagonal += v[6];
 			}
+
+			if (numFactors == 8)
+			{
+				xTilt += v[6]/printRadius;
+				yTilt += v[7]/printRadius;
+			}
+			else if (numFactors == 9)
+			{
+				xTilt += v[7]/printRadius;
+				yTilt += v[8]/printRadius;
+			}
 		}
 
 		Recalc();
@@ -198,13 +235,17 @@ void DeltaParameters::Adjust(size_t numFactors, const float v[])
 	const float heightError = GetHomedCarriageHeight(A_AXIS) - oldCarriageHeightA - v[0];
 	homedHeight -= heightError;
 	homedCarriageHeight -= heightError;
+
+	// Note: if we adjusted the X and Y tilts, and there are any endstop adjustments, then the homed position won't be exactly in the centre
+	// and changing the tilt will therefore affect the homed height. We ignore this for now. If it is ever significant, a second sutocalibration
+	// run will correct it.
 }
 
 void DeltaParameters::PrintParameters(StringRef& reply) const
 {
-	reply.printf("Endstops X%.2f Y%.2f Z%.2f, height %.2f, diagonal %.2f, radius %.2f, xcorr %.2f, ycorr %.2f, zcorr %.2f\n",
+	reply.printf("Stops X%.3f Y%.3f Z%.3f height %.3f diagonal %.3f radius %.3f xcorr %.2f ycorr %.2f zcorr %.2f xtilt %.3f%% ytilt %.3f%%\n",
 					endstopAdjustments[A_AXIS], endstopAdjustments[B_AXIS], endstopAdjustments[C_AXIS], homedHeight, diagonal, radius,
-					xCorrection, yCorrection, zCorrection);
+					xCorrection, yCorrection, zCorrection, xTilt * 100.0, yTilt * 100.0);
 }
 
 // End
diff --git a/src/Movement/DeltaParameters.h b/src/Movement/DeltaParameters.h
index 64cfee7..331e075 100644
--- a/src/Movement/DeltaParameters.h
+++ b/src/Movement/DeltaParameters.h
@@ -26,6 +26,8 @@ public:
     float GetEndstopAdjustment(size_t axis) const { return endstopAdjustments[axis]; }
     float GetHomedCarriageHeight(size_t axis) const { return homedCarriageHeight + endstopAdjustments[axis]; }
     float GetPrintRadiusSquared() const { return printRadiusSquared; }
+    float GetXTilt() const { return xTilt; }
+    float GetYTilt() const { return yTilt; }
 
     void Init();
     void SetDiagonal(float d) { diagonal = d; Recalc(); }
@@ -37,12 +39,14 @@ public:
     void SetXCorrection(float angle) { xCorrection = angle; Recalc(); }
     void SetYCorrection(float angle) { yCorrection = angle; Recalc(); }
     void SetZCorrection(float angle) { zCorrection = angle; Recalc(); }
+    void SetXTilt(float tilt) { xTilt = tilt; }
+    void SetYTilt(float tilt) { yTilt = tilt; }
 
-    float Transform(const float machinePos[AXES], size_t axis) const;				// Calculate the motor position for a single tower from a Cartesian coordinate
-    void InverseTransform(float Ha, float Hb, float Hc, float machinePos[AXES]) const;	// Calculate the Cartesian position from the motor positions
+    float Transform(const float machinePos[DELTA_AXES], size_t axis) const;			// Calculate the motor position for a single tower from a Cartesian coordinate
+    void InverseTransform(float Ha, float Hb, float Hc, float machinePos[DELTA_AXES]) const; // Calculate the Cartesian position from the motor positions
 
-    float ComputeDerivative(unsigned int deriv, float ha, float hb, float hc);		// Compute the derivative of height with respect to a parameter at a set of motor endpoints
-    void Adjust(size_t numFactors, const float v[]);								// Perform 3-, 4-, 6- or 7-factor adjustment
+    floatc_t ComputeDerivative(unsigned int deriv, float ha, float hb, float hc);	// Compute the derivative of height with respect to a parameter at a set of motor endpoints
+    void Adjust(size_t numFactors, const floatc_t v[]);								// Perform 3-, 4-, 6- or 7-factor adjustment
     void PrintParameters(StringRef& reply) const;									// Print all the parameters for debugging
 
 private:
@@ -56,14 +60,15 @@ private:
     float diagonal;										// The diagonal rod length, all 3 are assumed to be the same length
     float radius;										// The nominal delta radius, before any fine tuning of tower positions
     float xCorrection, yCorrection, zCorrection;		// Tower position corrections
-    float endstopAdjustments[AXES];						// How much above or below the ideal position each endstop is
+    float endstopAdjustments[DELTA_AXES];				// How much above or below the ideal position each endstop is
     float printRadius;
     float homedHeight;
+    float xTilt, yTilt;									// How much we need to raise Z for each unit of movement in the +X and +Y directions
 
     // Derived values
     bool deltaMode;										// True if this is a delta printer
-    float towerX[AXES];									// The X coordinate of each tower
-    float towerY[AXES];									// The Y coordinate of each tower
+    float towerX[DELTA_AXES];							// The X coordinate of each tower
+    float towerY[DELTA_AXES];							// The Y coordinate of each tower
     float printRadiusSquared;
     float homedCarriageHeight;
 	float Xbc, Xca, Xab, Ybc, Yca, Yab;
diff --git a/src/Movement/DriveMovement.cpp b/src/Movement/DriveMovement.cpp
index 9fae0c6..30fb68a 100644
--- a/src/Movement/DriveMovement.cpp
+++ b/src/Movement/DriveMovement.cpp
@@ -82,7 +82,7 @@ void DriveMovement::PrepareExtruder(const DDA& dda, const PrepParams& params, bo
 	mp.cart.twoCsquaredTimesMmPerStepDivA = (uint64_t)(((float)DDA::stepClockRate * (float)DDA::stepClockRate)/(stepsPerMm * dda.acceleration)) * 2;
 
 	// Calculate the pressure advance parameter
-	const float compensationTime = (doCompensation && dv > 0.0) ? reprap.GetPlatform()->GetPressureAdvance(drive - AXES) : 0.0;
+	const float compensationTime = (doCompensation && dv > 0.0) ? reprap.GetPlatform()->GetPressureAdvance(drive - reprap.GetGCodes()->GetNumAxes()) : 0.0;
 	const uint32_t compensationClocks = (uint32_t)(compensationTime * DDA::stepClockRate);
 
 	// Calculate the net total step count to allow for compensation. It may be negative.
diff --git a/src/Movement/Move.cpp b/src/Movement/Move.cpp
index ebfc207..450434c 100644
--- a/src/Movement/Move.cpp
+++ b/src/Movement/Move.cpp
@@ -29,7 +29,7 @@ void Move::Init()
 	// Reset Cartesian mode
 	deltaParams.Init();
 	coreXYMode = 0;
-	for (size_t axis = 0; axis < AXES; ++axis)
+	for (size_t axis = 0; axis < MAX_AXES; ++axis)
 	{
 		axisFactors[axis] = 1.0;
 	}
@@ -375,12 +375,14 @@ FilePosition Move::PausePrint(float positions[DRIVES], float& pausedFeedRate)
 
 	if (ddaRingAddPointer != savedDdaRingAddPointer)
 	{
+		const size_t numAxes = reprap.GetGCodes()->GetNumAxes();
+
 		// We are going to skip some moves. dda points to the last move we are going to print.
-		for (size_t axis = 0; axis < AXES; ++axis)
+		for (size_t axis = 0; axis < numAxes; ++axis)
 		{
 			positions[axis] = dda->GetEndCoordinate(axis, false);
 		}
-		for (size_t drive = AXES; drive < DRIVES; ++drive)
+		for (size_t drive = numAxes; drive < DRIVES; ++drive)
 		{
 			positions[drive] = 0.0;		// clear out extruder movement
 		}
@@ -390,7 +392,7 @@ FilePosition Move::PausePrint(float positions[DRIVES], float& pausedFeedRate)
 		dda = ddaRingAddPointer;
 		do
 		{
-			for (size_t drive = AXES; drive < DRIVES; ++drive)
+			for (size_t drive = numAxes; drive < DRIVES; ++drive)
 			{
 				positions[drive] += dda->GetEndCoordinate(drive, true);		// update the amount of extrusion we are going to skip
 			}
@@ -414,6 +416,7 @@ FilePosition Move::PausePrint(float positions[DRIVES], float& pausedFeedRate)
 uint32_t maxReps = 0;
 
 #if 0
+// For debugging
 extern uint32_t sqSum1, sqSum2, sqCount, sqErrors, lastRes1, lastRes2;
 extern uint64_t lastNum;
 #endif
@@ -428,6 +431,7 @@ void Move::Diagnostics(MessageType mtype)
 	longestGcodeWaitInterval = 0;
 
 #if 0
+	// For debugging
 	if (sqCount != 0)
 	{
 		reprap.GetPlatform()->AppendMessage(GENERIC_MESSAGE, "Average sqrt times %.2f, %.2f, count %u,  errors %u, last %" PRIu64 " %u %u\n",
@@ -453,7 +457,8 @@ void Move::SetPositions(const float move[DRIVES])
 void Move::EndPointToMachine(const float coords[], int32_t ep[], size_t numDrives) const
 {
 	MotorTransform(coords, ep);
-	for (size_t drive = AXES; drive < numDrives; ++drive)
+	const size_t numAxes = reprap.GetGCodes()->GetNumAxes();
+	for (size_t drive = numAxes; drive < numDrives; ++drive)
 	{
 		ep[drive] = MotorEndPointToMachine(drive, coords[drive]);
 	}
@@ -523,18 +528,18 @@ void Move::MachineToEndPoint(const int32_t motorPos[], float machinePos[], size_
 	}
 
 	// Convert the extruders
-	for (size_t drive = AXES; drive < numDrives; ++drive)
+	for (size_t drive = reprap.GetGCodes()->GetNumAxes(); drive < numDrives; ++drive)
 	{
 		machinePos[drive] = motorPos[drive]/stepsPerUnit[drive];
 	}
 }
 
 // Convert Cartesian coordinates to motor steps
-void Move::MotorTransform(const float machinePos[AXES], int32_t motorPos[AXES]) const
+void Move::MotorTransform(const float machinePos[MAX_AXES], int32_t motorPos[MAX_AXES]) const
 {
 	if (IsDeltaMode())
 	{
-		for (size_t axis = 0; axis < AXES; ++axis)
+		for (size_t axis = 0; axis < DELTA_AXES; ++axis)
 		{
 			motorPos[axis] = MotorEndPointToMachine(axis, deltaParams.Transform(machinePos, axis));
 		}
@@ -546,7 +551,8 @@ void Move::MotorTransform(const float machinePos[AXES], int32_t motorPos[AXES])
 	}
 	else
 	{
-		for (size_t axis = 0; axis < AXES; ++axis)
+		const size_t numAxes = reprap.GetGCodes()->GetNumAxes();
+		for (size_t axis = 0; axis < numAxes; ++axis)
 		{
 			motorPos[axis] = MotorEndPointToMachine(axis, MotorFactor(axis, machinePos));
 		}
@@ -602,33 +608,35 @@ float Move::MotorFactor(size_t drive, const float directionVector[]) const
 }
 
 // Do the Axis transform BEFORE the bed transform
-void Move::AxisTransform(float xyzPoint[AXES]) const
+void Move::AxisTransform(float xyzPoint[MAX_AXES]) const
 {
+	//TODO should we transform U axis instead of/as well as X if we are in dual carriage mode?
 	xyzPoint[X_AXIS] = xyzPoint[X_AXIS] + tanXY*xyzPoint[Y_AXIS] + tanXZ*xyzPoint[Z_AXIS];
 	xyzPoint[Y_AXIS] = xyzPoint[Y_AXIS] + tanYZ*xyzPoint[Z_AXIS];
 }
 
 // Invert the Axis transform AFTER the bed transform
-void Move::InverseAxisTransform(float xyzPoint[AXES]) const
+void Move::InverseAxisTransform(float xyzPoint[MAX_AXES]) const
 {
+	//TODO should we transform U axis instead of/as well as X if we are in dual carriage mode?
 	xyzPoint[Y_AXIS] = xyzPoint[Y_AXIS] - tanYZ*xyzPoint[Z_AXIS];
 	xyzPoint[X_AXIS] = xyzPoint[X_AXIS] - (tanXY*xyzPoint[Y_AXIS] + tanXZ*xyzPoint[Z_AXIS]);
 }
 
-void Move::Transform(float xyzPoint[AXES]) const
+void Move::Transform(float xyzPoint[MAX_AXES]) const
 {
 	AxisTransform(xyzPoint);
 	BedTransform(xyzPoint);
 }
 
-void Move::InverseTransform(float xyzPoint[AXES]) const
+void Move::InverseTransform(float xyzPoint[MAX_AXES]) const
 {
 	InverseBedTransform(xyzPoint);
 	InverseAxisTransform(xyzPoint);
 }
 
 // Do the bed transform AFTER the axis transform
-void Move::BedTransform(float xyzPoint[AXES]) const
+void Move::BedTransform(float xyzPoint[MAX_AXES]) const
 {
 	switch(numBedCompensationPoints)
 	{
@@ -636,15 +644,15 @@ void Move::BedTransform(float xyzPoint[AXES]) const
 		break;
 
 	case 3:
-		xyzPoint[Z_AXIS] = xyzPoint[Z_AXIS] + aX*xyzPoint[X_AXIS] + aY*xyzPoint[Y_AXIS] + aC;
+		xyzPoint[Z_AXIS] += aX*xyzPoint[X_AXIS] + aY*xyzPoint[Y_AXIS] + aC;
 		break;
 
 	case 4:
-		xyzPoint[Z_AXIS] = xyzPoint[Z_AXIS] + SecondDegreeTransformZ(xyzPoint[X_AXIS], xyzPoint[Y_AXIS]);
+		xyzPoint[Z_AXIS] += SecondDegreeTransformZ(xyzPoint[X_AXIS], xyzPoint[Y_AXIS]);
 		break;
 
 	case 5:
-		xyzPoint[Z_AXIS] = xyzPoint[Z_AXIS] + TriangleZ(xyzPoint[X_AXIS], xyzPoint[Y_AXIS]);
+		xyzPoint[Z_AXIS] += TriangleZ(xyzPoint[X_AXIS], xyzPoint[Y_AXIS]);
 		break;
 
 	default:
@@ -653,7 +661,7 @@ void Move::BedTransform(float xyzPoint[AXES]) const
 }
 
 // Invert the bed transform BEFORE the axis transform
-void Move::InverseBedTransform(float xyzPoint[AXES]) const
+void Move::InverseBedTransform(float xyzPoint[MAX_AXES]) const
 {
 	switch(numBedCompensationPoints)
 	{
@@ -661,15 +669,15 @@ void Move::InverseBedTransform(float xyzPoint[AXES]) const
 		break;
 
 	case 3:
-		xyzPoint[Z_AXIS] = xyzPoint[Z_AXIS] - (aX*xyzPoint[X_AXIS] + aY*xyzPoint[Y_AXIS] + aC);
+		xyzPoint[Z_AXIS] -= (aX*xyzPoint[X_AXIS] + aY*xyzPoint[Y_AXIS] + aC);
 		break;
 
 	case 4:
-		xyzPoint[Z_AXIS] = xyzPoint[Z_AXIS] - SecondDegreeTransformZ(xyzPoint[X_AXIS], xyzPoint[Y_AXIS]);
+		xyzPoint[Z_AXIS] -= SecondDegreeTransformZ(xyzPoint[X_AXIS], xyzPoint[Y_AXIS]);
 		break;
 
 	case 5:
-		xyzPoint[Z_AXIS] = xyzPoint[Z_AXIS] - TriangleZ(xyzPoint[X_AXIS], xyzPoint[Y_AXIS]);
+		xyzPoint[Z_AXIS] -= TriangleZ(xyzPoint[X_AXIS], xyzPoint[Y_AXIS]);
 		break;
 
 	default:
@@ -765,6 +773,7 @@ float Move::TriangleZ(float x, float y) const
 void Move::FinishedBedProbing(int sParam, StringRef& reply)
 {
 	const int numPoints = NumberOfProbePoints();
+
 	if (sParam < 0)
 	{
 		// A negative sParam just prints the probe heights
@@ -773,12 +782,19 @@ void Move::FinishedBedProbing(int sParam, StringRef& reply)
 		float sumOfSquares = 0.0;
 		for (size_t i = 0; (int)i < numPoints; ++i)
 		{
-			reply.catf(" %.3f", zBedProbePoints[i]);
-			sum += zBedProbePoints[i];
-			sumOfSquares += fsquare(zBedProbePoints[i]);
+			if ((probePointSet[i] & (xSet | ySet | zSet | probeError)) != (xSet | ySet | zSet))
+			{
+				reply.cat(" failed");
+			}
+			else
+			{
+				reply.catf(" %.3f", zBedProbePoints[i]);
+				sum += zBedProbePoints[i];
+				sumOfSquares += fsquare(zBedProbePoints[i]);
+			}
 		}
 		const float mean = sum/numPoints;
-		reply.catf(", mean %.3f, deviation from mean %.3f\n", mean, sqrt(sumOfSquares/numPoints - fsquare(mean)));
+		reply.catf(", mean %.3f, deviation from mean %.3f", mean, sqrt(sumOfSquares/numPoints - fsquare(mean)));
 	}
 	else if (numPoints < sParam)
 	{
@@ -807,7 +823,22 @@ void Move::FinishedBedProbing(int sParam, StringRef& reply)
 			sParam = numPoints;
 		}
 
-		if (IsDeltaMode())
+		// Check that all probe points are set and there were no errors
+		bool hadError = false;
+		for (size_t i = 0; (int)i < numPoints; ++i)
+		{
+			if ((probePointSet[i] & (xSet | ySet | zSet | probeError)) != (xSet | ySet | zSet))
+			{
+				hadError = true;
+				break;
+			}
+		}
+
+		if (hadError)
+		{
+			reply.cat("Compensation or calibration cancelled due to probing errors");
+		}
+		else if (IsDeltaMode())
 		{
 			DoDeltaCalibration(sParam, reply);
 		}
@@ -815,13 +846,13 @@ void Move::FinishedBedProbing(int sParam, StringRef& reply)
 		{
 			SetProbedBedEquation(sParam, reply);
 		}
+	}
 
-		// Clear out the Z heights so that we don't re-use old points.
-		// This allows us to use different numbers of probe point on different occasions.
-		for (size_t i = 0; i < MAX_PROBE_POINTS; ++i)
-		{
-			probePointSet[i] &= ~zSet;
-		}
+	// Clear out the Z heights so that we don't re-use old points.
+	// This allows us to use different numbers of probe point on different occasions.
+	for (size_t i = 0; i < MAX_PROBE_POINTS; ++i)
+	{
+		probePointSet[i] &= ~zSet;
 	}
 }
 
@@ -900,11 +931,11 @@ void Move::SetProbedBedEquation(size_t numPoints, StringRef& reply)
 }
 
 // Perform 3-, 4-, 6- or 7-factor delta adjustment
-void Move::AdjustDeltaParameters(const float v[], size_t numFactors)
+void Move::AdjustDeltaParameters(const floatc_t v[], size_t numFactors)
 {
 	// Save the old home carriage heights
-	float homedCarriageHeights[AXES];
-	for (size_t drive = 0; drive < AXES; ++drive)
+	float homedCarriageHeights[DELTA_AXES];
+	for (size_t drive = 0; drive < DELTA_AXES; ++drive)
 	{
 		homedCarriageHeights[drive] = deltaParams.GetHomedCarriageHeight(drive);
 	}
@@ -916,7 +947,7 @@ void Move::AdjustDeltaParameters(const float v[], size_t numFactors)
 	const int32_t *endCoordinates = lastQueuedMove->DriveCoordinates();
 	const float *driveStepsPerUnit = reprap.GetPlatform()->GetDriveStepsPerUnit();
 
-	for (size_t drive = 0; drive < AXES; ++drive)
+	for (size_t drive = 0; drive < DELTA_AXES; ++drive)
 	{
 		const float heightAdjust = deltaParams.GetHomedCarriageHeight(drive) - homedCarriageHeights[drive];
 		int32_t ep = endCoordinates[drive] + (int32_t)(heightAdjust * driveStepsPerUnit[drive]);
@@ -931,12 +962,12 @@ void Move::AdjustDeltaParameters(const float v[], size_t numFactors)
 // or the X positions of the front two towers, the Y position of the rear tower, and the diagonal rod length.
 void Move::DoDeltaCalibration(size_t numFactors, StringRef& reply)
 {
-	const size_t NumDeltaFactors = 7;		// number of delta machine factors we can adjust
+	const size_t NumDeltaFactors = 9;		// maximum number of delta machine factors we can adjust
 	const size_t numPoints = NumberOfProbePoints();
 
-	if (numFactors != 3 && numFactors != 4 && numFactors != 6 && numFactors != 7)
+	if (numFactors < 3 || numFactors > NumDeltaFactors || numFactors == 5)
 	{
-		reprap.GetPlatform()->MessageF(GENERIC_MESSAGE, "Delta calibration error: %d factors requested but only 3, 4, 6 and 7 supported\n", numFactors);
+		reprap.GetPlatform()->MessageF(GENERIC_MESSAGE, "Delta calibration error: %d factors requested but only 3, 4, 6, 7, 8 and 9 supported\n", numFactors);
 		return;
 	}
 
@@ -950,13 +981,13 @@ void Move::DoDeltaCalibration(size_t numFactors, StringRef& reply)
 	//uint32_t startTime = reprap.GetPlatform()->GetInterruptClocks();
 
 	// Transform the probing points to motor endpoints and store them in a matrix, so that we can do multiple iterations using the same data
-	FixedMatrix<float, MAX_DELTA_PROBE_POINTS, AXES> probeMotorPositions;
-	float corrections[MAX_DELTA_PROBE_POINTS];
-	float initialSumOfSquares = 0.0;
+	FixedMatrix<floatc_t, MAX_DELTA_PROBE_POINTS, DELTA_AXES> probeMotorPositions;
+	floatc_t corrections[MAX_DELTA_PROBE_POINTS];
+	float_t initialSumOfSquares = 0.0;
 	for (size_t i = 0; i < numPoints; ++i)
 	{
 		corrections[i] = 0.0;
-		float machinePos[AXES];
+		float machinePos[DELTA_AXES];
 		float xp = xBedProbePoints[i], yp = yBedProbePoints[i];
 		if (probePointSet[i] & xyCorrected)
 		{
@@ -981,14 +1012,15 @@ void Move::DoDeltaCalibration(size_t numFactors, StringRef& reply)
 	float expectedRmsError;
 	for (;;)
 	{
-		// Build a Nx7 matrix of derivatives with respect to xa, xb, yc, za, zb, zc, diagonal.
-		FixedMatrix<float, MAX_DELTA_PROBE_POINTS, NumDeltaFactors> derivativeMatrix;
+		// Build a Nx9 matrix of derivatives with respect to xa, xb, yc, za, zb, zc, diagonal.
+		FixedMatrix<floatc_t, MAX_DELTA_PROBE_POINTS, NumDeltaFactors> derivativeMatrix;
 		for (size_t i = 0; i < numPoints; ++i)
 		{
 			for (size_t j = 0; j < numFactors; ++j)
 			{
+				const size_t adjustedJ = (numFactors == 8 && j >= 6) ? j + 1 : j;		// skip diagonal rod length if doing 8-factor calibration
 				derivativeMatrix(i, j) =
-					deltaParams.ComputeDerivative(j, probeMotorPositions(i, A_AXIS), probeMotorPositions(i, B_AXIS), probeMotorPositions(i, C_AXIS));
+					deltaParams.ComputeDerivative(adjustedJ, probeMotorPositions(i, A_AXIS), probeMotorPositions(i, B_AXIS), probeMotorPositions(i, C_AXIS));
 			}
 		}
 
@@ -998,19 +1030,19 @@ void Move::DoDeltaCalibration(size_t numFactors, StringRef& reply)
 		}
 
 		// Now build the normal equations for least squares fitting
-		FixedMatrix<float, NumDeltaFactors, NumDeltaFactors + 1> normalMatrix;
+		FixedMatrix<floatc_t, NumDeltaFactors, NumDeltaFactors + 1> normalMatrix;
 		for (size_t i = 0; i < numFactors; ++i)
 		{
 			for (size_t j = 0; j < numFactors; ++j)
 			{
-				float temp = derivativeMatrix(0, i) * derivativeMatrix(0, j);
+				floatc_t temp = derivativeMatrix(0, i) * derivativeMatrix(0, j);
 				for (size_t k = 1; k < numPoints; ++k)
 				{
 					temp += derivativeMatrix(k, i) * derivativeMatrix(k, j);
 				}
 				normalMatrix(i, j) = temp;
 			}
-			float temp = derivativeMatrix(0, i) * -(zBedProbePoints[0] + corrections[0]);
+			floatc_t temp = derivativeMatrix(0, i) * -(zBedProbePoints[0] + corrections[0]);
 			for (size_t k = 1; k < numPoints; ++k)
 			{
 				temp += derivativeMatrix(k, i) * -(zBedProbePoints[k] + corrections[k]);
@@ -1023,7 +1055,7 @@ void Move::DoDeltaCalibration(size_t numFactors, StringRef& reply)
 			PrintMatrix("Normal matrix", normalMatrix, numFactors, numFactors + 1);
 		}
 
-		float solution[NumDeltaFactors];
+		floatc_t solution[NumDeltaFactors];
 		normalMatrix.GaussJordan(solution, numFactors);
 
 		if (reprap.Debug(moduleMove))
@@ -1032,7 +1064,7 @@ void Move::DoDeltaCalibration(size_t numFactors, StringRef& reply)
 			PrintVector("Solution", solution, numFactors);
 
 			// Calculate and display the residuals
-			float residuals[MAX_DELTA_PROBE_POINTS];
+			floatc_t residuals[MAX_DELTA_PROBE_POINTS];
 			for (size_t i = 0; i < numPoints; ++i)
 			{
 				residuals[i] = zBedProbePoints[i];
@@ -1045,20 +1077,19 @@ void Move::DoDeltaCalibration(size_t numFactors, StringRef& reply)
 			PrintVector("Residuals", residuals, numPoints);
 		}
 
-
 		AdjustDeltaParameters(solution, numFactors);
 
 		// Calculate the expected probe heights using the new parameters
 		{
-			float expectedResiduals[MAX_DELTA_PROBE_POINTS];
-			float sumOfSquares = 0.0;
+			floatc_t expectedResiduals[MAX_DELTA_PROBE_POINTS];
+			floatc_t sumOfSquares = 0.0;
 			for (size_t i = 0; i < numPoints; ++i)
 			{
-				for (size_t axis = 0; axis < AXES; ++axis)
+				for (size_t axis = 0; axis < DELTA_AXES; ++axis)
 				{
 					probeMotorPositions(i, axis) += solution[axis];
 				}
-				float newPosition[AXES];
+				float newPosition[DELTA_AXES];
 				deltaParams.InverseTransform(probeMotorPositions(i, A_AXIS), probeMotorPositions(i, B_AXIS), probeMotorPositions(i, C_AXIS), newPosition);
 				corrections[i] = newPosition[Z_AXIS];
 				expectedResiduals[i] = zBedProbePoints[i] + newPosition[Z_AXIS];
@@ -1073,7 +1104,7 @@ void Move::DoDeltaCalibration(size_t numFactors, StringRef& reply)
 			}
 		}
 
-		// Decide whether to do another iteration Two is slightly better than one, but three doesn't improve things.
+		// Decide whether to do another iteration. Two is slightly better than one, but three doesn't improve things.
 		// Alternatively, we could stop when the expected RMS error is only slightly worse than the RMS of the residuals.
 		++iteration;
 		if (iteration == 2) break;
@@ -1187,7 +1218,7 @@ bool Move::TryStartNextMove(uint32_t startTime)
 // This is called from the step ISR. Any variables it modifies that are also read by code outside the ISR must be declared 'volatile'.
 void Move::HitLowStop(size_t axis, DDA* hitDDA)
 {
-	if (axis < AXES && !IsDeltaMode())		// should always be true
+	if (axis < reprap.GetGCodes()->GetNumAxes() && !IsDeltaMode())		// should always be true
 	{
 		float hitPoint;
 		if (axis == Z_AXIS)
@@ -1207,7 +1238,7 @@ void Move::HitLowStop(size_t axis, DDA* hitDDA)
 // This is called from the step ISR. Any variables it modifies that are also read by code outside the ISR must be declared 'volatile'.
 void Move::HitHighStop(size_t axis, DDA* hitDDA)
 {
-	if (axis < AXES)		// should always be true
+	if (axis < reprap.GetGCodes()->GetNumAxes())		// should always be true
 	{
 		float hitPoint = (IsDeltaMode())
 							? deltaParams.GetHomedCarriageHeight(axis)
@@ -1223,13 +1254,13 @@ void Move::JustHomed(size_t axisHomed, float hitPoint, DDA* hitDDA)
 {
 	if (IsCoreXYAxis(axisHomed))
 	{
-		float tempCoordinates[AXES];
-		for (size_t axis = 0; axis < AXES; ++axis)
+		float tempCoordinates[CART_AXES];
+		for (size_t axis = 0; axis < CART_AXES; ++axis)
 		{
 			tempCoordinates[axis] = hitDDA->GetEndCoordinate(axis, false);
 		}
 		tempCoordinates[axisHomed] = hitPoint;
-		hitDDA->SetPositions(tempCoordinates, AXES);
+		hitDDA->SetPositions(tempCoordinates, CART_AXES);
 	}
 	else
 	{
@@ -1239,20 +1270,21 @@ void Move::JustHomed(size_t axisHomed, float hitPoint, DDA* hitDDA)
 
 }
 
-// This is called from the step ISR. Any variables it modifies that are also read by code outside the ISR must be declared 'volatile'.
+// This is called from the step ISR. Any variables it modifies that are also read by code outside the ISR should be declared 'volatile'.
 // The move has already been aborted when this is called, so the endpoints in the DDA are the current motor positions.
 void Move::ZProbeTriggered(DDA* hitDDA)
 {
-	// Currently, we don't need to do anything here
+	reprap.GetGCodes()->MoveStoppedByZProbe();
 }
 
 // Return the untransformed machine coordinates
 void Move::GetCurrentMachinePosition(float m[DRIVES], bool disableMotorMapping) const
 {
 	DDA *lastQueuedMove = ddaRingAddPointer->GetPrevious();
+	const size_t numAxes = reprap.GetGCodes()->GetNumAxes();
 	for (size_t i = 0; i < DRIVES; i++)
 	{
-		if (i < AXES)
+		if (i < numAxes)
 		{
 			m[i] = lastQueuedMove->GetEndCoordinate(i, disableMotorMapping);
 		}
@@ -1302,17 +1334,18 @@ void Move::LiveCoordinates(float m[DRIVES])
 	else
 	{
 		// Only the extruder coordinates are valid, so we need to convert the motor endpoints to coordinates
-		memcpy(m + AXES, const_cast<const float *>(liveCoordinates + AXES), sizeof(m[0]) * (DRIVES - AXES));
-		int32_t tempEndPoints[AXES];
+		const size_t numAxes = reprap.GetGCodes()->GetNumAxes();
+		memcpy(m + numAxes, const_cast<const float *>(liveCoordinates + numAxes), sizeof(m[0]) * (DRIVES - numAxes));
+		int32_t tempEndPoints[MAX_AXES];
 		memcpy(tempEndPoints, const_cast<const int32_t*>(liveEndPoints), sizeof(tempEndPoints));
 		cpu_irq_enable();
-		MachineToEndPoint(tempEndPoints, m, AXES);		// this is slow, so do it with interrupts enabled
+		MachineToEndPoint(tempEndPoints, m, numAxes);		// this is slow, so do it with interrupts enabled
 
 		// If the ISR has not updated the endpoints, store the live coordinates back so that we don't need to do it again
 		cpu_irq_disable();
 		if (memcmp(tempEndPoints, const_cast<const int32_t*>(liveEndPoints), sizeof(tempEndPoints)) == 0)
 		{
-			memcpy(const_cast<float *>(liveCoordinates), m, sizeof(m[0]) * AXES);
+			memcpy(const_cast<float *>(liveCoordinates), m, sizeof(m[0]) * numAxes);
 			liveCoordinatesValid = true;
 		}
 		cpu_irq_enable();
@@ -1330,14 +1363,14 @@ void Move::SetLiveCoordinates(const float coords[DRIVES])
 		liveCoordinates[drive] = coords[drive];
 	}
 	liveCoordinatesValid = true;
-	EndPointToMachine(coords, const_cast<int32_t *>(liveEndPoints), AXES);
+	EndPointToMachine(coords, const_cast<int32_t *>(liveEndPoints), reprap.GetGCodes()->GetNumAxes());
 	cpu_irq_enable();
 }
 
 void Move::ResetExtruderPositions()
 {
 	cpu_irq_disable();
-	for(size_t eDrive = AXES; eDrive < DRIVES; eDrive++)
+	for(size_t eDrive = reprap.GetGCodes()->GetNumAxes(); eDrive < DRIVES; eDrive++)
 	{
 		liveCoordinates[eDrive] = 0.0;
 	}
@@ -1531,7 +1564,7 @@ int Move::DoDeltaProbe(float frequency, float amplitude, float rate, float dista
 	return -1;
 }
 
-/*static*/ void Move::PrintMatrix(const char* s, const MathMatrix<float>& m, size_t maxRows, size_t maxCols)
+/*static*/ void Move::PrintMatrix(const char* s, const MathMatrix<floatc_t>& m, size_t maxRows, size_t maxCols)
 {
 	debugPrintf("%s\n", s);
 	if (maxRows == 0)
@@ -1552,7 +1585,7 @@ int Move::DoDeltaProbe(float frequency, float amplitude, float rate, float dista
 	}
 }
 
-/*static*/ void Move::PrintVector(const char *s, const float *v, size_t numElems)
+/*static*/ void Move::PrintVector(const char *s, const floatc_t *v, size_t numElems)
 {
 	debugPrintf("%s:", s);
 	for (size_t i = 0; i < numElems; ++i)
diff --git a/src/Movement/Move.h b/src/Movement/Move.h
index 693d763..dfe0260 100644
--- a/src/Movement/Move.h
+++ b/src/Movement/Move.h
@@ -10,16 +10,19 @@
 
 #include "DDA.h"
 #include "Libraries/Math/Matrix.h"
-#include "DeltaParameters.h"
-#include "DeltaProbe.h"
 
 #ifdef DUET_NG
 const unsigned int DdaRingLength = 40;
+typedef double floatc_t;					// type of matrix element used for delta calibration
 #else
 // We are more memory-constrained on the SAM3X
 const unsigned int DdaRingLength = 20;
+typedef float floatc_t;						// type of matrix element used for delta calibration
 #endif
 
+#include "DeltaParameters.h"
+#include "DeltaProbe.h"
+
 enum PointCoordinateSet
 {
 	unset = 0,
@@ -82,12 +85,12 @@ public:
     int GetCoreXYMode() const { return coreXYMode; }
     void SetCoreXYMode(int mode) { coreXYMode = mode; }
     float GetCoreAxisFactor(size_t axis) const { return axisFactors[axis]; }
-    void setCoreAxisFactor(size_t axis, float f) { axisFactors[axis] = f; }
+    void SetCoreAxisFactor(size_t axis, float f) { axisFactors[axis] = f; }
     bool IsCoreXYAxis(size_t axis) const;											// Return true if the specified axis shares its motors with another
 
     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 MotorTransform(const float machinePos[AXES], int32_t motorPos[AXES]) const;				// Convert Cartesian coordinates to delta motor coordinates
+    void MotorTransform(const float machinePos[MAX_AXES], int32_t motorPos[MAX_AXES]) const;				// Convert Cartesian coordinates to delta motor coordinates
     float MotorFactor(size_t drive, const float directionVector[]) const;							// Calculate the movement fraction for a single axis motor of a Cartesian or CoreXY printer
     void MachineToEndPoint(const int32_t motorPos[], float machinePos[], size_t numDrives) const;	// Convert motor coordinates to machine coordinates
     void EndPointToMachine(const float coords[], int32_t ep[], size_t numDrives) const;
@@ -116,21 +119,21 @@ private:
     bool StartNextMove(uint32_t startTime);											// start the next move, returning true if Step() needs to be called immediately
     void SetProbedBedEquation(size_t numPoints, StringRef& reply);					// When we have a full set of probed points, work out the bed's equation
     void DoDeltaCalibration(size_t numPoints, StringRef& reply);
-    void BedTransform(float move[AXES]) const;										// Take a position and apply the bed compensations
-    void GetCurrentMachinePosition(float m[DRIVES], bool disableMotorMapping) const;	// Get the current position in untransformed coords
-    void InverseBedTransform(float move[AXES]) const;								// Go from a bed-transformed point back to user coordinates
-    void AxisTransform(float move[AXES]) const;										// Take a position and apply the axis-angle compensations
-    void InverseAxisTransform(float move[AXES]) const;								// Go from an axis transformed point back to user coordinates
+    void BedTransform(float move[MAX_AXES]) const;									// Take a position and apply the bed compensations
+    void GetCurrentMachinePosition(float m[DRIVES], bool disableMotorMapping) const; // Get the current position in untransformed coords
+    void InverseBedTransform(float move[MAX_AXES]) const;							// Go from a bed-transformed point back to user coordinates
+    void AxisTransform(float move[MAX_AXES]) const;									// Take a position and apply the axis-angle compensations
+    void InverseAxisTransform(float move[MAX_AXES]) const;							// Go from an axis transformed point back to user coordinates
     void BarycentricCoordinates(size_t p0, size_t p1,   							// Compute the barycentric coordinates of a point in a triangle
     		size_t p2, float x, float y, float& l1,     							// (see http://en.wikipedia.org/wiki/Barycentric_coordinate_system).
     		float& l2, float& l3) const;
     float TriangleZ(float x, float y) const;										// Interpolate onto a triangular grid
-    void AdjustDeltaParameters(const float v[], size_t numFactors);					// Perform delta adjustment
+    void AdjustDeltaParameters(const floatc_t v[], size_t numFactors);				// Perform delta adjustment
     void JustHomed(size_t axis, float hitPoint, DDA* hitDDA);						// deal with setting positions after a drive has been homed
     void DeltaProbeInterrupt();														// step ISR when using the experimental delta probe
 
-    static void PrintMatrix(const char* s, const MathMatrix<float>& m, size_t numRows = 0, size_t maxCols = 0);	// for debugging
-    static void PrintVector(const char *s, const float *v, size_t numElems);		// for debugging
+    static void PrintMatrix(const char* s, const MathMatrix<floatc_t>& m, size_t numRows = 0, size_t maxCols = 0);	// for debugging
+    static void PrintVector(const char *s, const floatc_t *v, size_t numElems);		// for debugging
 
     bool DDARingAdd();									// Add a processed look-ahead entry to the DDA ring
     DDA* DDARingGet();									// Get the next DDA ring entry to be run
@@ -177,7 +180,7 @@ private:
     uint32_t deltaProbingStartTime;
     bool deltaProbing;
     int coreXYMode;										// 0 = Cartesian, 1 = CoreXY, 2 = CoreXZ, 3 = CoreYZ
-    float axisFactors[AXES];							// How much further the motors need to move for each axis movement, on a CoreXY/CoreXZ/CoreYZ machine
+    float axisFactors[MAX_AXES];						// How much further the motors need to move for each axis movement, on a CoreXY/CoreXZ/CoreYZ machine
     unsigned int stepErrors;							// count of step errors, for diagnostics
 };
 
diff --git a/src/Platform.cpp b/src/Platform.cpp
index 6be86a3..1ee8d9e 100644
--- a/src/Platform.cpp
+++ b/src/Platform.cpp
@@ -145,7 +145,7 @@ bool PidParameters::operator==(const PidParameters& other) const
 
 Platform::Platform() :
 		autoSaveEnabled(false), board(DEFAULT_BOARD_TYPE), active(false), errorCodeBits(0),
-		fileStructureInitialised(false), tickState(0), debugCode(0)
+		auxGCodeReply(nullptr), fileStructureInitialised(false), tickState(0), debugCode(0)
 {
 	// Output
 	auxOutput = new OutputStack();
@@ -184,6 +184,9 @@ void Platform::Init()
 	commsParams[2] = 0;
 #endif
 
+	auxDetected = false;
+	auxSeq = 0;
+
 	SERIAL_MAIN_DEVICE.begin(baudRates[0]);
 	SERIAL_AUX_DEVICE.begin(baudRates[1]);		// this can't be done in the constructor because the Arduino port initialisation isn't complete at that point
 #ifdef SERIAL_AUX2_DEVICE
@@ -291,7 +294,7 @@ void Platform::Init()
 	for (size_t drive = 0; drive < DRIVES; drive++)
 	{
 		// Map axes and extruders straight through
-		if (drive < AXES)
+		if (drive < MAX_AXES)
 		{
 			axisDrivers[drive].numDrivers = 1;
 			axisDrivers[drive].driverNumbers[0] = (uint8_t)drive;
@@ -305,10 +308,11 @@ void Platform::Init()
 #endif
 			endStopLogicLevel[drive] = true;					// assume all endstops use active high logic e.g. normally-closed switch to ground
 		}
-		else
+
+		if (drive >= MIN_AXES)
 		{
-			extruderDrivers[drive - AXES] = (uint8_t)drive;
-			SetPressureAdvance(drive - AXES, 0.0);
+			extruderDrivers[drive - MIN_AXES] = (uint8_t)drive;
+			SetPressureAdvance(drive - MIN_AXES, 0.0);
 		}
 		driveDriverBits[drive] = CalcDriverBitmap(drive);
 
@@ -570,26 +574,15 @@ int Platform::GetZProbeType() const
 	return nvData.zProbeType;
 }
 
-void Platform::SetZProbeAxes(const bool axes[AXES])
+void Platform::SetZProbeAxes(uint32_t axes)
 {
-	for (size_t axis=0; axis<AXES; axis++)
-	{
-		nvData.zProbeAxes[axis] = axes[axis];
-	}
+	nvData.zProbeAxes = axes;
 	if (autoSaveEnabled)
 	{
 		WriteNvData();
 	}
 }
 
-void Platform::GetZProbeAxes(bool (&axes)[AXES])
-{
-	for (size_t axis=0; axis<AXES; axis++)
-	{
-		axes[axis] = nvData.zProbeAxes[axis];
-	}
-}
-
 // Get our best estimate of the Z probe temperature
 float Platform::GetZProbeTemperature()
 {
@@ -737,7 +730,7 @@ bool Platform::SetZProbeParameters(const struct ZProbeParameters& params)
 // Return true if we must home X and Y before we home Z (i.e. we are using a bed probe)
 bool Platform::MustHomeXYBeforeZ() const
 {
-	return nvData.zProbeType != 0 && nvData.zProbeAxes[Z_AXIS];
+	return (nvData.zProbeType != 0) && ((nvData.zProbeAxes & (1 << Z_AXIS)) != 0);
 }
 
 void Platform::ResetNvData()
@@ -754,7 +747,7 @@ void Platform::ResetNvData()
 #endif
 
 	nvData.zProbeType = 0;	// Default is to use no Z probe switch
-	ARRAY_INIT(nvData.zProbeAxes, Z_PROBE_AXES);
+	nvData.zProbeAxes = Z_PROBE_AXES;
 	nvData.switchZProbeParameters.Init(0.0);
 	nvData.irZProbeParameters.Init(Z_PROBE_STOP_HEIGHT);
 	nvData.alternateZProbeParameters.Init(Z_PROBE_STOP_HEIGHT);
@@ -1044,6 +1037,12 @@ void Platform::Exit()
 		}
 	}
 
+	// Release the aux output stack (should release the others too!)
+	while (auxGCodeReply != nullptr)
+	{
+		auxGCodeReply = OutputBuffer::Release(auxGCodeReply);
+	}
+
 	// Stop processing data. Don't try to send a message because it will probably never get there.
 	active = false;
 }
@@ -1596,7 +1595,7 @@ float Platform::GetTemperature(size_t heater, TemperatureError& err)
 				return ABS_ZERO + p.GetBeta() / log(resistance / p.GetRInf());
 			}
 
-			// thermistor short circuit, return a high temperature
+			// Thermistor short circuit, return a high temperature
 			err = TemperatureError::shortCircuit;
 			return BAD_ERROR_TEMPERATURE;
 		}
@@ -1714,7 +1713,7 @@ EndStopHit Platform::Stopped(size_t drive) const
 	if (endStopType[drive] == EndStopType::noEndStop)
 	{
 		// No homing switch is configured for this axis, so see if we should use the Z probe
-		if (nvData.zProbeType > 0 && drive < AXES && nvData.zProbeAxes[drive])
+		if (nvData.zProbeType > 0 && drive < reprap.GetGCodes()->GetNumAxes() && (nvData.zProbeAxes & (1 << drive)) != 0)
 		{
 			return GetZProbeResult();			// using the Z probe as a low homing stop for this axis, so just get its result
 		}
@@ -1760,7 +1759,8 @@ EndStopHit Platform::GetZProbeResult() const
 // This is called from the step ISR as well as other places, so keep it fast
 void Platform::SetDirection(size_t drive, bool direction)
 {
-	if (drive < AXES)
+	const size_t numAxes = reprap.GetGCodes()->GetNumAxes();
+	if (drive < numAxes)
 	{
 		for (size_t i = 0; i < axisDrivers[drive].numDrivers; ++i)
 		{
@@ -1769,7 +1769,7 @@ void Platform::SetDirection(size_t drive, bool direction)
 	}
 	else if (drive < DRIVES)
 	{
-		SetDriverDirection(extruderDrivers[drive - AXES], direction);
+		SetDriverDirection(extruderDrivers[drive - numAxes], direction);
 	}
 }
 
@@ -1820,7 +1820,8 @@ void Platform::DisableDriver(size_t driver)
 // Enable the drivers for a drive. Must not be called from an ISR, or with interrupts disabled.
 void Platform::EnableDrive(size_t drive)
 {
-	if (drive < AXES)
+	const size_t numAxes = reprap.GetGCodes()->GetNumAxes();
+	if (drive < numAxes)
 	{
 		for (size_t i = 0; i < axisDrivers[drive].numDrivers; ++i)
 		{
@@ -1829,14 +1830,15 @@ void Platform::EnableDrive(size_t drive)
 	}
 	else if (drive < DRIVES)
 	{
-		EnableDriver(extruderDrivers[drive - AXES]);
+		EnableDriver(extruderDrivers[drive - numAxes]);
 	}
 }
 
 // Disable the drivers for a drive
 void Platform::DisableDrive(size_t drive)
 {
-	if (drive < AXES)
+	const size_t numAxes = reprap.GetGCodes()->GetNumAxes();
+	if (drive < numAxes)
 	{
 		for (size_t i = 0; i < axisDrivers[drive].numDrivers; ++i)
 		{
@@ -1845,7 +1847,7 @@ void Platform::DisableDrive(size_t drive)
 	}
 	else if (drive < DRIVES)
 	{
-		DisableDriver(extruderDrivers[drive - AXES]);
+		DisableDriver(extruderDrivers[drive - numAxes]);
 	}
 }
 
@@ -1883,7 +1885,8 @@ void Platform::SetDriverCurrent(size_t driver, float currentOrPercent, bool isPe
 // Set the current for all drivers on an axis or extruder. Current is in mA.
 void Platform::SetMotorCurrent(size_t drive, float currentOrPercent, bool isPercent)
 {
-	if (drive < AXES)
+	const size_t numAxes = reprap.GetGCodes()->GetNumAxes();
+	if (drive < numAxes)
 	{
 		for (size_t i = 0; i < axisDrivers[drive].numDrivers; ++i)
 		{
@@ -1893,7 +1896,7 @@ void Platform::SetMotorCurrent(size_t drive, float currentOrPercent, bool isPerc
 	}
 	else if (drive < DRIVES)
 	{
-		SetDriverCurrent(extruderDrivers[drive - AXES], currentOrPercent, isPercent);
+		SetDriverCurrent(extruderDrivers[drive - numAxes], currentOrPercent, isPercent);
 	}
 }
 
@@ -1968,7 +1971,8 @@ float Platform::GetMotorCurrent(size_t drive, bool isPercent) const
 {
 	if (drive < DRIVES)
 	{
-		uint8_t driver = (drive < AXES) ? axisDrivers[drive].driverNumbers[0] : extruderDrivers[drive - AXES];
+		const size_t numAxes = reprap.GetGCodes()->GetNumAxes();
+		const uint8_t driver = (drive < numAxes) ? axisDrivers[drive].driverNumbers[0] : extruderDrivers[drive - numAxes];
 		if (driver < DRIVES)
 		{
 			return (isPercent) ? motorCurrentFraction[driver] * 100.0 : motorCurrents[driver];
@@ -2016,7 +2020,8 @@ bool Platform::SetDriverMicrostepping(size_t driver, int microsteps, int mode)
 // Set the microstepping, returning true if successful. All drivers for the same axis must use the same microstepping.
 bool Platform::SetMicrostepping(size_t drive, int microsteps, int mode)
 {
-	if (drive < AXES)
+	const size_t numAxes = reprap.GetGCodes()->GetNumAxes();
+	if (drive < numAxes)
 	{
 		bool ok = true;
 		for (size_t i = 0; i < axisDrivers[drive].numDrivers; ++i)
@@ -2027,7 +2032,7 @@ bool Platform::SetMicrostepping(size_t drive, int microsteps, int mode)
 	}
 	else if (drive < DRIVES)
 	{
-		return SetDriverMicrostepping(extruderDrivers[drive - AXES], microsteps, mode);
+		return SetDriverMicrostepping(extruderDrivers[drive - numAxes], microsteps, mode);
 	}
 	return false;
 }
@@ -2049,13 +2054,14 @@ unsigned int Platform::GetDriverMicrostepping(size_t driver, bool& interpolation
 // Get the microstepping for an axis or extruder
 unsigned int Platform::GetMicrostepping(size_t drive, bool& interpolation) const
 {
-	if (drive < AXES)
+	const size_t numAxes = reprap.GetGCodes()->GetNumAxes();
+	if (drive < numAxes)
 	{
 		return GetDriverMicrostepping(axisDrivers[drive].driverNumbers[0], interpolation);
 	}
 	else if (drive < DRIVES)
 	{
-		return GetDriverMicrostepping(extruderDrivers[drive - AXES], interpolation);
+		return GetDriverMicrostepping(extruderDrivers[drive - numAxes], interpolation);
 	}
 	else
 	{
@@ -2079,7 +2085,7 @@ void Platform::SetAxisDriversConfig(size_t drive, const AxisDriversConfig& confi
 void Platform::SetExtruderDriver(size_t extruder, uint8_t driver)
 {
 	extruderDrivers[extruder] = driver;
-	driveDriverBits[extruder + AXES] = CalcDriverBitmap(driver);
+	driveDriverBits[extruder + reprap.GetGCodes()->GetNumAxes()] = CalcDriverBitmap(driver);
 }
 
 void Platform::SetDriverStepTiming(size_t driver, float microseconds)
@@ -2277,23 +2283,54 @@ FileStore* Platform::GetFileStore(const char* directory, const char* fileName, b
 	return NULL;
 }
 
+void Platform::AppendAuxReply(const char *msg)
+{
+	// Discard this response if either no aux device is attached or if the response is empty
+	if (msg[0] != 0 && HaveAux())
+	{
+		// Regular text-based responses for AUX are currently stored and processed by M105/M408
+		if (auxGCodeReply != nullptr || OutputBuffer::Allocate(auxGCodeReply))
+		{
+			auxSeq++;
+			auxGCodeReply->cat(msg);
+		}
+	}
+}
+
+void Platform::AppendAuxReply(OutputBuffer *reply)
+{
+	// Discard this response if either no aux device is attached or if the response is empty
+	if (reply == nullptr || reply->Length() == 0 || !HaveAux())
+	{
+		OutputBuffer::ReleaseAll(reply);
+	}
+	else if ((*reply)[0] == '{')
+	{
+		// JSON responses are always sent directly to the AUX device
+		// For big responses it makes sense to write big chunks of data in portions. Store this data here
+		auxOutput->Push(reply);
+	}
+	else
+	{
+		// Other responses are stored for M105/M408
+		auxSeq++;
+		if (auxGCodeReply == nullptr)
+		{
+			auxGCodeReply = reply;
+		}
+		else
+		{
+			auxGCodeReply->Append(reply);
+		}
+	}
+}
+
 void Platform::Message(MessageType type, const char *message)
 {
 	switch (type)
 	{
 		case AUX_MESSAGE:
-			// Message that is to be sent to the first auxiliary device
-			if (!auxOutput->IsEmpty())
-			{
-				// If we're still busy sending a response to the UART device, append this message to the output buffer
-				auxOutput->GetLastItem()->cat(message);
-			}
-			else
-			{
-				// Send short strings immediately through the aux channel. There is no flow control on this port, so it can't block for long
-				SERIAL_AUX_DEVICE.write(message);
-				SERIAL_AUX_DEVICE.flush();
-			}
+			AppendAuxReply(message);
 			break;
 
 		case AUX2_MESSAGE:
@@ -2385,15 +2422,7 @@ void Platform::Message(const MessageType type, OutputBuffer *buffer)
 	switch (type)
 	{
 		case AUX_MESSAGE:
-			// If no AUX device is attached, don't queue this buffer
-			if (!reprap.GetGCodes()->HaveAux())
-			{
-				OutputBuffer::ReleaseAll(buffer);
-				break;
-			}
-
-			// For big responses it makes sense to write big chunks of data in portions. Store this data here
-			auxOutput->Push(buffer);
+			AppendAuxReply(buffer);
 			break;
 
 		case AUX2_MESSAGE:
@@ -2491,7 +2520,7 @@ void Platform::SetAtxPower(bool on)
 
 void Platform::SetPressureAdvance(size_t extruder, float factor)
 {
-	if (extruder < DRIVES - AXES)
+	if (extruder < DRIVES - MIN_AXES)
 	{
 		pressureAdvance[extruder] = factor;
 	}
@@ -2500,9 +2529,10 @@ void Platform::SetPressureAdvance(size_t extruder, float factor)
 float Platform::ActualInstantDv(size_t drive) const
 {
 	const float idv = instantDvs[drive];
-	if (drive >= AXES)
+	const size_t numAxes = reprap.GetGCodes()->GetNumAxes();
+	if (drive >= numAxes)
 	{
-		const float eComp = pressureAdvance[drive - AXES];
+		const float eComp = pressureAdvance[drive - numAxes];
 		// If we are using pressure advance then we need to limit the extruder instantDv to avoid velocity mismatches.
 		// Assume that we want the extruder motor position to be accurate to within 0.01mm of extrusion.
 		// TODO remove this limit and add/remove steps to the previous and/or next move instead
diff --git a/src/Platform.h b/src/Platform.h
index 73ebbce..d7dbcc4 100644
--- a/src/Platform.h
+++ b/src/Platform.h
@@ -80,18 +80,6 @@ const float MillisToSeconds = 0.001;
 
 /**************************************************************************************************/
 
-#ifdef DUET_NG
-const int Z_PROBE_AD_VALUE = 500;						// Default for the Z probe - should be overwritten by experiment
-const bool Z_PROBE_AXES[AXES] = { false, false, true };	// Axes for which the Z-probe is normally used
-#else
-const int Z_PROBE_AD_VALUE = 400;						// Default for the Z probe - should be overwritten by experiment
-const bool Z_PROBE_AXES[AXES] = { true, false, true };	// Axes for which the Z-probe is normally used
-#endif
-
-const float Z_PROBE_STOP_HEIGHT = 0.7;					// Millimetres
-const unsigned int Z_PROBE_AVERAGE_READINGS = 8;		// We average this number of readings with IR on, and the same number with IR off
-const int ZProbeTypeDelta = 6;							// Z probe type for experimental delta probe
-
 #if SUPPORT_INKJET
 
 // Inkjet (if any - no inkjet is flagged by INKJET_BITS negative)
@@ -112,12 +100,26 @@ const float INSTANT_DVS[DRIVES] = DRIVES_(15.0, 15.0, 0.2, 2.0, 2.0, 2.0, 2.0, 2
 const size_t X_AXIS = 0, Y_AXIS = 1, Z_AXIS = 2, E0_AXIS = 3;	// The indices of the Cartesian axes in drive arrays
 const size_t A_AXIS = 0, B_AXIS = 1, C_AXIS = 2;				// The indices of the 3 tower motors of a delta printer in drive arrays
 
-const float AXIS_MINIMA[AXES] = { 0.0, 0.0, 0.0 };				// mm
-const float AXIS_MAXIMA[AXES] = { 230.0, 210.0, 200.0 };		// mm
+const float AXIS_MINIMA[MAX_AXES] = { 0.0, 0.0, 0.0 };			// mm
+const float AXIS_MAXIMA[MAX_AXES] = { 230.0, 210.0, 200.0 };	// mm
 
 const float defaultPrintRadius = 50;							// mm
 const float defaultDeltaHomedHeight = 200;						// mm
 
+// Z PROBE
+
+const float Z_PROBE_STOP_HEIGHT = 0.7;							// Millimetres
+const unsigned int Z_PROBE_AVERAGE_READINGS = 8;				// We average this number of readings with IR on, and the same number with IR off
+const int ZProbeTypeDelta = 6;									// Z probe type for experimental delta probe
+
+#ifdef DUET_NG
+const int Z_PROBE_AD_VALUE = 500;								// Default for the Z probe - should be overwritten by experiment
+const uint32_t Z_PROBE_AXES = (1 << Z_AXIS);					// Axes for which the Z-probe is normally used
+#else
+const int Z_PROBE_AD_VALUE = 400;								// Default for the Z probe - should be overwritten by experiment
+const uint32_t Z_PROBE_AXES = (1 << X_AXIS) | (1 << Z_AXIS);	// Axes for which the Z-probe is normally used
+#endif
+
 // HEATERS - The bed is assumed to be the at index 0
 
 // Bed thermistor: http://uk.farnell.com/epcos/b57863s103f040/sensor-miniature-ntc-10k/dp/1299930?Ntt=129-9930
@@ -464,6 +466,12 @@ public:
   
 	bool GCodeAvailable(const SerialSource source) const;
 	char ReadFromSource(const SerialSource source);
+	OutputBuffer *GetAuxGCodeReply();				// Returns cached G-Code reply for AUX devices and clears its reference
+	void AppendAuxReply(OutputBuffer *buf);
+	void AppendAuxReply(const char *msg);
+    uint32_t GetAuxSeq() { return auxSeq; }
+    bool HaveAux() const { return auxDetected; }	// Any device on the AUX line?
+    void SetAuxDetected() { auxDetected = true; }
 
 	void SetIPAddress(uint8_t ip[]);
 	const uint8_t* IPAddress() const;
@@ -576,8 +584,8 @@ public:
 	int GetZProbeSecondaryValues(int& v1, int& v2);
 	void SetZProbeType(int iZ);
 	int GetZProbeType() const;
-	void SetZProbeAxes(const bool axes[AXES]);
-	void GetZProbeAxes(bool (&axes)[AXES]);
+	void SetZProbeAxes(uint32_t axes);
+	uint32_t GetZProbeAxes() const { return nvData.zProbeAxes; }
 	const ZProbeParameters& GetZProbeParameters() const;
 	bool SetZProbeParameters(const struct ZProbeParameters& params);
 	bool MustHomeXYBeforeZ() const;
@@ -691,7 +699,7 @@ private:
 	struct FlashData
 	{
 		static const uint16_t magicValue = 0xE6C4;	// value we use to recognise that the flash data has been written
-		static const uint16_t versionValue = 2;		// increment this whenever this struct changes
+		static const uint16_t versionValue = 3;		// increment this whenever this struct changes
 		static const uint32_t nvAddress = (SoftwareResetData::nvAddress + sizeof(SoftwareResetData) + 3) & (~3);
 
 		uint16_t magic;
@@ -703,7 +711,7 @@ private:
 		ZProbeParameters irZProbeParameters;		// Z probe values for the IR sensor
 		ZProbeParameters alternateZProbeParameters;	// Z probe values for the alternate sensor
 		int zProbeType;								// the type of Z probe we are currently using
-		bool zProbeAxes[AXES];						// Z probe is used for these axes
+		uint32_t zProbeAxes;						// Z probe is used for these axes (bitmap)
 		PidParameters pidParams[HEATERS];
 		byte ipAddress[4];
 		byte netMask[4];
@@ -743,11 +751,11 @@ private:
 	float accelerations[DRIVES];
 	float driveStepsPerUnit[DRIVES];
 	float instantDvs[DRIVES];
-	float pressureAdvance[DRIVES - AXES];
+	float pressureAdvance[DRIVES - MIN_AXES];
 	float motorCurrents[DRIVES];					// the normal motor current for each stepper driver
 	float motorCurrentFraction[DRIVES];				// the percentages of normal motor current that each driver is set to
-	AxisDriversConfig axisDrivers[AXES];			// the driver numbers assigned to each axis
-	uint8_t extruderDrivers[DRIVES - AXES];			// the driver number assigned to each extruder
+	AxisDriversConfig axisDrivers[MAX_AXES];		// the driver numbers assigned to each axis
+	uint8_t extruderDrivers[DRIVES - MIN_AXES];		// the driver number assigned to each extruder
 	uint32_t driveDriverBits[DRIVES];				// the bitmap of driver port bits for each axis or extruder
 	uint32_t slowDriverStepPulseClocks;				// minimum high and low step pulse widths, in processor clocks
 	uint32_t slowDrivers;							// bitmap of driver port bits that need extended step pulse timing
@@ -783,10 +791,10 @@ private:
 
 	// Axes and endstops
 
-	float axisMaxima[AXES];
-	float axisMinima[AXES];
-	EndStopType endStopType[AXES+1];
-	bool endStopLogicLevel[AXES+1];
+	float axisMaxima[MAX_AXES];
+	float axisMinima[MAX_AXES];
+	EndStopType endStopType[MAX_AXES+1];
+	bool endStopLogicLevel[MAX_AXES+1];
   
 	// Heaters - bed is assumed to be the first
 
@@ -812,6 +820,9 @@ private:
 	OutputStack *auxOutput;
 	OutputStack *aux2Output;
 	OutputStack *usbOutput;
+    bool auxDetected;							// Have we processed at least one G-Code from an AUX device?
+	OutputBuffer *auxGCodeReply;				// G-Code reply for AUX devices (special one because it is actually encapsulated before sending)
+	uint32_t auxSeq;							// Sequence number for AUX devices
 
 	// Files
 
@@ -1228,7 +1239,7 @@ inline const uint8_t* Platform::MACAddress() const
 
 inline float Platform::GetPressureAdvance(size_t extruder) const
 {
-	return (extruder < DRIVES - AXES) ? pressureAdvance[extruder] : 0.0;
+	return (extruder < DRIVES - MIN_AXES) ? pressureAdvance[extruder] : 0.0;
 }
 
 inline void Platform::SetEndStopConfiguration(size_t axis, EndStopType esType, bool logicLevel)
@@ -1259,7 +1270,7 @@ inline uint16_t Platform::GetRawZProbeReading() const
 	case 4:
 		{
 			bool b = digitalRead(endStopPins[E0_AXIS]);
-			if (!endStopLogicLevel[AXES])
+			if (!endStopLogicLevel[MAX_AXES])
 			{
 				b = !b;
 			}
@@ -1299,6 +1310,13 @@ inline MassStorage* Platform::GetMassStorage() const
 	return massStorage;
 }
 
+inline OutputBuffer *Platform::GetAuxGCodeReply()
+{
+	OutputBuffer *temp = auxGCodeReply;
+	auxGCodeReply = nullptr;
+	return temp;
+}
+
 /*static*/ inline void Platform::EnableWatchdog()
 {
 	watchdogEnable(1000);
diff --git a/src/PrintMonitor.cpp b/src/PrintMonitor.cpp
index c938665..c6b92d9 100644
--- a/src/PrintMonitor.cpp
+++ b/src/PrintMonitor.cpp
@@ -302,7 +302,7 @@ bool PrintMonitor::GetFileInfo(const char *directory, const char *fileName, GCod
 		parsedFileInfo.layerHeight = 0.0;
 		parsedFileInfo.numFilaments = 0;
 		parsedFileInfo.generatedBy[0] = 0;
-		for(size_t extr = 0; extr < DRIVES - AXES; extr++)
+		for(size_t extr = 0; extr < DRIVES - MIN_AXES; extr++)
 		{
 			parsedFileInfo.filamentNeeded[extr] = 0.0;
 		}
@@ -374,7 +374,7 @@ bool PrintMonitor::GetFileInfo(const char *directory, const char *fileName, GCod
 			// Search for filament usage (Cura puts it at the beginning of a G-code file)
 			if (parsedFileInfo.numFilaments == 0)
 			{
-				parsedFileInfo.numFilaments = FindFilamentUsed(buf, sizeToScan, parsedFileInfo.filamentNeeded, DRIVES - AXES);
+				parsedFileInfo.numFilaments = FindFilamentUsed(buf, sizeToScan, parsedFileInfo.filamentNeeded, DRIVES - reprap.GetGCodes()->GetNumAxes());
 				headerInfoComplete &= (parsedFileInfo.numFilaments != 0);
 			}
 
@@ -529,7 +529,7 @@ bool PrintMonitor::GetFileInfo(const char *directory, const char *fileName, GCod
 			// Search for filament used
 			if (parsedFileInfo.numFilaments == 0)
 			{
-				parsedFileInfo.numFilaments = FindFilamentUsed(buf, sizeToScan, parsedFileInfo.filamentNeeded, DRIVES - AXES);
+				parsedFileInfo.numFilaments = FindFilamentUsed(buf, sizeToScan, parsedFileInfo.filamentNeeded, DRIVES - reprap.GetGCodes()->GetNumAxes());
 				if (parsedFileInfo.numFilaments == 0)
 				{
 					footerInfoComplete = false;
@@ -778,7 +778,7 @@ float PrintMonitor::EstimateTimeLeft(PrintEstimationMethod method) const
 			// Sum up the filament usage and the filament needed
 			float totalFilamentNeeded = 0.0;
 			const float extrRawTotal = gCodes->GetTotalRawExtrusion();
-			for(size_t extruder = 0; extruder < DRIVES - AXES; extruder++)
+			for(size_t extruder = 0; extruder < DRIVES - reprap.GetGCodes()->GetNumAxes(); extruder++)
 			{
 				totalFilamentNeeded += printingFileInfo.filamentNeeded[extruder];
 			}
diff --git a/src/PrintMonitor.h b/src/PrintMonitor.h
index 32ca213..875d43e 100644
--- a/src/PrintMonitor.h
+++ b/src/PrintMonitor.h
@@ -55,7 +55,7 @@ struct GCodeFileInfo
 	FilePosition fileSize;
 	float firstLayerHeight;
 	float objectHeight;
-	float filamentNeeded[DRIVES - AXES];
+	float filamentNeeded[DRIVES - MIN_AXES];
 	unsigned int numFilaments;
 	float layerHeight;
 	char generatedBy[50];
diff --git a/src/RepRapFirmware.cpp b/src/RepRapFirmware.cpp
index 043b81f..d73ec50 100644
--- a/src/RepRapFirmware.cpp
+++ b/src/RepRapFirmware.cpp
@@ -182,7 +182,7 @@ const char *moduleName[] =
 
 // Utilities and storage not part of any class
 
-static char scratchStringBuffer[120];		// this is now used only for short messages; needs to be long enough to print delta parameters
+static char scratchStringBuffer[140];		// this is now used only for short messages; needs to be long enough to print delta parameters
 StringRef scratchString(scratchStringBuffer, ARRAY_SIZE(scratchStringBuffer));
 
 // For debug use
diff --git a/src/RepRapFirmware.pdp b/src/RepRapFirmware.pdp
new file mode 100644
index 0000000..645411b
Binary files /dev/null and b/src/RepRapFirmware.pdp differ
diff --git a/src/Reprap.cpp b/src/Reprap.cpp
index ec24f4e..f31cbee 100644
--- a/src/Reprap.cpp
+++ b/src/Reprap.cpp
@@ -62,7 +62,7 @@ void RepRap::Init()
 	{
 		while (gCodes->DoingFileMacro())
 		{
-			// GCodes::Spin will read the macro and ensure DoFileMacro returns true when it's done
+			// GCodes::Spin will read the macro and ensure DoingFileMacro returns false when it's done
 			Spin();
 		}
 		platform->Message(HOST_MESSAGE, "Done!\n");
@@ -496,6 +496,7 @@ OutputBuffer *RepRap::GetStatusResponse(uint8_t type, ResponseSource source)
 
 	// Coordinates
 	{
+		const size_t numAxes = reprap.GetGCodes()->GetNumAxes();
 		float liveCoordinates[DRIVES + 1];
 #if SUPPORT_ROLAND
 		if (roland->Active())
@@ -511,24 +512,27 @@ OutputBuffer *RepRap::GetStatusResponse(uint8_t type, ResponseSource source)
 		if (currentTool != nullptr)
 		{
 			const float *offset = currentTool->GetOffset();
-			for (size_t i = 0; i < AXES; ++i)
+			for (size_t i = 0; i < numAxes; ++i)
 			{
 				liveCoordinates[i] += offset[i];
 			}
 		}
 
 		// Homed axes
-		response->catf("\"axesHomed\":[%d,%d,%d]",
-				(gCodes->GetAxisIsHomed(0)) ? 1 : 0,
-				(gCodes->GetAxisIsHomed(1)) ? 1 : 0,
-				(gCodes->GetAxisIsHomed(2)) ? 1 : 0);
+		response->cat("\"axesHomed\":");
+		ch = '[';
+		for (size_t axis = 0; axis < numAxes; ++axis)
+		{
+			response->catf("%c%d", ch, (gCodes->GetAxisIsHomed(axis)) ? 1 : 0);
+			ch = ',';
+		}
 
 		// Actual and theoretical extruder positions since power up, last G92 or last M23
-		response->catf(",\"extr\":");		// announce actual extruder positions
+		response->catf("],\"extr\":");		// announce actual extruder positions
 		ch = '[';
 		for (size_t extruder = 0; extruder < GetExtrudersInUse(); extruder++)
 		{
-			response->catf("%c%.1f", ch, liveCoordinates[AXES + extruder]);
+			response->catf("%c%.1f", ch, liveCoordinates[numAxes + extruder]);
 			ch = ',';
 		}
 		if (ch == '[')
@@ -547,7 +551,7 @@ OutputBuffer *RepRap::GetStatusResponse(uint8_t type, ResponseSource source)
 		{
 			// On Cartesian printers, the live coordinates are (usually) valid
 			ch = '[';
-			for (size_t axis = 0; axis < AXES; axis++)
+			for (size_t axis = 0; axis < numAxes; axis++)
 			{
 				response->catf("%c%.2f", ch, liveCoordinates[axis]);
 				ch = ',';
@@ -868,11 +872,11 @@ OutputBuffer *RepRap::GetStatusResponse(uint8_t type, ResponseSource source)
 
 	if (source == ResponseSource::AUX)
 	{
-		OutputBuffer *response = gCodes->GetAuxGCodeReply();
+		OutputBuffer *response = platform->GetAuxGCodeReply();
 		if (response != nullptr)
 		{
 			// Send the response to the last command. Do this last
-			response->catf(",\"seq\":%u,\"resp\":", gCodes->GetAuxSeq());			// send the response sequence number
+			response->catf(",\"seq\":%u,\"resp\":", platform->GetAuxSeq());			// send the response sequence number
 
 			// Send the JSON response
 			response->EncodeReply(response, true);									// also releases the OutputBuffer chain
@@ -892,10 +896,12 @@ OutputBuffer *RepRap::GetConfigResponse()
 		return nullptr;
 	}
 
+	const size_t numAxes = reprap.GetGCodes()->GetNumAxes();
+
 	// Axis minima
 	response->copy("{\"axisMins\":");
 	char ch = '[';
-	for (size_t axis = 0; axis < AXES; axis++)
+	for (size_t axis = 0; axis < numAxes; axis++)
 	{
 		response->catf("%c%.2f", ch, platform->AxisMinimum(axis));
 		ch = ',';
@@ -904,7 +910,7 @@ OutputBuffer *RepRap::GetConfigResponse()
 	// Axis maxima
 	response->cat("],\"axisMaxes\":");
 	ch = '[';
-	for (size_t axis = 0; axis < AXES; axis++)
+	for (size_t axis = 0; axis < numAxes; axis++)
 	{
 		response->catf("%c%.2f", ch, platform->AxisMaximum(axis));
 		ch = ',';
@@ -1066,20 +1072,21 @@ OutputBuffer *RepRap::GetLegacyStatusResponse(uint8_t type, int seq)
 	response->cat((ch == '[') ? "[]" : "]");
 
 	// Send XYZ positions
+	const size_t numAxes = reprap.GetGCodes()->GetNumAxes();
 	float liveCoordinates[DRIVES];
 	reprap.GetMove()->LiveCoordinates(liveCoordinates);
 	const Tool* currentTool = reprap.GetCurrentTool();
 	if (currentTool != nullptr)
 	{
 		const float *offset = currentTool->GetOffset();
-		for (size_t i = 0; i < AXES; ++i)
+		for (size_t i = 0; i < numAxes; ++i)
 		{
 			liveCoordinates[i] += offset[i];
 		}
 	}
 	response->catf(",\"pos\":");		// announce the XYZ position
 	ch = '[';
-	for (size_t drive = 0; drive < AXES; drive++)
+	for (size_t drive = 0; drive < numAxes; drive++)
 	{
 		response->catf("%c%.2f", ch, liveCoordinates[drive]);
 		ch = ',';
@@ -1138,10 +1145,14 @@ OutputBuffer *RepRap::GetLegacyStatusResponse(uint8_t type, int seq)
 	response->catf("],\"fanRPM\":%u", static_cast<unsigned int>(platform->GetFanRPM()));
 
 	// Send the home state. To keep the messages short, we send 1 for homed and 0 for not homed, instead of true and false.
-	response->catf(",\"homed\":[%d,%d,%d]",
-			(gCodes->GetAxisIsHomed(0)) ? 1 : 0,
-			(gCodes->GetAxisIsHomed(1)) ? 1 : 0,
-			(gCodes->GetAxisIsHomed(2)) ? 1 : 0);
+	response->cat(",\"homed\":");
+	ch = '[';
+	for (size_t axis = 0; axis < numAxes; ++axis)
+	{
+		response->catf("%c%d", ch, (gCodes->GetAxisIsHomed(axis)) ? 1 : 0);
+		ch = ',';
+	}
+	response->cat(']');
 
 	if (printMonitor->IsPrinting())
 	{
@@ -1170,7 +1181,7 @@ OutputBuffer *RepRap::GetLegacyStatusResponse(uint8_t type, int seq)
 		response->EncodeString(myName, ARRAY_SIZE(myName), false);
 	}
 
-	int auxSeq = (int)gCodes->GetAuxSeq();
+	int auxSeq = (int)platform->GetAuxSeq();
 	if (type < 2 || (seq != -1 && (int)auxSeq != seq))
 	{
 
@@ -1178,7 +1189,7 @@ OutputBuffer *RepRap::GetLegacyStatusResponse(uint8_t type, int seq)
 		response->catf(",\"seq\":%u,\"resp\":", auxSeq);					// send the response sequence number
 
 		// Send the JSON response
-		response->EncodeReply(gCodes->GetAuxGCodeReply(), true);			// also releases the OutputBuffer chain
+		response->EncodeReply(platform->GetAuxGCodeReply(), true);			// also releases the OutputBuffer chain
 	}
 
 	response->cat("}");
@@ -1328,7 +1339,7 @@ void RepRap::Beep(int freq, int ms)
 	beepFrequency = freq;
 	beepDuration = ms;
 
-	if (gCodes->HaveAux())
+	if (platform->HaveAux())
 	{
 		// If there is an LCD device present, make it beep
 		platform->Beep(freq, ms);
@@ -1341,7 +1352,7 @@ void RepRap::SetMessage(const char *msg)
 	strncpy(message, msg, MESSAGE_LENGTH);
 	message[MESSAGE_LENGTH] = 0;
 
-	if (gCodes->HaveAux())
+	if (platform->HaveAux())
 	{
 		platform->SendMessage(msg);
 	}
diff --git a/src/Tool.cpp b/src/Tool.cpp
index 0e57c39..da16bf8 100644
--- a/src/Tool.cpp
+++ b/src/Tool.cpp
@@ -29,7 +29,8 @@ Tool * Tool::freelist = nullptr;
 
 /*static*/Tool * Tool::Create(int toolNumber, long d[], size_t dCount, long h[], size_t hCount)
 {
-	if (dCount > DRIVES - AXES)
+	const size_t numAxes = reprap.GetGCodes()->GetNumAxes();
+	if (dCount > DRIVES - numAxes)
 	{
 		reprap.GetPlatform()->Message(GENERIC_MESSAGE,
 				"Error: Tool creation: attempt to use more drives than there are in the RepRap");
@@ -63,7 +64,7 @@ Tool * Tool::freelist = nullptr;
 	t->mixing = false;
 	t->displayColdExtrudeWarning = false;
 
-	for (size_t axis = 0; axis < AXES; axis++)
+	for (size_t axis = 0; axis < MAX_AXES; axis++)
 	{
 		t->offset[axis] = 0.0;
 	}
@@ -137,9 +138,10 @@ float Tool::MaxFeedrate() const
 		return 1.0;
 	}
 	float result = 0.0;
+	const size_t numAxes = reprap.GetGCodes()->GetNumAxes();
 	for (size_t d = 0; d < driveCount; d++)
 	{
-		float mf = reprap.GetPlatform()->MaxFeedrate(drives[d] + AXES);
+		float mf = reprap.GetPlatform()->MaxFeedrate(drives[d] + numAxes);
 		if (mf > result)
 		{
 			result = mf;
@@ -156,9 +158,10 @@ float Tool::InstantDv() const
 		return 1.0;
 	}
 	float result = FLT_MAX;
+	const size_t numAxes = reprap.GetGCodes()->GetNumAxes();
 	for (size_t d = 0; d < driveCount; d++)
 	{
-		float idv = reprap.GetPlatform()->ActualInstantDv(drives[d] + AXES);
+		float idv = reprap.GetPlatform()->ActualInstantDv(drives[d] + numAxes);
 		if (idv < result)
 		{
 			result = idv;
diff --git a/src/Tool.h b/src/Tool.h
index 1621025..223ebbe 100644
--- a/src/Tool.h
+++ b/src/Tool.h
@@ -34,7 +34,7 @@ public:
 	static void Delete(Tool *t);
 
 	const float *GetOffset() const;
-	void SetOffset(const float offs[AXES]);
+	void SetOffset(const float offs[MAX_AXES]);
 	size_t DriveCount() const;
 	int Drive(int driveNumber) const;
 	bool ToolCanDrive(bool extrude);
@@ -70,8 +70,8 @@ private:
 	void ResetTemperatureFault(int8_t wasDudHeater);
 	bool AllHeatersAtHighTemperature(bool forExtrusion) const;
 	int myNumber;
-	int drives[DRIVES - AXES];
-	float mix[DRIVES - AXES];
+	int drives[DRIVES - MIN_AXES];
+	float mix[DRIVES - MIN_AXES];
 	bool mixing;
 	size_t driveCount;
 	int heaters[HEATERS];
@@ -81,7 +81,7 @@ private:
 	Tool* next;
 	bool active;
 	bool heaterFault;
-	float offset[AXES];
+	float offset[MAX_AXES];
 
 	volatile bool displayColdExtrudeWarning;
 };
@@ -144,9 +144,9 @@ inline const float *Tool::GetOffset() const
 	return offset;
 }
 
-inline void Tool::SetOffset(const float offs[AXES])
+inline void Tool::SetOffset(const float offs[MAX_AXES])
 {
-	for(size_t i = 0; i < AXES; ++i)
+	for(size_t i = 0; i < MAX_AXES; ++i)
 	{
 		offset[i] = offs[i];
 	}