Bug fixes

Bed compensation is not computed at the XY position after adding the tool offset
Fixed pause/resume during arc moves
Fixed M579 applied to arc moves

16 files changed, 211 insertions, 160 deletions

diff --git a/src/GCodes/GCodes.cpp b/src/GCodes/GCodes.cpp
index 742e745f..68a10c6f 100644
--- a/src/GCodes/GCodes.cpp
+++ b/src/GCodes/GCodes.cpp
@@ -73,8 +73,7 @@ void GCodes::RawMove::SetDefaults(size_t firstDriveToZero)
 	hasExtrusion = false;
 	endStopsToCheck = 0;
 	filePos = noFilePosition;
-	xAxes = DefaultXAxisMapping;
-	yAxes = DefaultYAxisMapping;
+	tool = nullptr;
 	for (size_t drive = firstDriveToZero; drive < MaxTotalDrivers; ++drive)
 	{
 		coords[drive] = 0.0;			// clear extrusion
@@ -253,8 +252,7 @@ void GCodes::Reset()
 
 	currentZHop = 0.0;									// clear this before calling ToolOffsetInverseTransform
 	lastPrintingMoveHeight = -1.0;
-	moveBuffer.xAxes = DefaultXAxisMapping;
-	moveBuffer.yAxes = DefaultYAxisMapping;
+	moveBuffer.tool = nullptr;
 	moveBuffer.virtualExtruderPosition = 0.0;
 
 #if SUPPORT_LASER || SUPPORT_IOBITS
@@ -837,6 +835,8 @@ void GCodes::RunStateMachine(GCodeBuffer& gb, const StringRef& reply)
 			moveBuffer.virtualExtruderPosition = pauseRestorePoint.virtualExtruderPosition;
 			fileGCode->MachineState().feedRate = pauseRestorePoint.feedRate;
 			moveFractionToSkip = pauseRestorePoint.proportionDone;
+			restartInitialUserX = pauseRestorePoint.initialUserX;
+			restartInitialUserY = pauseRestorePoint.initialUserY;
 			isPaused = false;
 			reply.copy("Printing resumed");
 			platform.Message(LogMessage, "Printing resumed\n");
@@ -1467,10 +1467,9 @@ void GCodes::RunStateMachine(GCodeBuffer& gb, const StringRef& reply)
 		// We just did the retraction part of a firmware retraction, now we need to do the Z hop
 		if (segmentsLeft == 0)
 		{
-			const AxesBitmap xAxes = reprap.GetCurrentXAxes();
-			const AxesBitmap yAxes = reprap.GetCurrentYAxes();
-			reprap.GetMove().GetCurrentUserPosition(moveBuffer.coords, 0, xAxes, yAxes);
 			SetMoveBufferDefaults();
+			moveBuffer.tool = reprap.GetCurrentTool();
+			reprap.GetMove().GetCurrentUserPosition(moveBuffer.coords, 0, moveBuffer.tool);
 			moveBuffer.coords[Z_AXIS] += retractHop;
 			moveBuffer.feedRate = platform.MaxFeedrate(Z_AXIS);
 			moveBuffer.filePos = (&gb == fileGCode) ? gb.GetFilePosition(fileInput->BytesCached()) : noFilePosition;
@@ -1488,10 +1487,8 @@ void GCodes::RunStateMachine(GCodeBuffer& gb, const StringRef& reply)
 			const Tool * const tool = reprap.GetCurrentTool();
 			if (tool != nullptr)
 			{
-				const uint32_t xAxes = reprap.GetCurrentXAxes();
-				const uint32_t yAxes = reprap.GetCurrentYAxes();
-				reprap.GetMove().GetCurrentUserPosition(moveBuffer.coords, 0, xAxes, yAxes);
 				SetMoveBufferDefaults();
+				reprap.GetMove().GetCurrentUserPosition(moveBuffer.coords, 0, tool);
 				for (size_t i = 0; i < tool->DriveCount(); ++i)
 				{
 					moveBuffer.coords[numTotalAxes + tool->Drive(i)] = retractLength + retractExtra;
@@ -1861,6 +1858,8 @@ void GCodes::DoPause(GCodeBuffer& gb, PauseReason reason, const char *msg)
 			pauseRestorePoint.filePos = moveBuffer.filePos;
 			pauseRestorePoint.feedRate = moveBuffer.feedRate;
 			pauseRestorePoint.proportionDone = (float)(totalSegments - segmentsLeft)/(float)totalSegments;
+			pauseRestorePoint.initialUserX = moveBuffer.initialUserX;
+			pauseRestorePoint.initialUserY = moveBuffer.initialUserY;
 			ToolOffsetInverseTransform(pauseRestorePoint.moveCoords, currentUserPosition);	// transform the returned coordinates to user coordinates
 			ClearMove();
 		}
@@ -2024,6 +2023,8 @@ bool GCodes::DoEmergencyPause()
 		pauseRestorePoint.virtualExtruderPosition = moveBuffer.virtualExtruderPosition;
 		pauseRestorePoint.filePos = moveBuffer.filePos;
 		pauseRestorePoint.proportionDone = (float)(totalSegments - segmentsLeft)/(float)totalSegments;
+		pauseRestorePoint.initialUserX = moveBuffer.initialUserX;
+		pauseRestorePoint.initialUserY = moveBuffer.initialUserY;
 #if SUPPORT_LASER || SUPPORT_IOBITS
 		pauseRestorePoint.laserPwmOrIoBits = moveBuffer.laserPwmOrIoBits;
 #endif
@@ -2300,8 +2301,13 @@ void GCodes::SaveResumeInfo(bool wasPowerFailure)
 			}
 			if (ok)
 			{
-				buf.printf("M23 \"%s\"\nM26 S%" PRIu32 " P%.3f\n", printingFilename, pauseRestorePoint.filePos, (double)pauseRestorePoint.proportionDone);
-				ok = f->Write(buf.c_str());									// write filename and file position
+				buf.printf("M23 \"%s\"\nM26 S%" PRIu32, printingFilename, pauseRestorePoint.filePos);
+				if (pauseRestorePoint.proportionDone > 0.0)
+				{
+					buf.catf(" P%.3f X%.3f Y%.3f", (double)pauseRestorePoint.proportionDone, (double)pauseRestorePoint.initialUserX, (double)pauseRestorePoint.initialUserY);
+				}
+				buf.cat('\n');
+				ok = f->Write(buf.c_str());									// write filename and file position, and if necessary proportion done and initial XY position
 			}
 			if (ok)
 			{
@@ -2590,25 +2596,34 @@ bool GCodes::CheckEnoughAxesHomed(AxesBitmap axesMoved)
 // We have already acquired the movement lock and waited for the previous move to be taken.
 const char* GCodes::DoStraightMove(GCodeBuffer& gb, bool isCoordinated)
 {
+	if (moveFractionToSkip > 0.0)
+	{
+		moveBuffer.initialUserX = restartInitialUserX;
+		moveBuffer.initialUserY = restartInitialUserY;
+	}
+	else
+	{
+		moveBuffer.initialUserX = currentUserPosition[X_AXIS];
+		moveBuffer.initialUserY = currentUserPosition[Y_AXIS];
+	}
+
 	// Set up default move parameters
 	moveBuffer.isCoordinated = isCoordinated;
 	moveBuffer.endStopsToCheck = 0;
 	moveBuffer.moveType = 0;
-	moveBuffer.xAxes = reprap.GetCurrentXAxes();
-	moveBuffer.yAxes = reprap.GetCurrentYAxes();
+	moveBuffer.tool = reprap.GetCurrentTool();
 	moveBuffer.usePressureAdvance = false;
 	axesToSenseLength = 0;
 
 	// Check to see if the move is a 'homing' move that endstops are checked on.
-	// We handle S1 parameters affecting extrusion elsewhere.
+	// We handle H1 parameters affecting extrusion elsewhere.
 	if (gb.Seen('H') || (machineType != MachineType::laser && gb.Seen('S')))
 	{
 		const int ival = gb.GetIValue();
 		if (ival >= 1 && ival <= 3)
 		{
 			moveBuffer.moveType = ival;
-			moveBuffer.xAxes = DefaultXAxisMapping;
-			moveBuffer.yAxes = DefaultYAxisMapping;
+			moveBuffer.tool = nullptr;
 		}
 	}
 
@@ -2646,7 +2661,8 @@ const char* GCodes::DoStraightMove(GCodeBuffer& gb, bool isCoordinated)
 		}
 		else if (laserPowerSticky)
 		{
-			// leave the laser PWM alone because this is what LaserWeb expects
+			// Leave the laser PWM alone because this is what LaserWeb expects. If it is an uncoordinated move then the motion system will turn the laser off.
+			// This means that after a G0 move, the next G1 move will default to the same power as the previous G1 move, as LightBurn expects.
 		}
 		else
 		{
@@ -2675,7 +2691,7 @@ const char* GCodes::DoStraightMove(GCodeBuffer& gb, bool isCoordinated)
 		// This may be a raw motor move, in which case we need the current raw motor positions in moveBuffer.coords.
 		// If it isn't a raw motor move, it will still be applied without axis or bed transform applied,
 		// so make sure the initial coordinates don't have those either to avoid unwanted Z movement.
-		reprap.GetMove().GetCurrentUserPosition(moveBuffer.coords, moveBuffer.moveType, reprap.GetCurrentXAxes(), reprap.GetCurrentYAxes());
+		reprap.GetMove().GetCurrentUserPosition(moveBuffer.coords, moveBuffer.moveType, reprap.GetCurrentTool());
 	}
 
 	// Set up the initial coordinates
@@ -2702,7 +2718,7 @@ const char* GCodes::DoStraightMove(GCodeBuffer& gb, bool isCoordinated)
 				// Special moves update the move buffer directly, bypassing the user coordinates
 				if (gb.MachineState().axesRelative)
 				{
-					moveBuffer.coords[axis] += moveArg;
+					moveBuffer.coords[axis] += moveArg * (1.0 - moveFractionToSkip);
 				}
 				else
 				{
@@ -2718,7 +2734,7 @@ const char* GCodes::DoStraightMove(GCodeBuffer& gb, bool isCoordinated)
 			}
 			else if (gb.MachineState().axesRelative)
 			{
-				currentUserPosition[axis] += moveArg;
+				currentUserPosition[axis] += moveArg * (1.0 - moveFractionToSkip);
 			}
 			else if (gb.MachineState().g53Active)
 			{
@@ -2876,6 +2892,17 @@ const char* GCodes::DoStraightMove(GCodeBuffer& gb, bool isCoordinated)
 // Currently, we do not process new babystepping when executing an arc move
 const char* GCodes::DoArcMove(GCodeBuffer& gb, bool clockwise)
 {
+	if (moveFractionToSkip > 0.0)
+	{
+		moveBuffer.initialUserX = restartInitialUserX;
+		moveBuffer.initialUserY = restartInitialUserY;
+	}
+	else
+	{
+		moveBuffer.initialUserX = currentUserPosition[X_AXIS];
+		moveBuffer.initialUserY = currentUserPosition[Y_AXIS];
+	}
+
 	// Get the axis parameters
 	float newX, newY;
 	if (gb.Seen('X'))
@@ -2883,7 +2910,7 @@ const char* GCodes::DoArcMove(GCodeBuffer& gb, bool clockwise)
 		newX = gb.GetDistance();
 		if (gb.MachineState().axesRelative)
 		{
-			newX += currentUserPosition[X_AXIS];
+			newX += moveBuffer.initialUserX;
 		}
 		else if (gb.MachineState().g53Active)
 		{
@@ -2896,7 +2923,7 @@ const char* GCodes::DoArcMove(GCodeBuffer& gb, bool clockwise)
 	}
 	else
 	{
-		newX = currentUserPosition[X_AXIS];
+		newX = moveBuffer.initialUserX;
 	}
 
 	if (gb.Seen('Y'))
@@ -2904,7 +2931,7 @@ const char* GCodes::DoArcMove(GCodeBuffer& gb, bool clockwise)
 		newY = gb.GetDistance();
 		if (gb.MachineState().axesRelative)
 		{
-			newY += currentUserPosition[Y_AXIS];
+			newY += moveBuffer.initialUserY;
 		}
 		else if (gb.MachineState().g53Active)
 		{
@@ -2917,7 +2944,7 @@ const char* GCodes::DoArcMove(GCodeBuffer& gb, bool clockwise)
 	}
 	else
 	{
-		newY = currentUserPosition[Y_AXIS];
+		newY = moveBuffer.initialUserY;
 	}
 
 	float iParam, jParam;
@@ -2927,8 +2954,8 @@ const char* GCodes::DoArcMove(GCodeBuffer& gb, bool clockwise)
 		const float rParam = gb.GetDistance();
 
 		// Get the XY coordinates of the midpoints between the start and end points X and Y distances between start and end points
-		const float deltaX = newX - currentUserPosition[X_AXIS];
-		const float deltaY = newY - currentUserPosition[Y_AXIS];
+		const float deltaX = newX - moveBuffer.initialUserX;
+		const float deltaY = newY - moveBuffer.initialUserY;
 
 		const float dSquared = fsquare(deltaX) + fsquare(deltaY);	// square of the distance between start and end points
 		const float hSquared = fsquare(rParam) - dSquared/4;		// square of the length of the perpendicular from the mid point to the arc centre
@@ -2976,17 +3003,16 @@ const char* GCodes::DoArcMove(GCodeBuffer& gb, bool clockwise)
 	memcpy(moveBuffer.initialCoords, moveBuffer.coords, numVisibleAxes * sizeof(moveBuffer.initialCoords[0]));
 
 	// Save the arc centre user coordinates for later
-	const float userArcCentreX = currentUserPosition[X_AXIS] + iParam;
-	const float userArcCentreY = currentUserPosition[Y_AXIS] + jParam;
+	const float userArcCentreX = moveBuffer.initialUserX + iParam;
+	const float userArcCentreY = moveBuffer.initialUserY + jParam;
 
 	// Work out the new user position
-	const float initialX = currentUserPosition[X_AXIS], initialY = currentUserPosition[Y_AXIS];
 	currentUserPosition[X_AXIS] = newX;
 	currentUserPosition[Y_AXIS] = newY;
 
 	// CNC machines usually do a full circle if the initial and final XY coordinates are the same.
 	// Usually this is because X and Y were not given, but repeating the coordinates is permitted.
-	const bool wholeCircle = (initialX == currentUserPosition[X_AXIS] && initialY == currentUserPosition[Y_AXIS]);
+	const bool wholeCircle = (moveBuffer.initialUserX == currentUserPosition[X_AXIS] && moveBuffer.initialUserY == currentUserPosition[Y_AXIS]);
 
 	// Get any additional axes
 	AxesBitmap axesMentioned = MakeBitmap<AxesBitmap>(X_AXIS) | MakeBitmap<AxesBitmap>(Y_AXIS);
@@ -2997,7 +3023,7 @@ const char* GCodes::DoArcMove(GCodeBuffer& gb, bool clockwise)
 			const float moveArg = gb.GetDistance();
 			if (gb.MachineState().axesRelative)
 			{
-				currentUserPosition[axis] += moveArg;
+				currentUserPosition[axis] += moveArg * (1.0 - moveFractionToSkip);
 			}
 			else if (gb.MachineState().g53Active)
 			{
@@ -3035,22 +3061,23 @@ const char* GCodes::DoArcMove(GCodeBuffer& gb, bool clockwise)
 	// Set up default move parameters
 	moveBuffer.endStopsToCheck = 0;
 	moveBuffer.moveType = 0;
-	moveBuffer.xAxes = reprap.GetCurrentXAxes();
-	moveBuffer.yAxes = reprap.GetCurrentYAxes();
+	moveBuffer.tool = reprap.GetCurrentTool();
 	moveBuffer.isCoordinated = true;
 
 	// Set up the arc centre coordinates and record which axes behave like an X axis.
 	// The I and J parameters are always relative to present position.
 	// For X and Y we need to set up the arc centre for each axis that X or Y is mapped to.
+	const AxesBitmap xAxes = reprap.GetCurrentXAxes();
+	const AxesBitmap yAxes = reprap.GetCurrentYAxes();
 	for (size_t axis = 0; axis < numVisibleAxes; ++axis)
 	{
-		if (IsBitSet(moveBuffer.xAxes, axis))
+		if (IsBitSet(xAxes, axis))
 		{
-			arcCentre[axis] = moveBuffer.initialCoords[axis] + iParam;
+			arcCentre[axis] = (userArcCentreX * axisScaleFactors[axis]) + currentBabyStepOffsets[axis] - Tool::GetOffset(reprap.GetCurrentTool(), X_AXIS);
 		}
-		else if (IsBitSet(moveBuffer.yAxes, axis))
+		else if (IsBitSet(yAxes, axis))
 		{
-			arcCentre[axis] = moveBuffer.initialCoords[axis] + jParam;
+			arcCentre[axis] = (userArcCentreY * axisScaleFactors[axis]) + currentBabyStepOffsets[axis] - Tool::GetOffset(reprap.GetCurrentTool(), Y_AXIS);
 		}
 	}
 
@@ -3110,7 +3137,7 @@ const char* GCodes::DoArcMove(GCodeBuffer& gb, bool clockwise)
 		}
 	}
 
-	// Compute how many segments we need to move
+	// Compute how many segments to use
 	// For the arc to deviate up to MaxArcDeviation from the ideal, the segment length should be sqrt(8 * arcRadius * MaxArcDeviation + fsquare(MaxArcDeviation))
 	// We leave out the square term because it is very small
 	// In CNC applications even very small deviations can be visible, so we use a smaller segment length at low speeds
@@ -3191,7 +3218,7 @@ bool GCodes::ReadMove(RawMove& m)
 				m.coords[drive] *= (1.0 - firstSegmentFractionToSkip);
 			}
 		}
-		m.proportionLeft = 0.0;
+		m.proportionDone = 1.0;
 		if (doingArcMove)
 		{
 			m.canPauseAfter = true;			// we can pause after the final segment of an arc move
@@ -3209,15 +3236,15 @@ bool GCodes::ReadMove(RawMove& m)
 
 		for (size_t drive = 0; drive < numVisibleAxes; ++drive)
 		{
-			if (doingArcMove && drive != Z_AXIS && IsBitSet(moveBuffer.yAxes, drive))
+			if (doingArcMove && drive != Z_AXIS && IsBitSet(Tool::GetYAxes(moveBuffer.tool), drive))
 			{
 				// Y axis or a substitute Y axis
-				moveBuffer.initialCoords[drive] = arcCentre[drive] + arcRadius * sinf(arcCurrentAngle);
+				moveBuffer.initialCoords[drive] = arcCentre[drive] + arcRadius * axisScaleFactors[drive] * sinf(arcCurrentAngle);
 			}
-			else if (doingArcMove && drive != Z_AXIS && IsBitSet(moveBuffer.xAxes, drive))
+			else if (doingArcMove && drive != Z_AXIS && IsBitSet(Tool::GetXAxes(moveBuffer.tool), drive))
 			{
 				// X axis or a substitute X axis
-				moveBuffer.initialCoords[drive] = arcCentre[drive] + arcRadius * cosf(arcCurrentAngle);
+				moveBuffer.initialCoords[drive] = arcCentre[drive] + arcRadius * axisScaleFactors[drive] * cosf(arcCurrentAngle);
 			}
 			else
 			{
@@ -3253,7 +3280,7 @@ bool GCodes::ReadMove(RawMove& m)
 		}
 		--segmentsLeft;
 
-		m.proportionLeft = (float)segmentsLeft/(float)totalSegments;
+		m.proportionDone = (float)(totalSegments - segmentsLeft)/(float)totalSegments;
 	}
 
 	return true;
@@ -3601,7 +3628,7 @@ GCodeResult GCodes::SaveHeightMap(GCodeBuffer& gb, const StringRef& reply) const
 void GCodes::ClearBedMapping()
 {
 	reprap.GetMove().SetIdentityTransform();
-	reprap.GetMove().GetCurrentUserPosition(moveBuffer.coords, 0, reprap.GetCurrentXAxes(), reprap.GetCurrentYAxes());
+	reprap.GetMove().GetCurrentUserPosition(moveBuffer.coords, 0, reprap.GetCurrentTool());
 	ToolOffsetInverseTransform(moveBuffer.coords, currentUserPosition);		// update user coordinates to remove any height map offset there was at the current position
 }
 
@@ -3622,7 +3649,7 @@ void GCodes::GetCurrentCoordinates(const StringRef& s) const
 
 	// Get the live machine coordinates, we'll need them later
 	float liveCoordinates[MaxTotalDrivers];
-	reprap.GetMove().LiveCoordinates(liveCoordinates, reprap.GetCurrentXAxes(), reprap.GetCurrentYAxes());
+	reprap.GetMove().LiveCoordinates(liveCoordinates, reprap.GetCurrentTool());
 
 	// Now the extruder coordinates
 	for (size_t i = numTotalAxes; i < MaxTotalDrivers; i++)
@@ -3649,7 +3676,7 @@ void GCodes::GetCurrentCoordinates(const StringRef& s) const
 
 	// Add the bed compensation
 	const float machineZ = machineCoordinates[Z_AXIS];
-	reprap.GetMove().AxisAndBedTransform(machineCoordinates, reprap.GetCurrentXAxes(), reprap.GetCurrentYAxes(), true);
+	reprap.GetMove().AxisAndBedTransform(machineCoordinates, reprap.GetCurrentTool(), true);
 	s.catf(" Bed comp %.3f", (double)(machineCoordinates[Z_AXIS] - machineZ));
 }
 
@@ -3663,7 +3690,7 @@ bool GCodes::QueueFileToPrint(const char* fileName, const StringRef& reply)
 
 		fileToPrint.Set(f);
 		fileOffsetToPrint = 0;
-		moveFractionToStartAt = 0.0;
+		restartMoveFractionDone = 0.0;
 		return true;
 	}
 
@@ -4488,14 +4515,11 @@ GCodeResult GCodes::RetractFilament(GCodeBuffer& gb, bool retract)
 
 		// New code does the retraction and the Z hop as separate moves
 		// Get ready to generate a move
-		const uint32_t xAxes = reprap.GetCurrentXAxes();
-		const uint32_t yAxes = reprap.GetCurrentYAxes();
-		reprap.GetMove().GetCurrentUserPosition(moveBuffer.coords, 0, xAxes, yAxes);
+		moveBuffer.tool = reprap.GetCurrentTool();
+		reprap.GetMove().GetCurrentUserPosition(moveBuffer.coords, 0, moveBuffer.tool);
 		SetMoveBufferDefaults();
 		moveBuffer.isFirmwareRetraction = true;
 		moveBuffer.filePos = (&gb == fileGCode) ? gb.GetFilePosition(fileInput->BytesCached()) : noFilePosition;
-		moveBuffer.xAxes = xAxes;
-		moveBuffer.yAxes = yAxes;
 
 		if (retract)
 		{
@@ -4790,7 +4814,7 @@ void GCodes::SetMachinePosition(const float positionNow[MaxTotalDrivers], bool d
 // Get the current position from the Move class
 void GCodes::UpdateCurrentUserPosition()
 {
-	reprap.GetMove().GetCurrentUserPosition(moveBuffer.coords, 0, reprap.GetCurrentXAxes(), reprap.GetCurrentYAxes());
+	reprap.GetMove().GetCurrentUserPosition(moveBuffer.coords, 0, reprap.GetCurrentTool());
 	ToolOffsetInverseTransform(moveBuffer.coords, currentUserPosition);
 }
 
@@ -4824,6 +4848,9 @@ void GCodes::RestorePosition(const RestorePoint& rp, GCodeBuffer *gb)
 		gb->MachineState().feedRate = rp.feedRate;
 	}
 
+	moveBuffer.initialUserX = rp.initialUserX;
+	moveBuffer.initialUserY = rp.initialUserY;
+
 #if SUPPORT_LASER || SUPPORT_IOBITS
 	moveBuffer.laserPwmOrIoBits = rp.laserPwmOrIoBits;
 #endif
diff --git a/src/GCodes/GCodes.h b/src/GCodes/GCodes.h
index f66ca99c..08480caf 100644
--- a/src/GCodes/GCodes.h
+++ b/src/GCodes/GCodes.h
@@ -121,9 +121,9 @@ public:
 			float acceleration;											// the requested acceleration, for async moves
 		};
 		FilePosition filePos;											// offset in the file being printed at the start of reading this move
-		float proportionLeft;											// what proportion of the entire move remains after this segment
-		AxesBitmap xAxes;												// axes that X is mapped to
-		AxesBitmap yAxes;												// axes that Y is mapped to
+		float proportionDone;											// what proportion of the entire move has been done when this segment is complete
+		float initialUserX, initialUserY;								// if this is a segment of an arc move, the user X and Y coordinates at the start
+		const Tool *tool;												// which tool (if any) is being used
 		EndstopsBitmap endStopsToCheck;									// endstops to check
 #if SUPPORT_LASER || SUPPORT_IOBITS
 		LaserPwmOrIoBits laserPwmOrIoBits;								// the laser PWM or port bit settings required
@@ -482,10 +482,13 @@ private:
 	unsigned int totalSegments;					// The total number of segments left in the complete move
 
 	unsigned int segmentsLeftToStartAt;
-	float moveFractionToStartAt;				// how much of the next move was printed before the power failure
 	float moveFractionToSkip;
 	float firstSegmentFractionToSkip;
 
+	float restartMoveFractionDone;				// how much of the next move was printed before the pause or power failure (from M26)
+	float restartInitialUserX;					// if the print was paused during an arc move, the user X coordinate at the start of that move (from M26)
+	float restartInitialUserY;					// if the print was paused during an arc move, the user X coordinate at the start of that move (from M26)
+
 	float arcCentre[MaxAxes];
 	float arcRadius;
 	float arcCurrentAngle;
diff --git a/src/GCodes/GCodes2.cpp b/src/GCodes/GCodes2.cpp
index 1a97c42c..8dcb2a1d 100644
--- a/src/GCodes/GCodes2.cpp
+++ b/src/GCodes/GCodes2.cpp
@@ -795,7 +795,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply)
 						// We need to copy the absolute/relative and volumetric extrusion flags over
 						fileGCode->OriginalMachineState().CopyStateFrom(gb.MachineState());
 						fileToPrint.Seek(fileOffsetToPrint);
