Version 1.15dev6

Grid compensation now working except for G28 S2 and lack of segmentation
for long moves
Fixed pause command that was broken in previous 1.17 dev release
Support mixing extruders using absolute extruder coordinates, except
that pause/resume won't restore the correct extruder coordinates

12 files changed, 641 insertions, 341 deletions

diff --git a/src/Configuration.h b/src/Configuration.h
index 40d2b9e5..62d7c58c 100644
--- a/src/Configuration.h
+++ b/src/Configuration.h
@@ -28,11 +28,11 @@ Licence: GPL
 // Firmware name is now defined in the Pins file
 
 #ifndef VERSION
-# define VERSION "1.17dev5"
+# define VERSION "1.17dev6"
 #endif
 
 #ifndef DATE
-# define DATE "2016-11-19"
+# define DATE "2016-11-22"
 #endif
 
 #define AUTHORS "reprappro, dc42, zpl, t3p3, dnewman"
diff --git a/src/GCodes/GCodes.cpp b/src/GCodes/GCodes.cpp
index eb732666..37602f94 100644
--- a/src/GCodes/GCodes.cpp
+++ b/src/GCodes/GCodes.cpp
@@ -42,6 +42,7 @@ 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 char* DefaultHeightMapFile = "heightmap.csv";
 
 const size_t gcodeReplyLength = 2048;			// long enough to pass back a reasonable number of files in response to M20
 
@@ -139,6 +140,8 @@ void GCodes::Reset()
 	{
 		moveBuffer.coords[i] = 0.0;
 	}
+	moveBuffer.xAxes = DefaultXAxisMapping;
+
 	feedRate = DEFAULT_FEEDRATE/minutesToSeconds;
 	pauseRestorePoint.Init();
 	toolChangeRestorePoint.Init();
@@ -301,7 +304,11 @@ void GCodes::Spin()
 			break;
 
 		case GCodeState::pausing2:
-			reply.copy("Printing paused");
+			if (AllMovesAreFinishedAndMoveBufferIsLoaded())
+			{
+				reply.copy("Printing paused");
+				gb.SetState(GCodeState::normal);
+			}
 			break;
 
 		case GCodeState::resuming1:
@@ -379,7 +386,7 @@ void GCodes::Spin()
 				}
 			}
 #else
-			gb.SetState(GCodeState::flashing2;
+			gb.SetState(GCodeState::flashing2);
 #endif
 			break;
 
@@ -437,6 +444,7 @@ void GCodes::Spin()
 					moveBuffer.coords[Y_AXIS] = y - platform->GetZProbeParameters().yOffset;
 					moveBuffer.coords[Z_AXIS] = platform->GetZProbeDiveHeight();
 					moveBuffer.feedRate = platform->GetZProbeTravelSpeed();
+					moveBuffer.xAxes = 0;
 					moveAvailable = true;
 					gb.SetState(GCodeState::gridProbing2);
 				}
@@ -467,6 +475,7 @@ void GCodes::Spin()
 				moveBuffer.filePos = noFilePosition;
 				moveBuffer.coords[Z_AXIS] = -platform->GetZProbeDiveHeight();
 				moveBuffer.feedRate = platform->GetZProbeParameters().probeSpeed;
+				moveBuffer.xAxes = 0;
 				moveAvailable = true;
 				gb.SetState(GCodeState::gridProbing3);
 			}
@@ -496,6 +505,7 @@ void GCodes::Spin()
 				moveBuffer.filePos = noFilePosition;
 				moveBuffer.coords[Z_AXIS] = platform->GetZProbeDiveHeight();
 				moveBuffer.feedRate = platform->GetZProbeTravelSpeed();
+				moveBuffer.xAxes = 0;
 				moveAvailable = true;
 				gb.SetState(GCodeState::gridProbing4);
 			}
@@ -531,12 +541,14 @@ void GCodes::Spin()
 				}
 				if (gridYindex == grid.NumYpoints())
 				{
+					// Finished probing the grid
 					if (numPointsProbed >= 4)
 					{
-						reprap.GetMove()->UseHeightMap();
+						error = reprap.GetMove()->SaveHeightMapToFile(heightMapFile, reply);
 						const double mean = heightSum/numPointsProbed;
 						const double deviation = sqrt(((heightSquaredSum * numPointsProbed) - (heightSum * heightSum)))/numPointsProbed;
-						reply.printf("%u points probed, mean error %.2f, deviation %.2f", numPointsProbed, mean, deviation);
+						reply.catf(" - %u points probed, mean error %.2f, deviation %.2f", numPointsProbed, mean, deviation);
+						reprap.GetMove()->UseHeightMap();
 					}
 					else
 					{
@@ -579,16 +591,19 @@ void GCodes::Spin()
 // Start a new gcode, or continue to execute one that has already been started:
 void GCodes::StartNextGCode(GCodeBuffer& gb, StringRef& reply)
 {
-	if (gb.IsReady() || gb.IsExecuting())
+	if (isPaused && &gb == fileGCode)
+	{
+		// We are paused, so don't process any more gcodes from the file being printed.
+		// There is a potential issue here if fileGCode holds any locks, so unlock everything.
+		UnlockAll(gb);
+	}
+	else if (gb.IsReady() || gb.IsExecuting())
 	{
 		gb.SetFinished(ActOnCode(gb, reply));
 	}
 	else if (gb.MachineState().fileState.IsLive())
 	{
-		if (&gb != fileGCode || !isPaused)
-		{
-			DoFilePrint(gb, reply);
-		}
+		DoFilePrint(gb, reply);
 	}
 	else if (&gb == httpGCode)
 	{
@@ -758,38 +773,36 @@ void GCodes::CheckTriggers()
 	}
 
 	// If any triggers are pending, activate the one with the lowest number
-	if (lowestTriggerPending < MaxTriggers)
+	if (lowestTriggerPending == 0)
 	{
-
-		// Execute the trigger
-		switch(lowestTriggerPending)
+		triggersPending &= ~(1u << lowestTriggerPending);			// clear the trigger
+		DoEmergencyStop();
+	}
+	else if (lowestTriggerPending < MaxTriggers						// if a trigger is pending
+			 && !daemonGCode->MachineState().fileState.IsLive()
+			 && daemonGCode->GetState() == GCodeState::normal		// and we are not already executing a trigger or config.g
+			)
+	{
+		if (lowestTriggerPending == 1)
 		{
-		case 0:
-			// Trigger 0 does an emergency stop
-			triggersPending &= ~(1u << lowestTriggerPending);			// clear the trigger
-			DoEmergencyStop();
-			break;
-
-		case 1:
-			// Trigger 1 pauses the print, if printing from file
-			triggersPending &= ~(1u << lowestTriggerPending);			// clear the trigger
-			if (!isPaused && reprap.GetPrintMonitor()->IsPrinting())
+			if (isPaused || !reprap.GetPrintMonitor()->IsPrinting())
 			{
-				DoPause(true);
+				triggersPending &= ~(1u << lowestTriggerPending);	// ignore a pause trigger if we are already paused
 			}
-			break;
-
-		default:
-			// All other trigger numbers execute the corresponding macro file
-			if (!daemonGCode->MachineState().fileState.IsLive())		// if not already executing a trigger or config.g
+			else if (LockMovement(*daemonGCode))					// need to lock movement before executing the pause macro
 			{
-				triggersPending &= ~(1u << lowestTriggerPending);		// clear the trigger
-				char buffer[25];
-				StringRef filename(buffer, ARRAY_SIZE(buffer));
-				filename.printf(SYS_DIR "trigger%u.g", lowestTriggerPending);
-				DoFileMacro(*daemonGCode, filename.Pointer(), true);
+				triggersPending &= ~(1u << lowestTriggerPending);	// clear the trigger
+				DoPause(*daemonGCode);
 			}
 		}
+		else
+		{
+			triggersPending &= ~(1u << lowestTriggerPending);		// clear the trigger
+			char buffer[25];
+			StringRef filename(buffer, ARRAY_SIZE(buffer));
+			filename.printf(SYS_DIR "trigger%u.g", lowestTriggerPending);
+			DoFileMacro(*daemonGCode, filename.Pointer(), true);
+		}
 	}
 }
 
@@ -801,20 +814,33 @@ void GCodes::DoEmergencyStop()
 	platform->Message(GENERIC_MESSAGE, "Emergency Stop! Reset the controller to continue.");
 }
 
-// Pause the print. Before calling this, check that we are doing a file print that isn't already paused.
-void GCodes::DoPause(bool externalToFile)
+// Pause the print. Before calling this, check that we are doing a file print that isn't already paused and get the movement lock.
+void GCodes::DoPause(GCodeBuffer& gb)
 {
-	if (externalToFile)
+	if (&gb == fileGCode)
+	{
+		// Pausing a file print because of a command in the file itself
+		for (size_t drive = 0; drive < numAxes; ++drive)
+		{
+			pauseRestorePoint.moveCoords[drive] = moveBuffer.coords[drive];
+		}
+		for (size_t drive = numAxes; drive < DRIVES; ++drive)
+		{
+			pauseRestorePoint.moveCoords[drive] = lastRawExtruderPosition[drive - numAxes];	// get current extruder positions into pausedMoveBuffer
+		}
+		pauseRestorePoint.feedRate = feedRate;
+	}
+	else
 	{
 		// Pausing a file print via another input source
 		pauseRestorePoint.feedRate = feedRate;										// the call to PausePrint may or may not change this
-		FilePosition fPos = reprap.GetMove()->PausePrint(pauseRestorePoint.moveCoords, pauseRestorePoint.feedRate);	// tell Move we wish to pause the current print
+		FilePosition fPos = reprap.GetMove()->PausePrint(pauseRestorePoint.moveCoords, pauseRestorePoint.feedRate, reprap.GetCurrentXAxes());
+																					// tell Move we wish to pause the current print
 		FileData& fdata = fileGCode->MachineState().fileState;
 		if (fPos != noFilePosition && fdata.IsLive())
 		{
 			fdata.Seek(fPos);														// replay the abandoned instructions if/when we resume
 		}
-		fileGCode->Init();
 		if (moveAvailable)
 		{
 			for (size_t drive = numAxes; drive < DRIVES; ++drive)
@@ -829,30 +855,19 @@ void GCodes::DoPause(bool externalToFile)
 			pauseRestorePoint.moveCoords[drive] = lastRawExtruderPosition[drive - numAxes] - pauseRestorePoint.moveCoords[drive];
 		}
 
+		//TODO record the virtual extruder positions of mixing tools too. But that's very hard to do unless we store it in the move.
+
 		if (reprap.Debug(moduleGcodes))
 		{
 			platform->MessageF(GENERIC_MESSAGE, "Paused print, file offset=%u\n", fPos);
 		}
 	}
-	else
-	{
-		// Pausing a file print because of a command in the file itself
-		for (size_t drive = 0; drive < numAxes; ++drive)
-		{
-			pauseRestorePoint.moveCoords[drive] = moveBuffer.coords[drive];
-		}
-		for (size_t drive = numAxes; drive < DRIVES; ++drive)
-		{
-			pauseRestorePoint.moveCoords[drive] = lastRawExtruderPosition[drive - numAxes];	// get current extruder positions into pausedMoveBuffer
-		}
-		pauseRestorePoint.feedRate = feedRate;
-	}
 
 	for (size_t i = 0; i < NUM_FANS; ++i)
 	{
 		pausedFanValues[i] = platform->GetFanValue(i);
 	}
-	fileGCode->SetState(GCodeState::pausing1);
+	gb.SetState(GCodeState::pausing1);
 	isPaused = true;
 }
 
@@ -886,7 +901,7 @@ bool GCodes::AllMovesAreFinishedAndMoveBufferIsLoaded()
 	}
 
 	reprap.GetMove()->ResumeMoving();
