Finished implementation of StopDrivers and RevertPosition

5 files changed, 31 insertions, 1 deletions

diff --git a/src/Movement/DDA.cpp b/src/Movement/DDA.cpp
index 7abd7a5c..49aedf3f 100644
--- a/src/Movement/DDA.cpp
+++ b/src/Movement/DDA.cpp
@@ -876,7 +876,21 @@ bool DDA::InitFromRemote(const CanMessageMovementLinear& msg) noexcept
 	return true;
 }
 
-#endif
+void DDA::StopDrivers(uint16_t whichDrives) noexcept
+{
+	if (state == executing)
+	{
+		for (size_t drive = 0; drive < NumDirectDrivers; ++drive)
+		{
+			if (whichDrives & (1u << drive))
+			{
+				StopDrive(drive);
+			}
+		}
+	}
+}
+
+#endif	// SUPPORT_REMOTE_COMMANDS
 
 // Return true if this move is or might have been intended to be a deceleration-only move
 // A move planned as a deceleration-only move may have a short acceleration segment at the start because of rounding error
diff --git a/src/Movement/DDA.h b/src/Movement/DDA.h
index 96a1ab86..f4c2cf8a 100644
--- a/src/Movement/DDA.h
+++ b/src/Movement/DDA.h
@@ -131,6 +131,7 @@ public:
 # else
 	bool InitFromRemote(const CanMessageMovementLinear& msg) noexcept;
 # endif
+	void StopDrivers(uint16_t whichDrives) noexcept;
 #endif
 
 	const int32_t *DriveCoordinates() const noexcept { return endPoint; }			// Get endpoints of a move in machine coordinates
diff --git a/src/Movement/DDARing.cpp b/src/Movement/DDARing.cpp
index 6e5a57d8..9054705b 100644
--- a/src/Movement/DDARing.cpp
+++ b/src/Movement/DDARing.cpp
@@ -580,6 +580,13 @@ void DDARing::CurrentMoveCompleted() noexcept
 	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)
+	{
+		lastMoveStepsTaken[driver] = cdda->GetStepsTaken(driver);
+	}
+#endif
+
 	// Disable interrupts before we touch any extrusion accumulators until after we set currentDda to null, in case the filament monitor interrupt has higher priority than ours
 	{
 		AtomicCriticalSectionLocker lock;
diff --git a/src/Movement/DDARing.h b/src/Movement/DDARing.h
index 46565a66..e0c28685 100644
--- a/src/Movement/DDARing.h
+++ b/src/Movement/DDARing.h
@@ -95,6 +95,10 @@ public:
 	void StopDrivers(uint16_t whichDrives) noexcept;
 #endif
 
+#if SUPPORT_REMOTE_COMMANDS
+	const volatile int32_t *GetLastMoveStepsTaken() const noexcept { return lastMoveStepsTaken; }
+#endif
+
 protected:
 	DECLARE_OBJECT_MODEL
 
@@ -128,6 +132,9 @@ private:
 	unsigned int stepErrors;													// count of step errors, for diagnostics
 
 	float simulationTime;														// Print time since we started simulating
+#if SUPPORT_REMOTE_COMMANDS
+	volatile int32_t lastMoveStepsTaken[NumDirectDrivers];						// how many steps were taken in the last move we did
+#endif
 	volatile int32_t movementAccumulators[MaxAxesPlusExtruders]; 				// Accumulated motor steps, used by filament monitors
 	volatile uint32_t extrudersPrintingSince;									// The milliseconds clock time when extrudersPrinting was set to true
 
diff --git a/src/Movement/Move.cpp b/src/Movement/Move.cpp
index 29f42402..81f07e8c 100644
--- a/src/Movement/Move.cpp
+++ b/src/Movement/Move.cpp
@@ -1154,6 +1154,7 @@ void Move::RevertPosition(const CanMessageRevertPosition& msg) noexcept
 
 	size_t index = 0;
 	bool needSteps = false;
+	const volatile int32_t * const lastMoveStepsTaken = rings[0].GetLastMoveStepsTaken();
 	for (size_t driver = 0; driver < NumDirectDrivers; ++driver)
 	{
 		int32_t steps = 0;