More refactoring for multiple motion systems

3 files changed, 7 insertions, 13 deletions

diff --git a/src/GCodes/GCodes.cpp b/src/GCodes/GCodes.cpp
index 8eed8727..9142196f 100644
--- a/src/GCodes/GCodes.cpp
+++ b/src/GCodes/GCodes.cpp
@@ -1593,15 +1593,9 @@ bool GCodes::LockMovementSystemAndWaitForStandstill(GCodeBuffer& gb, unsigned in
 		// Get the current positions. These may not be the same as the ones we remembered from last time if we just did a special move.
 #if SUPPORT_ASYNC_MOVES
 		// Get the position of all axes by combining positions from the queues
-		Move& move = reprap.GetMove();
-		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
 		ms.SaveOwnAxisCoordinates();
-		memcpyf(ms.coords, MovementState::GetLastKnownMachinePositions(), MaxAxes);
-		move.InverseAxisAndBedTransform(ms.coords, ms.currentTool);
 		UpdateUserPositionFromMachinePosition(gb, ms);
-		collisionChecker.ResetPositions(ms.coords, ownedAxes);
+		collisionChecker.ResetPositions(ms.coords, ms.GetAxesAndExtrudersOwned());
 
 		// Release the axes and extruders that this movement system owns, except those used by the current tool
 		if (ms.currentTool != nullptr)
@@ -5117,7 +5111,6 @@ void GCodes::UpdateAllCoordinates(const GCodeBuffer& gb) noexcept
 {
 	const unsigned int msNumber = gb.GetOwnQueueNumber();
 	memcpyf(moveStates[msNumber].coords, MovementState::GetLastKnownMachinePositions(), MaxAxes);
-	reprap.GetMove().InverseAxisAndBedTransform(moveStates[msNumber].coords, moveStates[msNumber].currentTool);
 	UpdateUserPositionFromMachinePosition(gb, moveStates[msNumber]);
 	reprap.GetMove().SetNewPosition(moveStates[msNumber].coords, true, msNumber);
 }
diff --git a/src/Movement/RawMove.cpp b/src/Movement/RawMove.cpp
index 368f0b8f..a2e43fd3 100644
--- a/src/Movement/RawMove.cpp
+++ b/src/Movement/RawMove.cpp
@@ -280,13 +280,14 @@ AxesBitmap MovementState::AllocateAxes(AxesBitmap axes, ParameterLettersBitmap a
 	return unAvailable;
 }
 
-// Save the coordinates of axes we own to lastKnownMachinePositions
-void MovementState::SaveOwnAxisCoordinates() const noexcept
+// 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, GetAxesAndExtrudersOwned(), msNumber);
+	reprap.GetMove().GetPartialMachinePosition(lastKnownMachinePositions, axesAndExtrudersOwned, msNumber);
+	memcpyf(coords, lastKnownMachinePositions, MaxAxesPlusExtruders);
+	reprap.GetMove().InverseAxisAndBedTransform(coords, currentTool);
 }
 
-
 void AsyncMove::SetDefaults() noexcept
 {
 	for (float& f : movements)
diff --git a/src/Movement/RawMove.h b/src/Movement/RawMove.h
index 5f71197b..29e99ff3 100644
--- a/src/Movement/RawMove.h
+++ b/src/Movement/RawMove.h
@@ -91,7 +91,7 @@ public:
 	void ReleaseOwnedAxesAndExtruders() noexcept;
 	void ReleaseAxesAndExtruders(AxesBitmap axesToRelease) noexcept;
 	void ReleaseAxisLetter(char letter) noexcept;											// stop claiming that we own an axis letter (if we do) but don't release the associated axis
-	void SaveOwnAxisCoordinates() const noexcept;											// save the coordinates of axes we own to lastKnownMachinePositions
+	void SaveOwnAxisCoordinates() noexcept;													// fetch and save the coordinates of axes we own to lastKnownMachinePositions
 #endif
 
 	unsigned int GetMsNumber() const noexcept { return msNumber; }