-	reprap.GetMove()->GetCurrentUserPosition(moveBuffer.coords, 0);
+	reprap.GetMove()->GetCurrentUserPosition(moveBuffer.coords, 0, reprap.GetCurrentXAxes());
 	return true;
 }
 
@@ -941,17 +956,34 @@ bool GCodes::LoadMoveBufferFromGCode(GCodeBuffer& gb, int moveType)
 		size_t eMoveCount = tool->DriveCount();
 		if (eMoveCount > 0)
 		{
-			float eMovement[MaxExtruders];
+			// Set the drive values for this tool.
+			// chrishamm-2014-10-03: Do NOT check extruder temperatures here, because we may be executing queued codes like M116
 			if (tool->GetMixing())
 			{
-				float length = gb.GetFValue();
-				for (size_t drive = 0; drive < tool->DriveCount(); drive++)
+				const float moveArg = gb.GetFValue() * distanceScale;
+				if (moveType == -1)			// if doing G92
+				{
+					tool->virtualExtruderPosition = moveArg;
+				}
+				else
 				{
-					eMovement[drive] = length * tool->GetMix()[drive];
+					const float requestedExtrusionAmount = (gb.MachineState().drivesRelative)
+															? moveArg
+															: moveArg - tool->virtualExtruderPosition;
+					for (size_t eDrive = 0; eDrive < eMoveCount; eDrive++)
+					{
+						const int drive = tool->Drive(eDrive);
+						const float extrusionAmount = requestedExtrusionAmount * tool->GetMix()[eDrive];
+						lastRawExtruderPosition[drive] += extrusionAmount;
+						rawExtruderTotalByDrive[drive] += extrusionAmount;
+						rawExtruderTotal += extrusionAmount;
+						moveBuffer.coords[drive + numAxes] = extrusionAmount * extrusionFactors[drive];
+					}
 				}
 			}
 			else
 			{
+				float eMovement[MaxExtruders];
 				size_t mc = eMoveCount;
 				gb.GetFloatArray(eMovement, mc, false);
 				if (eMoveCount != mc)
@@ -959,28 +991,26 @@ bool GCodes::LoadMoveBufferFromGCode(GCodeBuffer& gb, int moveType)
 					platform->MessageF(GENERIC_MESSAGE, "Wrong number of extruder drives for the selected tool: %s\n", gb.Buffer());
 					return false;
 				}
-			}
 
-			// Set the drive values for this tool.
-			// chrishamm-2014-10-03: Do NOT check extruder temperatures here, because we may be executing queued codes like M116
-			for (size_t eDrive = 0; eDrive < eMoveCount; eDrive++)
-			{
-				int drive = tool->Drive(eDrive);
-				float moveArg = eMovement[eDrive] * distanceScale;
-				if (moveType == -1)
-				{
-					moveBuffer.coords[drive + numAxes] = moveArg;
-					lastRawExtruderPosition[drive] = moveArg;
-				}
-				else
+				for (size_t eDrive = 0; eDrive < eMoveCount; eDrive++)
 				{
-					float extrusionAmount = (gb.MachineState().drivesRelative)
-												? moveArg
-												: moveArg - lastRawExtruderPosition[drive];
-					lastRawExtruderPosition[drive] += extrusionAmount;
-					rawExtruderTotalByDrive[drive] += extrusionAmount;
-					rawExtruderTotal += extrusionAmount;
-					moveBuffer.coords[drive + numAxes] = extrusionAmount * extrusionFactors[drive];
+					const int drive = tool->Drive(eDrive);
+					const float moveArg = eMovement[eDrive] * distanceScale;
+					if (moveType == -1)
+					{
+						moveBuffer.coords[drive + numAxes] = moveArg;
+						lastRawExtruderPosition[drive] = moveArg;
+					}
+					else
+					{
+						const float extrusionAmount = (gb.MachineState().drivesRelative)
+													? moveArg
+													: moveArg - lastRawExtruderPosition[drive];
+						lastRawExtruderPosition[drive] += extrusionAmount;
+						rawExtruderTotalByDrive[drive] += extrusionAmount;
+						rawExtruderTotal += extrusionAmount;
+						moveBuffer.coords[drive + numAxes] = extrusionAmount * extrusionFactors[drive];
+					}
 				}
 			}
 		}
@@ -1001,19 +1031,22 @@ bool GCodes::LoadMoveBufferFromGCode(GCodeBuffer& gb, int moveType)
 			else if (axis == X_AXIS && moveType == 0 && currentTool != nullptr)
 			{
 				// Perform X axis mapping
-				for (size_t i = 0; i < currentTool->GetAxisMapCount(); ++i)
+				const uint32_t xMap = currentTool->GetXAxisMap();
+				for (size_t mappedAxis = 0; mappedAxis < numAxes; ++mappedAxis)
 				{
-					const size_t mappedAxis = currentTool->GetAxisMap()[i];
-					float mappedMoveArg = moveArg;
-					if (gb.MachineState().axesRelative)
-					{
-						mappedMoveArg += moveBuffer.coords[mappedAxis];
-					}
-					else
+					if ((xMap & (1u << mappedAxis)) != 0)
 					{
-						mappedMoveArg -= currentTool->GetOffset()[mappedAxis];	// adjust requested position to compensate for tool offset
+						float mappedMoveArg = moveArg;
+						if (gb.MachineState().axesRelative)
+						{
+							mappedMoveArg += moveBuffer.coords[mappedAxis];
+						}
+						else
+						{
+							mappedMoveArg -= currentTool->GetOffset()[mappedAxis];	// adjust requested position to compensate for tool offset
+						}
+						moveBuffer.coords[mappedAxis] = mappedMoveArg;
 					}
-					moveBuffer.coords[mappedAxis] = mappedMoveArg;
 				}
 			}
 			else
@@ -1094,12 +1127,14 @@ int GCodes::SetUpMove(GCodeBuffer& gb, StringRef& reply)
 	// Check to see if the move is a 'homing' move that endstops are checked on.
 	moveBuffer.endStopsToCheck = 0;
 	moveBuffer.moveType = 0;
+	moveBuffer.xAxes = reprap.GetCurrentXAxes();
 	if (gb.Seen('S'))
 	{
 		int ival = gb.GetIValue();
 		if (ival == 1 || ival == 2)
 		{
 			moveBuffer.moveType = ival;
+			moveBuffer.xAxes = 0;			// don't do bed compensation
 		}
 
 		if (ival == 1)
@@ -1150,7 +1185,7 @@ int GCodes::SetUpMove(GCodeBuffer& gb, StringRef& reply)
 	else
 #endif
 	{
-		reprap.GetMove()->GetCurrentUserPosition(moveBuffer.coords, moveBuffer.moveType);
+		reprap.GetMove()->GetCurrentUserPosition(moveBuffer.coords, moveBuffer.moveType, reprap.GetCurrentXAxes());
 	}
 
 	// Load the move buffer with either the absolute movement required or the relative movement required
@@ -1203,7 +1238,7 @@ void GCodes::ClearMove()
 // Return true if the file was found or it wasn't and we were asked to report that fact.
 bool GCodes::DoFileMacro(GCodeBuffer& gb, const char* fileName, bool reportMissing)
 {
-	FileStore *f = platform->GetFileStore(platform->GetSysDir(), fileName, false);
+	FileStore * const f = platform->GetFileStore(platform->GetSysDir(), fileName, false);
 	if (f == nullptr)
 	{
 		if (reportMissing)
@@ -1269,6 +1304,7 @@ bool GCodes::DoCannedCycleMove(GCodeBuffer& gb, EndstopChecks ce)
 			}
 		}
 		moveBuffer.feedRate = cannedFeedRate;
+		moveBuffer.xAxes = 0;
 		moveBuffer.endStopsToCheck = ce;
 		moveBuffer.filePos = noFilePosition;
 		moveBuffer.usePressureAdvance = false;
@@ -1278,7 +1314,7 @@ bool GCodes::DoCannedCycleMove(GCodeBuffer& gb, EndstopChecks ce)
 	return false;
 }
 
