Attempt fix for prepare underruns with sequences of short segments

3 files changed, 70 insertions, 51 deletions

diff --git a/src/Movement/DDARing.cpp b/src/Movement/DDARing.cpp
index 03789657..c190c6da 100644
--- a/src/Movement/DDARing.cpp
+++ b/src/Movement/DDARing.cpp
@@ -15,6 +15,8 @@
 # include "CAN/CanMotion.h"
 #endif
 
+constexpr uint32_t MoveStartPollInterval = 10;					// delay in milliseconds between checking whether we should start moves
+
 // Object model table and functions
 // Note: if using GCC version 7.3.1 20180622 and lambda functions are used in this table, you must compile this file with option -std=gnu++17.
 // Otherwise the table will be allocated in RAM instead of flash, which wastes too much RAM.
@@ -257,8 +259,9 @@ bool DDARing::AddAsyncMove(const AsyncMove& nextMove) noexcept
 
 #endif
 
-// Try to process moves in the ring
-void DDARing::Spin(uint8_t simulationMode, bool shouldStartMove) noexcept
+// Try to process moves in the ring. Called by the Move task.
+// Return the maximum time in milliseconds that should elapse before we prepare further unprepared moves that are already in the ring, or TaskBase::TimeoutUnlimited if there are no unprepared moves left.
+uint32_t DDARing::Spin(uint8_t simulationMode, bool shouldStartMove) noexcept
 {
 	DDA *cdda = currentDda;											// capture volatile variable
 
@@ -286,75 +289,79 @@ void DDARing::Spin(uint8_t simulationMode, bool shouldStartMove) noexcept
 			cdda = cdda->GetNext();
 			if (cdda == addPointer)
 			{
-				return;												// all moves are already prepared
+				return TaskBase::TimeoutUnlimited;				// all the moves we have are already prepared, so nothing to do until new moves arrive
 			}
 		}
 
-		PrepareMoves(cdda, preparedTime, preparedCount, simulationMode);
+		return PrepareMoves(cdda, preparedTime, preparedCount, simulationMode);
 	}
