Add per-tool pressure advance snapshots for queued moves and CAN motion for adaptive pressure advance

src/CAN/CanInterface.cpp

@@ -397,6 +397,7 @@ void CanInterface::SendAnnounce(CanMessageBuffer *buf) noexcept
 		msg->timeSinceStarted = millis();
 		msg->numDrivers = NumDirectDrivers;
 		msg->usesUf2Binary = BOARD_USES_UF2_BINARY;
+		msg->supportsMovementPaSnapshot = 1;
 		msg->zero = 0;
 		memcpy(msg->uniqueId, reprap.GetPlatform().GetUniqueId().GetRaw(), sizeof(msg->uniqueId));
 		// Note, board type name, firmware version, firmware date and firmware time are limited to 43 characters in the new

src/CAN/CanMotion.cpp

@@ -46,8 +46,10 @@ namespace CanMotion
 	static volatile uint32_t whenRevertedAll;
 	static Mutex stopListMutex;
 	static uint8_t nextSeq[CanId::MaxCanAddress + 1] = { 0 };
+	static bool warnedNoPaSnapshotSupport[CanId::MaxCanAddress + 1] = { false };
 
 	static CanMessageBuffer *_ecv_null GetBuffer(const PrepParams& params, DriverId canDriver) noexcept;
+	static bool BoardSupportsMovementPaSnapshot(CanAddress boardAddress) noexcept;
 	static void FreeMovementBuffers() noexcept;
 }
 
@@ -71,6 +73,16 @@ void CanMotion::FreeMovementBuffers() noexcept
 	}
 }
 
