Fixes to multiple motion systems. Tool selection not fully working.

3 files changed, 23 insertions, 11 deletions

diff --git a/src/GCodes/GCodes.cpp b/src/GCodes/GCodes.cpp
index 9142196f..ccfd4ac2 100644
--- a/src/GCodes/GCodes.cpp
+++ b/src/GCodes/GCodes.cpp
@@ -175,7 +175,7 @@ void GCodes::Init() noexcept
 	axisLetters[1] = 'Y';
 	axisLetters[2] = 'Z';
 #if SUPPORT_ASYNC_MOVES
-	allAxisLetters = ParameterLettersBitmap(ParameterLetterToBitNumber('X') | ParameterLetterToBitNumber('Y') | ParameterLetterToBitNumber('Z'));
+	allAxisLetters = ParameterLettersToBitmap("XYZ");
 #endif
 
 	numExtruders = 0;
@@ -4968,6 +4968,7 @@ void GCodes::AllocateAxes(const GCodeBuffer& gb, MovementState& ms, AxesBitmap a
 		//TODO report the lowest axis letter that is already allocated
 		gb.ThrowGCodeException("Axis is already used by a different motion system");
 	}
+	UpdateUserPositionFromMachinePosition(gb, ms);
 }
 
 // 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.
@@ -4989,7 +4990,6 @@ void GCodes::AllocateAxisLetters(const GCodeBuffer& gb, MovementState& ms, Param
 	}
 # endif
 
-	ms.SaveOwnAxisCoordinates();
 	AxesBitmap newAxes;
 	for (size_t axis = 0; axis < numVisibleAxes; ++axis)
 	{
@@ -5014,13 +5014,11 @@ void GCodes::AllocateAxisLetters(const GCodeBuffer& gb, MovementState& ms, Param
 		}
 	}
 	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)
 {
-	ms.SaveOwnAxisCoordinates();
 	AxesBitmap newAxes;
 	for (size_t axis = 0; axis < numVisibleAxes; ++axis)
 	{
@@ -5032,7 +5030,6 @@ void GCodes::AllocateAxesDirectFromLetters(const GCodeBuffer& gb, MovementState&
 		}
 	}
 	AllocateAxes(gb, ms, newAxes, ParameterLettersBitmap());			// don't own the letters!
-	UpdateAllCoordinates(gb);
 }
 
 // Synchronise motion systems and update user coordinates.
@@ -5047,7 +5044,7 @@ bool GCodes::SyncWith(GCodeBuffer& thisGb, const GCodeBuffer& otherGb) noexcept
 	switch (thisGb.syncState)
 	{
 	case GCodeBuffer::SyncState::running:
-		thisGb.syncState = GCodeBuffer::SyncState::syncing;					// tell other input channels that we are waiting for sync
+		thisGb.syncState = GCodeBuffer::SyncState::syncing;				// tell other input channels that we are waiting for sync
 		// no break
 	case GCodeBuffer::SyncState::syncing:
 		if (otherGb.syncState == GCodeBuffer::SyncState::running)
@@ -5107,6 +5104,7 @@ bool GCodes::DoSync(GCodeBuffer& gb) noexcept
 	return rslt;
 }
 
