Partial fixes to stopping drivers on main boards used as expansion

5 files changed, 72 insertions, 2 deletions

diff --git a/src/CAN/CommandProcessor.cpp b/src/CAN/CommandProcessor.cpp
index a91e8cd2..bd8a3137 100644
--- a/src/CAN/CommandProcessor.cpp
+++ b/src/CAN/CommandProcessor.cpp
@@ -414,11 +414,19 @@ void CommandProcessor::ProcessReceivedMessage(CanMessageBuffer *buf) noexcept
 				reprap.GetMove().AddMoveFromRemote(buf->msg.moveLinear);
 				return;							// no reply needed
 
-#if USE_REMOTE_INPUT_SHAPING
+# if USE_REMOTE_INPUT_SHAPING
 			case CanMessageType::movementLinearShaped:
 				reprap.GetMove().AddShapedMoveFromRemote(buf->msg.moveLinearShaped);
 				return;							// no reply needed
-#endif
+# endif
+
+			case CanMessageType::stopMovement:
+				reprap.GetMove().StopDrives(buf->msg.stopMovement.whichDrives);
+				return;							// no reply needed
+
+			case CanMessageType::revertPosition:
+				reprap.GetMove().RevertPosition(buf->msg.revertPosition);
+				return;							// no reply needed
 
 			case CanMessageType::acknowledgeAnnounce:
 				CanInterface::MainBoardAcknowledgedAnnounce();
diff --git a/src/Movement/DDARing.cpp b/src/Movement/DDARing.cpp
index ed8d9e00..6e5a57d8 100644
--- a/src/Movement/DDARing.cpp
+++ b/src/Movement/DDARing.cpp
@@ -1020,6 +1020,22 @@ void DDARing::AddMoveFromRemote(const CanMessageMovementLinear& msg) noexcept
 }
 
 # endif
+
+void DDARing::StopDrivers(uint16_t whichDrives) noexcept
+{
+	const uint32_t oldPrio = ChangeBasePriority(NvicPriorityStep);
+	DDA *cdda = currentDda;							// capture volatile
+	if (cdda != nullptr)
+	{
+		cdda->StopDrivers(whichDrives);
+		if (cdda->GetState() == DDA::completed)
+		{
+			CurrentMoveCompleted();					// tell the DDA ring that the current move is complete
+		}
+	}
+	RestoreBasePriority(oldPrio);
+}
+
 #endif
 
 // End
diff --git a/src/Movement/DDARing.h b/src/Movement/DDARing.h
index a9fd9a2f..46565a66 100644
--- a/src/Movement/DDARing.h
+++ b/src/Movement/DDARing.h
@@ -92,6 +92,7 @@ public:
 # else
 	void AddMoveFromRemote(const CanMessageMovementLinear& msg) noexcept;				// add a move from the ATE to the movement queue
 # endif
+	void StopDrivers(uint16_t whichDrives) noexcept;
 #endif
 
 protected:
diff --git a/src/Movement/Move.cpp b/src/Movement/Move.cpp
index 403f0a63..29f42402 100644
--- a/src/Movement/Move.cpp
+++ b/src/Movement/Move.cpp
@@ -1138,6 +1138,44 @@ GCodeResult Move::EutSetRemotePressureAdvance(const CanMessageMultipleDrivesRequ
 	return rslt;
 }
 
+void Move::RevertPosition(const CanMessageRevertPosition& msg) noexcept
+{
+	// Construct a MovementLinear message to revert the position. The move must be shorter than clocksAllowed.
+	// When writing this, clocksAllowed was equivalent to 40ms.
+	// We allow 10ms delay time to allow the motor to stop and reverse direction, 10ms acceleration time, 5ms steady time and 10ms deceleration time.
+	CanMessageMovementLinear msg2;
+	msg2.accelerationClocks = msg2.decelClocks = msg.clocksAllowed/4;
+	msg2.steadyClocks = msg.clocksAllowed/8;
+	msg2.whenToExecute = StepTimer::GetTimerTicks() + msg.clocksAllowed/4;
+	msg2.numDrivers = NumDirectDrivers;
+	msg2.pressureAdvanceDrives = 0;
+	msg2.seq = 0;
+	msg2.initialSpeedFraction = msg2.finalSpeedFraction = 0.0;
+
+	size_t index = 0;
+	bool needSteps = false;
+	for (size_t driver = 0; driver < NumDirectDrivers; ++driver)
+	{
+		int32_t steps = 0;
+		if (msg.whichDrives & (1u << driver))
+		{
+			const int32_t stepsWanted = msg.finalStepCounts[index++];
+			const int32_t stepsTaken = lastMoveStepsTaken[driver];
+			if (((stepsWanted >= 0 && stepsTaken > stepsWanted) || (stepsWanted <= 0 && stepsTaken < stepsWanted)))
+			{
+				steps = stepsWanted - stepsTaken;
+				needSteps = true;
+			}
+		}
+		msg2.perDrive[driver].steps = steps;
+	}
+
+	if (needSteps)
+	{
+		AddMoveFromRemote(msg2);
+	}
+}
+
 #endif
 
 // Return the current live XYZ and extruder coordinates
diff --git a/src/Movement/Move.h b/src/Movement/Move.h
index 6007f9d1..d16b3e7e 100644
--- a/src/Movement/Move.h
+++ b/src/Movement/Move.h
@@ -214,6 +214,13 @@ public:
 		MoveAvailable();
 	}
 # endif
+
+	void StopDrivers(uint16_t whichDrives) noexcept
+	{
+		rings[0].StopDrivers(whichDrives);
+	}
+
+	void RevertPosition(const CanMessageRevertPosition& msg) noexcept;
 #endif
 
 protected: