Revert axis position after endstop or Z probe triggered3.4-precise-stop

7 files changed, 102 insertions, 74 deletions

diff --git a/src/CAN/CanMotion.cpp b/src/CAN/CanMotion.cpp
index 47bb2f27..0219ae51 100644
--- a/src/CAN/CanMotion.cpp
+++ b/src/CAN/CanMotion.cpp
@@ -24,11 +24,12 @@ namespace CanMotion
 	public:
 		DECLARE_FREELIST_NEW_DELETE(DriversStopList)
 
-		DriversStopList(DriversStopList *p_next, CanAddress p_ba) noexcept : next(p_next), boardAddress(p_ba) { }
+		DriversStopList(DriversStopList *p_next, CanAddress p_ba) noexcept : next(p_next), boardAddress(p_ba), sentRevertRequest(0) { }
 
 		DriversStopList *next;
 		CanAddress boardAddress;
 		uint8_t numDrivers;
+		bool sentRevertRequest;
 		volatile DriverStopState stopStates[MaxLinearDriversPerCanSlave];
 		volatile int32_t stopSteps[MaxLinearDriversPerCanSlave];
 	};
@@ -38,6 +39,9 @@ namespace CanMotion
 	static DriversStopList *volatile stopList = nullptr;
 	static uint32_t currentMoveClocks;
 	static volatile uint32_t hiccupToInsert = 0;
+	static volatile bool revertAll = false;
+	static volatile bool revertedAll = false;
+	static volatile uint32_t whenRevertedAll;
 	static Mutex stopListMutex;
 	static uint8_t nextSeq[CanId::MaxCanAddress + 1] = { 0 };
 
@@ -70,6 +74,7 @@ void CanMotion::StartMovement() noexcept
 	// Free up any stop list items left over from the previous move
 	MutexLocker lock(stopListMutex);
 
+	revertAll = revertedAll = false;
 	for (;;)
 	{
 		DriversStopList *p = stopList;
@@ -263,33 +268,64 @@ bool CanMotion::CanPrepareMove() noexcept
 }
 
 // This is called by the CanSender task to check if we have any urgent messages to send
-// The only urgent messages we may have currently are messages to stop drivers.
+// The only urgent messages we may have currently are messages to stop drivers, or to tell them that all drivers have now been stopped and they need to revert to the requested stop position.
 CanMessageBuffer *CanMotion::GetUrgentMessage() noexcept
 {
-	MutexLocker lock(stopListMutex);			// make sure the list isn't being changed while we traverse it
-
-	for (DriversStopList *sl = stopList; sl != nullptr; sl = sl->next)
+	if (!revertedAll)
 	{
-		auto msg = urgentMessageBuffer.SetupRequestMessage<CanMessageStopMovement>(0, CanInterface::GetCanAddress(), sl->boardAddress);
-		uint16_t driversBeingStopped = 0;
-		size_t numDriversStopped = 0;
-		for (size_t driver = 0; driver < sl->numDrivers; ++driver)
+		MutexLocker lock(stopListMutex);					// make sure the list isn't being changed while we traverse it
+
+		// We have to be careful of race conditions here. The stop list links won't change while we are scanning it because we hold the mutex,
+		// but ISR may change the stop states to StopRequested up until the time at which it changes revertAll from false to true.
+		const bool revertingAll = revertAll;
+		for (DriversStopList *sl = stopList; sl != nullptr; sl = sl->next)
 		{
-			if (sl->stopStates[driver] == DriverStopState::stopRequested)
+			if (!sl->sentRevertRequest)						// if we've already reverted the drivers on this board, no more to do
 			{
-				driversBeingStopped |= 1u << driver;
-				msg->finalStepCounts[numDriversStopped++] = sl->stopSteps[driver];
-				sl->stopStates[driver] = DriverStopState::stopSent;
+				// Set up a reversion message in case we are going to revert the drivers on this board
+				auto revertMsg = urgentMessageBuffer.SetupRequestMessage<CanMessageRevertPosition>(0, CanInterface::GetCanAddress(), sl->boardAddress);
+				uint16_t driversToStop = 0, driversToRevert = 0;
+				size_t numDriversReverted = 0;
+				for (size_t driver = 0; driver < sl->numDrivers; ++driver)
+				{
+					const DriverStopState ss = sl->stopStates[driver];
+					if (ss == DriverStopState::stopRequested)
+					{
+						driversToStop |= 1u << driver;
+						sl->stopStates[driver] = DriverStopState::stopSent;
+					}
+					else if (revertingAll && ss == DriverStopState::stopSent)
+					{
+						driversToRevert |= 1u << driver;
+						revertMsg->finalStepCounts[numDriversReverted++] = sl->stopSteps[driver];
+					}
+				}
+
+				if (driversToStop != 0)
+				{
+					auto stopMsg = urgentMessageBuffer.SetupRequestMessage<CanMessageStopMovement>(0, CanInterface::GetCanAddress(), sl->boardAddress);
+					stopMsg->whichDrives = driversToStop;
+					return &urgentMessageBuffer;
+				}
+
+				if (driversToRevert != 0)
+				{
+					sl->sentRevertRequest = true;
+					revertMsg->whichDrives = driversToRevert;
+					urgentMessageBuffer.dataLength = revertMsg->GetActualDataLength(numDriversReverted);
+					return &urgentMessageBuffer;
+				}
 			}
 		}
-		if (driversBeingStopped != 0)
+
+		// We found nothing to send
+		if (revertingAll)
 		{
-			msg->whichDrives = driversBeingStopped;
-			urgentMessageBuffer.dataLength = msg->GetActualDataLength(numDriversStopped);
-			return &urgentMessageBuffer;
+			// All drivers have been stopped and reverted where requested
+			whenRevertedAll = millis();
+			revertedAll = true;
 		}
 	}
-
 	return nullptr;
 }
 