+// Update our machine coordinates from the set of last stored coordinates. If we have moved any axes then we must call ms.SaveOwnAxisPositions before calling this.
 void GCodes::UpdateAllCoordinates(const GCodeBuffer& gb) noexcept
 {
 	const unsigned int msNumber = gb.GetOwnQueueNumber();
diff --git a/src/Movement/Move.h b/src/Movement/Move.h
index ff2a37cb..2221c4f1 100644
--- a/src/Movement/Move.h
+++ b/src/Movement/Move.h
@@ -59,7 +59,10 @@ public:
 
 	void GetCurrentMachinePosition(float m[MaxAxes], bool disableMotorMapping) const noexcept; // Get the current position in untransformed coords
 #if SUPPORT_ASYNC_MOVES
-	void GetPartialMachinePosition(float m[MaxAxes], AxesBitmap whichAxes, unsigned int queueNumber) const noexcept;	// Get the current position of some axes from one of the rings
+	void GetPartialMachinePosition(float m[MaxAxes], AxesBitmap whichAxes, unsigned int queueNumber) const noexcept
+			pre(queueNumber < NumMovementSystems);							// Get the current position of some axes from one of the rings
+	void SetRawPosition(const float positions[MaxAxesPlusExtruders], unsigned int queueNumber) noexcept
+			pre(queueNumber < NumMovementSystems);							// Set the current position to be this without transforming them first
 #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
@@ -332,6 +335,12 @@ inline void Move::GetPartialMachinePosition(float m[MaxAxes], AxesBitmap whichAx
 	rings[queueNumber].GetPartialMachinePosition(m, whichAxes);
 }
 
+// Set the current position to be this without transforming them first
+inline void Move::SetRawPosition(const float positions[MaxAxesPlusExtruders], unsigned int queueNumber) noexcept
+{
+	rings[queueNumber].SetPositions(positions);
+}
+
 #endif
 
 inline void Move::GetLivePositions(int32_t pos[MaxAxesPlusExtruders]) const noexcept
diff --git a/src/Movement/RawMove.cpp b/src/Movement/RawMove.cpp
index a2e43fd3..5b45bf25 100644
--- a/src/Movement/RawMove.cpp
+++ b/src/Movement/RawMove.cpp
@@ -59,7 +59,7 @@ float MovementState::GetProportionDone() const noexcept
 	return (float)(totalSegments - segmentsLeft)/(float)totalSegments;
 }
 
-// Initialise this MovementState. If SUPPORT_ASYNC_MOVES is set then must call MovementState::GlobalInit before calling this.
+// Initialise this MovementState. If SUPPORT_ASYNC_MOVES is set then must call MovementState::GlobalInit before calling this to initialise lastKnownMachinePositions.
 void MovementState::Init(unsigned int p_msNumber) noexcept
 {
 	msNumber = p_msNumber;
@@ -70,7 +70,9 @@ void MovementState::Init(unsigned int p_msNumber) noexcept
 	pausedInMacro = false;
 
 #if SUPPORT_ASYNC_MOVES
-	memcpyf(coords, MovementState::GetLastKnownMachinePositions(), MaxAxesPlusExtruders);
+	memcpyf(coords, lastKnownMachinePositions, MaxAxesPlusExtruders);
+	axesAndExtrudersOwned.Clear();
+	ownedAxisLetters.Clear();
 #else
 	for (float& f : coords)
 	{
@@ -270,6 +272,7 @@ void MovementState::ReleaseAxesAndExtruders(AxesBitmap axesToRelease) noexcept
 
 AxesBitmap MovementState::AllocateAxes(AxesBitmap axes, ParameterLettersBitmap axisLetters) noexcept
 {
+	SaveOwnAxisCoordinates();										// we must do this before we allocate new axis to ourselves
 	const AxesBitmap unAvailable = axes & ~axesAndExtrudersOwned & axesAndExtrudersMoved;
 	if (unAvailable.IsEmpty())
 	{
@@ -283,9 +286,11 @@ AxesBitmap MovementState::AllocateAxes(AxesBitmap axes, ParameterLettersBitmap a
 // Fetch and save the coordinates of axes we own to lastKnownMachinePositions, also copy them to our own coordinates in case we just did a homing move
 void MovementState::SaveOwnAxisCoordinates() noexcept
 {
-	reprap.GetMove().GetPartialMachinePosition(lastKnownMachinePositions, axesAndExtrudersOwned, msNumber);
+	Move& move = reprap.GetMove();
+	move.GetPartialMachinePosition(lastKnownMachinePositions, axesAndExtrudersOwned, msNumber);
 	memcpyf(coords, lastKnownMachinePositions, MaxAxesPlusExtruders);
-	reprap.GetMove().InverseAxisAndBedTransform(coords, currentTool);
+	move.SetRawPosition(lastKnownMachinePositions, msNumber);
+	move.InverseAxisAndBedTransform(coords, currentTool);
 }
 
 void AsyncMove::SetDefaults() noexcept