-// This sets positions.  I.e. it handles G92.
+// This handles G92
 bool GCodes::SetPositions(GCodeBuffer& gb)
 {
 	// Don't pause the machine if only extruder drives are being reset (DC, 2015-09-06).
@@ -1305,7 +1341,7 @@ bool GCodes::SetPositions(GCodeBuffer& gb)
 		return false;
 	}
 
-	reprap.GetMove()->GetCurrentUserPosition(moveBuffer.coords, 0);		// make sure move buffer is up to date
+	reprap.GetMove()->GetCurrentUserPosition(moveBuffer.coords, 0, reprap.GetCurrentXAxes());	// make sure move buffer is up to date
 	bool ok = LoadMoveBufferFromGCode(gb, -1);
 	if (ok && includingAxes)
 	{
@@ -1864,12 +1900,68 @@ bool GCodes::DefineGrid(GCodeBuffer& gb, StringRef &reply)
 	}
 }
 
+// Start probing the grid, returning true if we didn't because of an error.
+// Prior to calling this the movement system must be locked.
+bool GCodes::ProbeGrid(GCodeBuffer& gb, StringRef& reply)
+{
+	long sParam = 0;
+	bool dummy;
+	gb.TryGetIValue('S', sParam, dummy);
+
+	if (gb.Seen('P'))
+	{
+		heightMapFile = gb.GetString();
+	}
+	else
+	{
+		heightMapFile = DefaultHeightMapFile;
+	}
+
+	switch(sParam)
+	{
+	case 0:	// Probe the bed and save to file
+		if (!reprap.GetMove()->GetBedProbeGrid().IsValid())
+		{
+			reply.copy("No valid grid defined for G29 bed probing");
+			return true;
+		}
+
+		if (!AllAxesAreHomed())
+		{
+			reply.copy("Must home printer before G29 bed probing");
+			return true;
+		}
+
+		gridXindex = gridYindex = 0;
+		numPointsProbed = 0;
+		heightSum = heightSquaredSum = 0.0;
+
+		reprap.GetMove()->ClearGridHeights();
+		reprap.GetMove()->SetIdentityTransform();
+		gb.SetState(GCodeState::gridProbing1);
+		return false;
+
+	case 1:	// Load height map from file
+		return reprap.GetMove()->LoadHeightMapFromFile(heightMapFile, reply);
+
+	case 2:	// Clear height map
+		reprap.GetMove()->ClearGridHeights();
+		return false;
+
+	default:
+		reply.copy("Invalid S parameter in G29 command");
+		return true;
+	}
+
+
+}
+
 // 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.
 void GCodes::GetCurrentCoordinates(StringRef& s) const
 {
 	float liveCoordinates[DRIVES];
-	reprap.GetMove()->LiveCoordinates(liveCoordinates);
+	reprap.GetMove()->LiveCoordinates(liveCoordinates, reprap.GetCurrentXAxes());
 	const Tool *currentTool = reprap.GetCurrentTool();
 	if (currentTool != nullptr)
 	{
@@ -1992,7 +2084,7 @@ void GCodes::WriteGCodeToFile(GCodeBuffer& gb)
 // Set up a file to print, but don't print it yet.
 void GCodes::QueueFileToPrint(const char* fileName)
 {
-	FileStore *f = platform->GetFileStore(platform->GetGCodeDir(), fileName, false);
+	FileStore * const f = platform->GetFileStore(platform->GetGCodeDir(), fileName, false);
 	if (f != nullptr)
 	{
 		// Cancel current print if there is any
@@ -2220,17 +2312,18 @@ void GCodes::ManageTool(GCodeBuffer& gb, StringRef& reply)
 	}
 
 	// Check X axis mapping
-	long xMapping[MAX_AXES];
-	size_t xCount = numAxes;
+	uint32_t xMap;
 	if (gb.Seen('X'))
 	{
+		long xMapping[MAX_AXES];
+		size_t xCount = numAxes;
 		gb.GetLongArray(xMapping, xCount);
+		xMap = LongArrayToBitMap(xMapping, xCount) & ((1u << numAxes) - 1);
 		seen = true;
 	}
 	else
 	{
-		xCount = 1;
-		xMapping[0] = 0;
+		xMap = 1;					// by default map X axis straight through
 	}
 
 	// Check for fan mapping
@@ -2240,15 +2333,7 @@ void GCodes::ManageTool(GCodeBuffer& gb, StringRef& reply)
 		long fanMapping[NUM_FANS];
 		size_t fanCount = NUM_FANS;
 		gb.GetLongArray(fanMapping, fanCount);
-		fanMap = 0;
-		for (size_t i = 0; i < fanCount; ++i)
-		{
-			const long f = fanMapping[i];
-			if (f >= 0 && (unsigned long)f < NUM_FANS)
-			{
-				fanMap |= 1u << (unsigned int)f;
-			}
-		}
+		fanMap = LongArrayToBitMap(fanMapping, fanCount) & ((1u << NUM_FANS) - 1);
 		seen = true;
 	}
 	else
@@ -2268,7 +2353,7 @@ void GCodes::ManageTool(GCodeBuffer& gb, StringRef& reply)
 		}
 		else
 		{
-			Tool* tool = Tool::Create(toolNumber, drives, dCount, heaters, hCount, xMapping, xCount, fanMap);
+			Tool* tool = Tool::Create(toolNumber, drives, dCount, heaters, hCount, xMap, fanMap);
 			if (tool != nullptr)
 			{
 				reprap.AddTool(tool);
@@ -2766,7 +2851,7 @@ bool GCodes::RetractFilament(bool retract)
 					return false;
 				}
 
-				reprap.GetMove()->GetCurrentUserPosition(moveBuffer.coords, 0);
+				reprap.GetMove()->GetCurrentUserPosition(moveBuffer.coords, 0, reprap.GetCurrentXAxes());
 				for (size_t i = numAxes; i < DRIVES; ++i)
 				{
 					moveBuffer.coords[i] = 0.0;
@@ -2786,6 +2871,7 @@ bool GCodes::RetractFilament(bool retract)
 				moveBuffer.isFirmwareRetraction = true;
 				moveBuffer.usePressureAdvance = false;
 				moveBuffer.filePos = filePos;
+				moveBuffer.xAxes = reprap.GetCurrentXAxes();
 				moveAvailable = true;
 			}
 		}
@@ -2929,27 +3015,11 @@ bool GCodes::HandleGcode(GCodeBuffer& gb, StringRef& reply)
 		break;
 
 	case 29: // Grid-based bed probing
-		LockMovementAndWaitForStandstill(gb);		// do this first to make sure that a new grid isn't being defined
-
-		if (!reprap.GetMove()->GetBedProbeGrid().IsValid())
-		{
-			reply.copy("No valid grid defined for G29 bed probing");
-			error = true;
-		}
-		else if (!AllAxesAreHomed())
-		{
-			reply.copy("Must home printer before G29 bed probing");
-			error = true;
-		}
-		else
+		if (!LockMovementAndWaitForStandstill(gb))		// do this first to make sure that a new grid isn't being defined
 		{
-			gridXindex = gridYindex = 0;
-			numPointsProbed = 0;
-			heightSum = heightSquaredSum = 0.0;
-
-			reprap.GetMove()->ClearGridHeights();
-			gb.SetState(GCodeState::gridProbing1);
+			return false;
 		}
+		error = ProbeGrid(gb, reply);
 		break;
 
 	case 30: // Z probe/manually set at a position and set that as point P
@@ -3029,7 +3099,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply)
 	bool result = true;
 	bool error = false;
 
-	int code = gb.GetIValue();
+	const int code = gb.GetIValue();
 	if (simulationMode != 0 && (code < 20 || code > 37) && code != 0 && code != 1 && code != 82 && code != 83 && code != 105 && code != 111 && code != 112 && code != 122 && code != 408 && code != 999)
 	{
 		return true;			// we don't yet simulate most M codes
@@ -3291,11 +3361,11 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply)
 		break;
 
 	case 226: // Gcode Initiated Pause
-		if (!LockMovementAndWaitForStandstill(gb))
+		if (&gb == fileGCode)			// ignore M226 if it did't come from within a file being printed
 		{
-			return false;
+			DoPause(gb);
 		}
-		// no break
+		break;
 
 	case 25: // Pause the print
 		if (isPaused)
@@ -3310,7 +3380,11 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply)
 		}
 		else
 		{
-			DoPause(code == 25 && &gb != fileGCode);
+			if (!LockMovement(gb))					// lock movement before calling DoPause
+			{
+				return false;
+			}
+			DoPause(gb);
 		}
 		break;
 
@@ -3437,7 +3511,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply)
 				if (!wasSimulating)
 				{
 					// Starting a new simulation, so save the current position
-					reprap.GetMove()->GetCurrentUserPosition(simulationRestorePoint.moveCoords, 0);
+					reprap.GetMove()->GetCurrentUserPosition(simulationRestorePoint.moveCoords, 0, reprap.GetCurrentXAxes());
 					simulationRestorePoint.feedRate = feedRate;
 				}
 			}
@@ -3562,7 +3636,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply)
 			// Save the current positions as we may need them later
 			float positionNow[DRIVES];
 			Move *move = reprap.GetMove();
-			move->GetCurrentUserPosition(positionNow, 0);
+			move->GetCurrentUserPosition(positionNow, 0, reprap.GetCurrentXAxes());
 
 			bool seen = false;
 			for (size_t axis = 0; axis < numAxes; axis++)
@@ -4813,7 +4887,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply)
 			}
 
 			// Read the entire file
-			FileStore *f = platform->GetFileStore(platform->GetSysDir(), platform->GetConfigFile(), false);
+			FileStore * const f = platform->GetFileStore(platform->GetSysDir(), platform->GetConfigFile(), false);
 			if (f == nullptr)
 			{
 				error = true;
@@ -5797,7 +5871,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply)
 		{
 			float positionNow[DRIVES];
 			Move *move = reprap.GetMove();
-			move->GetCurrentUserPosition(positionNow, 0);					// get the current position, we may need it later
+			move->GetCurrentUserPosition(positionNow, 0, reprap.GetCurrentXAxes()); // get the current position, we may need it later
 			DeltaParameters& params = move->AccessDeltaParams();
 			bool wasInDeltaMode = params.IsDeltaMode();						// remember whether we were in delta mode
 			bool seen = false;
@@ -5929,7 +6003,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply)
 			Move* move = reprap.GetMove();
 			bool seen = false;
 			float positionNow[DRIVES];