+bool CanMotion::BoardSupportsMovementPaSnapshot(CanAddress boardAddress) noexcept
+{
+	const ExpansionBoardData *const boardData = reprap.GetExpansion().GetBoardDetails(boardAddress);
+	if (boardData == nullptr)
+	{
+		return false;
+	}
+	return boardData->supportsMovementPaSnapshot;
+}
+
 // This is called by DDA::Prepare at the start of preparing a movement
 void CanMotion::StartMovement() noexcept
 {
@@ -140,6 +152,7 @@ CanMessageBuffer *_ecv_null CanMotion::GetBuffer(const PrepParams& params, Drive
 
 		move->acceleration = params.acceleration/params.totalDistance;					// scale the acceleration to correspond to unit distance
 		move->deceleration = -params.deceleration/params.totalDistance;					// scale the deceleration to correspond to unit distance
+		move->pressureAdvanceClocks = 0.0;
 		move->extruderDrives = 0;
 		move->numDrivers = canDriver.localDriver + 1;
 		move->zero1 = move->zero2 = 0;
@@ -168,14 +181,15 @@ void CanMotion::AddAxisMovement(const PrepParams& params, DriverId canDriver, in
 	}
 }
 
-void CanMotion::AddExtruderMovement(const PrepParams& params, DriverId canDriver, float extrusion, bool usePressureAdvance) noexcept
+void CanMotion::AddExtruderMovement(const PrepParams& params, DriverId canDriver, float extrusion, float pressureAdvanceClocks) noexcept
 {
 	CanMessageBuffer * const buf = GetBuffer(params, canDriver);
 	if (buf != nullptr)
 	{
 		buf->msg.moveLinearShaped.perDrive[canDriver.localDriver].extrusion = extrusion;
 		buf->msg.moveLinearShaped.extruderDrives |= 1u << canDriver.localDriver;
-		buf->msg.moveLinearShaped.usePressureAdvance = usePressureAdvance;
+		buf->msg.moveLinearShaped.usePressureAdvance = (pressureAdvanceClocks > 0.0);
+		buf->msg.moveLinearShaped.pressureAdvanceClocks = pressureAdvanceClocks;
 	}
 }
 
@@ -199,6 +213,21 @@ uint32_t CanMotion::FinishMovement(const DDA& dda, uint32_t moveStartTime, bool
 				CanMessageMovementLinearShaped& msg = buf->msg.moveLinearShaped;
 				if (msg.HasMotion())
 				{
+					if (!BoardSupportsMovementPaSnapshot(buf->id.Dst()))
+					{
+						if (!warnedNoPaSnapshotSupport[buf->id.Dst()])
+						{
+							warnedNoPaSnapshotSupport[buf->id.Dst()] = true;
+							reprap.GetPlatform().MessageF(ErrorMessage,
+								"CAN board %u is missing movement PA snapshot support; flash matching firmware on all boards\n",
+								buf->id.Dst());
+						}
+						reprap.EmergencyStop();
+						CanMessageBuffer::Free(buf);
+						buf = nextBuffer;
+						continue;
+					}
+
 					msg.whenToExecute = moveStartTime;
 					uint8_t& seq = nextSeq[buf->id.Dst()];
 					msg.seq = seq;

src/CAN/CanMotion.h

@@ -21,7 +21,7 @@ namespace CanMotion
 	void Init() noexcept;
 	void StartMovement() noexcept;
 	void AddAxisMovement(const PrepParams& params, DriverId canDriver, int32_t steps) noexcept;
-	void AddExtruderMovement(const PrepParams& params, DriverId canDriver, float extrusion, bool usePressureAdvance) noexcept;
+	void AddExtruderMovement(const PrepParams& params, DriverId canDriver, float extrusion, float pressureAdvanceClocks) noexcept;
 	uint32_t FinishMovement(const DDA& dda, uint32_t moveStartTime, bool simulating) noexcept;
 	bool CanPrepareMove() noexcept;
 	CanMessageBuffer *GetUrgentMessage() noexcept;

src/CAN/CommandProcessor.cpp

@@ -327,10 +327,22 @@ void CommandProcessor::ProcessReceivedMessage(CanMessageBuffer *buf) noexcept
 				reprap.ScheduleReset();
 				return;							// no reply needed
 
-			case CanMessageType::movementLinearShaped:
-				// Check for duplicate and out-of-sequence message
-				// We can get out-of-sequence messages because of a bug in the CAN hardware; so use only the sequence number to detect duplicates
-				{
+				case CanMessageType::movementLinearShaped:
+					{
+						const CanMessageMovementLinearShaped& msg = buf->msg.moveLinearShaped;
+						const size_t minLength = sizeof(msg) - sizeof(msg.perDrive);
+						const size_t expectedLength = minLength + (size_t)msg.numDrivers * sizeof(msg.perDrive[0]);
+						if (msg.numDrivers == 0 || msg.numDrivers > MaxLinearDriversPerCanSlave || buf->dataLength < expectedLength)
+						{
+							++oosMessagesOther;
+							CanInterface::LogIgnoredMovementMessage();
+							return;
+						}
+					}
+
+					// Check for duplicate and out-of-sequence message
+					// We can get out-of-sequence messages because of a bug in the CAN hardware; so use only the sequence number to detect duplicates
+					{
 					const int8_t seq = buf->msg.moveLinearShaped.seq;
 					if (((seq + 1) & CanMessageMovementLinearShaped::SeqMask) == expectedSeq)
 					{

src/CAN/ExpansionManager.cpp

@@ -118,6 +118,7 @@ ExpansionBoardData::ExpansionBoardData() noexcept
 	  driverData(nullptr),
 	  accelerometerRuns(0), closedLoopRuns(0),
 	  hasMcuTemp(false), hasVin(false), hasV12(false), hasAccelerometer(false),
+	  supportsMovementPaSnapshot(false),
 	  state(BoardState::unknown), numDrivers(0)
 {
 }
@@ -183,10 +184,12 @@ void ExpansionManager::ProcessAnnouncement(CanMessageBuffer *buf, bool isNewForm
 			if (isNewFormat)
 			{
 				boardTypeAndFirmwareVersion.copy(buf->msg.announceNew.boardTypeAndFirmwareVersion, CanMessageAnnounceNew::GetMaxTextLength(buf->dataLength));
+				board.supportsMovementPaSnapshot = buf->msg.announceNew.supportsMovementPaSnapshot;
 			}
 			else
 			{
 				boardTypeAndFirmwareVersion.copy(buf->msg.announceOld.boardTypeAndFirmwareVersion, CanMessageAnnounceOld::GetMaxTextLength(buf->dataLength));
+				board.supportsMovementPaSnapshot = false;
 			}
 			UpdateBoardState(src, BoardState::unknown);
 			if (board.typeName == nullptr || strcmp(board.typeName, boardTypeAndFirmwareVersion.c_str()) != 0)

src/CAN/ExpansionManager.h

@@ -44,7 +44,8 @@ struct ExpansionBoardData
 			 hasClosedLoop : 1,
 			 hasInductiveSensor : 1,
 			 usesUf2Binary : 1,
-			 spare : 9;
+			 supportsMovementPaSnapshot : 1,
+			 spare : 8;
 	BoardState state;
 	uint8_t numDrivers;
 	uint8_t accelerometerOrientation = DefaultAccelerometerOrientation;

src/GCodes/GCodes.cpp

@@ -2100,6 +2100,7 @@ bool GCodes::DoStraightMove(GCodeBuffer& gb, bool isCoordinated) THROWS(GCodeExc
 	ms.checkEndstops = false;
 	ms.reduceAcceleration = false;
 	ms.usePressureAdvance = false;
+	ms.pressureAdvance = 0.0;
 	ms.linearAxesMentioned = false;
 	ms.rotationalAxesMentioned = false;
 
@@ -2564,6 +2565,7 @@ bool GCodes::DoStraightMove(GCodeBuffer& gb, bool isCoordinated) THROWS(GCodeExc
 				AxesBitmap axesMentionedExceptZ = axesMentioned;
 				axesMentionedExceptZ.ClearBit(Z_AXIS);
 				ms.usePressureAdvance = axesMentionedExceptZ.IsNonEmpty();
+				ms.pressureAdvance = (ms.usePressureAdvance && ms.movementTool != nullptr) ? ms.movementTool->GetPressureAdvance() : 0.0;
 			}
 
 			// Apply segmentation if necessary
@@ -3026,6 +3028,7 @@ bool GCodes::DoArcMove(GCodeBuffer& gb, bool clockwise) THROWS(GCodeException)
 #endif
 
 	ms.usePressureAdvance = ms.hasPositiveExtrusion;
+	ms.pressureAdvance = (ms.usePressureAdvance && ms.movementTool != nullptr) ? ms.movementTool->GetPressureAdvance() : 0.0;
 
 	// Calculate the total angle moved, which depends on which way round we are going
 	float totalArc;

src/Movement/DDA.cpp

@@ -170,6 +170,7 @@ DDA::DDA(DDA *_ecv_null n) noexcept : next(n), prev(nullptr)
 
 	flags.all = 0;						// in particular we need to set endCoordinatesValid, usePressureAdvance to false, stateBits to empty, also checkEndstops false for the ATE build
 	SetState(empty);					// should alrrady be covered by the above
+	pressureAdvanceClocks = 0.0;
 	virtualExtruderPosition = 0.0;
 	filePos = noFilePosition;
 
@@ -377,7 +378,7 @@ MovementError DDA::InitStandardMove(DDARing& ring, const RawMove &nextMove, bool
 				}
 				if (flags.xyMoving && nextMove.usePressureAdvance)
 				{
-					const float compensationClocks = move.GetPressureAdvanceClocksForLogicalDrive(drive);
+					const float compensationClocks = (float)nextMove.pressureAdvance * (float)StepClockRate;
 					if (compensationClocks > 0.0)
 					{
 						// Compensation causes instant velocity changes equal to acceleration * k, so we may need to limit the acceleration
@@ -417,6 +418,7 @@ MovementError DDA::InitStandardMove(DDARing& ring, const RawMove &nextMove, bool
 	initialUserC0 = nextMove.initialUserC0;
 	initialUserC1 = nextMove.initialUserC1;
 	originalFeedRate = nextMove.originalFeedRate;
+	pressureAdvanceClocks = (nextMove.usePressureAdvance) ? (float)nextMove.pressureAdvance * (float)StepClockRate : 0.0;
 
 	// These 4 or 5 bits can be copied in one go by the compiler generating a ubfx instruction
 	flags.canPauseAfter = nextMove.canPauseAfter;
@@ -1112,7 +1114,7 @@ void DDA::Prepare(DDARing& ring, uint32_t prepareAdvanceTime, SimulationMode sim
 						else		// we don't generate segments for leadscrew adjustment moves to remote drivers
 #endif
 						{
-							move.AddLinearSegments(driver.localDriver + MaxAxesPlusExtruders, afterPrepare.moveStartTime, params, (motioncalc_t)delta, segFlags);
+							move.AddLinearSegments(driver.localDriver + MaxAxesPlusExtruders, afterPrepare.moveStartTime, params, (motioncalc_t)delta, segFlags, 0.0);
 						}
 					}
 				}
@@ -1146,7 +1148,7 @@ void DDA::Prepare(DDARing& ring, uint32_t prepareAdvanceTime, SimulationMode sim
 						delta = move.ApplyBacklashCompensation(drive, delta);
 
 						// We generate segments even for nonlocal drivers so that the final position is correct and to track the position in near real time
-						move.AddLinearSegments(drive, afterPrepare.moveStartTime, params, (motioncalc_t)delta, segFlags);
+						move.AddLinearSegments(drive, afterPrepare.moveStartTime, params, (motioncalc_t)delta, segFlags, 0.0);
 						afterPrepare.drivesMoving.SetBit(drive);
 
 #if SUPPORT_CAN_EXPANSION
@@ -1198,14 +1200,14 @@ void DDA::Prepare(DDARing& ring, uint32_t prepareAdvanceTime, SimulationMode sim
 							const motioncalc_t delta = totalDistance * directionVector[drive] * move.DriveStepsPerMm(drive);
 
 							// We generate segments even for nonlocal extruders in order to track extruder position
-							move.AddLinearSegments(drive, afterPrepare.moveStartTime, params, delta, segFlags.AddIsExtruder());
+							move.AddLinearSegments(drive, afterPrepare.moveStartTime, params, delta, segFlags.AddIsExtruder(), pressureAdvanceClocks);
 
 #if SUPPORT_CAN_EXPANSION
 							const DriverId driver = move.GetExtruderDriver(extruder);
 							if (driver.IsRemote())
 							{
 								// The MovementLinearShaped message requires the extrusion amount in steps to be passed as a float. The remote board adds the PA and handles fractional steps.
-								CanMotion::AddExtruderMovement(params, driver, delta, flags.usePressureAdvance);
+								CanMotion::AddExtruderMovement(params, driver, delta, flags.usePressureAdvance ? pressureAdvanceClocks : 0.0);
 							}
 #endif
 							afterPrepare.drivesMoving.SetBit(drive);

src/Movement/DDA.h

@@ -245,6 +245,7 @@ class DDA final
     float totalDistance;							// How long is the move in hypercuboid space
     float maxAcceleration, maxDeceleration;			// The maximum acceleration and deceleration to use, always positive
     float requestedSpeed;							// The speed that the user asked for
+    float pressureAdvanceClocks;					// pressure advance to use for this move, in step clocks
     float virtualExtruderPosition;					// the virtual extruder position at the end of this move, used for pause/resume
 
     // These vary depending on how we connect the move with its predecessor and successor, but remain constant while the move is being executed

src/Movement/Move.cpp

@@ -1814,7 +1814,7 @@ MoveSegment *Move::AddSegment(MoveSegment *list, uint32_t startTime, uint32_t du
 
 // Add some linear segments to be executed by a driver, taking account of possible input shaping. This is used by linear axes and by extruders.
 // We never add a segment that starts earlier than the earliest existing segment (if any).
-void Move::AddLinearSegments(size_t logicalDrive, uint32_t startTime, const PrepParams& params, motioncalc_t steps, MovementFlags moveFlags) noexcept
+void Move::AddLinearSegments(size_t logicalDrive, uint32_t startTime, const PrepParams& params, motioncalc_t steps, MovementFlags moveFlags, float pressureAdvanceClocks) noexcept
 {
 	if (reprap.GetDebugFlags(Module::Move).IsBitSet(MoveDebugFlags::Segments))
 	{
@@ -1899,7 +1899,7 @@ void Move::AddLinearSegments(size_t logicalDrive, uint32_t startTime, const Prep
 	else
 	{
 		accelDistance = (params.decelClocks + params.steadyClocks == 0) ? totalDistance : (motioncalc_t)params.accelDistance;
-		accelPressureAdvance = (moveFlags.isExtruder && !moveFlags.nonPrintingMove) ? (motioncalc_t)(params.accelClocks * dm.extruderShaper.GetKclocks()) : (motioncalc_t)0.0;
+		accelPressureAdvance = (moveFlags.isExtruder && !moveFlags.nonPrintingMove) ? (motioncalc_t)(params.accelClocks * pressureAdvanceClocks) : (motioncalc_t)0.0;
 	}
 
 	motioncalc_t decelDistance, decelPressureAdvance;
@@ -1911,7 +1911,7 @@ void Move::AddLinearSegments(size_t logicalDrive, uint32_t startTime, const Prep
 	else
 	{
 		decelDistance = totalDistance - ((params.steadyClocks == 0) ? accelDistance : (motioncalc_t)params.decelStartDistance);
-		decelPressureAdvance = (moveFlags.isExtruder && !moveFlags.nonPrintingMove) ? (motioncalc_t)(params.decelClocks * dm.extruderShaper.GetKclocks()) : (motioncalc_t)0.0;
+		decelPressureAdvance = (moveFlags.isExtruder && !moveFlags.nonPrintingMove) ? (motioncalc_t)(params.decelClocks * pressureAdvanceClocks) : (motioncalc_t)0.0;
 	}
 
 	const motioncalc_t steadyDistance = (params.steadyClocks == 0) ? (motioncalc_t)0.0 : totalDistance - accelDistance - decelDistance;

src/Movement/Move.h

@@ -346,7 +346,7 @@ class Move final INHERIT_OBJECT_MODEL
 	AxisShaper& GetAxisShaper() noexcept { return axisShaper; }
 
 	// Functions called by DDA::Prepare to generate segments for executing DDAs
-	void AddLinearSegments(size_t logicalDrive, uint32_t startTime, const PrepParams& params, motioncalc_t steps, MovementFlags moveFlags) noexcept;
+	void AddLinearSegments(size_t logicalDrive, uint32_t startTime, const PrepParams& params, motioncalc_t steps, MovementFlags moveFlags, float pressureAdvanceClocks) noexcept;
 
 	bool AreDrivesStopped(LogicalDrivesBitmap drives) const noexcept;						// return true if none of the drives passed has any movement pending