Machine coordinates in OM are now live

Also we can call Move::SetPositions as long as the ring is not full
(previously we could only call it if the ring was empty)

8 files changed, 90 insertions, 95 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 1d60b9a6..acb86014 100644
--- a/src/GCodes/GCodes.cpp
+++ b/src/GCodes/GCodes.cpp
@@ -3104,9 +3104,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
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..ac6d885b 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,37 @@ 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;
+	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 +699,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 +731,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 53ffd9b1..b3ff43cd 100644
--- a/src/Movement/Move.cpp
+++ b/src/Movement/Move.cpp
@@ -565,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);
 }
 
@@ -1204,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