-			move->GetCurrentUserPosition(positionNow, 0);					// get the current position, we may need it later
+			move->GetCurrentUserPosition(positionNow, 0, reprap.GetCurrentXAxes());	// get the current position, we may need it later
 			if (gb.Seen('S'))
 			{
 				move->SetCoreXYMode(gb.GetIValue());
@@ -6301,7 +6375,7 @@ void GCodes::SetPositions(float positionNow[DRIVES])
 {
 	// Transform the position so that e.g. if the user does G92 Z0,
 	// the position we report (which gets inverse-transformed) really is Z=0 afterwards
-	reprap.GetMove()->Transform(positionNow);
+	reprap.GetMove()->Transform(positionNow, reprap.GetCurrentXAxes());
 	reprap.GetMove()->SetLiveCoordinates(positionNow);
 	reprap.GetMove()->SetPositions(positionNow);
 }
@@ -6513,4 +6587,19 @@ void GCodes::UnlockAll(const GCodeBuffer& gb)
 	}
 }
 
+// Convert an array of longs to a bit map
+/*static*/ uint32_t GCodes::LongArrayToBitMap(const long *arr, size_t numEntries)
+{
+	uint32_t res = 0;
+	for (size_t i = 0; i < numEntries; ++i)
+	{
+		const long f = arr[i];
+		if (f >= 0 && f < 32)
+		{
+			res |= 1u << (unsigned int)f;
+		}
+	}
+	return res;
+}
+
 // End
diff --git a/src/GCodes/GCodes.h b/src/GCodes/GCodes.h
index c1513714..c6b6b476 100644
--- a/src/GCodes/GCodes.h
+++ b/src/GCodes/GCodes.h
@@ -68,6 +68,7 @@ public:
 		float coords[DRIVES];											// new positions for the axes, amount of movement for the extruders
 		float feedRate;													// feed rate of this move
 		FilePosition filePos;											// offset in the file being printed that this move was read from
+		uint32_t xAxes;													// axes that X is mapped to
 		EndstopChecks endStopsToCheck;									// endstops to check
 		uint8_t moveType;												// the S parameter from the G0 or G1 command, 0 for a normal move
 		bool isFirmwareRetraction;										// true if this is a firmware retraction/un-retraction move
@@ -203,13 +204,16 @@ private:
 	void ListTriggers(StringRef reply, TriggerMask mask);				// Append a list of trigger endstops to a message
 	void CheckTriggers();												// Check for and execute triggers
 	void DoEmergencyStop();												// Execute an emergency stop
-	void DoPause(bool externalToFile);									// Pause the print
+	void DoPause(GCodeBuffer& gb);										// Pause the print
 	void SetMappedFanSpeed();											// Set the speeds of fans mapped for the current tool
 
 	bool DefineGrid(GCodeBuffer& gb, StringRef &reply);					// Define the probing grid, returning true if error
+	bool ProbeGrid(GCodeBuffer& gb, StringRef& reply);					// Start probing the grid, returning true if we didn't because of an error
+
+	static uint32_t LongArrayToBitMap(const long *arr, size_t numEntries);	// Convert an array of longs to a bit map
 
 	Platform* platform;													// The RepRap machine
-	Webserver* webserver;												// The webserver class
+	Webserver* webserver;												// The web server class
 
 	GCodeBuffer* gcodeSources[6];										// The various sources of gcodes
 
@@ -270,6 +274,7 @@ private:
 	size_t gridXindex, gridYindex;				// Which grid probe point is next
 	size_t numPointsProbed;
 	double heightSum, heightSquaredSum;
+	const char *heightMapFile;
 
 	float simulationTime;						// Accumulated simulation time
 	uint8_t simulationMode;						// 0 = not simulating, 1 = simulating, >1 are simulation modes for debugging
diff --git a/src/Movement/Grid.cpp b/src/Movement/Grid.cpp
index d338ceb7..ba194537 100644
--- a/src/Movement/Grid.cpp
+++ b/src/Movement/Grid.cpp
@@ -6,9 +6,12 @@
  */
 
 #include "Grid.h"
-#include "Configuration.h"
+#include "RepRapFirmware.h"
 #include <cmath>
 
+// Increase the version number in the following string whenever we change the format of the height map file.
+const char *HeightMapComment = "RepRapFirmware height map file v1";
+
 // Initialise the grid to be invalid
 GridDefinition::GridDefinition()
 	: xMin(0.0), xMax(-1.0), yMin(0.0), yMax(-1.0), radius(-1.0), spacing(DefaultGridSpacing), gridHeights(nullptr),
@@ -17,10 +20,10 @@ GridDefinition::GridDefinition()
 }
 
 GridDefinition::GridDefinition(const float xRange[2], const float yRange[2], float pRadius, float pSpacing)
-	: xMin(xRange[0]), xMax(xRange[1]), yMin(yRange[0]), yMax(yRange[1]), radius(pRadius), spacing(pSpacing)
+	: xMin(xRange[0]), xMax(xRange[1]), yMin(yRange[0]), yMax(yRange[1]), radius(pRadius), spacing(pSpacing), recipSpacing(1.0/spacing)
 {
-	numX = (xMax - xMin >= MinRange && spacing >= MinSpacing) ? (uint32_t)((xMax - xMin)/spacing) + 1 : 0;
-	numY = (yMax - yMin >= MinRange && spacing >= MinSpacing) ? (uint32_t)((xMax - xMin)/spacing) + 1 : 0;
+	numX = (xMax - xMin >= MinRange && spacing >= MinSpacing) ? (uint32_t)((xMax - xMin) * recipSpacing) + 1 : 0;
+	numY = (yMax - yMin >= MinRange && spacing >= MinSpacing) ? (uint32_t)((yMax - yMin) * recipSpacing) + 1 : 0;
 	isValid = NumPoints() != 0 && NumPoints() <= MaxGridProbePoints && (radius < 0.0 || radius >= 1.0);
 }
 
@@ -77,6 +80,75 @@ void GridDefinition::PrintError(StringRef& r) const
 	}
 }
 
+// Save the grid to file returning true if an error occurred
+bool GridDefinition::SaveToFile(FileStore *f) const
+{
+	char bufferSpace[500];
+	StringRef buf(bufferSpace, ARRAY_SIZE(bufferSpace));
+
+	// Write the header comment
+	buf.copy(HeightMapComment);
+	if (reprap.GetPlatform()->IsDateTimeSet())
+	{
+		time_t timeNow = reprap.GetPlatform()->GetDateTime();
+		const struct tm * const timeInfo = gmtime(&timeNow);
+		buf.catf(" generated at %04u-%02u-%02u %02u:%02u",
+						timeInfo->tm_year, timeInfo->tm_mon, timeInfo->tm_mday, timeInfo->tm_hour, timeInfo->tm_min);
+	}
+	buf.cat('\n');
+	if (!f->Write(buf.Pointer()))
+	{
+		return true;
+	}
+
+	// Write the grid parameters
+	buf.printf("xmin,xmax,ymin,ymax,radius,spacing,xnum,ynum\n"
+				 "%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%u,%u\n",
+				 xMin, xMax, yMin, yMax, radius, spacing, numX, numY
+				);
+	if (!f->Write(buf.Pointer()))
+	{
+		return true;
+	}
+
+	// Write the grid heights
+	uint32_t index = 0;
+	for (uint32_t i = 0; i < numY; ++i)
+	{
+		buf.Clear();
+		for (uint32_t j = 0; j < numX; ++j)
+		{
+			if (j != 0)
+			{
+				buf.cat(',');
+			}
+			if (IsHeightSet(index))
+			{
+				buf.catf("%.3f%", gridHeights[index]);
+			}
+			else
+			{
+				buf.cat("0");					// write 0 with no decimal point where we didn't probe, so we can tell when we reload it
+			}
+			++index;
+		}
+		buf.cat('\n');
+		if (!f->Write(buf.Pointer()))
+		{
+			return true;
+		}
+	}
+
+	return false;
+}
+
+// Load the grid from file returning true if an error occurred
+bool GridDefinition::LoadFromFile(FileStore *f)
+{
+	//TODO
+	return true;
+}
+
 // Compute the height error at the specified point
 float GridDefinition::ComputeHeightError(float x, float y) const
 {
@@ -185,32 +257,43 @@ float GridDefinition::InterpolateXY(uint32_t xIndex, uint32_t yIndex, float xFra
 	case 2:		// (X1,Y0) defined
 		return gridHeights[indexX1Y0];
 	case 3:		// (X0,Y0) and (X1,Y0) defined
-		return Interpolate2(indexX0Y0, indexX1Y0, xFrac);
+		return (xFrac * gridHeights[indexX1Y0]) + ((1.0 - xFrac) * gridHeights[indexX0Y0]);
 	case 4:		// (X0,Y1) defined
 		return gridHeights[indexX0Y1];
 	case 5:		// (X0,Y0) and (X0,Y1) defined
-		return Interpolate2(indexX0Y0, indexX0Y1, yFrac);
+		return (yFrac * gridHeights[indexX0Y1]) + ((1.0 - yFrac) * gridHeights[indexX0Y0]);
 	case 6:		// (X1,Y0) and (X0,Y1) defined - diagonal interpolation
-		return DiagonalInterpolate(indexX1Y0, indexX0Y1, 1.0 - xFrac, yFrac);
+		return (((xFrac + 1.0 - yFrac) * gridHeights[indexX1Y0]) + ((yFrac + 1.0 - xFrac) * gridHeights[indexX0Y1]))/2;
 	case 7:		// (X0,Y0), (X1,Y0) and (X0,Y1) defined - 3-way interpolation
-		return Interpolate3(indexX0Y0, indexX1Y0, indexX0Y1, xFrac, yFrac);
+		return InterpolateCorner(indexX0Y0, indexX1Y0, indexX0Y1, xFrac, yFrac);
 	case 8:		// (X1,Y1) defined
 		return gridHeights[indexX1Y1];
 	case 9:		// (X0,Y0) and (X1,Y1) defined - diagonal interpolation
-		return DiagonalInterpolate(indexX0Y0, indexX1Y1, xFrac, yFrac);
+		return ((xFrac + yFrac) * gridHeights[indexX1Y1]) + ((2.0 - (xFrac + yFrac)) * gridHeights[indexX0Y0])/2;
 	case 10:	// (X1,Y0) and (X1,Y1) defined
-		return Interpolate2(indexX1Y0, indexX1Y1, yFrac);
+		return (yFrac * gridHeights[indexX1Y1]) + ((1.0 - yFrac) * gridHeights[indexX1Y0]);
 	case 11:	// (X0,Y0), (X1,Y0) and (X1,Y1) defined - 3-way interpolation
-		return Interpolate3(indexX1Y0, indexX0Y0, indexX1Y1, xFrac, yFrac);
+		return InterpolateCorner(indexX1Y0, indexX0Y0, indexX1Y1, xFrac, yFrac);
 	case 12:	// (X0,Y1) and (X1,Y1) defined
-		return Interpolate2(indexX0Y1, indexX1Y1, yFrac);
+		return (xFrac * gridHeights[indexX1Y1]) + ((1.0 - xFrac) * gridHeights[indexX0Y1]);
 	case 13:	// (X0,Y0), (X0,Y1) and (X1,Y1) defined - 3-way interpolation
-		return Interpolate3(indexX0Y1, indexX1Y1, indexX0Y0, xFrac, 1.0 - yFrac);
+		return InterpolateCorner(indexX0Y1, indexX1Y1, indexX0Y0, xFrac, 1.0 - yFrac);
 	case 14:	// (X1,Y0), (X0,Y1) and (X1,Y1) defined - 3-way interpolation
-		return Interpolate3(indexX1Y1, indexX0Y1, indexX1Y0, 1.0 - xFrac, 1.0 - yFrac);
+		return InterpolateCorner(indexX1Y1, indexX0Y1, indexX1Y0, 1.0 - xFrac, 1.0 - yFrac);
 	case 15:	// All points defined
-		return Interpolate4(indexX0Y0, indexX1Y0, indexX0Y1, indexX1Y1, xFrac, yFrac);
+		{
+			const float xyFrac = xFrac * yFrac;
+			return (gridHeights[indexX0Y0] * (1.0 - xFrac - yFrac + xyFrac))
+				 + (gridHeights[indexX1Y0] * (xFrac - xyFrac))
+				 + (gridHeights[indexX0Y1] * (yFrac - xyFrac))
+				 + (gridHeights[indexX1Y1] * xyFrac);
+		}
 	}
 }
 
