Fixes for homing CAN drives when the endstop switch is already triggered

3 files changed, 58 insertions, 45 deletions

diff --git a/src/CAN/CanMotion.cpp b/src/CAN/CanMotion.cpp
index ebf98fc6..7aece70d 100644
--- a/src/CAN/CanMotion.cpp
+++ b/src/CAN/CanMotion.cpp
@@ -48,6 +48,7 @@ namespace CanMotion
 	static CanMessageBuffer *GetBuffer(const PrepParams& params, DriverId canDriver) noexcept;
 	static void InternalStopDriverWhenProvisional(DriverId driver) noexcept;
 	static bool InternalStopDriverWhenMoving(DriverId driver, int32_t steps) noexcept;
+	static void FreeMovementBuffers() noexcept;
 }
 
 void CanMotion::Init() noexcept
@@ -56,10 +57,8 @@ void CanMotion::Init() noexcept
 	stopListMutex.Create("stopList");
 }
 
-// This is called by DDA::Prepare at the start of preparing a movement
-void CanMotion::StartMovement() noexcept
+void CanMotion::FreeMovementBuffers() noexcept
 {
-	// There shouldn't be any movement buffers in the list, but free any that there may be
 	for (;;)
 	{
 		CanMessageBuffer *p = movementBufferList;
@@ -70,6 +69,12 @@ void CanMotion::StartMovement() noexcept
 		movementBufferList = p->next;
 		CanMessageBuffer::Free(p);
 	}
+}
+
+// This is called by DDA::Prepare at the start of preparing a movement
+void CanMotion::StartMovement() noexcept
+{
+	FreeMovementBuffers();					// there shouldn't be any movement buffers in the list, but free any that there may be
 
 	// Free up any stop list items left over from the previous move
 	MutexLocker lock(stopListMutex);
@@ -212,54 +217,60 @@ void CanMotion::AddExtruderMovement(const PrepParams& params, DriverId canDriver
 #endif
 
 // This is called by DDA::Prepare when all DMs for CAN drives have been processed. Return the calculated move time in steps, or 0 if there are no CAN moves
-uint32_t CanMotion::FinishMovement(uint32_t moveStartTime, bool simulating, bool checkingEndstops) noexcept
+uint32_t CanMotion::FinishMovement(const DDA& dda, uint32_t moveStartTime, bool simulating) noexcept
 {
-	CanMessageBuffer *buf = movementBufferList;
-	if (buf == nullptr)
+	uint32_t clocks = 0;
+	if (simulating || dda.GetState() == DDA::completed)
 	{
-		return 0;
+		FreeMovementBuffers();											// it turned out that there was nothing to move
 	}
-
-	MutexLocker lock(stopListMutex);
-
-	do
+	else
 	{
-		CanMessageBuffer * const nextBuffer = buf->next;				// must get this before sending the buffer, because sending the buffer releases it
-		if (simulating)
-		{
-			CanMessageBuffer::Free(buf);
-		}
-		else
+		CanMessageBuffer *buf = movementBufferList;
+		if (buf != nullptr)
 		{
+			MutexLocker lock((dda.IsCheckingEndstops()) ? &stopListMutex : nullptr);
+			do
+			{
+				CanMessageBuffer * const nextBuffer = buf->next;		// must get this before sending the buffer, because sending the buffer releases it
 #if USE_REMOTE_INPUT_SHAPING
-			CanMessageMovementLinear& msg = buf->msg.moveLinearShaped;
+				CanMessageMovementLinear& msg = buf->msg.moveLinearShaped;
 #else
-			CanMessageMovementLinear& msg = buf->msg.moveLinear;
+				CanMessageMovementLinear& msg = buf->msg.moveLinear;
 #endif
-			msg.whenToExecute = moveStartTime;
-			uint8_t& seq = nextSeq[buf->id.Dst()];
-			msg.seq = seq;
-			seq = (seq + 1) & 0x7F;
-			buf->dataLength = msg.GetActualDataLength();
-			if (checkingEndstops)
-			{
-				// Set up the stop list
-				DriversStopList * const sl = new DriversStopList(stopList, buf->id.Dst());
-				const size_t nd = msg.numDrivers;
-				sl->numDrivers = (uint8_t)nd;
-				for (size_t i = 0; i < nd; ++i)
+				if (msg.HasMotion())
 				{
-					sl->stopStates[i] = (msg.perDrive[i].steps != 0) ? DriverStopState::active : DriverStopState::inactive;
+					msg.whenToExecute = moveStartTime;
+					uint8_t& seq = nextSeq[buf->id.Dst()];
+					msg.seq = seq;
+					seq = (seq + 1) & 0x7F;
+					buf->dataLength = msg.GetActualDataLength();
+					if (dda.IsCheckingEndstops())
+					{
+						// Set up the stop list
+						DriversStopList * const sl = new DriversStopList(stopList, buf->id.Dst());
+						const size_t nd = msg.numDrivers;
+						sl->numDrivers = (uint8_t)nd;
+						for (size_t i = 0; i < nd; ++i)
+						{
+							sl->stopStates[i] = (msg.perDrive[i].steps != 0) ? DriverStopState::active : DriverStopState::inactive;
+						}
+						stopList = sl;
+					}
+					CanInterface::SendMotion(buf);								// queues the buffer for sending and frees it when done
+					clocks = currentMoveClocks;
 				}
-				stopList = sl;
-			}
-			CanInterface::SendMotion(buf);								// queues the buffer for sending and frees it when done
-		}
-		buf = nextBuffer;
-	} while (buf != nullptr);
+				else
+				{
+					CanMessageBuffer::Free(buf);
+				}
+				buf = nextBuffer;
+			} while (buf != nullptr);
 
-	movementBufferList = nullptr;
-	return currentMoveClocks;
+			movementBufferList = nullptr;
+		}
+	}
+	return clocks;
 }
 
 bool CanMotion::CanPrepareMove() noexcept
