Fixed G923.5-dev

4 files changed, 25 insertions, 14 deletions

diff --git a/src/GCodes/GCodes3.cpp b/src/GCodes/GCodes3.cpp
index 51db8901..a511b714 100644
--- a/src/GCodes/GCodes3.cpp
+++ b/src/GCodes/GCodes3.cpp
@@ -79,6 +79,10 @@ GCodeResult GCodes::SetPositions(GCodeBuffer& gb, const StringRef& reply) THROWS
 	axisLettersMentioned.ClearBits(ms.GetOwnedAxisLetters());
 	if (axisLettersMentioned.IsNonEmpty())
 	{
+		if (!LockCurrentMovementSystemAndWaitForStandstill(gb))	// lock movement and get current coordinates before we try to allocate any axes
+		{
+			return GCodeResult::notFinished;
+		}
 		AllocateAxisLetters(gb, ms, axisLettersMentioned);
 	}
 #endif
@@ -91,6 +95,7 @@ GCodeResult GCodes::SetPositions(GCodeBuffer& gb, const StringRef& reply) THROWS
 		if (gb.Seen(axisLetters[axis]))
 		{
 			const float axisValue = gb.GetFValue();
+#if !SUPPORT_ASYNC_MOVES
 			if (axesIncluded.IsEmpty())
 			{
 				if (!LockCurrentMovementSystemAndWaitForStandstill(gb))	// lock movement and get current coordinates
@@ -98,6 +103,7 @@ GCodeResult GCodes::SetPositions(GCodeBuffer& gb, const StringRef& reply) THROWS
 					return GCodeResult::notFinished;
 				}
 			}
+#endif
 			axesIncluded.SetBit(axis);
 			ms.currentUserPosition[axis] = gb.ConvertDistance(axisValue);
 		}
@@ -115,12 +121,12 @@ GCodeResult GCodes::SetPositions(GCodeBuffer& gb, const StringRef& reply) THROWS
 
 		if (reprap.GetMove().GetKinematics().LimitPosition(ms.coords, nullptr, numVisibleAxes, axesIncluded, false, limitAxes) != LimitPositionResult::ok)
 		{
-			ToolOffsetInverseTransform(ms);		// make sure the limits are reflected in the user position
+			ToolOffsetInverseTransform(ms);					// make sure the limits are reflected in the user position
 		}
-		reprap.GetMove().SetNewPosition(ms.coords, true, gb.GetActiveQueueNumber());
 #if SUPPORT_ASYNC_MOVES
-		ms.SaveOwnAxisCoordinates();
+		ms.OwnedAxisCoordinatesUpdated(axesIncluded);		// save coordinates of any owned axes we changed
 #endif
+		reprap.GetMove().SetNewPosition(ms.coords, true, gb.GetActiveQueueNumber());
 		if (!IsSimulating())
 		{
 			axesHomed |= reprap.GetMove().GetKinematics().AxesAssumedHomed(axesIncluded);
@@ -131,7 +137,6 @@ GCodeResult GCodes::SetPositions(GCodeBuffer& gb, const StringRef& reply) THROWS
 			}
 			reprap.MoveUpdated();				// because we may have updated axesHomed or zDatumSetByProbing
 		}
-
 	}
 
 	return GCodeResult::ok;
diff --git a/src/GCodes/GCodes4.cpp b/src/GCodes/GCodes4.cpp
index 293bd405..51f06764 100644
--- a/src/GCodes/GCodes4.cpp
+++ b/src/GCodes/GCodes4.cpp
@@ -1120,7 +1120,10 @@ void GCodes::RunStateMachine(GCodeBuffer& gb, const StringRef& reply) noexcept
 				if (tapsDone <= 1 && !hadProbingError)
 				{
 					// Reset the Z axis origin according to the height error so that we can move back up to the dive height
-					ms.UpdateOwnedAxisCoordinate(Z_AXIS, zp->GetActualTriggerHeight());
+					ms.coords[Z_AXIS] = zp->GetActualTriggerHeight();
+#if SUPPORT_ASYNC_MOVES
+					ms.OwnedAxisCoordinateUpdated(Z_AXIS);
+#endif
 					reprap.GetMove().SetNewPosition(ms.coords, false, gb.GetActiveQueueNumber());
 
 					// Find the coordinates of the Z probe to pass to SetZeroHeightError
diff --git a/src/Movement/RawMove.cpp b/src/Movement/RawMove.cpp
index e0d68dc8..451817c6 100644
--- a/src/Movement/RawMove.cpp
+++ b/src/Movement/RawMove.cpp
@@ -293,16 +293,18 @@ void MovementState::SaveOwnAxisCoordinates() noexcept
 	move.InverseAxisAndBedTransform(coords, currentTool);
 }
 
+// Update changed coordinates of some owned axes - called after G92
+void MovementState::OwnedAxisCoordinatesUpdated(AxesBitmap axesIncluded) noexcept
+{
+	axesIncluded.Iterate([this](unsigned int bitNumber, unsigned int count)->void
+							{ lastKnownMachinePositions[bitNumber] = coords[bitNumber]; }
+						);
+}
+
 // Update the machine coordinate of an axis we own - called after Z probing
-void MovementState::UpdateOwnedAxisCoordinate(size_t axis, float coordinate) noexcept
+void MovementState::OwnedAxisCoordinateUpdated(size_t axis) noexcept
 {
-	coords[axis] = coordinate;
-#if SUPPORT_ASYNC_MOVES
-	if (axesAndExtrudersOwned.IsBitSet(axis))
-	{
-		lastKnownMachinePositions[axis] = coordinate;
-	}
-#endif
+	lastKnownMachinePositions[axis] = coords[axis];
 }
 
 void AsyncMove::SetDefaults() noexcept
diff --git a/src/Movement/RawMove.h b/src/Movement/RawMove.h
index fee725d0..9ac05b36 100644
--- a/src/Movement/RawMove.h
+++ b/src/Movement/RawMove.h
@@ -92,6 +92,8 @@ public:
 	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() noexcept;													// fetch and save the coordinates of axes we own to lastKnownMachinePositions
+	void OwnedAxisCoordinatesUpdated(AxesBitmap axesIncluded) noexcept;						// update changed coordinates of some owned axes - called after G92
+	void OwnedAxisCoordinateUpdated(size_t axis) noexcept;									// update the machine coordinate of an axis we own - called after Z probing
 #endif
 
 	unsigned int GetMsNumber() const noexcept { return msNumber; }
@@ -102,7 +104,6 @@ public:
 	void ClearMove() noexcept;
 	void SavePosition(unsigned int restorePointNumber, size_t numAxes, float p_feedRate, FilePosition p_filePos) noexcept
 		pre(restorePointNumber < NumTotalRestorePoints);
-	void UpdateOwnedAxisCoordinate(size_t axis, float coordinate) noexcept;					// update the machine coordinate of an axis we own - called after Z probing
 
 	// Tool management
 	void SelectTool(int toolNumber, bool simulating) noexcept;