-						moveFractionToSkip = moveFractionToStartAt;
+						moveFractionToSkip = restartMoveFractionDone;
 					}
 					StartPrinting(fromStart);
 				}
@@ -869,10 +869,9 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply)
 		if (gb.Seen('S'))
 		{
 			fileOffsetToPrint = (FilePosition)gb.GetUIValue();
-			if (gb.Seen('P'))
-			{
-				moveFractionToStartAt = constrain<float>(gb.GetFValue(), 0.0, 1.0);
-			}
+			restartMoveFractionDone = (gb.Seen('P')) ? constrain<float>(gb.GetFValue(), 0.0, 1.0) : 0.0;
+			restartInitialUserX = (gb.Seen('X')) ? gb.GetFValue() : 0.0;
+			restartInitialUserY = (gb.Seen('Y')) ? gb.GetFValue() : 0.0;
 		}
 		break;
 
diff --git a/src/GCodes/GCodes3.cpp b/src/GCodes/GCodes3.cpp
index da1c26df..11d6360e 100644
--- a/src/GCodes/GCodes3.cpp
+++ b/src/GCodes/GCodes3.cpp
@@ -847,7 +847,10 @@ GCodeResult GCodes::ProbeTool(GCodeBuffer& gb, const StringRef& reply)
 			SavePosition(toolChangeRestorePoint, gb);
 
 			// Prepare another move similar to G1 .. S3