@@ -340,6 +351,7 @@ void CanMotion::InsertHiccup(uint32_t numClocks) noexcept
 	CanInterface::WakeAsyncSenderFromIsr();
 }
 
+// Flag a CAN-connected driver as not moving when we haven't sent the movement message yet
 void CanMotion::InternalStopDriverWhenProvisional(DriverId driver) noexcept
 {
 	// Search for the correct movement buffer
@@ -359,6 +371,7 @@ void CanMotion::InternalStopDriverWhenProvisional(DriverId driver) noexcept
 	}
 }
 
+// Tell a CAN-connected driver to stop moving after we have sent the movement message
 bool CanMotion::InternalStopDriverWhenMoving(DriverId driver, int32_t steps) noexcept
 {
 	DriversStopList *sl = stopList;
@@ -379,7 +392,7 @@ bool CanMotion::InternalStopDriverWhenMoving(DriverId driver, int32_t steps) noe
 	return false;
 }
 
-// This is called from the step ISR with isBeingPrepared false, or from the Move task with isBeingPrepared true
+// This is called from the step ISR with DDA state executing, or from the Move task with DDA state provisional
 void CanMotion::StopDriver(const DDA& dda, size_t axis, DriverId driver) noexcept
 {
 	if (dda.GetState() == DDA::DDAState::provisional)
@@ -399,7 +412,7 @@ void CanMotion::StopDriver(const DDA& dda, size_t axis, DriverId driver) noexcep
 	}
 }
 
-// This is called from the step ISR with isBeingPrepared false, or from the Move task with isBeingPrepared true
+// This is called from the step ISR with DDA state executing, or from the Move task with DDA state provisional
 void CanMotion::StopAxis(const DDA& dda, size_t axis) noexcept
 {
 	const Platform& p = reprap.GetPlatform();
@@ -446,7 +459,7 @@ void CanMotion::StopAxis(const DDA& dda, size_t axis) noexcept
 	}
 }
 
-// This is called from the step ISR with isBeingPrepared false, or from the Move task with isBeingPrepared true
+// This is called from the step ISR with DDA state executing, or from the Move task with DDA state provisional
 void CanMotion::StopAll(const DDA& dda) noexcept
 {
 	if (dda.GetState() == DDA::DDAState::provisional)
diff --git a/src/CAN/CanMotion.h b/src/CAN/CanMotion.h
index 9dfcb758..aae4d562 100644
--- a/src/CAN/CanMotion.h
+++ b/src/CAN/CanMotion.h
@@ -24,7 +24,7 @@ namespace CanMotion
 #else
 	void AddMovement(const PrepParams& params, DriverId canDriver, int32_t steps, bool usePressureAdvance) noexcept;
 #endif
-	uint32_t FinishMovement(uint32_t moveStartTime, bool simulating, bool checkingEndstops) noexcept;
+	uint32_t FinishMovement(const DDA& dda, uint32_t moveStartTime, bool simulating) noexcept;
 	bool CanPrepareMove() noexcept;
 	CanMessageBuffer *GetUrgentMessage() noexcept;
 
diff --git a/src/Movement/DDA.cpp b/src/Movement/DDA.cpp
index 880f02f5..f0adb8ac 100644
--- a/src/Movement/DDA.cpp
+++ b/src/Movement/DDA.cpp
@@ -1624,7 +1624,7 @@ void DDA::Prepare(SimulationMode simMode) noexcept
 		}
 
 #if SUPPORT_CAN_EXPANSION
-		const uint32_t canClocksNeeded = CanMotion::FinishMovement(afterPrepare.moveStartTime, simMode != SimulationMode::off, flags.checkEndstops);
+		const uint32_t canClocksNeeded = CanMotion::FinishMovement(*this, afterPrepare.moveStartTime, simMode != SimulationMode::off);
 		if (canClocksNeeded > clocksNeeded)
 		{
 			// Due to rounding error in the calculations, we quite often calculate the CAN move as being longer than our previously-calculated value, normally by just one clock.