+float GridDefinition::InterpolateCorner(uint32_t cornerIndex, uint32_t indexX, uint32_t indexY, float xFrac, float yFrac) const
+{
+	return ((xFrac * gridHeights[indexX]) + (yFrac * gridHeights[indexY]) + ((2.0 - xFrac - yFrac) * gridHeights[cornerIndex]))/2;
+}
+
 // End
diff --git a/src/Movement/Grid.h b/src/Movement/Grid.h
index f03ec4e5..9146d394 100644
--- a/src/Movement/Grid.h
+++ b/src/Movement/Grid.h
@@ -12,6 +12,8 @@
 #include "ecv.h"
 #include "Libraries/General/StringRef.h"
 
+class FileStore;
+
 // This class defines the bed probing grid
 class GridDefinition
 {
@@ -29,9 +31,15 @@ public:
 	bool IsValid() const { return isValid; }
 
 	void PrintParameters(StringRef& r) const;
+
 	void PrintError(StringRef& r) const
 	pre(!IsValid());
 
+	bool SaveToFile(FileStore *f) const					// Save the grid to file returning true if an error occurred
+	pre(IsValid());
+
+	bool LoadFromFile(FileStore *f);					// Load the grid from file returning true if an error occurred
+
 	float ComputeHeightError(float x, float y) const		// Compute the height error at the specified point
 	pre(IsValid(); gridHeights != nullptr; gridHeights.upb >= NumPoints());
 
@@ -58,9 +66,7 @@ private:
 	float InterpolateY(uint32_t xIndex, uint32_t yIndex, float yFrac) const;
 	float InterpolateXY(uint32_t xIndex, uint32_t yIndex, float xFrac, float yFrac) const;
 	float Interpolate2(uint32_t index1, uint32_t index2, float frac) const;
-	float DiagonalInterpolate(uint32_t index1, uint32_t index2, float xFrac, float yFrac) const;
-	float Interpolate3(uint32_t cornerIndex, uint32_t indexX, uint32_t indexY, float xFrac, float yFrac) const;
-	float Interpolate4(uint32_t index1, uint32_t index2, uint32_t index3, uint32_t index4, float xFrac, float yFrac) const;
+	float InterpolateCorner(uint32_t cornerIndex, uint32_t indexX, uint32_t indexY, float xFrac, float yFrac) const;
 };
 
 #endif /* SRC_MOVEMENT_GRID_H_ */
diff --git a/src/Movement/Move.cpp b/src/Movement/Move.cpp
index 9d630f34..0d7ccf78 100644
--- a/src/Movement/Move.cpp
+++ b/src/Movement/Move.cpp
@@ -209,7 +209,7 @@ void Move::Spin()
 					bool doMotorMapping = (nextMove.moveType == 0) || (nextMove.moveType == 1 && !IsDeltaMode());
 					if (doMotorMapping)
 					{
-						Transform(nextMove.coords);
+						Transform(nextMove.coords, nextMove.xAxes);
 					}
 					if (ddaRingAddPointer->Init(nextMove, doMotorMapping))
 					{
@@ -323,7 +323,7 @@ void Move::Spin()
 // Returns the file position of the first queue move we are going to skip, or noFilePosition we we are not skipping any moves.
 // We update 'positions' to the positions and feed rate expected for the next move, and the amount of extrusion in the moves we skipped.
 // If we are not skipping any moves then the feed rate is left alone, therefore the caller should set this up first.
-FilePosition Move::PausePrint(float positions[DRIVES], float& pausedFeedRate)
+FilePosition Move::PausePrint(float positions[DRIVES], float& pausedFeedRate, uint32_t xAxes)
 {
 	// Find a move we can pause after.
 	// Ideally, we would adjust a move if necessary and possible so that we can pause after it, but for now we don't do that.
@@ -408,7 +408,7 @@ FilePosition Move::PausePrint(float positions[DRIVES], float& pausedFeedRate)
 	}
 	else
 	{
-		GetCurrentUserPosition(positions, 0);		// gets positions and clears out extrusion values
+		GetCurrentUserPosition(positions, 0, xAxes);		// gets positions and clears out extrusion values
 	}
 
 	return fPos;
@@ -627,78 +627,124 @@ float Move::MotorFactor(size_t drive, const float directionVector[]) 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];
+	xyzPoint[X_AXIS] += tanXY*xyzPoint[Y_AXIS] + tanXZ*xyzPoint[Z_AXIS];
+	xyzPoint[Y_AXIS] += tanYZ*xyzPoint[Z_AXIS];
 }
 
 // Invert the Axis transform AFTER the bed transform
 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]);
+	xyzPoint[Y_AXIS] -= tanYZ*xyzPoint[Z_AXIS];
+	xyzPoint[X_AXIS] -= (tanXY*xyzPoint[Y_AXIS] + tanXZ*xyzPoint[Z_AXIS]);
 }
 
-void Move::Transform(float xyzPoint[MAX_AXES]) const
+void Move::Transform(float xyzPoint[MAX_AXES], uint32_t xAxes) const
 {
 	AxisTransform(xyzPoint);
-	BedTransform(xyzPoint);
+	BedTransform(xyzPoint, xAxes);
 }
 
-void Move::InverseTransform(float xyzPoint[MAX_AXES]) const
+void Move::InverseTransform(float xyzPoint[MAX_AXES], uint32_t xAxes) const
 {
-	InverseBedTransform(xyzPoint);
+	InverseBedTransform(xyzPoint, xAxes);
 	InverseAxisTransform(xyzPoint);
 }
 
 // Do the bed transform AFTER the axis transform
-void Move::BedTransform(float xyzPoint[MAX_AXES]) const
+void Move::BedTransform(float xyzPoint[MAX_AXES], uint32_t xAxes) const
 {
-	switch(numBedCompensationPoints)
+	float zCorrection = 0.0;
+	const size_t numAxes = reprap.GetGCodes()->GetNumAxes();
+	unsigned int numXAxes = 0;
+
+	// Transform the Z coordinate based on the average correction for each axis used as an X axis.
+	// We are assuming that the tool Y offsets are small enough to be ignored.
+	for (uint32_t axis = 0; axis < numAxes; ++axis)
 	{
-	case 0:
-		break;
+		if ((xAxes & (1u << axis)) != 0)
+		{
+			const float xCoord = xyzPoint[axis];
+			switch(numBedCompensationPoints)
+			{
+			case 0:
+				if (useGridHeights)
+				{
+					zCorrection += grid.ComputeHeightError(xCoord, xyzPoint[Y_AXIS]);
+				}
+				break;
 
-	case 3:
-		xyzPoint[Z_AXIS] += aX*xyzPoint[X_AXIS] + aY*xyzPoint[Y_AXIS] + aC;
-		break;
+			case 3:
+				zCorrection += aX * xCoord + aY * xyzPoint[Y_AXIS] + aC;
+				break;
 
-	case 4:
-		xyzPoint[Z_AXIS] += SecondDegreeTransformZ(xyzPoint[X_AXIS], xyzPoint[Y_AXIS]);
-		break;
+			case 4:
+				zCorrection += SecondDegreeTransformZ(xCoord, xyzPoint[Y_AXIS]);
+				break;
 
-	case 5:
-		xyzPoint[Z_AXIS] += TriangleZ(xyzPoint[X_AXIS], xyzPoint[Y_AXIS]);
-		break;
+			case 5:
+				zCorrection += TriangleZ(xCoord, xyzPoint[Y_AXIS]);
+				break;
 
-	default:
-		reprap.GetPlatform()->Message(GENERIC_MESSAGE, "BedTransform: wrong number of sample points.");
+			default:
+				break;
+			}
+			++numXAxes;
+		}
+	}
+	if (numXAxes > 1)
+	{
+		zCorrection /= numXAxes;			// take an average
 	}
+	xyzPoint[Z_AXIS] += zCorrection;
 }
 
 // Invert the bed transform BEFORE the axis transform