+			moveBuffer.SetDefaults(numVisibleAxes);
 			moveBuffer.moveType = 3;
+			moveBuffer.canPauseAfter = false;
+
 			if (endStopToUse < 0)
 			{
 				moveBuffer.endStopsToCheck = 0;
@@ -864,11 +867,6 @@ GCodeResult GCodes::ProbeTool(GCodeBuffer& gb, const StringRef& reply)
 					moveBuffer.endStopsToCheck |= ActiveLowEndstop;
 				}
 			}
-			moveBuffer.xAxes = DefaultXAxisMapping;
-			moveBuffer.yAxes = DefaultYAxisMapping;
-			moveBuffer.usePressureAdvance = false;
-			moveBuffer.filePos = noFilePosition;
-			moveBuffer.canPauseAfter = false;
 
 			// Decide which way and how far to go
 			if (gb.Seen('R'))
diff --git a/src/GCodes/RestorePoint.cpp b/src/GCodes/RestorePoint.cpp
index 3a9ca286..68fcac1d 100644
--- a/src/GCodes/RestorePoint.cpp
+++ b/src/GCodes/RestorePoint.cpp
@@ -23,6 +23,7 @@ void RestorePoint::Init()
 	virtualExtruderPosition = 0.0;
 	filePos = noFilePosition;
 	proportionDone = 0.0;