-	else
-	{
-		// No DDA is executing, so start executing a new one if possible
-		DDA * dda = getPointer;										// capture volatile variable
-		if (   shouldStartMove										// if the Move code told us that we should start a move...
-			|| waitingForRingToEmpty								// ...or GCodes is waiting for all moves to finish...
-			|| !CanAddMove()										// ...or the ring is full...
+
+	// No DDA is executing, so start executing a new one if possible
+	DDA * dda = getPointer;										// capture volatile variable
+	if (   shouldStartMove										// if the Move code told us that we should start a move...
+		|| waitingForRingToEmpty								// ...or GCodes is waiting for all moves to finish...
+		|| !CanAddMove()										// ...or the ring is full...
 #if SUPPORT_REMOTE_COMMANDS
-			|| dda->GetState() == DDA::frozen						// ...or the move has already been frozen (it's probably a remote move)
+		|| dda->GetState() == DDA::frozen						// ...or the move has already been frozen (it's probably a remote move)
 #endif
-		   )
-		{
-			PrepareMoves(dda, 0, 0, simulationMode);
+	   )
+	{
+		const uint32_t ret = PrepareMoves(dda, 0, 0, simulationMode);
 
-			if (dda->GetState() == DDA::completed)
+		if (dda->GetState() == DDA::completed)
+		{
+			// We prepared the move but found there was nothing to do because endstops are already triggered
+			getPointer = dda = dda->GetNext();
+			completedMoves++;
+		}
+		else if (dda->GetState() == DDA::frozen)
+		{
+			if (simulationMode != 0)
 			{
-				// We prepared the move but found there was nothing to do because endstops are already triggered
-				getPointer = dda = dda->GetNext();
-				completedMoves++;
+				currentDda = dda;									// pretend we are executing this move
 			}
-			else if (dda->GetState() == DDA::frozen)
+			else
 			{
-				if (simulationMode != 0)
+				Platform& p = reprap.GetPlatform();
+				SetBasePriority(NvicPriorityStep);					// shut out step interrupt
+				const bool wakeLaser = StartNextMove(p, StepTimer::GetTimerTicks());
+				if (ScheduleNextStepInterrupt())
 				{
-					currentDda = dda;									// pretend we are executing this move
+					Interrupt(p);
 				}
-				else
-				{
-					Platform& p = reprap.GetPlatform();
-					SetBasePriority(NvicPriorityStep);					// shut out step interrupt
-					const bool wakeLaser = StartNextMove(p, StepTimer::GetTimerTicks());
-					if (ScheduleNextStepInterrupt())
-					{
-						Interrupt(p);
-					}
-					SetBasePriority(0);
+				SetBasePriority(0);
 
 #if SUPPORT_LASER || SUPPORT_IOBITS
-					if (wakeLaser)
-					{
-						Move::WakeLaserTask();
-					}
-					else
-					{
-						p.SetLaserPwm(0);
-					}
+				if (wakeLaser)
+				{
+					Move::WakeLaserTask();
+				}
+				else
+				{
+					p.SetLaserPwm(0);
+				}
 #else
-					(void)wakeLaser;
+				(void)wakeLaser;
 #endif
-				}
 			}
 		}
+		return ret;
 	}
+
+	return (dda->GetState() == DDA::provisional)
+			? MoveStartPollInterval									// there are moves in the queue but it is not time to prepare them yet
+				: TaskBase::TimeoutUnlimited;						// the queue is empty, nothing to do until new moves arrive
 }
 
 // Prepare some moves. moveTimeLeft is the total length remaining of moves that are already executing or prepared.
-void DDARing::PrepareMoves(DDA *firstUnpreparedMove, int32_t moveTimeLeft, unsigned int alreadyPrepared, uint8_t simulationMode) noexcept
+// Return the maximum time in milliseconds that should elapse before we prepare further unprepared moves that are already in the ring, or TaskBase::TimeoutUnlimited if there are no unprepared moves left.
+uint32_t DDARing::PrepareMoves(DDA *firstUnpreparedMove, int32_t moveTimeLeft, unsigned int alreadyPrepared, uint8_t simulationMode) noexcept
 {
 	// If the number of prepared moves will execute in less than the minimum time, prepare another move.
 	// Try to avoid preparing deceleration-only moves too early
 	while (	  firstUnpreparedMove->GetState() == DDA::provisional
 		   && moveTimeLeft < (int32_t)DDA::UsualMinimumPreparedTime		// prepare moves one tenth of a second ahead of when they will be needed
-		   && alreadyPrepared * 2 < numDdasInRing					// but don't prepare more than half the ring
+		   && alreadyPrepared * 2 < numDdasInRing						// but don't prepare more than half the ring, to handle accelerate/decelerate moves in small segments
 		   && (firstUnpreparedMove->IsGoodToPrepare() || moveTimeLeft < (int32_t)DDA::AbsoluteMinimumPreparedTime)
 #if SUPPORT_CAN_EXPANSION
 		   && CanMotion::CanPrepareMove()
@@ -366,6 +373,16 @@ void DDARing::PrepareMoves(DDA *firstUnpreparedMove, int32_t moveTimeLeft, unsig
 		++alreadyPrepared;
 		firstUnpreparedMove = firstUnpreparedMove->GetNext();
 	}
+
+	// Decide how soon we want to be called again to prepare further moves
+	if (firstUnpreparedMove->GetState() == DDA::provisional)
+	{
+		// There are more moves waiting to be prepared, so ask to be woken up early
+		const int32_t clocksTillWakeup = moveTimeLeft - (int32_t)DDA::UsualMinimumPreparedTime;						// calculate how long before we run out of prepared moves, less the usual advance prepare time
+		return (clocksTillWakeup <= 0) ? 2 : min<uint32_t>((uint32_t)clocksTillWakeup/(StepClockRate/1000), 2);		// wake up at that time, but delay for at least 2 ticks
+	}
+
+	return TaskBase::TimeoutUnlimited;
 }
 
 // Return true if this DDA ring is idle
diff --git a/src/Movement/DDARing.h b/src/Movement/DDARing.h
index 256036d1..51355512 100644
--- a/src/Movement/DDARing.h
+++ b/src/Movement/DDARing.h
@@ -29,7 +29,7 @@ public:
 	bool AddAsyncMove(const AsyncMove& nextMove) noexcept;
 #endif
 
-	void Spin(uint8_t simulationMode, bool shouldStartMove) noexcept SPEED_CRITICAL;	// Try to process moves in the ring
+	uint32_t Spin(uint8_t simulationMode, bool shouldStartMove) noexcept SPEED_CRITICAL;	// Try to process moves in the ring
 	bool IsIdle() const noexcept;														// Return true if this DDA ring is idle
 	uint32_t GetGracePeriod() const noexcept { return gracePeriod; }					// Return the minimum idle time, before we should start a move. Better to have a few moves in the queue so that we can do lookahead
 
@@ -99,7 +99,7 @@ protected:
 
 private:
 	bool StartNextMove(Platform& p, uint32_t startTime) noexcept SPEED_CRITICAL;		// Start the next move, returning true if laser or IObits need to be controlled
-	void PrepareMoves(DDA *firstUnpreparedMove, int32_t moveTimeLeft, unsigned int alreadyPrepared, uint8_t simulationMode) noexcept;
+	uint32_t PrepareMoves(DDA *firstUnpreparedMove, int32_t moveTimeLeft, unsigned int alreadyPrepared, uint8_t simulationMode) noexcept;
 
 	static void TimerCallback(CallbackParameter p) noexcept;
 
diff --git a/src/Movement/Move.cpp b/src/Movement/Move.cpp
index ad1e51e5..a8d62b25 100644
--- a/src/Movement/Move.cpp
+++ b/src/Movement/Move.cpp
@@ -52,8 +52,6 @@
 
 Task<Move::MoveTaskStackWords> Move::moveTask;
 
-constexpr uint32_t MoveTimeout = 20;					// normal timeout in milliseconds when the Move process is waiting for a new move
-
 // Object model table and functions
 // Note: if using GCC version 7.3.1 20180622 and lambda functions are used in this table, you must compile this file with option -std=gnu++17.
 // Otherwise the table will be allocated in RAM instead of flash, which wastes too much RAM.
@@ -303,7 +301,7 @@ void Move::Exit() noexcept
 		}
 
 		// Let the DDA ring process moves. Better to have a few moves in the queue so that we can do lookahead, hence the test on idleCount and idleTime.
-		mainDDARing.Spin(simulationMode, !canAddMove || millis() - idleStartTime >= mainDDARing.GetGracePeriod());
+		uint32_t nextPrepareDelay = mainDDARing.Spin(simulationMode, !canAddMove || millis() - idleStartTime >= mainDDARing.GetGracePeriod());
 
 #if SUPPORT_ASYNC_MOVES
 		if (auxMoveAvailable && auxDDARing.CanAddMove())
@@ -314,7 +312,11 @@ void Move::Exit() noexcept
 			}
 			auxMoveAvailable = false;
 		}
-		auxDDARing.Spin(simulationMode, true);				// let the DDA ring process moves
+		const uint32_t auxPrepareDelay = auxDDARing.Spin(simulationMode, true);				// let the DDA ring process moves
+		if (auxPrepareDelay < nextPrepareDelay)
+		{
+			nextPrepareDelay = auxPrepareDelay;
+		}
 #endif
 
 		// Reduce motor current to standby if the rings have been idle for long enough
@@ -344,7 +346,7 @@ void Move::Exit() noexcept
 
 		if (!moveRead)
 		{
-			TaskBase::Take(MoveTimeout);
+			TaskBase::Take(nextPrepareDelay);
 		}
 	}
 }