-void Move::InverseBedTransform(float xyzPoint[MAX_AXES]) const
+void Move::InverseBedTransform(float xyzPoint[MAX_AXES], uint32_t xAxes) const
 {
-	switch(numBedCompensationPoints)
+	float zCorrection = 0.0;
+	const size_t numAxes = reprap.GetGCodes()->GetNumAxes();
+	unsigned int numXAxes = 0;
+
+	// Transform the Z coordinate based on the average correction for each axis used as an X axis.
+	// We are assuming that the tool Y offsets are small enough to be ignored.
+	for (uint32_t axis = 0; axis < numAxes; ++axis)
 	{
-	case 0:
-		break;
+		if ((xAxes & (1u << axis)) != 0)
+		{
+			const float xCoord = xyzPoint[axis];
+			switch(numBedCompensationPoints)
+			{
+			case 0:
+				if (useGridHeights)
+				{
+					zCorrection += grid.ComputeHeightError(xCoord, xyzPoint[Y_AXIS]);
+				}
+				break;
 
-	case 3:
-		xyzPoint[Z_AXIS] -= (aX*xyzPoint[X_AXIS] + aY*xyzPoint[Y_AXIS] + aC);
-		break;
+			case 3:
+				zCorrection += aX * xCoord + aY * xyzPoint[Y_AXIS] + aC;
+				break;
 
-	case 4:
-		xyzPoint[Z_AXIS] -= SecondDegreeTransformZ(xyzPoint[X_AXIS], xyzPoint[Y_AXIS]);
-		break;
+			case 4:
+				zCorrection += SecondDegreeTransformZ(xCoord, xyzPoint[Y_AXIS]);
+				break;
 
-	case 5:
-		xyzPoint[Z_AXIS] -= TriangleZ(xyzPoint[X_AXIS], xyzPoint[Y_AXIS]);
-		break;
+			case 5:
+				zCorrection += TriangleZ(xCoord, xyzPoint[Y_AXIS]);
+				break;
 
-	default:
-		reprap.GetPlatform()->Message(GENERIC_MESSAGE, "InverseBedTransform: wrong number of sample points.");
+			default:
+				break;
+			}
+			++numXAxes;
+		}
+	}
+	if (numXAxes > 1)
+	{
+		zCorrection /= numXAxes;			// take an average
 	}
+	xyzPoint[Z_AXIS] -= zCorrection;
 }
 
 void Move::SetIdentityTransform()
@@ -1315,18 +1361,18 @@ bool Move::IsExtruding() const
 }
 
 // Return the transformed machine coordinates
-void Move::GetCurrentUserPosition(float m[DRIVES], uint8_t moveType) const
+void Move::GetCurrentUserPosition(float m[DRIVES], uint8_t moveType, uint32_t xAxes) const
 {
 	GetCurrentMachinePosition(m, moveType == 2 || (moveType == 1 && IsDeltaMode()));
 	if (moveType == 0)
 	{
-		InverseTransform(m);
+		InverseTransform(m, xAxes);
 	}
 }
 
 // Return the current live XYZ and extruder coordinates
 // Interrupts are assumed enabled on entry, so do not call this from an ISR
-void Move::LiveCoordinates(float m[DRIVES])
+void Move::LiveCoordinates(float m[DRIVES], uint32_t xAxes)
 {
 	// The live coordinates and live endpoints are modified by the ISR, so be careful to get a self-consistent set of them
 	cpu_irq_disable();
@@ -1355,7 +1401,7 @@ void Move::LiveCoordinates(float m[DRIVES])
 		}
 		cpu_irq_enable();
 	}
-	InverseTransform(m);
+	InverseTransform(m, xAxes);
 }
 
 // These are the actual numbers that we want to be the coordinates, so don't transform them.
@@ -1494,7 +1540,6 @@ void Move::SetBedProbeGrid(const GridDefinition& newGrid)
 void Move::ClearGridHeights()
 {
 	useGridHeights = false;
-	SetIdentityTransform();
 	for (size_t i = 0; i < ARRAY_SIZE(gridHeightSet); ++i)
 	{
 		gridHeightSet[i] = 0;
@@ -1512,6 +1557,60 @@ void Move::SetGridHeight(size_t xIndex, size_t yIndex, float height)
 	}
 }
 