@@ -430,13 +466,12 @@ void CanMotion::StopAll(const DDA& dda) noexcept
 #endif
 		}
 	}
-	else
+	else if (stopList != nullptr)
 	{
 		// Loop through the axes that are actually moving
 		const GCodes& gc = reprap.GetGCodes();
 		const size_t totalAxes = gc.GetTotalAxes();
 		const Platform& p = reprap.GetPlatform();
-		bool somethingStopped = false;
 		for (size_t axis = 0; axis < totalAxes; ++axis)
 		{
 			const DriveMovement* const dm = dda.FindDM(axis);
@@ -447,9 +482,9 @@ void CanMotion::StopAll(const DDA& dda) noexcept
 				for (size_t i = 0; i < cfg.numDrivers; ++i)
 				{
 					const DriverId driver = cfg.driverNumbers[i];
-					if (driver.IsRemote() && InternalStopDriverWhenMoving(driver, steps))
+					if (driver.IsRemote())
 					{
-						somethingStopped = true;
+						(void)InternalStopDriverWhenMoving(driver, steps);
 					}
 				}
 			}
@@ -463,21 +498,33 @@ void CanMotion::StopAll(const DDA& dda) noexcept
 				const DriveMovement* const dm = dda.FindDM(extruder);
 				if (dm != nullptr)
 				{
-					if (InternalStopDriverWhenMoving(driver, dm->GetNetStepsTaken()))
-					{
-						somethingStopped = true;
-					}
+					(void)InternalStopDriverWhenMoving(driver, dm->GetNetStepsTaken());
 				}
 			}
 		}
 
-		if (somethingStopped)
-		{
-			CanInterface::WakeAsyncSenderFromIsr();
-		}
+		revertAll = true;
+		CanInterface::WakeAsyncSenderFromIsr();
 	}
 }
 