+	initialUserX = initialUserY = 0.0;
 #if SUPPORT_LASER || SUPPORT_IOBITS
 	laserPwmOrIoBits.Clear();
 #endif
diff --git a/src/GCodes/RestorePoint.h b/src/GCodes/RestorePoint.h
index d277744e..7d758a6a 100644
--- a/src/GCodes/RestorePoint.h
+++ b/src/GCodes/RestorePoint.h
@@ -21,6 +21,7 @@ struct RestorePoint
 	float virtualExtruderPosition;			// The virtual extruder position at the start of this move
 	float proportionDone;					// How much of this move we have already done (zero unless we interrupted a move)
 	FilePosition filePos;					// The file position that this move was read from
+	float initialUserX, initialUserY;		// If we paused during an arc move and proportionDone is nonzero, the X and Y user coordinates at the start of the move
 	int toolNumber;							// The tool number that was active
 
 #if SUPPORT_LASER || SUPPORT_IOBITS
diff --git a/src/Movement/DDA.cpp b/src/Movement/DDA.cpp
index 14d37e97..c0aee505 100644
--- a/src/Movement/DDA.cpp
+++ b/src/Movement/DDA.cpp
@@ -11,6 +11,7 @@
 #include "Move.h"
 #include "StepTimer.h"
 #include "Kinematics/LinearDeltaKinematics.h"		// for DELTA_AXES
+#include "Tools/Tool.h"
 
 #if SUPPORT_CAN_EXPANSION
 # include "CAN/CanInterface.h"
