Merge remote-tracking branch 'origin/3.4-dev' into v3-chrishamm

7 files changed, 72 insertions, 20 deletions

diff --git a/src/Movement/AxisShaper.cpp b/src/Movement/AxisShaper.cpp
index f7dc64b9..55de9ad0 100644
--- a/src/Movement/AxisShaper.cpp
+++ b/src/Movement/AxisShaper.cpp
@@ -210,6 +210,18 @@ GCodeResult AxisShaper::Configure(GCodeBuffer& gb, const StringRef& reply) THROW
 			numExtraImpulses = 3;
 			break;
 
+		case InputShaperType::zvddd:
+			{
+				const float j = fsquare(fsquare(1.0 + k));
+				coefficients[0] = 1.0/j;
+				coefficients[1] = coefficients[0] + 4.0 * k/j;
+				coefficients[2] = coefficients[1] + 6.0 * fsquare(k)/j;
+				coefficients[3] = coefficients[2] + 4.0 * fcube(k)/j;
+			}
+			durations[0] = durations[1] = durations[2] = durations[3] = 0.5 * dampedPeriod;
+			numExtraImpulses = 4;
+			break;
+
 		case InputShaperType::ei2:		// see http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.465.1337&rep=rep1&type=pdf. United States patent #4,916,635.
 			{
 				const float zetaSquared = fsquare(zeta);
@@ -491,6 +503,7 @@ void AxisShaper::PlanShaping(DDA& dda, PrepParams& params, bool shapingEnabled)
 	case InputShaperType::zvd:
 	case InputShaperType::mzv:
 	case InputShaperType::zvdd:
+	case InputShaperType::zvddd:
 	case InputShaperType::ei2:
 	case InputShaperType::ei3:
 		params.SetFromDDA(dda);															// set up the provisional parameters
diff --git a/src/Movement/AxisShaper.h b/src/Movement/AxisShaper.h
index 18ee9e02..d4740a5f 100644
--- a/src/Movement/AxisShaper.h
+++ b/src/Movement/AxisShaper.h
@@ -27,6 +27,7 @@ NamedEnum(InputShaperType, uint8_t,
 	none,
 	zvd,
 	zvdd,
+	zvddd,
 );
 
 class DDA;
diff --git a/src/Movement/DDA.cpp b/src/Movement/DDA.cpp
index 69626146..6c08ce0f 100644
--- a/src/Movement/DDA.cpp
+++ b/src/Movement/DDA.cpp
@@ -388,7 +388,7 @@ bool DDA::InitStandardMove(DDARing& ring, const RawMove &nextMove, bool doMotorM
 			// It's an extruder drive. We defer calculating the steps because they may be affected by nonlinear extrusion, which we can't calculate until we
 			// know the speed of the move, and because extruder movement is relative so we need to accumulate fractions of a whole step between moves.
 			const float movement = nextMove.coords[drive];
-			directionVector[drive] = movement;
+			endCoordinates[drive] = directionVector[drive] = movement;			// for an extruder, endCoordinates is the amount of movement
 			if (movement != 0.0)
 			{
 				extrudersMoving = true;
@@ -2326,6 +2326,50 @@ void DDA::LimitSpeedAndAcceleration(float maxSpeed, float maxAcceleration) noexc
 	}
 }
 
+// Update the movement accumulators to account for the move that has just finished.
+// Only drives that correspond to extruders need to be updated, but it doesn't matter if we update them all.
+// This is called with interrupts disabled.
+void DDA::UpdateMovementAccumulators(volatile int32_t *accumulators) const noexcept
+{
+	// To identify all the extruder movement, we can either loop through extruder numbers and search both DM lists for a DM for that drive,
+	// or we can iterate through both DM lists, checking whether the drive it is for is an extruder.
+#if 1
+	// Loop through DMs, checking whether each associated drive is an extruder and updating the movement accumulator if so.
+	// We could omit the check that the drive is an accumulator so that we update all accumulators, but we would still need to check for leadscrew adjustment moves.
+	const unsigned int firstExtruderDrive = ExtruderToLogicalDrive(reprap.GetGCodes().GetNumExtruders() - 1);
+	for (const DriveMovement* dm = activeDMs; dm != nullptr; )
+	{
+		const uint8_t drv = dm->drive;
+		if (   drv >= firstExtruderDrive						// check that it's an extruder (to save the call to GetStepsTaken)
+			&& drv < MaxAxesPlusExtruders						// check that it's not a direct leadscrew move
+		   )
+		{
+			accumulators[drv] += dm->GetNetStepsTaken();
+		}
+		dm = dm->nextDM;
+	}
+	for (const DriveMovement* dm = completedDMs; dm != nullptr; )
+	{
+		const uint8_t drv = dm->drive;
+		if (   drv >= firstExtruderDrive						// check that it's an extruder (to save the call to GetStepsTaken)
+			&& drv < MaxAxesPlusExtruders						// check that it's not a direct leadscrew move
+		   )
+		{
+			accumulators[drv] += dm->GetNetStepsTaken();
+		}
+		dm = dm->nextDM;
+	}
+#else
+	// Loop through extruders
+	const size_t numExtruders = reprap.GetGCodes().GetNumExtruders();
+	for (size_t extruder = 0; extruder < numExtruders; ++extruder)
+	{
+		const size_t drv = ExtruderToLogicalDrive(extruder);
+		accumulators[drv] += GetStepsTaken(drv);
+	}
+#endif
+}
+
 #if SUPPORT_LASER
 
 // Manage the laser power. Return the number of ticks until we should be called again, or 0 to be called at the start of the next move.
diff --git a/src/Movement/DDA.h b/src/Movement/DDA.h
index 2c693d98..035b8f42 100644
--- a/src/Movement/DDA.h
+++ b/src/Movement/DDA.h
@@ -160,6 +160,7 @@ public:
 	uint32_t GetClocksNeeded() const noexcept { return clocksNeeded; }
 	bool IsGoodToPrepare() const noexcept;
 	bool IsNonPrintingExtruderMove() const noexcept { return flags.isNonPrintingExtruderMove; }
+	void UpdateMovementAccumulators(volatile int32_t *accumulators) const noexcept;
 
 #if SUPPORT_LASER || SUPPORT_IOBITS
 	LaserPwmOrIoBits GetLaserPwmOrIoBits() const noexcept { return laserPwmOrIoBits; }
diff --git a/src/Movement/DDARing.cpp b/src/Movement/DDARing.cpp
index 23270b35..4aeb39a7 100644
--- a/src/Movement/DDARing.cpp
+++ b/src/Movement/DDARing.cpp
@@ -572,18 +572,12 @@ void DDARing::CurrentMoveCompleted() noexcept
 	liveCoordinatesValid = cdda->FetchEndPosition(const_cast<int32_t*>(liveEndPoints), const_cast<float *>(liveCoordinates));
 	liveCoordinatesChanged = true;
 
-	// In the following it doesn't currently matter whether we process all drives or just the extruders
-	// Instead of looping through extruders, we could loop through DMs.
+	// Disable interrupts before we touch any extrusion accumulators until after we set currentDda to null, in case the filament monitor interrupt has higher priority than ours
 	{
-		const size_t numExtruders = reprap.GetGCodes().GetNumExtruders();
-		for (size_t extruder = 0; extruder < numExtruders; ++extruder)
-		{
-			const size_t drv = ExtruderToLogicalDrive(extruder);
-			movementAccumulators[drv] += cdda->GetStepsTaken(drv);
-		}
+		AtomicCriticalSectionLocker lock;
+		cdda->UpdateMovementAccumulators(movementAccumulators);
+		currentDda = nullptr;
 	}
-
-	currentDda = nullptr;
 	if (cdda->IsCheckingEndstops())
 	{
 		Move::WakeMoveTaskFromISR();				// wake the Move task if we were checking endstops
@@ -607,13 +601,12 @@ bool DDARing::SetWaitingToEmpty() noexcept
 
 int32_t DDARing::GetAccumulatedMovement(size_t drive, bool& isPrinting) noexcept
 {
-	const uint32_t basepri = ChangeBasePriority(NvicPriorityStep);
+	AtomicCriticalSectionLocker lock;
 	const int32_t ret = movementAccumulators[drive];
 	const DDA * const cdda = currentDda;						// capture volatile variable
 	const int32_t adjustment = (cdda == nullptr) ? 0 : cdda->GetStepsTaken(drive);
 	movementAccumulators[drive] = -adjustment;
 	isPrinting = extrudersPrinting;
-	RestoreBasePriority(basepri);
 	return ret + adjustment;
 }
 
diff --git a/src/Movement/Move.cpp b/src/Movement/Move.cpp
index 04781a55..08f167e0 100644
--- a/src/Movement/Move.cpp
+++ b/src/Movement/Move.cpp
@@ -894,13 +894,6 @@ void Move::GetCurrentUserPosition(float m[MaxAxes], uint8_t moveType, const Tool
 	}
 }
 
-// Get the accumulated extruder motor steps taken by an extruder since the last call. Used by the filament monitoring code.
-// Returns the number of motor steps moves since the last call, and sets isPrinting true unless we are currently executing an extruding but non-printing move
-int32_t Move::GetAccumulatedExtrusion(size_t drive, bool& isPrinting) noexcept
-{
-	return mainDDARing.GetAccumulatedMovement(drive, isPrinting);
-}
-
 void Move::SetXYBedProbePoint(size_t index, float x, float y) noexcept
 {
 	if (index >= MaxProbePoints)
diff --git a/src/Movement/Move.h b/src/Movement/Move.h
index 239e8ea6..4caf011f 100644
--- a/src/Movement/Move.h
+++ b/src/Movement/Move.h
@@ -343,6 +343,13 @@ inline float Move::GetPressureAdvanceClocks(size_t extruder) const noexcept
 	return (extruder < MaxExtruders) ? extruderShapers[extruder].GetKclocks() : 0.0;
 }
 
+// Get the accumulated extruder motor steps taken by an extruder since the last call. Used by the filament monitoring code.
+// Returns the number of motor steps moves since the last call, and sets isPrinting true unless we are currently executing an extruding but non-printing move
+inline int32_t Move::GetAccumulatedExtrusion(size_t drive, bool& isPrinting) noexcept
+{
+	return mainDDARing.GetAccumulatedMovement(drive, isPrinting);
+}
+
 #if HAS_SMART_DRIVERS
 
 // Get the current step interval for this axis or extruder, or 0 if it is not moving