+// This is called by the step ISR when a movement that uses endstops or a probe has completed.
+// We must make sure that all boards have been told to adjust their stepper motor positions to the points at which we wanted them to stop.
+void CanMotion::FinishMoveUsingEndstops() noexcept
+{
+	if (!revertAll)
+	{
+		revertAll = true;
+		CanInterface::WakeAsyncSenderFromIsr();
+	}
+}
+
+// This is called by the main task when it is waiting for the move to complete, after checking that the DDA ring is empty and there is no current move
+bool CanMotion::FinishedReverting() noexcept
+{
+	return !revertAll || (revertedAll && millis() - whenRevertedAll >= AllowedDriverPositionRevertMillis);
+}
+
 #endif
 
 // End
diff --git a/src/CAN/CanMotion.h b/src/CAN/CanMotion.h
index e02b0f76..9dfcb758 100644
--- a/src/CAN/CanMotion.h
+++ b/src/CAN/CanMotion.h
@@ -34,6 +34,8 @@ namespace CanMotion
 	void StopAxis(const DDA& dda, size_t axis) noexcept;
 	void StopDriver(const DDA& dda, size_t axis, DriverId driver) noexcept
 		pre(driver.IsRemote());
+	void FinishMoveUsingEndstops() noexcept;
+	bool FinishedReverting() noexcept;
 }
 
 #endif
diff --git a/src/GCodes/GCodes4.cpp b/src/GCodes/GCodes4.cpp
index 52ac4c03..97950e37 100644
--- a/src/GCodes/GCodes4.cpp
+++ b/src/GCodes/GCodes4.cpp
@@ -9,6 +9,10 @@
 #include <Heating/Heat.h>
 #include <Endstops/ZProbe.h>
 
+#if SUPPORT_CAN_EXPANSION
+# include <CAN/CanMotion.h>
+#endif
+
 #if HAS_SBC_INTERFACE
 # include <SBC/SbcInterface.h>
 #endif