@@ -306,7 +307,7 @@ bool DDA::InitStandardMove(DDARing& ring, GCodes::RawMove &nextMove, bool doMoto
 				{
 					const float positionDelta = endCoordinates[drive] - prev->GetEndCoordinate(drive, false);
 					directionVector[drive] = positionDelta;
-					if (positionDelta != 0.0 && (IsBitSet(nextMove.yAxes, drive) || IsBitSet(nextMove.xAxes, drive)))
+					if (positionDelta != 0.0 && (IsBitSet(Tool::GetXAxes(nextMove.tool), drive) || IsBitSet(Tool::GetYAxes(nextMove.tool), drive)))
 					{
 						flags.xyMoving = true;
 					}
@@ -365,13 +366,12 @@ bool DDA::InitStandardMove(DDARing& ring, GCodes::RawMove &nextMove, bool doMoto
 	}
 
 	// 3. Store some values
-	xAxes = nextMove.xAxes;
-	yAxes = nextMove.yAxes;
+	tool = nextMove.tool;
 	flags.usesEndstops = (nextMove.endStopsToCheck != 0);
 	endStopsToCheck = nextMove.endStopsToCheck;					//TODO move this to DDARing
 	filePos = nextMove.filePos;
 	virtualExtruderPosition = nextMove.virtualExtruderPosition;
-	proportionLeft = nextMove.proportionLeft;
+	proportionDone = nextMove.proportionDone;
 
 	flags.canPauseAfter = nextMove.canPauseAfter;
 	flags.usingStandardFeedrate = nextMove.usingStandardFeedrate;
@@ -535,8 +535,7 @@ bool DDA::InitLeadscrewMove(DDARing& ring, float feedrate, const float adjustmen
 	flags.continuousRotationShortcut = false;
 	endStopsToCheck = 0;
 	virtualExtruderPosition = prev->virtualExtruderPosition;
-	xAxes = prev->xAxes;
-	yAxes = prev->yAxes;
+	tool = nullptr;
 	filePos = prev->filePos;
 	flags.endCoordinatesValid = prev->flags.endCoordinatesValid;
 	acceleration = deceleration = reprap.GetPlatform().Accelerations()[Z_AXIS];
@@ -1491,6 +1490,8 @@ float DDA::NormaliseXYZ()
 	// First calculate the magnitude of the vector. If there is more than one X or Y axis, take an average of their movements (they should be equal).
 	float xMagSquared = 0.0, yMagSquared = 0.0;
 	unsigned int numXaxes = 0, numYaxes = 0;
+	const AxesBitmap xAxes = Tool::GetXAxes(tool);
+	const AxesBitmap yAxes = Tool::GetYAxes(tool);
 	for (size_t d = 0; d < MaxAxes; ++d)
 	{
 		if (IsBitSet(xAxes, d))
@@ -1881,14 +1882,13 @@ void DDA::MoveAborted()
 float DDA::GetProportionDone(bool moveWasAborted) const
 {
 	// Get the proportion of extrusion already done at the start of this segment
-	float proportionDone = (filePos != noFilePosition && filePos == prev->filePos)
-									? 1.0 - prev->proportionLeft
+	float proportionDoneSoFar = (filePos != noFilePosition && filePos == prev->filePos)
+									? prev->proportionDone
 										: 0.0;
 	if (moveWasAborted)
 	{
 		// The move was aborted, so subtract how much was done
-		const float proportionDoneAtEnd = 1.0 - proportionLeft;
-		if (proportionDoneAtEnd > proportionDone)
+		if (proportionDone > proportionDoneSoFar)
 		{
 			int32_t taken = 0, left = 0;
 			for (size_t drive = reprap.GetGCodes().GetTotalAxes(); drive < NumDirectDrivers; ++drive)
@@ -1903,11 +1903,11 @@ float DDA::GetProportionDone(bool moveWasAborted) const
 			const int32_t total = taken + left;
 			if (total > 0)										// if the move has net extrusion
 			{
-				proportionDone += (((proportionDoneAtEnd - proportionDone) * taken) + (total/2)) / total;
+				proportionDoneSoFar += (((proportionDone - proportionDoneSoFar) * taken) + (total/2)) / total;
 			}
 		}
 	}
-	return proportionDone;
+	return proportionDoneSoFar;
 }
 
 // Reduce the speed of this move to the indicated speed.
diff --git a/src/Movement/DDA.h b/src/Movement/DDA.h
index dced9d7d..9cd9ab2f 100644
--- a/src/Movement/DDA.h
+++ b/src/Movement/DDA.h
@@ -75,8 +75,7 @@ public:
     float GetVirtualExtruderPosition() const { return virtualExtruderPosition; }
 	float AdvanceBabyStepping(DDARing& ring, size_t axis, float amount);					// Try to push babystepping earlier in the move queue
 	bool IsHomingAxes() const { return (endStopsToCheck & HomeAxes) != 0; }
-	uint32_t GetXAxes() const { return xAxes; }
-	uint32_t GetYAxes() const { return yAxes; }
+	const Tool *GetTool() const { return tool; }
 	float GetTotalDistance() const { return totalDistance; }
 	void LimitSpeedAndAcceleration(float maxSpeed, float maxAcceleration);	// Limit the speed an acceleration of this move
 
@@ -84,6 +83,8 @@ public:
 	int32_t GetStepsTaken(size_t drive) const;
 
 	float GetProportionDone(bool moveWasAborted) const;						// Return the proportion of extrusion for the complete multi-segment move already done
+	float GetInitialUserX() const { return initialUserX; }
+	float GetInitialUserY() const { return initialUserY; }
 
 	void MoveAborted();
 
@@ -207,8 +208,7 @@ private:
 #endif
 
 	EndstopsBitmap endStopsToCheck;			// Which endstops we are checking on this move
-    AxesBitmap xAxes;						// Which axes are behaving as X axes
-    AxesBitmap yAxes;						// Which axes are behaving as Y axes
+	const Tool *tool;								// which tool (if any) is active
 
     FilePosition filePos;					// The position in the SD card file after this move was read, or zero if not read from SD card
 
@@ -226,7 +226,8 @@ private:
 	float endSpeed;
 	float topSpeed;
 
-	float proportionLeft;					// what proportion of the extrusion in the G1 or G0 move of which this is a part remains to be done after this segment is complete
+	float proportionDone;							// what proportion of the extrusion in the G1 or G0 move of which this is a part has been done after this segment is complete
+	float initialUserX, initialUserY;				// if this is a segment of an arc move, the user X and Y coordinates at the start
 	uint32_t clocksNeeded;
 
 	union
diff --git a/src/Movement/DDARing.cpp b/src/Movement/DDARing.cpp
index 323adfd7..5fb63300 100644
--- a/src/Movement/DDARing.cpp
+++ b/src/Movement/DDARing.cpp
@@ -535,7 +535,7 @@ bool DDARing::PauseMoves(RestorePoint& rp)
 		rp.moveCoords[axis] = prevDda->GetEndCoordinate(axis, false);
 	}
 
-	reprap.GetMove().InverseAxisAndBedTransform(rp.moveCoords, prevDda->GetXAxes(), prevDda->GetYAxes());	// we assume that xAxes hasn't changed between the moves
+	reprap.GetMove().InverseAxisAndBedTransform(rp.moveCoords, prevDda->GetTool());
 
 #if SUPPORT_LASER || SUPPORT_IOBITS
 	rp.laserPwmOrIoBits = dda->GetLaserPwmOrIoBits();
@@ -548,6 +548,8 @@ bool DDARing::PauseMoves(RestorePoint& rp)
 
 	dda = addPointer;
 	rp.proportionDone = dda->GetProportionDone(false);	// get the proportion of the current multi-segment move that has been completed
+	rp.initialUserX = dda->GetInitialUserX();
+	rp.initialUserY = dda->GetInitialUserY();
 	if (dda->UsingStandardFeedrate())
 	{
 		rp.feedRate = dda->GetRequestedSpeed();
@@ -623,6 +625,8 @@ bool DDARing::LowPowerOrStallPause(RestorePoint& rp)
 	rp.virtualExtruderPosition = dda->GetVirtualExtruderPosition();
 	rp.filePos = dda->GetFilePosition();
 	rp.proportionDone = dda->GetProportionDone(abortedMove);	// store how much of the complete multi-segment move's extrusion has been done
+	rp.initialUserX = dda->GetInitialUserX();
+	rp.initialUserY = dda->GetInitialUserY();
 
 #if SUPPORT_LASER || SUPPORT_IOBITS
 	rp.laserPwmOrIoBits = dda->GetLaserPwmOrIoBits();
@@ -638,7 +642,7 @@ bool DDARing::LowPowerOrStallPause(RestorePoint& rp)
 		rp.moveCoords[axis] = prevDda->GetEndCoordinate(axis, false);
 	}
 
-	reprap.GetMove().InverseAxisAndBedTransform(rp.moveCoords, prevDda->GetXAxes(), prevDda->GetYAxes());	// we assume that xAxes and yAxes have't changed between the moves
+	reprap.GetMove().InverseAxisAndBedTransform(rp.moveCoords, prevDda->GetTool());
 
 	// Free the DDAs for the moves we are going to skip
 	for (dda = addPointer; dda != savedDdaRingAddPointer; dda = dda->GetNext())
diff --git a/src/Movement/Move.cpp b/src/Movement/Move.cpp
index 70d79f4f..b421e3e2 100644
--- a/src/Movement/Move.cpp
+++ b/src/Movement/Move.cpp
@@ -171,7 +171,7 @@ void Move::Spin()
 				{
 					if (nextMove.moveType == 0)
 					{
-						AxisAndBedTransform(nextMove.coords, nextMove.xAxes, nextMove.yAxes, true);
+						AxisAndBedTransform(nextMove.coords, nextMove.tool, true);
 					}
 
 					if (mainDDARing.AddStandardMove(nextMove, !IsRawMotorMove(nextMove.moveType)))
@@ -236,7 +236,7 @@ bool Move::SetKinematics(KinematicsType k)
 {
 	if (kinematics->GetKinematicsType() != k)
 	{
-		Kinematics *nk = Kinematics::Create(k);
+		Kinematics * const nk = Kinematics::Create(k);
 		if (nk == nullptr)
 		{
 			return false;
@@ -340,7 +340,7 @@ void Move::SetNewPosition(const float positionNow[MaxTotalDrivers], bool doBedCo
 {
 	float newPos[MaxTotalDrivers];
 	memcpy(newPos, positionNow, sizeof(newPos));			// copy to local storage because Transform modifies it
-	AxisAndBedTransform(newPos, reprap.GetCurrentXAxes(), reprap.GetCurrentYAxes(), doBedCompensation);
+	AxisAndBedTransform(newPos, reprap.GetCurrentTool(), doBedCompensation);
 	SetLiveCoordinates(newPos);
 	SetPositions(newPos);
 }
@@ -350,8 +350,7 @@ void Move::EndPointToMachine(const float coords[], int32_t ep[], size_t numDrive
 {
 	if (CartesianToMotorSteps(coords, ep, true))
 	{
-		const size_t numAxes = reprap.GetGCodes().GetTotalAxes();
-		for (size_t drive = numAxes; drive < numDrives; ++drive)
+		for (size_t drive = reprap.GetGCodes().GetTotalAxes(); drive < numDrives; ++drive)
 		{
 			ep[drive] = MotorMovementToSteps(drive, coords[drive]);
 		}
@@ -412,100 +411,99 @@ bool Move::CartesianToMotorSteps(const float machinePos[MaxAxes], int32_t motorP
 	return b;
 }
 
-void Move::AxisAndBedTransform(float xyzPoint[MaxAxes], AxesBitmap xAxes, AxesBitmap yAxes, bool useBedCompensation) const
+void Move::AxisAndBedTransform(float xyzPoint[MaxAxes], const Tool *tool, bool useBedCompensation) const
 {
-	AxisTransform(xyzPoint, xAxes, yAxes);
+	AxisTransform(xyzPoint, tool);
 	if (useBedCompensation)
 	{
-		BedTransform(xyzPoint, xAxes, yAxes);
+		BedTransform(xyzPoint, tool);
 	}
 }
 
-void Move::InverseAxisAndBedTransform(float xyzPoint[MaxAxes], AxesBitmap xAxes, AxesBitmap yAxes) const
+void Move::InverseAxisAndBedTransform(float xyzPoint[MaxAxes], const Tool *tool) const
 {
-	InverseBedTransform(xyzPoint, xAxes, yAxes);
-	InverseAxisTransform(xyzPoint, xAxes, yAxes);
+	InverseBedTransform(xyzPoint, tool);
+	InverseAxisTransform(xyzPoint, tool);
 }
 
 // Do the Axis transform BEFORE the bed transform
-void Move::AxisTransform(float xyzPoint[MaxAxes], AxesBitmap xAxes, AxesBitmap yAxes) const
+void Move::AxisTransform(float xyzPoint[MaxAxes], const Tool *tool) const
 {
 	// Identify the lowest Y axis
-	const size_t NumVisibleAxes = reprap.GetGCodes().GetVisibleAxes();
-	for (size_t yAxis = Y_AXIS; yAxis < NumVisibleAxes; ++yAxis)
-	{
-		if (IsBitSet(yAxes, yAxis))
+	const size_t numVisibleAxes = reprap.GetGCodes().GetVisibleAxes();
+	const AxesBitmap yAxes = Tool::GetYAxes(tool);
+	const size_t lowestYAxis = LowestSetBit(yAxes);
+	if (lowestYAxis < numVisibleAxes)
+	{
+		// Found a Y axis. Use this one when correcting the X coordinate.
+		const AxesBitmap xAxes = Tool::GetXAxes(tool);
+		for (size_t axis = 0; axis < numVisibleAxes; ++axis)
 		{
-			// Found a Y axis. Use this one when correcting the X coordinate.
-			for (size_t axis = 0; axis < NumVisibleAxes; ++axis)
+			if (IsBitSet(xAxes, axis))
 			{
-				if (IsBitSet(xAxes, axis))
-				{
-					xyzPoint[axis] += tanXY*xyzPoint[yAxis] + tanXZ*xyzPoint[Z_AXIS];
-				}
-				if (IsBitSet(yAxes, axis))
-				{
-					xyzPoint[axis] += tanYZ*xyzPoint[Z_AXIS];
-				}
+				xyzPoint[axis] += tanXY*xyzPoint[lowestYAxis] + tanXZ*xyzPoint[Z_AXIS];
+			}
+			if (IsBitSet(yAxes, axis))
+			{
+				xyzPoint[axis] += tanYZ*xyzPoint[Z_AXIS];
 			}
-			break;
 		}
 	}
 }
 
-// Get the height error at an XY position
+// Get the height error at a bed XY position
 float Move::GetInterpolatedHeightError(float xCoord, float yCoord) const
 {
 	return (usingMesh) ? heightMap.GetInterpolatedHeightError(xCoord, yCoord) : probePoints.GetInterpolatedHeightError(xCoord, yCoord);
 }
 
 // Invert the Axis transform AFTER the bed transform
-void Move::InverseAxisTransform(float xyzPoint[MaxAxes], AxesBitmap xAxes, AxesBitmap yAxes) const
+void Move::InverseAxisTransform(float xyzPoint[MaxAxes], const Tool *tool) const
 {
 	// Identify the lowest Y axis
-	const size_t NumVisibleAxes = reprap.GetGCodes().GetVisibleAxes();
-	for (size_t yAxis = Y_AXIS; yAxis < NumVisibleAxes; ++yAxis)
-	{
-		if (IsBitSet(yAxes, yAxis))
+	const size_t numVisibleAxes = reprap.GetGCodes().GetVisibleAxes();
+	const AxesBitmap yAxes = Tool::GetYAxes(tool);
+	const size_t lowestYAxis = LowestSetBit(yAxes);
+	if (lowestYAxis < numVisibleAxes)
+	{
+		// Found a Y axis. Use this one when correcting the X coordinate.
+		const AxesBitmap xAxes = Tool::GetXAxes(tool);
+		for (size_t axis = 0; axis < numVisibleAxes; ++axis)
 		{
-			// Found a Y axis. Use this one when correcting the X coordinate.
-			for (size_t axis = 0; axis < NumVisibleAxes; ++axis)
+			if (IsBitSet(yAxes, axis))
 			{
-				if (IsBitSet(yAxes, axis))
-				{
-					xyzPoint[axis] -= tanYZ*xyzPoint[Z_AXIS];
-				}
-				if (IsBitSet(xAxes, axis))
-				{
-					xyzPoint[axis] -= (tanXY*xyzPoint[yAxis] + tanXZ*xyzPoint[Z_AXIS]);
-				}
+				xyzPoint[axis] -= tanYZ*xyzPoint[Z_AXIS];
+			}
+			if (IsBitSet(xAxes, axis))
+			{
+				xyzPoint[axis] -= (tanXY*xyzPoint[lowestYAxis] + tanXZ*xyzPoint[Z_AXIS]);
 			}
-			break;
 		}
 	}
 }
 
 // Do the bed transform AFTER the axis transform
-void Move::BedTransform(float xyzPoint[MaxAxes], AxesBitmap xAxes, AxesBitmap yAxes) const
+void Move::BedTransform(float xyzPoint[MaxAxes], const Tool *tool) const
 {
 	if (!useTaper || xyzPoint[Z_AXIS] < taperHeight)
 	{
 		float zCorrection = 0.0;
 		const size_t numAxes = reprap.GetGCodes().GetVisibleAxes();
+		const AxesBitmap xAxes = Tool::GetXAxes(tool);
+		const AxesBitmap yAxes = Tool::GetYAxes(tool);
 		unsigned int numCorrections = 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 xAxis = 0; xAxis < numAxes; ++xAxis)
 		{
 			if (IsBitSet(xAxes, xAxis))
 			{
-				const float xCoord = xyzPoint[xAxis];
+				const float xCoord = xyzPoint[xAxis] + Tool::GetOffset(tool, xAxis);
 				for (uint32_t yAxis = 0; yAxis < numAxes; ++yAxis)
 				{
 					if (IsBitSet(yAxes, yAxis))
 					{
-						const float yCoord = xyzPoint[yAxis];
+						const float yCoord = xyzPoint[yAxis] + Tool::GetOffset(tool, yAxis);
 						zCorrection += GetInterpolatedHeightError(xCoord, yCoord);
 						++numCorrections;
 					}
@@ -524,24 +522,25 @@ void Move::BedTransform(float xyzPoint[MaxAxes], AxesBitmap xAxes, AxesBitmap yA
 }
 
 // Invert the bed transform BEFORE the axis transform
-void Move::InverseBedTransform(float xyzPoint[MaxAxes], AxesBitmap xAxes, AxesBitmap yAxes) const
+void Move::InverseBedTransform(float xyzPoint[MaxAxes], const Tool *tool) const
 {
 	float zCorrection = 0.0;
 	const size_t numAxes = reprap.GetGCodes().GetVisibleAxes();
+	const AxesBitmap xAxes = Tool::GetXAxes(tool);
+	const AxesBitmap yAxes = Tool::GetYAxes(tool);
 	unsigned int numCorrections = 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.
+	// Transform the Z coordinate based on the average correction for each axis used as an X or Y axis.
 	for (uint32_t xAxis = 0; xAxis < numAxes; ++xAxis)
 	{
 		if (IsBitSet(xAxes, xAxis))
 		{
-			const float xCoord = xyzPoint[xAxis];
+			const float xCoord = xyzPoint[xAxis] + Tool::GetOffset(tool, xAxis);
 			for (uint32_t yAxis = 0; yAxis < numAxes; ++yAxis)
 			{
 				if (IsBitSet(yAxes, yAxis))
 				{
-					const float yCoord = xyzPoint[yAxis];
+					const float yCoord = xyzPoint[yAxis]+ Tool::GetOffset(tool, yAxis);
 					zCorrection += GetInterpolatedHeightError(xCoord, yCoord);
 					++numCorrections;
 				}
@@ -570,12 +569,12 @@ void Move::InverseBedTransform(float xyzPoint[MaxAxes], AxesBitmap xAxes, AxesBi
 	}
 }
 
-// Normalise the bed transform to have zero height error at these coordinates
+// Normalise the bed transform to have zero height error at these bed coordinates
 void Move::SetZeroHeightError(const float coords[MaxAxes])
 {
 	float tempCoords[MaxAxes];
 	memcpy(tempCoords, coords, sizeof(tempCoords));
-	AxisTransform(tempCoords, DefaultXAxisMapping, DefaultYAxisMapping);
+	AxisTransform(tempCoords, nullptr);
 	zShift = -GetInterpolatedHeightError(tempCoords[X_AXIS], tempCoords[Y_AXIS]);
 }
 
@@ -731,12 +730,12 @@ void Move::Interrupt()
 }
 
 // Return the transformed machine coordinates
-void Move::GetCurrentUserPosition(float m[MaxAxes], uint8_t moveType, AxesBitmap xAxes, AxesBitmap yAxes) const
+void Move::GetCurrentUserPosition(float m[MaxAxes], uint8_t moveType, const Tool *tool) const
 {
 	GetCurrentMachinePosition(m, IsRawMotorMove(moveType));
 	if (moveType == 0)
 	{
-		InverseAxisAndBedTransform(m, xAxes, yAxes);
+		InverseAxisAndBedTransform(m, tool);
 	}
 }
 
diff --git a/src/Movement/Move.h b/src/Movement/Move.h
index 11166580..2dd6a066 100644
--- a/src/Movement/Move.h
+++ b/src/Movement/Move.h
@@ -51,10 +51,10 @@ public:
 	void Exit();													// Shut down
 
 	void GetCurrentMachinePosition(float m[MaxAxes], bool disableMotorMapping) const; // Get the current position in untransformed coords
-	void GetCurrentUserPosition(float m[MaxAxes], uint8_t moveType, AxesBitmap xAxes, AxesBitmap yAxes) const;
+	void GetCurrentUserPosition(float m[MaxAxes], uint8_t moveType, const Tool *tool) const;
 																	// Return the position (after all queued moves have been executed) in transformed coords
 	int32_t GetEndPoint(size_t drive) const;					 	// Get the current position of a motor
-	void LiveCoordinates(float m[MaxTotalDrivers], AxesBitmap xAxes, AxesBitmap yAxes);	// Gives the last point at the end of the last complete DDA transformed to user coords
+	void LiveCoordinates(float m[MaxTotalDrivers], const Tool *tool);	// Gives the last point at the end of the last complete DDA transformed to user coords
 	void Interrupt() __attribute__ ((hot));							// The hardware's (i.e. platform's)  interrupt should call this.
 	bool AllMovesAreFinished();										// Is the look-ahead ring empty?  Stops more moves being added as well.
 	void DoLookAhead() __attribute__ ((hot));						// Run the look-ahead procedure
@@ -68,9 +68,9 @@ public:
 	void SetAxisCompensation(unsigned int axis, float tangent);		// Set an axis-pair compensation angle
 	float AxisCompensation(unsigned int axis) const;				// The tangent value
 	void SetIdentityTransform();									// Cancel the bed equation; does not reset axis angle compensation
-	void AxisAndBedTransform(float move[], AxesBitmap xAxes, AxesBitmap yAxes, bool useBedCompensation) const;
+	void AxisAndBedTransform(float move[], const Tool *tool, bool useBedCompensation) const;
 																	// Take a position and apply the bed and the axis-angle compensations
-	void InverseAxisAndBedTransform(float move[], AxesBitmap xAxes, AxesBitmap yAxes) const;
+	void InverseAxisAndBedTransform(float move[], const Tool *tool) const;
 																	// Go from a transformed point back to user coordinates
 	void SetZeroHeightError(const float coords[MaxAxes]);			// Set zero height error at these coordinates
 	float GetTaperHeight() const { return (useTaper) ? taperHeight : 0.0; }
@@ -169,12 +169,12 @@ private:
 		timing			// no moves being executed or in queue, motors are at full current
 	};
 
-	void BedTransform(float move[MaxAxes], AxesBitmap xAxes, AxesBitmap yAxes) const;			// Take a position and apply the bed compensations
-	void InverseBedTransform(float move[MaxAxes], AxesBitmap xAxes, AxesBitmap yAxes) const;	// Go from a bed-transformed point back to user coordinates
-	void AxisTransform(float move[MaxAxes], AxesBitmap xAxes, AxesBitmap yAxes) const;			// Take a position and apply the axis-angle compensations
-	void InverseAxisTransform(float move[MaxAxes], AxesBitmap xAxes, AxesBitmap yAxes) const;	// Go from an axis transformed point back to user coordinates
+	void BedTransform(float move[MaxAxes], const Tool *tool) const;			// Take a position and apply the bed compensations
+	void InverseBedTransform(float move[MaxAxes], const Tool *tool) const;	// Go from a bed-transformed point back to user coordinates
+	void AxisTransform(float move[MaxAxes], const Tool *tool) const;		// Take a position and apply the axis-angle compensations
+	void InverseAxisTransform(float move[MaxAxes], const Tool *tool) const;	// Go from an axis transformed point back to user coordinates
 	void SetPositions(const float move[MaxTotalDrivers]) { return mainDDARing.SetPositions(move); }	// Force the machine coordinates to be these;
-	float GetInterpolatedHeightError(float xCoord, float yCoord) const;							// Get the height error at an XY position
+	float GetInterpolatedHeightError(float xCoord, float yCoord) const;		// Get the height error at an XY position
 
 	DDARing mainDDARing;								// The DDA ring used for regular moves
 
@@ -237,10 +237,10 @@ inline void Move::AdjustMotorPositions(const float adjustment[], size_t numMotor
 
 // Return the current live XYZ and extruder coordinates
 // Interrupts are assumed enabled on entry
-inline void Move::LiveCoordinates(float m[MaxTotalDrivers], AxesBitmap xAxes, AxesBitmap yAxes)
+inline void Move::LiveCoordinates(float m[MaxTotalDrivers], const Tool *tool)
 {
 	mainDDARing.LiveCoordinates(m);
-	InverseAxisAndBedTransform(m, xAxes, yAxes);
+	InverseAxisAndBedTransform(m, tool);
 }
 
 // These are the actual numbers that we want to be the coordinates, so don't transform them.
diff --git a/src/Platform.cpp b/src/Platform.cpp
index 7819771d..db59d619 100644
--- a/src/Platform.cpp
+++ b/src/Platform.cpp
@@ -1621,7 +1621,7 @@ void Platform::Spin()
 				ListDrivers(scratchString.GetRef(), stalledDriversToLog);
 				stalledDriversToLog = 0;
 				float liveCoordinates[MaxTotalDrivers];
-				reprap.GetMove().LiveCoordinates(liveCoordinates, reprap.GetCurrentXAxes(), reprap.GetCurrentYAxes());
+				reprap.GetMove().LiveCoordinates(liveCoordinates, reprap.GetCurrentTool());
 				MessageF(WarningMessage, "Driver(s)%s stalled at Z height %.2f", scratchString.c_str(), (double)liveCoordinates[Z_AXIS]);
 				reported = true;
 			}
diff --git a/src/RepRap.cpp b/src/RepRap.cpp
index 4587b1f6..b9913d56 100644
--- a/src/RepRap.cpp
+++ b/src/RepRap.cpp
@@ -883,7 +883,7 @@ OutputBuffer *RepRap::GetStatusResponse(uint8_t type, ResponseSource source)
 		else
 #endif
 		{
-			move->LiveCoordinates(liveCoordinates, GetCurrentXAxes(), GetCurrentYAxes());
+			move->LiveCoordinates(liveCoordinates, currentTool);
 		}
 
 		// Machine coordinates
@@ -1703,7 +1703,7 @@ OutputBuffer *RepRap::GetLegacyStatusResponse(uint8_t type, int seq)
 
 	// Now the machine coordinates
 	float liveCoordinates[MaxTotalDrivers];
-	move->LiveCoordinates(liveCoordinates, GetCurrentXAxes(), GetCurrentYAxes());
+	move->LiveCoordinates(liveCoordinates, currentTool);
 	response->catf("],\"machine\":");		// announce the machine position
 	ch = '[';
 	for (size_t drive = 0; drive < numVisibleAxes; drive++)
diff --git a/src/Tools/Tool.cpp b/src/Tools/Tool.cpp
index 8fd40684..aa059e0e 100644
--- a/src/Tools/Tool.cpp
+++ b/src/Tools/Tool.cpp
@@ -158,6 +158,21 @@ Tool * Tool::freelist = nullptr;
 	}
 }
 
+/*static*/ AxesBitmap Tool::GetXAxes(const Tool *tool)
+{
+	return (tool == nullptr) ? DefaultXAxisMapping : tool->xMapping;
+}
+
+/*static*/ AxesBitmap Tool::GetYAxes(const Tool *tool)
+{
+	return (tool == nullptr) ? DefaultYAxisMapping : tool->yMapping;
+}
+
+/*static*/ float Tool::GetOffset(const Tool *tool, size_t axis)
+{
+	return (tool == nullptr) ? 0.0 : tool->offset[axis];
+}
+
 void Tool::Print(const StringRef& reply) const
 {
 	reply.printf("Tool %u - ", myNumber);
diff --git a/src/Tools/Tool.h b/src/Tools/Tool.h
index d8bd330f..b43be51a 100644
--- a/src/Tools/Tool.h
+++ b/src/Tools/Tool.h
@@ -51,6 +51,9 @@ public:
 
 	static Tool *Create(unsigned int toolNumber, const char *name, int32_t d[], size_t dCount, int32_t h[], size_t hCount, AxesBitmap xMap, AxesBitmap yMap, FansBitmap fanMap, const StringRef& reply);
 	static void Delete(Tool *t);
+	static AxesBitmap GetXAxes(const Tool *tool);
+	static AxesBitmap GetYAxes(const Tool *tool);
+	static float GetOffset(const Tool *tool, size_t axis) pre(axis < MaxAxes);
 
 	float GetOffset(size_t axis) const pre(axis < MaxAxes);
 	void SetOffset(size_t axis, float offs, bool byProbing) pre(axis < MaxAxes);
diff --git a/src/Version.h b/src/Version.h
index bd1e213e..a5ddffeb 100644
--- a/src/Version.h
+++ b/src/Version.h
@@ -20,7 +20,7 @@
 #endif
 
 #ifndef DATE
-# define DATE "2019-11-27b1"
+# define DATE "2019-11-28b1"
 #endif
 
 #define AUTHORS "reprappro, dc42, chrishamm, t3p3, dnewman, printm3d"