+// Load the height map
+bool Move::LoadHeightMapFromFile(const char *fname, StringRef& reply)
+{
+	Platform *platform = reprap.GetPlatform();
+	FileStore * const f = platform->GetFileStore(platform->GetSysDir(), fname, false);
+	bool err;
+	if (f == nullptr)
+	{
+		reply.printf("Height map file %s not found", fname);
+		err = true;
+	}
+	else
+	{
+		//TODO
+		err = grid.LoadFromFile(f);
+		f->Close();
+	}
+
+	if (err)
+	{
+		ClearGridHeights();			// make sure we don't end up with a partial height map
+	}
+	return err;
+}
+
+// Save the height map and write the success or error message to 'reply'
+// Returning true if an error occurred
+bool Move::SaveHeightMapToFile(const char *fname, StringRef& reply) const
+{
+	Platform *platform = reprap.GetPlatform();
+	FileStore * const f = platform->GetFileStore(platform->GetSysDir(), fname, true);
+	bool err;
+	if (f == nullptr)
+	{
+		reply.printf("Failed to create height map file %s", fname);
+		err = true;
+	}
+	else
+	{
+		err = grid.SaveToFile(f);
+		f->Close();
+		if (err)
+		{
+			platform->GetMassStorage()->Delete(platform->GetSysDir(), fname);
+			reply.printf("Failed to save height map to file %s", fname);
+		}
+		else
+		{
+			reply.printf("Height map saved to file %s", fname);
+		}
+	}
+	return err;
+}
+
 // Enter or leave simulation mode
 void Move::Simulate(uint8_t simMode)
 {
diff --git a/src/Movement/Move.h b/src/Movement/Move.h
index efd6f73e..401f0c1d 100644
--- a/src/Movement/Move.h
+++ b/src/Movement/Move.h
@@ -42,41 +42,41 @@ class Move
 public:
 
     Move(Platform* p, GCodes* g);
-    void Init();										// Start me up
-    void Spin();										// Called in a tight loop to keep the class going
-    void Exit();										// Shut down
-    void GetCurrentUserPosition(float m[DRIVES], uint8_t moveType) const;	// Return the position (after all queued moves have been executed) in transformed coords
-    int32_t GetEndPoint(size_t drive) const { return liveEndPoints[drive]; }	// Get the current position of a motor
-    void LiveCoordinates(float m[DRIVES]);				// Gives the last point at the end of the last complete DDA transformed to user coords
-    void Interrupt();									// The hardware's (i.e. platform's)  interrupt should call this.
-    void InterruptTime();								// Test function - not used
-    bool AllMovesAreFinished();							// Is the look-ahead ring empty?  Stops more moves being added as well.
-    void ResumeMoving();								// Allow moves to be added after a call to AllMovesAreFinished()
-    void DoLookAhead();									// Run the look-ahead procedure
-    void HitLowStop(size_t axis, DDA* hitDDA);			// What to do when a low endstop is hit
-    void HitHighStop(size_t axis, DDA* hitDDA);			// What to do when a high endstop is hit
-    void ZProbeTriggered(DDA* hitDDA);					// What to do when a the Z probe is triggered
-    void SetPositions(const float move[DRIVES]);		// Force the coordinates to be these
-    void SetLiveCoordinates(const float coords[DRIVES]); // Force the live coordinates (see above) to be these
-	void ResetExtruderPositions();						// Resets the extrusion amounts of the live coordinates
-    void SetXBedProbePoint(size_t index, float x);		// Record the X coordinate of a probe point
-    void SetYBedProbePoint(size_t index, float y);		// Record the Y coordinate of a probe point
-    void SetZBedProbePoint(size_t index, float z, bool wasXyCorrected, bool wasError);	// Record the Z coordinate of a probe point
-    float XBedProbePoint(size_t index) const;			// Get the X coordinate of a probe point
-    float YBedProbePoint(size_t index) const;			// Get the Y coordinate of a probe point
-    float ZBedProbePoint(size_t index)const ;			// Get the Z coordinate of a probe point
-    size_t NumberOfProbePoints() const;					// How many points to probe have been set?  0 if incomplete
-    size_t NumberOfXYProbePoints() const;				// How many XY coordinates of probe points have been set (Zs may not have been probed yet)
-    bool AllProbeCoordinatesSet(int index) const;		// XY, and Z all set for this one?
-    bool XYProbeCoordinatesSet(int index) const;		// Just XY set for this one?
-    void FinishedBedProbing(int sParam, StringRef& reply); // Calibrate or set the bed equation after probing
-    float SecondDegreeTransformZ(float x, float y) const; // Used for second degree bed equation
-    void SetAxisCompensation(int8_t axis, float tangent); // Set an axis-pair compensation angle
-    float AxisCompensation(int8_t axis) const;			// The tangent value
-    void SetIdentityTransform();						// Cancel the bed equation; does not reset axis angle compensation
-    void Transform(float move[]) const;					// Take a position and apply the bed and the axis-angle compensations
-    void InverseTransform(float move[]) const;			// Go from a transformed point back to user coordinates
-    void Diagnostics(MessageType mtype);				// Report useful stuff
+    void Init();													// Start me up
+    void Spin();													// Called in a tight loop to keep the class going
+    void Exit();													// Shut down
+    void GetCurrentUserPosition(float m[DRIVES], uint8_t moveType, uint32_t xAxes) const; // Return the position (after all queued moves have been executed) in transformed coords
+    int32_t GetEndPoint(size_t drive) const { return liveEndPoints[drive]; } // Get the current position of a motor
+    void LiveCoordinates(float m[DRIVES], uint32_t xAxes);			// Gives the last point at the end of the last complete DDA transformed to user coords
+    void Interrupt();												// The hardware's (i.e. platform's)  interrupt should call this.
+    void InterruptTime();											// Test function - not used
+    bool AllMovesAreFinished();										// Is the look-ahead ring empty?  Stops more moves being added as well.
+    void ResumeMoving();											// Allow moves to be added after a call to AllMovesAreFinished()
+    void DoLookAhead();												// Run the look-ahead procedure
+    void HitLowStop(size_t axis, DDA* hitDDA);						// What to do when a low endstop is hit
+    void HitHighStop(size_t axis, DDA* hitDDA);						// What to do when a high endstop is hit
+    void ZProbeTriggered(DDA* hitDDA);								// What to do when a the Z probe is triggered
+    void SetPositions(const float move[DRIVES]);					// Force the coordinates to be these
+    void SetLiveCoordinates(const float coords[DRIVES]);			// Force the live coordinates (see above) to be these
+	void ResetExtruderPositions();									// Resets the extrusion amounts of the live coordinates
+    void SetXBedProbePoint(size_t index, float x);					// Record the X coordinate of a probe point
+    void SetYBedProbePoint(size_t index, float y);					// Record the Y coordinate of a probe point
+    void SetZBedProbePoint(size_t index, float z, bool wasXyCorrected, bool wasError); // Record the Z coordinate of a probe point
+    float XBedProbePoint(size_t index) const;						// Get the X coordinate of a probe point
+    float YBedProbePoint(size_t index) const;						// Get the Y coordinate of a probe point
+    float ZBedProbePoint(size_t index) const;						// Get the Z coordinate of a probe point
+    size_t NumberOfProbePoints() const;								// How many points to probe have been set?  0 if incomplete
+    size_t NumberOfXYProbePoints() const;							// How many XY coordinates of probe points have been set (Zs may not have been probed yet)
+    bool AllProbeCoordinatesSet(int index) const;					// XY, and Z all set for this one?
+    bool XYProbeCoordinatesSet(int index) const;					// Just XY set for this one?
+    void FinishedBedProbing(int sParam, StringRef& reply);			// Calibrate or set the bed equation after probing
+    float SecondDegreeTransformZ(float x, float y) const;			// Used for second degree bed equation
+    void SetAxisCompensation(int8_t axis, float tangent);			// Set an axis-pair compensation angle
+    float AxisCompensation(int8_t axis) const;						// The tangent value
+    void SetIdentityTransform();									// Cancel the bed equation; does not reset axis angle compensation
+    void Transform(float move[], uint32_t xAxes) const;				// Take a position and apply the bed and the axis-angle compensations
+    void InverseTransform(float move[], uint32_t xAxes) const;		// Go from a transformed point back to user coordinates
+    void Diagnostics(MessageType mtype);							// Report useful stuff
 
     const DeltaParameters& GetDeltaParams() const { return deltaParams; }
     DeltaParameters& AccessDeltaParams() { return deltaParams; }
@@ -91,7 +91,7 @@ public:
 
     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[MAX_AXES], int32_t motorPos[MAX_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;
@@ -103,7 +103,7 @@ public:
     float GetSimulationTime() const { return simulationTime; }						// Get the accumulated simulation time
     void PrintCurrentDda() const;													// For debugging
 
-    FilePosition PausePrint(float positions[DRIVES], float& pausedFeedRate);		// Pause the print as soon as we can
+    FilePosition PausePrint(float positions[DRIVES], float& pausedFeedRate, uint32_t xAxes); // Pause the print as soon as we can
     bool NoLiveMovement() const;													// Is a move running, or are there any queued?
 
     int DoDeltaProbe(float frequency, float amplitude, float rate, float distance);
@@ -121,82 +121,84 @@ public:
 	void ClearGridHeights();														// Clear all grid height corrections
 	void SetGridHeight(size_t xIndex, size_t yIndex, float height);					// Set the height of a grid point
 	void UseHeightMap() { useGridHeights = true; }									// Start using the height map
+	bool LoadHeightMapFromFile(const char *fname, StringRef& reply);				// Load the height map and *append* any error message to 'reply'
+	bool SaveHeightMapToFile(const char *fname, StringRef& reply) const;			// Save the height map
 
 private:
 
-    enum class IdleState : uint8_t { idle, busy, timing };
-
-    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[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 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<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
-    bool DDARingEmpty() const;							// Anything there?
-
-    DDA* volatile currentDda;
-    DDA* ddaRingAddPointer;
-    DDA* volatile ddaRingGetPointer;
-    DDA* ddaRingCheckPointer;
-
-    bool addNoMoreMoves;								// If true, allow no more moves to be added to the look-ahead
-    bool active;										// Are we live and running?
-    uint8_t simulationMode;								// Are we simulating, or really printing?
-    bool waitingForMove;								// True if we are waiting for a new move
-    unsigned int numLookaheadUnderruns;					// How many times we have run out of moves to adjust during lookahead
-    unsigned int numPrepareUnderruns;					// How many times we wanted a new move but there were only un-prepared moves in the queue
-    unsigned int idleCount;								// The number of times Spin was called and had no new moves to process
-    uint32_t longestGcodeWaitInterval;					// the longest we had to wait for a new gcode
-    uint32_t gcodeWaitStartTime;						// When we last asked for a gcode and didn't get one
-    float simulationTime;								// Print time since we started simulating
-    volatile float liveCoordinates[DRIVES];				// The endpoint that the machine moved to in the last completed move
-    volatile bool liveCoordinatesValid;					// True if the XYZ live coordinates are reliable (the extruder ones always are)
-    volatile int32_t liveEndPoints[DRIVES];				// The XYZ endpoints of the last completed move in motor coordinates
-
-    // Variable for G32 bed probing, for bed compensation and delta calibration
-    float xBedProbePoints[MaxProbePoints];				// The X coordinates of the points on the bed at which to probe
-    float yBedProbePoints[MaxProbePoints];				// The Y coordinates of the points on the bed at which to probe
-    float zBedProbePoints[MaxGridProbePoints];			// The Z coordinates of the points on the bed that were probed
-    float baryXBedProbePoints[5];						// The X coordinates of the triangle corner points
-    float baryYBedProbePoints[5];						// The Y coordinates of the triangle corner points
-    float baryZBedProbePoints[5];						// The Z coordinates of the triangle corner points
-    uint8_t probePointSet[MaxProbePoints];				// Has the XY of this point been set?  Has the Z been probed?
-    float aX, aY, aC; 									// Bed plane explicit equation z' = z + aX*x + aY*y + aC
-    float tanXY, tanYZ, tanXZ; 							// Axis compensation - 90 degrees + angle gives angle between axes
-    int numBedCompensationPoints;						// The number of points we are actually using for bed compensation, 0 means identity bed transform
-    float xRectangle, yRectangle;						// The side lengths of the rectangle used for second-degree bed compensation
-
-    GridDefinition grid;    							// Grid definition for G29 bed probing. The probe heights are stored in zBedProbePoints, see above.
-    uint32_t gridHeightSet[MaxGridProbePoints/32];		// Bitmap of which points have been probed
-    bool useGridHeights;								// True if the zBedProbePoints came from valid bed probing and relate to the current grid
-
-    float idleTimeout;									// How long we wait with no activity before we reduce motor currents to idle
-    float lastMoveTime;									// The approximate time at which the last move was completed, or 0
-    float longWait;										// A long time for things that need to be done occasionally
-    IdleState iState;									// whether the idle timer is active
-
-    DeltaParameters deltaParams;						// Information about the delta parameters of this machine
-    DeltaProbe deltaProbe;								// Delta probing state
-    uint32_t deltaProbingStartTime;
-    bool deltaProbing;
-    int coreXYMode;										// 0 = Cartesian, 1 = CoreXY, 2 = CoreXZ, 3 = CoreYZ
-    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
+	enum class IdleState : uint8_t { idle, busy, timing };
+
+	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[MAX_AXES], uint32_t xAxes) 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], uint32_t xAxes) 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 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<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
+	bool DDARingEmpty() const;							// Anything there?
+
+	DDA* volatile currentDda;
+	DDA* ddaRingAddPointer;
+	DDA* volatile ddaRingGetPointer;
+	DDA* ddaRingCheckPointer;
+
+	bool addNoMoreMoves;								// If true, allow no more moves to be added to the look-ahead
+	bool active;										// Are we live and running?
+	uint8_t simulationMode;								// Are we simulating, or really printing?
+	bool waitingForMove;								// True if we are waiting for a new move
+	unsigned int numLookaheadUnderruns;					// How many times we have run out of moves to adjust during lookahead
+	unsigned int numPrepareUnderruns;					// How many times we wanted a new move but there were only un-prepared moves in the queue
+	unsigned int idleCount;								// The number of times Spin was called and had no new moves to process
+	uint32_t longestGcodeWaitInterval;					// the longest we had to wait for a new gcode
+	uint32_t gcodeWaitStartTime;						// When we last asked for a gcode and didn't get one
+	float simulationTime;								// Print time since we started simulating
+	volatile float liveCoordinates[DRIVES];				// The endpoint that the machine moved to in the last completed move
+	volatile bool liveCoordinatesValid;					// True if the XYZ live coordinates are reliable (the extruder ones always are)
+	volatile int32_t liveEndPoints[DRIVES];				// The XYZ endpoints of the last completed move in motor coordinates
+
+	// Variable for G32 bed probing, for bed compensation and delta calibration
+	float xBedProbePoints[MaxProbePoints];				// The X coordinates of the points on the bed at which to probe
+	float yBedProbePoints[MaxProbePoints];				// The Y coordinates of the points on the bed at which to probe
+	float zBedProbePoints[MaxGridProbePoints];			// The Z coordinates of the points on the bed that were probed
+	float baryXBedProbePoints[5];						// The X coordinates of the triangle corner points
+	float baryYBedProbePoints[5];						// The Y coordinates of the triangle corner points
+	float baryZBedProbePoints[5];						// The Z coordinates of the triangle corner points
+	uint8_t probePointSet[MaxProbePoints];				// Has the XY of this point been set?  Has the Z been probed?
+	float aX, aY, aC; 									// Bed plane explicit equation z' = z + aX*x + aY*y + aC
+	float tanXY, tanYZ, tanXZ; 							// Axis compensation - 90 degrees + angle gives angle between axes
+	int numBedCompensationPoints;						// The number of points we are actually using for bed compensation, 0 means identity bed transform
+	float xRectangle, yRectangle;						// The side lengths of the rectangle used for second-degree bed compensation
+
+	GridDefinition grid;    							// Grid definition for G29 bed probing. The probe heights are stored in zBedProbePoints, see above.
+	uint32_t gridHeightSet[MaxGridProbePoints/32];		// Bitmap of which points have been probed
+	bool useGridHeights;								// True if the zBedProbePoints came from valid bed probing and relate to the current grid
+
+	float idleTimeout;									// How long we wait with no activity before we reduce motor currents to idle
+	float lastMoveTime;									// The approximate time at which the last move was completed, or 0
+	float longWait;										// A long time for things that need to be done occasionally
+	IdleState iState;									// whether the idle timer is active
+
+	DeltaParameters deltaParams;						// Information about the delta parameters of this machine
+	DeltaProbe deltaProbe;								// Delta probing state
+	uint32_t deltaProbingStartTime;
+	bool deltaProbing;
+	int coreXYMode;										// 0 = Cartesian, 1 = CoreXY, 2 = CoreXZ, 3 = CoreYZ
+	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/PrintMonitor.cpp b/src/PrintMonitor.cpp
index 6901e555..b2181ee4 100644
--- a/src/PrintMonitor.cpp
+++ b/src/PrintMonitor.cpp
@@ -110,7 +110,7 @@ void PrintMonitor::Spin()
 		else if (!gCodes->DoingFileMacro() && reprap.GetMove()->IsExtruding())
 		{
 			float liveCoordinates[DRIVES];
-			reprap.GetMove()->LiveCoordinates(liveCoordinates);
+			reprap.GetMove()->LiveCoordinates(liveCoordinates, reprap.GetCurrentXAxes());
 
 			// See if we need to determine the first layer height (usually smaller than the nozzle diameter)
 			if (printingFileInfo.firstLayerHeight == 0.0)
diff --git a/src/Reprap.cpp b/src/Reprap.cpp
index a7df8e3f..d3cd3288 100644
--- a/src/Reprap.cpp
+++ b/src/Reprap.cpp
@@ -507,7 +507,7 @@ OutputBuffer *RepRap::GetStatusResponse(uint8_t type, ResponseSource source)
 		else
 #endif
 		{
-			move->LiveCoordinates(liveCoordinates);
+			move->LiveCoordinates(liveCoordinates, GetCurrentXAxes());
 		}
 
 		if (currentTool != nullptr)
@@ -807,14 +807,22 @@ OutputBuffer *RepRap::GetStatusResponse(uint8_t type, ResponseSource source)
 					}
 				}
 