@@ -40,7 +44,11 @@ void GCodes::RunStateMachine(GCodeBuffer& gb, const StringRef& reply) noexcept
 	{
 	case GCodeState::waitingForSpecialMoveToComplete:
 	case GCodeState::abortWhenMovementFinished:
-		if (LockMovementAndWaitForStandstill(gb))		// movement should already be locked, but we need to wait for standstill and fetch the current position
+		if (   LockMovementAndWaitForStandstill(gb)		// movement should already be locked, but we need to wait for standstill and fetch the current position
+#if SUPPORT_CAN_EXPANSION
+			&& CanMotion::FinishedReverting()
+#endif
+		   )
 		{
 			// Check whether we made any G1 S3 moves and need to set the axis limits
 			axesToSenseLength.Iterate([this](unsigned int axis, unsigned int)
diff --git a/src/Movement/DDA.cpp b/src/Movement/DDA.cpp
index c0a81800..dcf733e7 100644
--- a/src/Movement/DDA.cpp
+++ b/src/Movement/DDA.cpp
@@ -1813,14 +1813,7 @@ void DDA::CheckEndstops(Platform& platform) noexcept
 		case EndstopHitAction::stopAxis:
 			StopDrive(hitDetails.axis);								// we must stop the drive before we mess with its coordinates
 #if SUPPORT_CAN_EXPANSION
-			if (state == completed)									// if the call to StopDrive flagged the move as completed
-			{
-				CanMotion::StopAll(*this);
-			}
-			else
-			{
-				CanMotion::StopAxis(*this, hitDetails.axis);
-			}
+			CanMotion::StopAxis(*this, hitDetails.axis);
 #endif
 			if (hitDetails.setAxisLow)
 			{
@@ -1861,34 +1854,6 @@ void DDA::CheckEndstops(Platform& platform) noexcept
 			return;
 		}
 	}
-
-#if DDA_LOG_PROBE_CHANGES
-	else if ((endStopsToCheck & LogProbeChanges) != 0)
-	{
-		switch (platform.GetZProbeResult())
-		{
-		case EndStopHit::lowHit:
-			if (!probeTriggered)
-			{
-				probeTriggered = true;
-				LogProbePosition();
-			}
-			break;
-
-		case EndStopHit::nearStop:
-		case EndStopHit::noStop:
-			if (probeTriggered)
-			{
-				probeTriggered = false;
-				LogProbePosition();
-			}
-			break;
-
-		default:
-			break;
-		}
-	}
-#endif
 }
 
 // Start executing this move. Must be called with interrupts disabled or basepri >= set interrupt priority, to avoid a race condition.
diff --git a/src/Movement/DDARing.cpp b/src/Movement/DDARing.cpp
index df20b658..bae5b3c4 100644
--- a/src/Movement/DDARing.cpp
+++ b/src/Movement/DDARing.cpp
@@ -341,6 +341,7 @@ uint32_t DDARing::Spin(SimulationMode simulationMode, bool waitingForSpace, bool
 	if (   shouldStartMove											// if the Move code told us that we should start a move in any case...
 		|| waitingForSpace											// ...or the Move code told us it was waiting for space in the ring...
 		|| waitingForRingToEmpty									// ...or GCodes is waiting for all moves to finish...
+		|| dda->IsCheckingEndstops()								// ...or checking endstops, so we can't schedule the following move
 #if SUPPORT_REMOTE_COMMANDS
 		|| dda->GetState() == DDA::frozen							// ...or the move has already been frozen (it's probably a remote move)
 #endif
@@ -455,7 +456,7 @@ float DDARing::PushBabyStepping(size_t axis, float amount) noexcept
 // ISR for the step interrupt
 void DDARing::Interrupt(Platform& p) noexcept
 {
-	DDA* cdda = currentDda;								// capture volatile variable
+	DDA* cdda = currentDda;										// capture volatile variable
 	if (cdda != nullptr)
 	{
 		uint32_t now = StepTimer::GetTimerTicks();
@@ -463,9 +464,15 @@ void DDARing::Interrupt(Platform& p) noexcept
 		for (;;)
 		{
 			// Generate a step for the current move
-			cdda->StepDrivers(p, now);						// check endstops if necessary and step the drivers
+			cdda->StepDrivers(p, now);							// check endstops if necessary and step the drivers
 			if (cdda->GetState() == DDA::completed)
 			{
+#if SUPPORT_CAN_EXPANSION
+				if (cdda->IsCheckingEndstops())
+				{
+					CanMotion::FinishMoveUsingEndstops();		// Tell CAN-connected drivers to revert their position
+				}
+#endif
 				OnMoveCompleted(cdda, p);
 				cdda = currentDda;
 				if (cdda == nullptr)
diff --git a/src/Movement/Move.cpp b/src/Movement/Move.cpp
index 93486ac4..2c8678a1 100644
--- a/src/Movement/Move.cpp
+++ b/src/Movement/Move.cpp
@@ -376,10 +376,9 @@ void Move::Exit() noexcept
 		}
 
 		// We need to be woken when one of the following is true:
-		// 1. If noMoveAvailable is true, when a new move becomes available.
-		// 2. If moves are being executed and there are unprepared moves in the queue, when it is time to prepare more moves.
-		// 3. If the queue was full and all moves in it were prepared and noMoveAvailable is false, when we have completed one or more moves.
-		// 4. In order to implement idle timeout, we must wake up regularly anyway, say every half second
+		// 1. If moves are being executed and there are unprepared moves in the queue, when it is time to prepare more moves.
+		// 2. If the queue was full and all moves in it were prepared, when we have completed one or more moves.
+		// 3. In order to implement idle timeout, we must wake up regularly anyway, say every half second
 		if (!moveRead && nextPrepareDelay != 0)
 		{
 			TaskBase::Take(min<uint32_t>(nextPrepareDelay, 500));
diff --git a/src/Version.h b/src/Version.h
index 2c5f5e68..4f366169 100644
--- a/src/Version.h
+++ b/src/Version.h
@@ -10,7 +10,7 @@
 
 #ifndef VERSION
 // Note: the complete VERSION string must be in standard version number format and must not contain spaces! This is so that DWC can parse it.
-# define MAIN_VERSION	"3.4.0beta7+1"
+# define MAIN_VERSION	"3.4.0beta7+2"
 # ifdef USE_CAN0
 #  define VERSION_SUFFIX	"(CAN0)"
 # else