Merge remote-tracking branch 'origin/3.5-dev' into v3-chrishamm

18 files changed, 357 insertions, 240 deletions

diff --git a/src/CAN/CommandProcessor.cpp b/src/CAN/CommandProcessor.cpp
index 545cc677..a82d778e 100644
--- a/src/CAN/CommandProcessor.cpp
+++ b/src/CAN/CommandProcessor.cpp
@@ -355,11 +355,11 @@ static GCodeResult EutGetInfo(const CanMessageReturnInfo& msg, const StringRef&
 			const size_t driver = msg.type - (CanMessageReturnInfo::typeDiagnosticsPart0 + 1);
 			if (driver < NumDirectDrivers)			// we have up to 7 drivers on the Duet 3 Mini but only 6 on the 6HC and 6XD
 			{
-				reply.lcatf("Driver %u: position %" PRIi32 ", %.1f steps/mm"
+				reply.lcatf("Driver %u: %.1f steps/mm"
 #if HAS_SMART_DRIVERS
 					","
 #endif
-					, driver, reprap.GetMove().GetEndPoint(driver), (double)reprap.GetPlatform().DriveStepsPerUnit(driver));
+					, driver, (double)reprap.GetPlatform().DriveStepsPerUnit(driver));
 #if HAS_SMART_DRIVERS
 				SmartDrivers::AppendDriverStatus(driver, reply);
 #endif
diff --git a/src/GCodes/GCodes.cpp b/src/GCodes/GCodes.cpp
index 6d5d58e0..db68c372 100644
--- a/src/GCodes/GCodes.cpp
+++ b/src/GCodes/GCodes.cpp
@@ -1555,7 +1555,6 @@ bool GCodes::LockAllMovementSystemsAndWaitForStandstill(GCodeBuffer& gb) noexcep
 // Lock movement and wait for pending moves to finish.
 // Return true if successful, false if we need to try again later.
 // As a side-effect it updates the user coordinates from the machine coordinates.
-
 bool GCodes::LockMovementSystemAndWaitForStandstill(GCodeBuffer& gb, unsigned int msNumber) noexcept
 {
 	// Lock movement to stop another source adding moves to the queue
@@ -1596,17 +1595,19 @@ bool GCodes::LockMovementSystemAndWaitForStandstill(GCodeBuffer& gb, unsigned in
 #if SUPPORT_ASYNC_MOVES
 		// Get the position of all axes by combining positions from the queues
 		Move& move = reprap.GetMove();
-		const AxesBitmap ownedAxes = moveStates[msNumber].axesAndExtrudersOwned;
+		const AxesBitmap ownedAxes = ms.GetAxesAndExtrudersOwned();
+
+		// Whenever we release axes, we must update lastKnownMachinePositions for those axes first so that whoever allocated them next gets the correct positions
 		move.GetPartialMachinePosition(lastKnownMachinePositions, ownedAxes, msNumber);
-		memcpyf(moveStates[msNumber].coords, lastKnownMachinePositions, MaxAxes);
-		move.InverseAxisAndBedTransform(lastKnownMachinePositions, moveStates[msNumber].currentTool);
-		UpdateUserPositionFromMachinePosition(gb, moveStates[msNumber]);
+
+		memcpyf(ms.coords, lastKnownMachinePositions, MaxAxes);
+		move.InverseAxisAndBedTransform(lastKnownMachinePositions, ms.currentTool);
+		UpdateUserPositionFromMachinePosition(gb, ms);
 		collisionChecker.ResetPositions(lastKnownMachinePositions, ownedAxes);
 
 		// Release the axes and extruders that this movement system owns
 		axesAndExtrudersMoved.ClearBits(ownedAxes);
-		moveStates[msNumber].axesAndExtrudersOwned.Clear();
-		ms.ownedAxisLetters.Clear();
+		ms.ReleaseOwnedAxesAndExtruders();
 #else
 		UpdateCurrentUserPosition(gb);
 #endif
@@ -1688,8 +1689,10 @@ void GCodes::LoadExtrusionAndFeedrateFromGCode(GCodeBuffer& gb, MovementState& m
 	}
 	ms.hasPositiveExtrusion = false;
 	ms.moveStartVirtualExtruderPosition = ms.latestVirtualExtruderPosition;	// save this before we update it
-	AxesBitmap logicalDrivesMoving;
 	ExtrudersBitmap extrudersMoving;
+#if SUPPORT_ASYNC_MOVES
+	AxesBitmap logicalDrivesMoving;
+#endif
 
 	// Check if we are extruding
 	if (gb.Seen(extrudeLetter))												// DC 2018-08-07: at E3D's request, extrusion is now recognised even on uncoordinated moves
@@ -1761,7 +1764,9 @@ void GCodes::LoadExtrusionAndFeedrateFromGCode(GCodeBuffer& gb, MovementState& m
 																		? extrusionAmount * extrusionFactors[extruder]
 																		: extrusionAmount;
 						extrudersMoving.SetBit(extruder);
+#if SUPPORT_ASYNC_MOVES
 						logicalDrivesMoving.SetBit(ExtruderToLogicalDrive(extruder));
+#endif
 					}
 				}
 				if (!isPrintingMove && ms.usingStandardFeedrate)
@@ -1801,7 +1806,9 @@ void GCodes::LoadExtrusionAndFeedrateFromGCode(GCodeBuffer& gb, MovementState& m
 																			? extrusionAmount * extrusionFactors[extruder]
 																			: extrusionAmount;
 							extrudersMoving.SetBit(extruder);
+#if SUPPORT_ASYNC_MOVES
 							logicalDrivesMoving.SetBit(ExtruderToLogicalDrive(extruder));
+#endif
 						}
 					}
 				}
@@ -1814,7 +1821,7 @@ void GCodes::LoadExtrusionAndFeedrateFromGCode(GCodeBuffer& gb, MovementState& m
 	}
 
 #if SUPPORT_ASYNC_MOVES
-	AllocateAxes(gb, ms, logicalDrivesMoving);
+	AllocateAxes(gb, ms, logicalDrivesMoving, ParameterLettersBitmap());
 #endif
 
 	if (ms.moveType == 1 || ms.moveType == 4)
@@ -1838,14 +1845,46 @@ bool GCodes::DoStraightMove(GCodeBuffer& gb, bool isCoordinated) THROWS(GCodeExc
 {
 	MovementState& ms = GetMovementState(gb);
 
+	// Set up default move parameters
+	ms.movementTool = ms.currentTool;
+	ms.moveType = 0;
+	ms.isCoordinated = isCoordinated;
+	ms.checkEndstops = false;
+	ms.reduceAcceleration = false;
+	ms.movementTool = ms.currentTool;
+	ms.usePressureAdvance = false;
+	axesToSenseLength.Clear();
+
+	// Check to see if the move is a 'homing' move that endstops are checked on and for which X and Y axis mapping is not applied
+	{
+		uint32_t moveType;
+		bool dummy;
+		if (gb.TryGetLimitedUIValue('H', moveType, dummy, 5))
+		{
+			if (!LockCurrentMovementSystemAndWaitForStandstill(gb))
+			{
+				return false;
+			}
+			ms.moveType = moveType;
+			ms.movementTool = nullptr;
+		}
+	}
+
+
 #if SUPPORT_ASYNC_MOVES
 	// We need to check for moving unowned axes right at the start in case we need to fetch axis positions before processing the command
 	ParameterLettersBitmap axisLettersMentioned = gb.AllParameters() & allAxisLetters;
-	axisLettersMentioned.ClearBits(ms.ownedAxisLetters);
-	if (axisLettersMentioned.IsNonEmpty())
+	if (ms.moveType == 0)
 	{
-		//TODO problem! this ignores axis mapping and coordinate rotation!!!
-		AllocateAxisLetters(gb, ms, axisLettersMentioned);
+		axisLettersMentioned.ClearBits(ms.GetOwnedAxisLetters());
+		if (axisLettersMentioned.IsNonEmpty())
+		{
+			AllocateAxisLetters(gb, ms, axisLettersMentioned);
+		}
+	}
+	else
+	{
+		AllocateAxesDirectFromLetters(gb, ms, axisLettersMentioned);
 	}
 #endif
 
@@ -1861,31 +1900,6 @@ bool GCodes::DoStraightMove(GCodeBuffer& gb, bool isCoordinated) THROWS(GCodeExc
 		ms.initialUserC1 = ms.currentUserPosition[(selectedPlane == 0) ? Y_AXIS : Z_AXIS];
 	}
 
-	// Set up default move parameters
-	ms.isCoordinated = isCoordinated;
-	ms.checkEndstops = false;
-	ms.reduceAcceleration = false;
-	ms.movementTool = ms.currentTool;
-	ms.moveType = 0;
-	ms.usePressureAdvance = false;
-	axesToSenseLength.Clear();
-
-	// Check to see if the move is a 'homing' move that endstops are checked on.
-	// We handle H1 parameters affecting extrusion elsewhere.
-	if (gb.Seen('H'))
-	{
-		const int ival = gb.GetIValue();
-		if (ival >= 1 && ival <= 4)
-		{
-			if (!LockCurrentMovementSystemAndWaitForStandstill(gb))
-			{
-				return false;
-			}
-			ms.moveType = ival;
-			ms.movementTool = nullptr;
-		}
-	}
-
 	// Check for 'R' parameter to move relative to a restore point
 	const RestorePoint * rp = nullptr;
 	if (ms.moveType == 0 && gb.Seen('R'))
@@ -2043,9 +2057,6 @@ bool GCodes::DoStraightMove(GCodeBuffer& gb, bool isCoordinated) THROWS(GCodeExc
 			}
 		}
 
-#if SUPPORT_ASYNC_MOVES
-		AllocateAxes(gb, ms, realAxesMoving);
-#endif
 		if (!doingManualBedProbe && CheckEnoughAxesHomed(axesMentioned))
 		{
 			gb.ThrowGCodeException("G0/G1: insufficient axes homed");
@@ -2053,9 +2064,6 @@ bool GCodes::DoStraightMove(GCodeBuffer& gb, bool isCoordinated) THROWS(GCodeExc
 	}
 	else
 	{
-#if SUPPORT_ASYNC_MOVES
-		AllocateAxes(gb, ms, axesMentioned);
-#endif
 		switch (ms.moveType)
 		{
 		case 3:
@@ -2259,8 +2267,20 @@ bool GCodes::DoArcMove(GCodeBuffer& gb, bool clockwise)
 	const unsigned int selectedPlane = gb.LatestMachineState().selectedPlane;
 	const unsigned int axis0 = (unsigned int[]){ X_AXIS, Z_AXIS, Y_AXIS }[selectedPlane];
 	const unsigned int axis1 = (axis0 + 1) % 3;
-
 	MovementState& ms = GetMovementState(gb);
+
+#if SUPPORT_ASYNC_MOVES
+	// We need to check for moving unowned axes right at the start in case we need to fetch axis positions before processing the command
+	ParameterLettersBitmap axisLettersMentioned = gb.AllParameters() & allAxisLetters;
+	axisLettersMentioned.SetBit(ParameterLetterToBitNumber('X') + axis0);		// add in the implicit axes
+	axisLettersMentioned.SetBit(ParameterLetterToBitNumber('X') + axis1);
+	axisLettersMentioned.ClearBits(ms.GetOwnedAxisLetters());
+	if (axisLettersMentioned.IsNonEmpty())
+	{
+		AllocateAxisLetters(gb, ms, axisLettersMentioned);
+	}
+#endif
+
 	if (ms.moveFractionToSkip > 0.0)
 	{
 		ms.initialUserC0 = ms.restartInitialUserC0;
@@ -2454,10 +2474,6 @@ bool GCodes::DoArcMove(GCodeBuffer& gb, bool clockwise)
 		gb.ThrowGCodeException("G2/G3: insufficient axes homed");
 	}
 
-#if SUPPORT_ASYNC_MOVES
-	AllocateAxes(gb, ms, realAxesMoving);
-#endif
-
 	// Compute the initial and final angles. Do this before we possible rotate the coordinates of the arc centre.
 	float finalTheta = atan2(ms.currentUserPosition[axis1] - userArcCentre[1], ms.currentUserPosition[axis0] - userArcCentre[0]);
 	ms.arcRadius = fastSqrtf(iParam * iParam + jParam * jParam);
@@ -3091,9 +3107,11 @@ void GCodes::HandleM114(GCodeBuffer& gb, const StringRef& s) const noexcept
 	// Print the axis stepper motor positions as Marlin does, as an aid to debugging.
 	// Don't bother with the extruder endpoints, they are zero after any non-extruding move.
 	s.cat("Count");
+	int32_t positions[MaxAxesPlusExtruders];
+	reprap.GetMove().GetLivePositions(positions);
 	for (size_t i = 0; i < numVisibleAxes; ++i)
 	{
-		s.catf(" %" PRIi32, reprap.GetMove().GetEndPoint(i));
+		s.catf(" %" PRIi32, positions[i]);
 	}
 
 	// Add the machine coordinates because they may be different from the user coordinates under some conditions
@@ -3909,7 +3927,7 @@ GCodeResult GCodes::RetractFilament(GCodeBuffer& gb, bool retract) THROWS(GCodeE
 					ms.feedRate = currentTool->GetRetractSpeed() * currentTool->DriveCount();
 					ms.canPauseAfter = false;			// don't pause after a retraction because that could cause too much retraction
 #if SUPPORT_ASYNC_MOVES
-					AllocateAxes(gb, ms, drivesMoving);
+					AllocateAxes(gb, ms, drivesMoving, ParameterLettersBitmap());
 #endif
 					NewSingleSegmentMoveAvailable(ms);
 				}
@@ -3927,7 +3945,7 @@ GCodeResult GCodes::RetractFilament(GCodeBuffer& gb, bool retract) THROWS(GCodeE
 				ms.canPauseAfter = false;			// don't pause in the middle of a command
 				ms.linearAxesMentioned = true;
 #if SUPPORT_ASYNC_MOVES
-				AllocateAxes(gb, ms, AxesBitmap::MakeFromBits(Z_AXIS));
+				AllocateAxes(gb, ms, AxesBitmap::MakeFromBits(Z_AXIS), ParameterLettersBitmap('Z'));
 #endif
 				NewSingleSegmentMoveAvailable(ms);
 				gb.SetState(GCodeState::doingFirmwareUnRetraction);
@@ -3947,7 +3965,7 @@ GCodeResult GCodes::RetractFilament(GCodeBuffer& gb, bool retract) THROWS(GCodeE
 					ms.feedRate = currentTool->GetUnRetractSpeed() * currentTool->DriveCount();
 					ms.canPauseAfter = true;
 #if SUPPORT_ASYNC_MOVES
-					AllocateAxes(gb, ms, drivesMoving);
+					AllocateAxes(gb, ms, drivesMoving, ParameterLettersBitmap());
 #endif
 					NewSingleSegmentMoveAvailable(ms);
 				}
@@ -4696,8 +4714,6 @@ void GCodes:: SetMoveBufferDefaults(MovementState& ms) noexcept
 // Locking the same resource more than once only locks it once, there is no lock count held.
 bool GCodes::LockResource(const GCodeBuffer& gb, Resource r) noexcept
 {
-	TaskCriticalSectionLocker lock;
-
 	if (resourceOwners[r] == &gb)
 	{
 		return true;
@@ -4714,8 +4730,6 @@ bool GCodes::LockResource(const GCodeBuffer& gb, Resource r) noexcept
 // Grab the movement lock even if another GCode source has it
 void GCodes::GrabResource(const GCodeBuffer& gb, Resource r) noexcept
 {
-	TaskCriticalSectionLocker lock;
-
 	if (resourceOwners[r] != &gb)
 	{
 		// Note, we now leave the resource bit set in the original owning GCodeBuffer machine state
@@ -4784,8 +4798,6 @@ void GCodes::GrabMovement(const GCodeBuffer& gb) noexcept
 // Unlock the resource if we own it
 void GCodes::UnlockResource(const GCodeBuffer& gb, Resource r) noexcept
 {
-	TaskCriticalSectionLocker lock;
-
 	if (resourceOwners[r] == &gb)
 	{
 		// Note, we leave the bit set in previous stack levels! This is needed e.g. to allow M291 blocking messages to be used in homing files.
@@ -4797,17 +4809,16 @@ void GCodes::UnlockResource(const GCodeBuffer& gb, Resource r) noexcept
 // Release all locks, except those that were owned when the current macro was started
 void GCodes::UnlockAll(const GCodeBuffer& gb) noexcept
 {
-	TaskCriticalSectionLocker lock;
-
 	const GCodeMachineState * const mc = gb.LatestMachineState().GetPrevious();
 	const GCodeMachineState::ResourceBitmap resourcesToKeep = (mc == nullptr) ? GCodeMachineState::ResourceBitmap() : mc->lockedResources;
 	for (size_t i = 0; i < NumResources; ++i)
 	{
 		if (resourceOwners[i] == &gb && !resourcesToKeep.IsBitSet(i))
 		{
-			if (i >= MoveResourceBase && i < MoveResourceBase + NumResources)
+			if (i >= MoveResourceBase && i < MoveResourceBase + NumMovementSystems && mc == nullptr)
 			{
 				// In case homing was aborted because of an exception, we need to clear toBeHomed when releasing the movement lock
+				//debugPrintf("tbh %04x clearing\n", (unsigned int)toBeHomed.GetRaw());
 				toBeHomed.Clear();
 			}
 			resourceOwners[i] = nullptr;
@@ -4928,31 +4939,75 @@ const MovementState& GCodes::GetCurrentMovementState(const ObjectExplorationCont
 
 // Allocate additional axes and/or extruders to a movement state returning true if successful, false if another movement state owns it already
 // This relies on cooperative scheduling between different GCodeBuffer objects
-void GCodes::AllocateAxes(const GCodeBuffer& gb, MovementState& ms, AxesBitmap axes) THROWS(GCodeException)
+void GCodes::AllocateAxes(const GCodeBuffer& gb, MovementState& ms, AxesBitmap axes, ParameterLettersBitmap axLetters) THROWS(GCodeException)
 {
-	if ((axes & axesAndExtrudersMoved & ~ms.axesAndExtrudersOwned).IsNonEmpty())
+	if ((axes & axesAndExtrudersMoved & ~ms.GetAxesAndExtrudersOwned()).IsNonEmpty())
 	{
 		gb.ThrowGCodeException("Axis is already used by a different motion system");
 	}
 	axesAndExtrudersMoved |= axes;
-	ms.axesAndExtrudersOwned |= axes;
+	ms.AllocateAxes(axes, axLetters);
 }
 
-// Allocate additional axes by letter
+// Allocate additional axes by letter when we are doing a standard move. The axis letters are as in the GCode, before we account for coordinate rotation and axis mapping.
+// We must clear out owned axis letters on a tool change, or when coordinate rotation is changed from zero to nonzero
 void GCodes::AllocateAxisLetters(const GCodeBuffer& gb, MovementState& ms, ParameterLettersBitmap axLetters) THROWS(GCodeException)
 {
+#if SUPPORT_COORDINATE_ROTATION
+	// If we are rotating coordinates then X implies Y and vice versa
+	if (g68Angle != 0.0)
+	{
+		if (axLetters.IsBitSet(ParameterLetterToBitNumber('X')))
+		{
+			axLetters.SetBit(ParameterLetterToBitNumber('Y'));
+		}
+		if (axLetters.IsBitSet(ParameterLetterToBitNumber('Y')))
+		{
+			axLetters.SetBit(ParameterLetterToBitNumber('X'));
+		}
+	}
+#endif
+
+	AxesBitmap newAxes;
+	for (size_t axis = 0; axis < numVisibleAxes; ++axis)
+	{
+		const char c = axisLetters[axis];
+		const unsigned int axisLetterBitNumber = ParameterLetterToBitNumber(c);
+		if (axLetters.IsBitSet(axisLetterBitNumber))
+		{
+			if (axis == 0)			// axis 0 is always X
+			{
+				newAxes |= Tool::GetXAxes(ms.currentTool);
+			}
+			else if (axis == 1)		// axis 1 is always Y
+			{
+				newAxes |= Tool::GetYAxes(ms.currentTool);
+			}
+			else
+			{
+				newAxes.SetBit(axis);
+			}
+		}
+	}
+	AllocateAxes(gb, ms, newAxes, axLetters);
+	UpdateAllCoordinates(gb);
+}
+
+// Allocate axes by letter when we are doing a special move. Do not update the map of owned axes letters.
+void GCodes::AllocateAxesDirectFromLetters(const GCodeBuffer& gb, MovementState& ms, ParameterLettersBitmap axLetters) THROWS(GCodeException)
+{
 	AxesBitmap newAxes;
 	for (size_t axis = 0; axis < numVisibleAxes; ++axis)
 	{
 		const char c = axisLetters[axis];
-		const unsigned int axisLetterBit = (c >= 'a') ? c - ('a' - 26) : c - 'A';
-		if (axLetters.IsBitSet(axisLetterBit))
+		const unsigned int axisLetterBitNumber = ParameterLetterToBitNumber(c);
+		if (axLetters.IsBitSet(axisLetterBitNumber))
 		{
 			newAxes.SetBit(axis);
 		}
 	}
-	AllocateAxes(gb, ms, newAxes);
-	ms.ownedAxisLetters |= axLetters;
+	AllocateAxes(gb, ms, newAxes, ParameterLettersBitmap());			// don't own the letters!
+	UpdateAllCoordinates(gb);
 }
 
 // Synchronise motion systems and update user coordinates.
@@ -5027,7 +5082,7 @@ bool GCodes::DoSync(GCodeBuffer& gb) noexcept
 	return rslt;
 }
 
-void GCodes::UpdateAllCoordinates(GCodeBuffer& gb) noexcept
+void GCodes::UpdateAllCoordinates(const GCodeBuffer& gb) noexcept
 {
 	const unsigned int msNumber = gb.GetOwnQueueNumber();
 	memcpyf(moveStates[msNumber].coords, lastKnownMachinePositions, MaxAxes);
diff --git a/src/GCodes/GCodes.h b/src/GCodes/GCodes.h
index c5118440..084fe9e1 100644
--- a/src/GCodes/GCodes.h
+++ b/src/GCodes/GCodes.h
@@ -554,12 +554,14 @@ private:
 	GCodeResult CollisionAvoidance(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeException);			// Handle M597
 	GCodeResult SyncMovementSystems(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeException);		// Handle M598
 	GCodeResult ExecuteM400(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeException);				// Handle M400
-	void AllocateAxes(const GCodeBuffer& gb, MovementState& ms, AxesBitmap axes) THROWS(GCodeException);	// allocate axes to a movement state
+	void AllocateAxes(const GCodeBuffer& gb, MovementState& ms, AxesBitmap axes, ParameterLettersBitmap axLetters) THROWS(GCodeException);	// allocate axes to a movement state
 	void AllocateAxisLetters(const GCodeBuffer& gb, MovementState& ms, ParameterLettersBitmap axLetters) THROWS(GCodeException);
 																											// allocate axes by letter
+	void AllocateAxesDirectFromLetters(const GCodeBuffer& gb, MovementState& ms, ParameterLettersBitmap axLetters) THROWS(GCodeException);
+																											// allocate axes by letter for a special move
 	bool DoSync(GCodeBuffer& gb) noexcept;																	// sync with the other stream returning true if done, false if we need to wait for it
 	bool SyncWith(GCodeBuffer& thisGb, const GCodeBuffer& otherGb) noexcept;								// synchronise motion systems
-	void UpdateAllCoordinates(GCodeBuffer& gb) noexcept;
+	void UpdateAllCoordinates(const GCodeBuffer& gb) noexcept;
 #endif
 
 #if SUPPORT_COORDINATE_ROTATION
diff --git a/src/GCodes/GCodes2.cpp b/src/GCodes/GCodes2.cpp
index 8b0f75ed..7bdb66c4 100644
--- a/src/GCodes/GCodes2.cpp
+++ b/src/GCodes/GCodes2.cpp
@@ -2390,7 +2390,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx
 					}
 					else
 					{
-						reply.printf("Max speeds (%s)): ", (usingMmPerSec) ? "mm/sec" : "mm/min");
+						reply.printf("Max speeds (%s): ", (usingMmPerSec) ? "mm/sec" : "mm/min");
 						for (size_t axis = 0; axis < numTotalAxes; ++axis)
 						{
 							reply.catf("%c: %.1f, ", axisLetters[axis], (double)InverseConvertSpeedToMm(platform.MaxFeedrate(axis), usingMmPerSec));
diff --git a/src/GCodes/GCodes3.cpp b/src/GCodes/GCodes3.cpp
index 779043fb..04aadd9f 100644
--- a/src/GCodes/GCodes3.cpp
+++ b/src/GCodes/GCodes3.cpp
@@ -71,10 +71,21 @@ GCodeResult GCodes::SetPositions(GCodeBuffer& gb, const StringRef& reply) THROWS
 	}
 #endif
 
+	MovementState& ms = GetMovementState(gb);
+
+#if SUPPORT_ASYNC_MOVES
+	// Check for setting unowned axes before processing the command
+	ParameterLettersBitmap axisLettersMentioned = gb.AllParameters() & allAxisLetters;
+	axisLettersMentioned.ClearBits(ms.GetOwnedAxisLetters());
+	if (axisLettersMentioned.IsNonEmpty())
+	{
+		AllocateAxisLetters(gb, ms, axisLettersMentioned);
+	}
+#endif
+
 	// Don't wait for the machine to stop if only extruder drives are being reset.
 	// This avoids blobs and seams when the gcode uses absolute E coordinates and periodically includes G92 E0.
 	AxesBitmap axesIncluded;
-	MovementState& ms = GetMovementState(gb);
 	for (size_t axis = 0; axis < numVisibleAxes; ++axis)
 	{
 		if (gb.Seen(axisLetters[axis]))
@@ -82,7 +93,7 @@ GCodeResult GCodes::SetPositions(GCodeBuffer& gb, const StringRef& reply) THROWS
 			const float axisValue = gb.GetFValue();
 			if (axesIncluded.IsEmpty())
 			{
-				if (!LockAllMovementSystemsAndWaitForStandstill(gb))	// lock movement and get current coordinates
+				if (!LockCurrentMovementSystemAndWaitForStandstill(gb))	// lock movement and get current coordinates
 				{
 					return GCodeResult::notFinished;
 				}
@@ -1537,10 +1548,13 @@ GCodeResult GCodes::HandleG68(GCodeBuffer& gb, const StringRef& reply) THROWS(GC
 		gb.MustSee('A', 'X');
 		centreX = gb.GetFValue();
 		gb.MustSee('B', 'Y');
-		centreY= gb.GetFValue();
+		centreY = gb.GetFValue();
 
 		g68Centre[0] = centreX + GetWorkplaceOffset(gb, 0);
 		g68Centre[1] = centreY + GetWorkplaceOffset(gb, 1);
+#if SUPPORT_ASYNC_MOVES
+		const float oldG68Angle = g68Angle;
+#endif
 		if (gb.Seen('I'))
 		{
 			g68Angle += angle;
@@ -1549,6 +1563,16 @@ GCodeResult GCodes::HandleG68(GCodeBuffer& gb, const StringRef& reply) THROWS(GC
 		{
 			g68Angle = angle;
 		}
+#if SUPPORT_ASYNC_MOVES
+		if (g68Angle != 0.0 && oldG68Angle == 0.0)
+		{
+			// We have just started doing coordinate rotation, so if we own axis letter X we need to own Y and vice versa
+			// Simplest is just to say we don't own either in the axis letters bitmap
+			MovementState& ms = GetMovementState(gb);
+			ms.ReleaseAxisLetter('X');
+			ms.ReleaseAxisLetter('Y');
+		}
+#endif
 		UpdateCurrentUserPosition(gb);
 	}
 	return GCodeResult::ok;
diff --git a/src/GCodes/GCodes4.cpp b/src/GCodes/GCodes4.cpp
index 4a9e169f..db6be018 100644
--- a/src/GCodes/GCodes4.cpp
+++ b/src/GCodes/GCodes4.cpp
@@ -310,6 +310,7 @@ void GCodes::RunStateMachine(GCodeBuffer& gb, const StringRef& reply) noexcept
 			// Test whether the previous homing move homed any axes
 			if (toBeHomed.Disjoint(axesHomed))
 			{
+				//debugPrintf("tbh %04x, ah %04x\n", (unsigned int)toBeHomed.GetRaw(), (unsigned int)axesHomed.GetRaw());
 				reply.copy("Failed to home axes ");
 				AppendAxes(reply, toBeHomed);
 				stateMachineResult = GCodeResult::error;
diff --git a/src/Movement/DDA.cpp b/src/Movement/DDA.cpp
index 2621020e..4804a439 100644
--- a/src/Movement/DDA.cpp
+++ b/src/Movement/DDA.cpp
@@ -1233,30 +1233,14 @@ void DDA::MatchSpeeds() noexcept
 	}
 }
 
-// This is called by Move::CurrentMoveCompleted to update the live coordinates from the move that has just finished
-bool DDA::FetchEndPosition(volatile int32_t ep[MaxAxesPlusExtruders], volatile float endCoords[MaxAxesPlusExtruders]) noexcept
+// This is called by DDARing::LiveCoordinates to get the endpoints of a move that is being executed
+void DDA::FetchCurrentPositions(int32_t ep[MaxAxesPlusExtruders]) const noexcept
 {
-	for (size_t drive = 0; drive < MaxAxesPlusExtruders; ++drive)
-	{
-		ep[drive] = endPoint[drive];
-	}
-	if (flags.endCoordinatesValid)
+	memcpyi32(ep, endPoint, MaxAxesPlusExtruders);
+	for (const DriveMovement* dm = activeDMs; dm != nullptr; dm = dm->nextDM)
 	{
-		const size_t visibleAxes = reprap.GetGCodes().GetVisibleAxes();
-		for (size_t axis = 0; axis < visibleAxes; ++axis)
-		{
-			endCoords[axis] = endCoordinates[axis];
-		}
+		ep[dm->drive] -= dm->GetNetStepsLeft();
 	}
-
-	// Extrusion amounts are always valid
-	const size_t numExtruders = reprap.GetGCodes().GetNumExtruders();
-	for (size_t extruder = 0; extruder < numExtruders; ++extruder)
-	{
-		endCoords[ExtruderToLogicalDrive(extruder)] += endCoordinates[ExtruderToLogicalDrive(extruder)];
-	}
-
-	return flags.endCoordinatesValid;
 }
 
 // This may be called from an ISR, e.g. via Kinematics::OnHomingSwitchTriggered
diff --git a/src/Movement/DDA.h b/src/Movement/DDA.h
index d9d2143e..7bf40d3d 100644
--- a/src/Movement/DDA.h
+++ b/src/Movement/DDA.h
@@ -138,7 +138,8 @@ public:
 	void SetDriveCoordinate(int32_t a, size_t drive) noexcept;						// Force an end point
 	void SetFeedRate(float rate) noexcept { requestedSpeed = rate; }
 	float GetEndCoordinate(size_t drive, bool disableMotorMapping) noexcept;
-	bool FetchEndPosition(volatile int32_t ep[MaxAxesPlusExtruders], volatile float endCoords[MaxAxesPlusExtruders]) noexcept;
+	void FetchEndPoints(int32_t ep[MaxAxesPlusExtruders]) const noexcept;
+	void FetchCurrentPositions(int32_t ep[MaxAxesPlusExtruders]) const noexcept;
 	void SetPositions(const float move[]) noexcept;									// Force the endpoints to be these
 	FilePosition GetFilePosition() const noexcept { return filePos; }
 	float GetRequestedSpeedMmPerClock() const noexcept { return requestedSpeed; }
@@ -454,6 +455,12 @@ inline int32_t DDA::GetStepsTaken(size_t drive) const noexcept
 	return (dmp != nullptr) ? dmp->GetNetStepsTaken() : 0;
 }
 
+// This is called by DDARing::LiveCoordinates to get the endpoints of a move that has been completed
+inline void DDA::FetchEndPoints(int32_t ep[MaxAxesPlusExtruders]) const noexcept
+{
+	memcpyi32(ep, endPoint, MaxAxesPlusExtruders);
+}
+
 #if SUPPORT_CAN_EXPANSION
 
 // Insert a hiccup, returning the amount of time inserted
diff --git a/src/Movement/DDARing.cpp b/src/Movement/DDARing.cpp
index 5b375636..96382faa 100644
--- a/src/Movement/DDARing.cpp
+++ b/src/Movement/DDARing.cpp
@@ -79,9 +79,7 @@ void DDARing::Init2() noexcept
 		for (size_t i = 0; i < MaxAxesPlusExtruders; i++)
 		{
 			pos[i] = 0.0;
-			liveEndPoints[i] = 0;								// not actually right for a delta, but better than printing random values in response to M114
 		}
-		SetLiveCoordinates(pos);
 		SetPositions(pos);
 	}
 
@@ -380,6 +378,7 @@ uint32_t DDARing::Spin(SimulationMode simulationMode, bool waitingForSpace, bool
 				Interrupt(p);
 			}
 			SetBasePriority(0);
+			liveCoordinatesValid = false;
 
 #if SUPPORT_LASER || SUPPORT_IOBITS
 			if (wakeLaser)
@@ -576,9 +575,6 @@ void DDARing::OnMoveCompleted(DDA *cdda, Platform& p) noexcept
 void DDARing::CurrentMoveCompleted() noexcept
 {
 	DDA * const cdda = currentDda;					// capture volatile variable
-	// Save the current motor coordinates, and the machine Cartesian coordinates if known
-	liveCoordinatesValid = cdda->FetchEndPosition(const_cast<int32_t*>(liveEndPoints), const_cast<float *>(liveCoordinates));
-	liveCoordinatesChanged = true;
 
 #if SUPPORT_REMOTE_COMMANDS
 	for (size_t driver = 0; driver < NumDirectDrivers; ++driver)
@@ -658,15 +654,38 @@ void DDARing::GetPartialMachinePosition(float m[MaxAxes], AxesBitmap whichAxes)
 // These are the actual numbers we want in the positions, so don't transform them.
 void DDARing::SetPositions(const float move[MaxAxesPlusExtruders]) noexcept
 {
-	if (   getPointer == addPointer								// by itself this means the ring is empty or full
-		&& addPointer->GetState() == DDA::DDAState::empty
+	AtomicCriticalSectionLocker lock;
+	liveCoordinatesValid = false;
+	DDA::DDAState state;
+	if (   getPointer != addPointer								// OK if the ring is neither empty or full
+		|| (state = addPointer->GetState()) == DDA::DDAState::empty	 || state == DDA::DDAState::completed	// also OK if the ring is not full
 	   )
 	{
 		addPointer->GetPrevious()->SetPositions(move);
 	}
 	else
 	{
-		reprap.GetPlatform().Message(ErrorMessage, "SetPositions called when DDA ring not empty\n");
+		reprap.GetPlatform().Message(ErrorMessage, "SetPositions called when DDA ring is full\n");
+	}
+}
+
+void DDARing::ResetExtruderPositions() noexcept
+{
+	AtomicCriticalSectionLocker lock;
+	liveCoordinatesValid = false;
+	DDA::DDAState state;
+	if (   getPointer != addPointer								// OK if the ring is neither empty or full
+		|| (state = addPointer->GetState()) == DDA::DDAState::empty	 || state == DDA::DDAState::completed	// also OK if the ring is not full
+	   )
+	{
+		for (size_t eDrive = MaxAxesPlusExtruders - reprap.GetGCodes().GetNumExtruders(); eDrive < MaxAxesPlusExtruders; eDrive++)
+		{
+			liveCoordinates[eDrive] = 0.0;
+		}
+	}
+	else
+	{
+		reprap.GetPlatform().Message(ErrorMessage, "ResetExtruderPositions called when DDA ring is full\n");
 	}
 }
 
@@ -681,22 +700,30 @@ void DDARing::AdjustMotorPositions(const float adjustment[], size_t numMotors) n
 	{
 		const int32_t ep = endCoordinates[drive] + lrintf(adjustment[drive] * driveStepsPerUnit[drive]);
 		lastQueuedMove->SetDriveCoordinate(ep, drive);
-		liveEndPoints[drive] = ep;
 	}
 
 	liveCoordinatesValid = false;		// force the live XYZ position to be recalculated
-	liveCoordinatesChanged = true;
 }
 
-// Fetch the current live XYZ and extruder coordinates if they have changed since this was lass called
-// Interrupts are assumed enabled on entry
-bool DDARing::LiveCoordinates(float m[MaxAxesPlusExtruders]) noexcept
+// Get the live motor positions
+void DDARing::GetCurrentMotorPositions(int32_t pos[MaxAxesPlusExtruders]) const noexcept
 {
-	if (!liveCoordinatesChanged)
+	AtomicCriticalSectionLocker lock;
+	const DDA *const cdda = currentDda;									// capture volatile variable
+	if (cdda == nullptr)
 	{
-		return false;
+		getPointer->GetPrevious()->FetchEndPoints(pos);
 	}
+	else
+	{
+		cdda->FetchCurrentPositions(pos);
+	}
+}
 
+// Fetch the current axis and extruder coordinates
+// Interrupts are assumed enabled on entry
+void DDARing::LiveCoordinates(float m[MaxAxesPlusExtruders]) noexcept
+{
 	// The live coordinates and live endpoints are modified by the ISR, so be careful to get a self-consistent set of them
 	const size_t numVisibleAxes = reprap.GetGCodes().GetVisibleAxes();		// do this before we disable interrupts
 	const size_t numTotalAxes = reprap.GetGCodes().GetTotalAxes();			// do this before we disable interrupts
@@ -705,55 +732,34 @@ bool DDARing::LiveCoordinates(float m[MaxAxesPlusExtruders]) noexcept
 	{
 		// All coordinates are valid, so copy them across
 		memcpyf(m, const_cast<const float *>(liveCoordinates), MaxAxesPlusExtruders);
-		liveCoordinatesChanged = false;
 		IrqEnable();
 	}
 	else
 	{
-		// Only the extruder coordinates are valid, so we need to convert the motor endpoints to coordinates
-		memcpyf(m + numTotalAxes, const_cast<const float *>(liveCoordinates + numTotalAxes), MaxAxesPlusExtruders - numTotalAxes);
-		int32_t tempEndPoints[MaxAxes];
-		memcpyi32(tempEndPoints, const_cast<const int32_t*>(liveEndPoints), ARRAY_SIZE(tempEndPoints));
+		// Get the positions of each motor
+		int32_t currentMotorPositions[MaxAxesPlusExtruders];
+		GetCurrentMotorPositions(currentMotorPositions);
+		const DDA * const cdda = currentDda;
 		IrqEnable();
 
-		reprap.GetMove().MotorStepsToCartesian(tempEndPoints, numVisibleAxes, numTotalAxes, m);		// this is slow, so do it with interrupts enabled
-
-		// If the ISR has not updated the endpoints, store the live coordinates back so that we don't need to do it again
-		IrqDisable();
-		if (memcmp(tempEndPoints, const_cast<const int32_t*>(liveEndPoints), sizeof(tempEndPoints)) == 0)
+		reprap.GetMove().MotorStepsToCartesian(currentMotorPositions, numVisibleAxes, numTotalAxes, m);		// this is slow, so do it with interrupts enabled
+		for (size_t i = MaxAxesPlusExtruders - reprap.GetGCodes().GetNumExtruders(); i < MaxAxesPlusExtruders; ++i)
 		{
-			memcpyf(const_cast<float *>(liveCoordinates), m, numVisibleAxes);
-			liveCoordinatesValid = true;
-			liveCoordinatesChanged = false;
+			m[i] = currentMotorPositions[i] / reprap.GetPlatform().DriveStepsPerUnit(i);
 		}
-		IrqEnable();
-	}
-	return true;
-}
 
-// These are the actual numbers that we want to be the coordinates, so don't transform them.
-// The caller must make sure that no moves are in progress or pending when calling this
-void DDARing::SetLiveCoordinates(const float coords[MaxAxesPlusExtruders]) noexcept
-{
-	const size_t numAxes = reprap.GetGCodes().GetVisibleAxes();
-	for (size_t drive = 0; drive < numAxes; drive++)
-	{
-		liveCoordinates[drive] = coords[drive];
-	}
-	liveCoordinatesValid = true;
-	liveCoordinatesChanged = true;
-	(void)reprap.GetMove().CartesianToMotorSteps(coords, const_cast<int32_t *>(liveEndPoints), true);
-}
-
-void DDARing::ResetExtruderPositions() noexcept
-{
-	IrqDisable();
-	for (size_t eDrive = reprap.GetGCodes().GetTotalAxes(); eDrive < MaxAxesPlusExtruders; eDrive++)
-	{
-		liveCoordinates[eDrive] = 0.0;
+		// Optimisation: if no movement, save the positions for next time
+		if (cdda == nullptr)
+		{
+			IrqDisable();
+			if (currentDda == nullptr)
+			{
+				memcpyf(const_cast<float *>(liveCoordinates), m, MaxAxesPlusExtruders);
+				liveCoordinatesValid = true;
+			}
+			IrqEnable();
+		}
 	}
-	IrqEnable();
-	liveCoordinatesChanged = true;
 }
 
 float DDARing::GetRequestedSpeedMmPerSec() const noexcept
diff --git a/src/Movement/DDARing.h b/src/Movement/DDARing.h
index ea15bf34..0a2d31d6 100644
--- a/src/Movement/DDARing.h
+++ b/src/Movement/DDARing.h
@@ -62,17 +62,15 @@ public:
 	float GetDecelerationMmPerSecSquared() const noexcept;
 	float GetTotalExtrusionRate() const noexcept;
 
-	int32_t GetEndPoint(size_t drive) const noexcept { return liveEndPoints[drive]; } 	// Get the current position of a motor
-	void GetCurrentMachinePosition(float m[MaxAxes], bool disableMotorMapping) const noexcept; // Get the current position in untransformed coords
+	void GetCurrentMachinePosition(float m[MaxAxes], bool disableMotorMapping) const noexcept; // Get the position at the end of the last queued move in untransformed coords
 #if SUPPORT_ASYNC_MOVES
 	void GetPartialMachinePosition(float m[MaxAxes], AxesBitmap whichAxes) const noexcept;	// Return the machine coordinates of just some axes
 #endif
 
 	void SetPositions(const float move[MaxAxesPlusExtruders]) noexcept;					// Force the machine coordinates to be these
 	void AdjustMotorPositions(const float adjustment[], size_t numMotors) noexcept;		// Perform motor endpoint adjustment
-	bool LiveCoordinates(float m[MaxAxesPlusExtruders]) noexcept;						// Fetch the last point at the end of the last completed DDA if it has changed since we last called this
-	void SetLiveCoordinates(const float coords[MaxAxesPlusExtruders]) noexcept;			// Force the live coordinates (see above) to be these
-	bool HaveLiveCoordinatesChanged() const noexcept { return liveCoordinatesChanged; }
+	void GetCurrentMotorPositions(int32_t pos[MaxAxesPlusExtruders]) const noexcept;	// Get the live motor positions
+	void LiveCoordinates(float m[MaxAxesPlusExtruders]) noexcept;						// Fetch the last point at the end of the last completed DDA
 	void ResetExtruderPositions() noexcept;												// Resets the extrusion amounts of the live coordinates
 
 	bool PauseMoves(RestorePoint& rp) noexcept;											// Pause the print as soon as we can, returning true if we were able to skip any
@@ -121,7 +119,6 @@ private:
 	StepTimer timer;															// Timer object to control getting step interrupts
 
 	volatile float liveCoordinates[MaxAxesPlusExtruders];						// The endpoint that the machine moved to in the last completed move
-	volatile int32_t liveEndPoints[MaxAxesPlusExtruders];						// The XYZ endpoints of the last completed move in motor coordinates
 
 	unsigned int numDdasInRing;
 	uint32_t gracePeriod;														// The minimum idle time in milliseconds, before we should start a move. Better to have a few moves in the queue so that we can do lookahead
@@ -145,7 +142,6 @@ private:
 
 	volatile bool extrudersPrinting;											// Set whenever an extruder starts a printing move, cleared by a non-printing extruder move
 	volatile bool liveCoordinatesValid;											// True if the XYZ live coordinates in liveCoordinates are reliable (the extruder ones always are)
-	volatile bool liveCoordinatesChanged;										// True if the live coordinates have changed since LiveCoordinates was last called
 	volatile bool waitingForRingToEmpty;										// True if Move has signalled that we are waiting for this ring to empty
 };
 
diff --git a/src/Movement/Move.cpp b/src/Movement/Move.cpp
index 26277d12..b3ff43cd 100644
--- a/src/Movement/Move.cpp
+++ b/src/Movement/Move.cpp
@@ -507,7 +507,7 @@ void Move::Diagnostics(MessageType mtype) noexcept
 #if 0	// debug only
 	String<StringLength256> scratchString;
 #else
-	String<StringLength50> scratchString;
+	String<StringLength100> scratchString;
 #endif
 	scratchString.copy(GetCompensationTypeString());
 
@@ -549,6 +549,10 @@ void Move::Diagnostics(MessageType mtype) noexcept
 	maxDelay = maxDelayIncrease = 0;
 #endif
 
+	scratchString.Clear();
+	StepTimer::Diagnostics(scratchString.GetRef());
+	p.MessageF(mtype, "%s\n", scratchString.c_str());
+
 	for (size_t i = 0; i < ARRAY_SIZE(rings); ++i)
 	{
 		rings[i].Diagnostics(mtype, i);
@@ -561,8 +565,6 @@ void Move::SetNewPosition(const float positionNow[MaxAxesPlusExtruders], bool do
 	float newPos[MaxAxesPlusExtruders];
 	memcpyf(newPos, positionNow, ARRAY_SIZE(newPos));			// copy to local storage because Transform modifies it
 	AxisAndBedTransform(newPos, reprap.GetGCodes().GetMovementState(queueNumber).currentTool, doBedCompensation);
-
-	rings[queueNumber].SetLiveCoordinates(newPos);
 	rings[queueNumber].SetPositions(newPos);
 }
 
@@ -1200,14 +1202,17 @@ void Move::RevertPosition(const CanMessageRevertPosition& msg) noexcept
 
 #endif
 
-// Return the current live XYZ and extruder coordinates
+// Return the current machine axis and extruder coordinates. They are needed only to service status requests from DWC, PanelDue, M114.
+// Transforming the machine motor coordinates to Cartesian coordinates is quite expensive, and a status request or object model request will call this for each axis.
+// So we cache the latest coordinates and only update them if it is some time since we last did
 // Interrupts are assumed enabled on entry
 float Move::LiveCoordinate(unsigned int axisOrExtruder, const Tool *tool) noexcept
 {
-	if (rings[0].HaveLiveCoordinatesChanged())
+	if (millis() - latestLiveCoordinatesFetchedAt > 200)
 	{
 		rings[0].LiveCoordinates(latestLiveCoordinates);
 		InverseAxisAndBedTransform(latestLiveCoordinates, tool);
+		latestLiveCoordinatesFetchedAt = millis();
 	}
 	return latestLiveCoordinates[axisOrExtruder];
 }
diff --git a/src/Movement/Move.h b/src/Movement/Move.h
index 27726ea5..ff2a37cb 100644
--- a/src/Movement/Move.h
+++ b/src/Movement/Move.h
@@ -63,7 +63,7 @@ public:
 #endif
 	void GetCurrentUserPosition(float m[MaxAxes], uint8_t moveType, const Tool *tool) const noexcept;
 																			// Return the position (after all queued moves have been executed) in transformed coords
-	int32_t GetEndPoint(size_t drive) const noexcept;					 	// Get the current position of a motor
+	void GetLivePositions(int32_t pos[MaxAxesPlusExtruders]) const noexcept;
 	float LiveCoordinate(unsigned int axisOrExtruder, const Tool *tool) noexcept; // Gives the last point at the end of the last complete DDA
 	void MoveAvailable() noexcept;											// Called from GCodes to tell the Move task that a move is available
 	bool WaitingForAllMovesFinished(size_t queueNumber) noexcept
@@ -300,7 +300,8 @@ private:
 	AxisShaper axisShaper;
 	ExtruderShaper extruderShapers[MaxExtruders];
 
-	float latestLiveCoordinates[MaxAxesPlusExtruders];
+	float latestLiveCoordinates[MaxAxesPlusExtruders];	// the most recent set of live coordinates that we fetched
+	uint32_t latestLiveCoordinatesFetchedAt = 0;		// when we fetched the live coordinates
 	float specialMoveCoords[MaxDriversPerAxis];			// Amounts by which to move individual Z motors (leadscrew adjustment move)
 
 	uint8_t numCalibratedFactors;
@@ -333,10 +334,9 @@ inline void Move::GetPartialMachinePosition(float m[MaxAxes], AxesBitmap whichAx
 
 #endif
 
-// Get the current position of a motor
-inline int32_t Move::GetEndPoint(size_t drive) const noexcept
+inline void Move::GetLivePositions(int32_t pos[MaxAxesPlusExtruders]) const noexcept
 {
-	return rings[0].GetEndPoint(drive);
+	return rings[0].GetCurrentMotorPositions(pos);
 }
 
 // Perform motor endpoint adjustment
diff --git a/src/Movement/RawMove.cpp b/src/Movement/RawMove.cpp
index 9d78fa10..91162dd7 100644
--- a/src/Movement/RawMove.cpp
+++ b/src/Movement/RawMove.cpp
@@ -106,7 +106,7 @@ void MovementState::Diagnostics(MessageType mtype, unsigned int moveSystemNumber
 {
 	reprap.GetPlatform().MessageF(mtype, "Q%u segments left %u"
 #if SUPPORT_ASYNC_MOVES
-											", axes/extruders owned %03x"
+											", axes/extruders owned 0x%07x"
 #endif
 												"\n",
 													moveSystemNumber,
@@ -226,6 +226,13 @@ void MovementState::InitObjectCancellation() noexcept
 	currentObjectCancelled = printingJustResumed = false;
 }
 
+// When releasing axes we must also release the corresponding axis letters, because they serve as a cache
+void MovementState::ReleaseOwnedAxesAndExtruders() noexcept
+{
+	axesAndExtrudersOwned.Clear();
+	ownedAxisLetters.Clear();
+}
+
 #if SUPPORT_ASYNC_MOVES
 
 void AsyncMove::SetDefaults() noexcept
diff --git a/src/Movement/RawMove.h b/src/Movement/RawMove.h
index 99b11d1b..e3f72656 100644
--- a/src/Movement/RawMove.h
+++ b/src/Movement/RawMove.h
@@ -76,11 +76,46 @@ constexpr size_t ResumeObjectRestorePointNumber = NumVisibleRestorePoints + 1;
 
 // Details of a move that are needed only by GCodes
 // CAUTION: segmentsLeft should ONLY be changed from 0 to not 0 by calling NewMoveAvailable()!
-struct MovementState : public RawMove
+class MovementState : public RawMove
 {
+public:
+	AxesBitmap GetAxesAndExtrudersOwned() const noexcept { return axesAndExtrudersOwned; }		// Get the axes and extruders that this movement system owns
+	ParameterLettersBitmap GetOwnedAxisLetters() const noexcept { return ownedAxisLetters; }	// Get the letters denoting axes that this movement system owns
+	void AllocateAxes(AxesBitmap axes, ParameterLettersBitmap axisLetters) noexcept;
+	void ReleaseOwnedAxesAndExtruders() noexcept;
+	void ReleaseAxisLetter(char letter) noexcept;											// stop claiming that we own an axis letter (if we do) but don't release the associated axis
+
+	float GetProportionDone() const noexcept;												// get the proportion of this whole move that has been completed, based on segmentsLeft and totalSegments
+	void Reset() noexcept;
+	void ChangeExtrusionFactor(unsigned int extruder, float multiplier) noexcept;			// change the extrusion factor of an extruder
+	const RestorePoint& GetRestorePoint(size_t n) const pre(n < NumTotalRestorePoints) { return restorePoints[n]; }
+	void ClearMove() noexcept;
+	void SavePosition(unsigned int restorePointNumber, size_t numAxes, float p_feedRate, FilePosition p_filePos) noexcept
+		pre(restorePointNumber < NumTotalRestorePoints);
+
+	// Tool management
+	void SelectTool(int toolNumber, bool simulating) noexcept;
+	ReadLockedPointer<Tool> GetLockedCurrentTool() const noexcept;
+	ReadLockedPointer<Tool> GetLockedCurrentOrDefaultTool() const noexcept;
+	int GetCurrentToolNumber() const noexcept;
+	void SetPreviousToolNumber() noexcept;
+	AxesBitmap GetCurrentXAxes() const noexcept;											// Get the current axes used as X axes
+	AxesBitmap GetCurrentYAxes() const noexcept;											// Get the current axes used as Y axes
+	AxesBitmap GetCurrentAxisMapping(unsigned int axis) const noexcept;
+	float GetCurrentToolOffset(size_t axis) const noexcept;									// Get an axis offset of the current tool
+
+	// Object cancellation support
+	void InitObjectCancellation() noexcept;
+	bool IsCurrentObjectCancelled() const noexcept { return currentObjectCancelled; }
+	bool IsFirstMoveSincePrintingResumed() const noexcept { return printingJustResumed; }
+	void DoneMoveSincePrintingResumed() noexcept { printingJustResumed = false; }
+	void StopPrinting(GCodeBuffer& gb) noexcept;
+	void ResumePrinting(GCodeBuffer& gb) noexcept;
+
+	void Diagnostics(MessageType mtype, unsigned int moveSystemNumber) noexcept;
+
+	// These variables are currently all public, but we ought to make most of them private
 	Tool *currentTool;												// the current tool of this movement system
-	AxesBitmap axesAndExtrudersOwned;								// axes and extruders that this movement system has moved since the last sync
-	ParameterLettersBitmap ownedAxisLetters;						// letters denoting axes that this movement system owns
 
 	// The current user position now holds the requested user position after applying workplace coordinate offsets.
 	// So we must subtract the workplace coordinate offsets when we want to display them.
@@ -142,35 +177,22 @@ struct MovementState : public RawMove
 	bool currentObjectCancelled;									// true if the current object should not be printed
 	bool printingJustResumed;										// true if we have just restarted printing
 
-	float GetProportionDone() const noexcept;						// get the proportion of this whole move that has been completed, based on segmentsLeft and totalSegments
-	void Reset() noexcept;
-	void ChangeExtrusionFactor(unsigned int extruder, float multiplier) noexcept;	// change the extrusion factor of an extruder
-	const RestorePoint& GetRestorePoint(size_t n) const pre(n < NumTotalRestorePoints) { return restorePoints[n]; }
-	void ClearMove() noexcept;
-	void SavePosition(unsigned int restorePointNumber, size_t numAxes, float p_feedRate, FilePosition p_filePos) noexcept
-		pre(restorePointNumber < NumTotalRestorePoints);
-
-	// Tool management
-	void SelectTool(int toolNumber, bool simulating) noexcept;
-	ReadLockedPointer<Tool> GetLockedCurrentTool() const noexcept;
-	ReadLockedPointer<Tool> GetLockedCurrentOrDefaultTool() const noexcept;
-	int GetCurrentToolNumber() const noexcept;
-	void SetPreviousToolNumber() noexcept;
-	AxesBitmap GetCurrentXAxes() const noexcept;											// Get the current axes used as X axes
-	AxesBitmap GetCurrentYAxes() const noexcept;											// Get the current axes used as Y axes
-	AxesBitmap GetCurrentAxisMapping(unsigned int axis) const noexcept;
-	float GetCurrentToolOffset(size_t axis) const noexcept;									// Get an axis offset of the current tool
+private:
+	AxesBitmap axesAndExtrudersOwned;								// axes and extruders that this movement system has moved since the last sync
+	ParameterLettersBitmap ownedAxisLetters;						// letters denoting axes that this movement system owns
+};
 
-	// Object cancellation support
-	void InitObjectCancellation() noexcept;
-	bool IsCurrentObjectCancelled() const noexcept { return currentObjectCancelled; }
-	bool IsFirstMoveSincePrintingResumed() const noexcept { return printingJustResumed; }
-	void DoneMoveSincePrintingResumed() noexcept { printingJustResumed = false; }
-	void StopPrinting(GCodeBuffer& gb) noexcept;
-	void ResumePrinting(GCodeBuffer& gb) noexcept;
+inline void MovementState::AllocateAxes(AxesBitmap axes, ParameterLettersBitmap axisLetters) noexcept
+{
+	axesAndExtrudersOwned |= axes;
+	ownedAxisLetters |= axisLetters;
+}
 
-	void Diagnostics(MessageType mtype, unsigned int moveSystemNumber) noexcept;
-};
+// Stop claiming that we own an axis letter (if we do) but don't release the associated axis
+inline void MovementState::ReleaseAxisLetter(char letter) noexcept
+{
+	ownedAxisLetters.ClearBit(ParameterLetterToBitNumber(letter));
+}
 
 #if SUPPORT_ASYNC_MOVES
 
diff --git a/src/Movement/StepTimer.cpp b/src/Movement/StepTimer.cpp
index 5edb2b00..e14a8859 100644
--- a/src/Movement/StepTimer.cpp
+++ b/src/Movement/StepTimer.cpp
@@ -220,9 +220,6 @@ bool StepTimer::ScheduleTimerInterrupt(uint32_t tim) noexcept
 	while (StepTc->SYNCBUSY.reg & TC_SYNCBUSY_CC0) { }
 	StepTc->INTFLAG.reg = TC_INTFLAG_MC0;							// clear any existing compare match
 	StepTc->INTENSET.reg = TC_INTFLAG_MC0;
-#elif defined(__LPC17xx__)
-	STEP_TC->MR[0] = tim;											// set MR0 compare register
-	STEP_TC->MCR |= (1u<<SBIT_MR0I);									// enable interrupt on MR0 match
 #else
 	STEP_TC->TC_CHANNEL[STEP_TC_CHAN].TC_RB = tim;					// set up the compare register
 	(void)STEP_TC->TC_CHANNEL[STEP_TC_CHAN].TC_SR;					// read the status register, which clears the status bits and any pending interrupt
@@ -353,7 +350,7 @@ void StepTimer::DisableTimerInterrupt() noexcept
 #endif
 
 // The guts of the ISR
-/*static*/ void StepTimer::Interrupt() noexcept
+/*static*/ inline void StepTimer::Interrupt() noexcept
 {
 	StepTimer * tmr = pendingList;
 	if (tmr != nullptr)
@@ -442,7 +439,8 @@ bool StepTimer::ScheduleCallbackFromIsr() noexcept
 	}
 
 	// Optimise the common case i.e. no other timer is pending
-	if (pendingList == nullptr)
+	StepTimer *tmr = pendingList;			// capture volatile variable
+	if (tmr == nullptr)
 	{
 		if (ScheduleTimerInterrupt(whenDue))
 		{
@@ -450,32 +448,33 @@ bool StepTimer::ScheduleCallbackFromIsr() noexcept
 		}
 		next = nullptr;
 		pendingList = this;
-		active = true;
-		return false;
 	}
-
-	// Another timer is already pending
-	const Ticks now = GetTimerTicks();
-	const int32_t howSoon = (int32_t)(whenDue - now);
-	StepTimer** ppst = const_cast<StepTimer**>(&pendingList);
-	if (howSoon < (int32_t)((*ppst)->whenDue - now))
+	else
 	{
-		// This one is due earlier than the first existing one
-		if (ScheduleTimerInterrupt(whenDue))
+		// Another timer is already pending
+		const Ticks now = GetTimerTicks();
+		const int32_t howSoon = (int32_t)(whenDue - now);
+		if (howSoon < (int32_t)(tmr->whenDue - now))
 		{
-			return true;
+			// This one is due earlier than the first existing one
+			if (ScheduleTimerInterrupt(whenDue))
+			{
+				return true;
+			}
+			next = tmr;
+			pendingList = this;
 		}
-	}
-	else
-	{
-		while (*ppst != nullptr && (int32_t)((*ppst)->whenDue - now) < howSoon)
+		else
 		{
-			ppst = &((*ppst)->next);
+			while (tmr->next != nullptr && (int32_t)(tmr->next->whenDue - now) < howSoon)
+			{
+				tmr = tmr->next;
+			}
+			next = tmr->next;
+			tmr->next = this;
 		}
 	}
 
-	next = *ppst;
-	*ppst = this;
 	active = true;
 	return false;
 }
@@ -515,16 +514,17 @@ extern "C" uint32_t StepTimerGetTimerTicks() noexcept
 	return StepTimer::GetTimerTicks();
 }
 
-#if SUPPORT_REMOTE_COMMANDS
-
-// Remote diagnostics
 /*static*/ void StepTimer::Diagnostics(const StringRef& reply) noexcept
 {
-	reply.lcatf("Peak sync jitter %" PRIi32 "/%" PRIi32 ", peak Rx sync delay %" PRIu32 ", resyncs %u/%u, ", peakNegJitter, peakPosJitter, peakReceiveDelay, numTimeoutResyncs, numJitterResyncs);
-	gotJitter = false;
-	numTimeoutResyncs = numJitterResyncs = 0;
-	peakReceiveDelay = 0;
-
+#if SUPPORT_REMOTE_COMMANDS
+	if (CanInterface::InExpansionMode())
+	{
+		reply.lcatf("Peak sync jitter %" PRIi32 "/%" PRIi32 ", peak Rx sync delay %" PRIu32 ", resyncs %u/%u, ", peakNegJitter, peakPosJitter, peakReceiveDelay, numTimeoutResyncs, numJitterResyncs);
+		gotJitter = false;
+		numTimeoutResyncs = numJitterResyncs = 0;
+		peakReceiveDelay = 0;
+	}
+#endif
 	StepTimer *pst = pendingList;
 	if (pst == nullptr)
 	{
@@ -536,13 +536,15 @@ extern "C" uint32_t StepTimerGetTimerTicks() noexcept
 					pst->whenDue - GetTimerTicks(),
 # if SAME5x
 					((StepTc->INTENSET.reg & TC_INTFLAG_MC0) == 0)
-# elif SAME70
+# elif SAME70 || SAM4E || SAM4S
 					((STEP_TC->TC_CHANNEL[STEP_TC_CHAN].TC_IER & TC_IER_CPBS) == 0)
 # endif
 						? "disabled" : "enabled");
 # if SAME5x
 		if (StepTc->CC[0].reg != pst->whenDue)
-# elif SAME70
+# elif SAM4E
+		if (STEP_TC->TC_CHANNEL[STEP_TC_CHAN].TC_RB != pst->whenDue)
+# elif SAME70 || SAM4S
 		if (STEP_TC->TC_CHANNEL[STEP_TC_CHAN].TC_RB != (uint16_t)pst->whenDue)
 # endif
 		{
@@ -551,6 +553,4 @@ extern "C" uint32_t StepTimerGetTimerTicks() noexcept
 	}
 }
 
-#endif
-
 // End
diff --git a/src/Movement/StepTimer.h b/src/Movement/StepTimer.h
index f0080cd7..ae36797e 100644
--- a/src/Movement/StepTimer.h
+++ b/src/Movement/StepTimer.h
@@ -63,6 +63,9 @@ public:
 	// ISR called from StepTimer
 	static void Interrupt() noexcept;
 
+	// Append diagnostics to reply string
+	static void Diagnostics(const StringRef& reply) noexcept;
+
 	static constexpr uint32_t MinInterruptInterval = 6;							// Minimum interval between step timer interrupts, in step clocks; about 6us
 
 #if SUPPORT_REMOTE_COMMANDS
@@ -71,9 +74,7 @@ public:
 	static uint32_t ConvertToLocalTime(uint32_t masterTime) noexcept { return masterTime + localTimeOffset; }
 	static uint32_t ConvertToMasterTime(uint32_t localTime) noexcept { return localTime - localTimeOffset; }
 	static uint32_t GetMasterTime() noexcept { return ConvertToMasterTime(GetTimerTicks()); }
-
 	static bool IsSynced() noexcept;
-	static void Diagnostics(const StringRef& reply) noexcept;
 
 	static constexpr uint32_t MinSyncInterval = 2000;							// maximum interval in milliseconds between sync messages for us to remain synced
 																				// increased from 1000 because of workaround we added for bad Tx time stamps on SAME70
diff --git a/src/Platform/Platform.cpp b/src/Platform/Platform.cpp
index 5dc74632..72ebaa4f 100644
--- a/src/Platform/Platform.cpp
+++ b/src/Platform/Platform.cpp
@@ -5143,6 +5143,8 @@ GCodeResult Platform::EutProcessM569Point7(const CanMessageGeneric& msg, const S
 		String<StringLength20> portName;
 		parser.GetStringParam('C', portName.GetRef());
 		return GetGCodeResultFromSuccess(brakePorts[drive].AssignPort(portName.c_str(), reply, PinUsedBy::gpout, PinAccess::write0));
+		//TODO use the following instead when we track the enable state of each driver individually
+		//return GetGCodeResultFromSuccess(brakePorts[drive].AssignPort(portName.c_str(), reply, PinUsedBy::gpout, (driverDisabled[drive]) ? PinAccess::write0 : PinAccess::write1));
 	}
 
 	reply.printf("Driver %u.%u uses brake port ", CanInterface::GetCanAddress(), drive);
diff --git a/src/RepRapFirmware.h b/src/RepRapFirmware.h
index a994f734..1f1e1a9d 100644
--- a/src/RepRapFirmware.h
+++ b/src/RepRapFirmware.h
@@ -382,6 +382,11 @@ union LaserPwmOrIoBits
 };
 #endif
 
+// Find the bit number corresponding to a parameter letter
+constexpr unsigned int ParameterLetterToBitNumber(char c) noexcept
+{
+	return (c <= 'Z') ? c - 'A' : c - ('a' - 26);
+}
 // Debugging support
 extern "C" void debugPrintf(const char* fmt, ...) noexcept __attribute__ ((format (printf, 1, 2)));
 #define DEBUG_HERE do { debugPrintf("At " __FILE__ " line %d\n", __LINE__); delay(50); } while (false)