-				// Axis mapping. Currently we only map the X axis, but we return an array of arrays to allow for mapping otyher axes in future.
+				// Axis mapping. Currently we only map the X axis, but we return an array of arrays to allow for mapping other axes in future.
 				response->cat("],\"axisMap\":[[");
-				for(size_t xi = 0; xi < tool->GetAxisMapCount(); ++xi)
+				bool first = true;
+				for (size_t xi = 0; xi < MAX_AXES; ++xi)
 				{
-					response->catf("%d", tool->GetAxisMap()[xi]);
-					if (xi + 1 < tool->GetAxisMapCount())
+					if ((tool->GetXAxisMap() & (1u << xi)) != 0)
 					{
-						response->cat(",");
+						if (first)
+						{
+							first = false;
+						}
+						else
+						{
+							response->cat(",");
+						}
+						response->catf("%u", xi);
 					}
 				}
 
@@ -1108,7 +1116,7 @@ OutputBuffer *RepRap::GetLegacyStatusResponse(uint8_t type, int seq)
 	// Send XYZ positions
 	const size_t numAxes = reprap.GetGCodes()->GetNumAxes();
 	float liveCoordinates[DRIVES];
-	reprap.GetMove()->LiveCoordinates(liveCoordinates);
+	reprap.GetMove()->LiveCoordinates(liveCoordinates, GetCurrentXAxes());
 	const Tool* currentTool = reprap.GetCurrentTool();
 	if (currentTool != nullptr)
 	{
@@ -1521,4 +1529,10 @@ void RepRap::ClearTemperatureFault(int8_t wasDudHeater)
 	}
 }
 
+// Get the current axes used as X axes
+uint32_t RepRap::GetCurrentXAxes() const
+{
+	return (currentTool == nullptr) ? DefaultXAxisMapping : currentTool->GetXAxisMap();
+}
+
 // End
diff --git a/src/Reprap.h b/src/Reprap.h
index 79c94bce..f7408bdd 100644
--- a/src/Reprap.h
+++ b/src/Reprap.h
@@ -59,6 +59,7 @@ public:
     Tool* GetCurrentTool() const;
     Tool* GetTool(int toolNumber) const;
     Tool* GetOnlyTool() const;
+	uint32_t GetCurrentXAxes() const;									// Get the current axes used as X axes
     //Tool* GetToolByDrive(int driveNumber);
     void SetToolVariables(int toolNumber, const float* standbyTemperatures, const float* activeTemperatures);
 	bool ToolWarningsAllowed();
diff --git a/src/Tool.cpp b/src/Tool.cpp
index a7abb201..5c6dd0be 100644
--- a/src/Tool.cpp
+++ b/src/Tool.cpp
@@ -27,7 +27,7 @@
 
 Tool * Tool::freelist = nullptr;
 
-/*static*/Tool * Tool::Create(int toolNumber, long d[], size_t dCount, long h[], size_t hCount, long xMap[], size_t xCount, uint32_t fanMap)
+/*static*/Tool * Tool::Create(int toolNumber, long d[], size_t dCount, long h[], size_t hCount, uint32_t xMap, uint32_t fanMap)
 {
 	const size_t numExtruders = reprap.GetGCodes()->GetNumExtruders();
 	if (dCount > ARRAY_SIZE(Tool::drives))
@@ -76,11 +76,12 @@ Tool * Tool::freelist = nullptr;
 	t->active = false;
 	t->driveCount = dCount;
 	t->heaterCount = hCount;
-	t->xmapCount = xCount;
+	t->xMapping = xMap;
 	t->fanMapping = fanMap;
 	t->heaterFault = false;
 	t->mixing = false;
 	t->displayColdExtrudeWarning = false;
+	t->virtualExtruderPosition = 0.0;
 
 	for (size_t axis = 0; axis < MAX_AXES; axis++)
 	{
@@ -104,11 +105,6 @@ Tool * Tool::freelist = nullptr;
 		t->standbyTemperatures[heater] = ABS_ZERO;
 	}
 
-	for (size_t xi = 0; xi < t->xmapCount; ++xi)
-	{
-		t->xMapping[xi] = xMap[xi];
-	}
-
 	return t;
 }
 
@@ -141,10 +137,13 @@ void Tool::Print(StringRef& reply)
 
 	reply.cat("; xmap:");
 	sep = ' ';
-	for (size_t xi = 0; xi < xmapCount; ++xi)
+	for (size_t xi = 0; xi < MAX_AXES; ++xi)
 	{
-		reply.catf("%c%c", sep, GCodes::axisLetters[xi]);
-		sep = ',';
+		if ((xMapping & (1u << xi)) != 0)
+		{
+			reply.catf("%c%c", sep, GCodes::axisLetters[xi]);
+			sep = ',';
+		}
 	}
 
 	reply.cat("; fans:");
diff --git a/src/Tool.h b/src/Tool.h
index 259cb6d6..042285fd 100644
--- a/src/Tool.h
+++ b/src/Tool.h
@@ -26,11 +26,13 @@ Licence: GPL
 #ifndef TOOL_H_
 #define TOOL_H_
 
+const uint32_t DefaultXAxisMapping = 0x0001;	// by default, X is mapped to X
+
 class Tool
 {
 public:
 
-	static Tool * Create(int toolNumber, long d[], size_t dCount, long h[], size_t hCount, long xMap[], size_t xCount, uint32_t fanMap);
+	static Tool * Create(int toolNumber, long d[], size_t dCount, long h[], size_t hCount, uint32_t xMap, uint32_t fanMap);
 	static void Delete(Tool *t);
 
 	const float *GetOffset() const;
@@ -50,10 +52,11 @@ public:
 	float MaxFeedrate() const;
 	float InstantDv() const;
 	void Print(StringRef& reply);
-	size_t GetAxisMapCount() const { return xmapCount; }
-	const int *GetAxisMap() const { return xMapping; }
+	uint32_t GetXAxisMap() const { return xMapping; }
 	uint32_t GetFanMapping() const { return fanMapping; }
 
+	float virtualExtruderPosition;
+
 	friend class RepRap;
 
 protected:
@@ -81,11 +84,10 @@ private:
 	float activeTemperatures[HEATERS];
 	float standbyTemperatures[HEATERS];
 	size_t heaterCount;
-	int xMapping[MAX_AXES];
-	size_t xmapCount;
-	Tool* next;
 	float offset[MAX_AXES];
+	uint32_t xMapping;
 	uint32_t fanMapping;
+	Tool* next;
 
 	bool mixing;
 	bool active;