Version 2.02beta12.02beta1

Fixed issues with dynamic acceleration adjustment
More work on rotary delta kinematics

8 files changed, 192 insertions, 137 deletions

diff --git a/src/GCodes/GCodes2.cpp b/src/GCodes/GCodes2.cpp
index 0e4e598d..d0e1e392 100644
--- a/src/GCodes/GCodes2.cpp
+++ b/src/GCodes/GCodes2.cpp
@@ -1929,34 +1929,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply)
 		break;
 
 	case 204: // Set max travel and printing accelerations
-		{
-			Move& move = reprap.GetMove();
-			bool seen = false;
-			if (gb.Seen('S'))
-			{
-				// For backwards compatibility with old versions of Marlin (e.g. for Cura and the Prusa fork of slic3r), set both accelerations
-				const float acc = gb.GetFValue();
-				move.SetMaxPrintingAcceleration(acc);
-				move.SetMaxTravelAcceleration(acc);
-				seen = true;
-			}
-			if (gb.Seen('P'))
-			{
-				move.SetMaxPrintingAcceleration(gb.GetFValue());
-				seen = true;
-			}
-			if (gb.Seen('T'))
-			{
-				move.SetMaxTravelAcceleration(gb.GetFValue());
-				seen = true;
-			}
-			if (!seen)
-			{
-				reply.printf("Maximum printing acceleration %.1f, maximum travel acceleration %.1f%s",
-					(double)move.GetMaxPrintingAcceleration(), (double)move.GetMaxTravelAcceleration(),
-					(move.IsDRCenabled()) ? " (both currently ignored because DRC is enabled)" : "");
-			}
-		}
+		result = reprap.GetMove().ConfigureAccelerations(gb, reply);
 		break;
 
 	case 206: // Offset axes
@@ -3671,18 +3644,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply)
 #endif
 
 	case 593: // Configure dynamic ringing cancellation
-		if (gb.Seen('F'))
-		{
-			reprap.GetMove().SetDRCfreq(gb.GetFValue());
-		}
-		else if (reprap.GetMove().IsDRCenabled())
-		{
-			reply.printf("Dynamic ringing cancellation at %.1fHz", (double)reprap.GetMove().GetDRCfreq());
-		}
-		else
-		{
-			reply.copy("Dynamic ringing cancellation is disabled");
-		}
+		result = reprap.GetMove().ConfigureDynamicAcceleration(gb, reply);
 		break;
 
 	case 665: // Set delta configuration
diff --git a/src/Movement/DDA.cpp b/src/Movement/DDA.cpp
index d37a4a95..9ca2034c 100644
--- a/src/Movement/DDA.cpp
+++ b/src/Movement/DDA.cpp
@@ -394,18 +394,22 @@ bool DDA::Init(GCodes::RawMove &nextMove, bool doMotorMapping)
 	xAxes = nextMove.xAxes;
 	yAxes = nextMove.yAxes;
 	endStopsToCheck = nextMove.endStopsToCheck;
-	canPauseAfter = nextMove.canPauseAfter;
-	usingStandardFeedrate = nextMove.usingStandardFeedrate;
 	filePos = nextMove.filePos;
-	isPrintingMove = xyMoving && extruding;
-	usePressureAdvance = nextMove.usePressureAdvance;
 	virtualExtruderPosition = nextMove.virtualExtruderPosition;
 	proportionLeft = nextMove.proportionLeft;
 
+	canPauseAfter = nextMove.canPauseAfter;
+	usingStandardFeedrate = nextMove.usingStandardFeedrate;
+	isPrintingMove = xyMoving && extruding;
+	usePressureAdvance = nextMove.usePressureAdvance;
 	hadLookaheadUnderrun = false;
+	hadHiccup = false;
 	isLeadscrewAdjustmentMove = false;
 	goingSlow = false;
 
+	// The end coordinates will be valid at the end of this move if it does not involve endstop checks and is not a raw motor move
+	endCoordinatesValid = (endStopsToCheck == 0) && doMotorMapping;
+
 #if SUPPORT_IOBITS
 	laserPwmOrIoBits = nextMove.laserPwmOrIoBits;
 #endif
@@ -416,9 +420,6 @@ bool DDA::Init(GCodes::RawMove &nextMove, bool doMotorMapping)
 		accelerations[Z_AXIS] = ZProbeMaxAcceleration;
 	}
 
-	// The end coordinates will be valid at the end of this move if it does not involve endstop checks and is not a raw motor move
-	endCoordinatesValid = (endStopsToCheck == 0) && doMotorMapping;
-
 	// 4. Normalise the direction vector and compute the amount of motion.
 	if (xyMoving)
 	{
@@ -448,11 +449,11 @@ bool DDA::Init(GCodes::RawMove &nextMove, bool doMotorMapping)
 	}
 
 	// 5. Compute the maximum acceleration available
-	float normalisedDirectionVector[DRIVES];			// Used to hold a unit-length vector in the direction of motion
+	float normalisedDirectionVector[DRIVES];			// used to hold a unit-length vector in the direction of motion
 	memcpy(normalisedDirectionVector, directionVector, sizeof(normalisedDirectionVector));
 	Absolute(normalisedDirectionVector, DRIVES);
 	acceleration = maxAcceleration = VectorBoxIntersection(normalisedDirectionVector, accelerations, DRIVES);
-	if (xyMoving && !move.IsDRCenabled())				// apply M204 acceleration limits to XY moves
+	if (xyMoving)										// apply M204 acceleration limits to XY moves
 	{
 		acceleration = min<float>(acceleration, (isPrintingMove) ? move.GetMaxPrintingAcceleration() : move.GetMaxTravelAcceleration());
 	}
@@ -556,17 +557,18 @@ bool DDA::Init(const float_t adjustments[DRIVES])
 	isDeltaMovement = false;
 	isPrintingMove = false;
 	xyMoving = false;
-	endStopsToCheck = 0;
 	canPauseAfter = true;
 	usingStandardFeedrate = false;
 	usePressureAdvance = false;
-	virtualExtruderPosition = prev->virtualExtruderPosition;
 	hadLookaheadUnderrun = false;
+	hadHiccup = false;
+	goingSlow = false;
+	endStopsToCheck = 0;
+	virtualExtruderPosition = prev->virtualExtruderPosition;
 	xAxes = prev->xAxes;
 	yAxes = prev->yAxes;
 	filePos = prev->filePos;
 	endCoordinatesValid = prev->endCoordinatesValid;
-	goingSlow = false;
 
 #if SUPPORT_LASER && SUPPORT_IOBITS
 	if (reprap.GetGCodes().GetMachineType() == MachineType::laser)
@@ -1051,67 +1053,118 @@ pre(disableDeltaMapping || drive < MaxAxes)
 }
 
 // Adjust the acceleration and deceleration to reduce ringing
+// Only called if topSpeed > startSpeed & topSpeed > endSpeed
 // This is only called once, so inlined for speed
 inline void DDA::AdjustAcceleration()
 {
 	// Try to reduce the acceleration/deceleration of the move to cancel ringing
 	const float idealPeriod = reprap.GetMove().GetDRCperiod();
-
 	const float accTime = (topSpeed - startSpeed)/acceleration;
 	const bool adjustAcceleration =
-		(accTime < idealPeriod && accTime > 0.2 * idealPeriod && ((prev->state != DDAState::frozen && prev->state != DDAState::executing) || !prev->IsAccelerationMove()));
+		(accTime < idealPeriod && ((prev->state != DDAState::frozen && prev->state != DDAState::executing) || !prev->IsAccelerationMove()));
+	float proposedAcceleration, proposedAccelDistance;
 	if (adjustAcceleration)
 	{
-		acceleration = (topSpeed - startSpeed)/idealPeriod;
-		accelDistance = (fsquare(topSpeed) - fsquare(startSpeed))/(2 *acceleration);
+		proposedAcceleration = (topSpeed - startSpeed)/idealPeriod;
+		proposedAccelDistance = (fsquare(topSpeed) - fsquare(startSpeed))/(2 * proposedAcceleration);
+	}
+	else
+	{
+		proposedAcceleration = acceleration;
+		proposedAccelDistance = accelDistance;
 	}
 
 	const float decelTime = (topSpeed - endSpeed)/deceleration;
 	const float adjustDeceleration =
-		(decelTime < idealPeriod && decelTime > 0.2 * idealPeriod && (next->state != DDAState::provisional || !next->IsDecelerationMove()));
+		(decelTime < idealPeriod && (next->state != DDAState::provisional || !next->IsDecelerationMove()));
+	float proposedDeceleration, proposedDecelDistance;
 	if (adjustDeceleration)
 	{
-		deceleration = (topSpeed - endSpeed)/idealPeriod;
-		decelDistance = (fsquare(topSpeed) - fsquare(endSpeed))/(2 * deceleration);
+		proposedDeceleration = (topSpeed - endSpeed)/idealPeriod;
+		proposedDecelDistance = (fsquare(topSpeed) - fsquare(endSpeed))/(2 * proposedDeceleration);
+	}
+	else
+	{
+		proposedDeceleration = deceleration;
+		proposedDecelDistance = decelDistance;
 	}
 
 	if (adjustAcceleration || adjustDeceleration)
 	{
-		if (accelDistance + decelDistance > totalDistance)
+		const float drcMinimumAcceleration = reprap.GetMove().GetDRCminimumAcceleration();
+		if (proposedAccelDistance + proposedDecelDistance <= totalDistance)
 		{
-			// We can't keep this as a trapezoidal move
+			if (proposedAcceleration < drcMinimumAcceleration || proposedDeceleration < drcMinimumAcceleration)
+			{
+				return;
+			}
+			acceleration = proposedAcceleration;
+			deceleration = proposedDeceleration;
+			accelDistance = proposedAccelDistance;
+			decelDistance = proposedDecelDistance;
+		}
+		else
+		{
+			// We can't keep this as a trapezoidal move with the original top speed.
+			// Try an accelerate-decelerate move with acceleration and deceleration times equal to the ideal period.
 			const float twiceTotalDistance = 2 * totalDistance;
-			if (adjustAcceleration && adjustDeceleration && (startSpeed + endSpeed) * idealPeriod < totalDistance)
+			float proposedTopSpeed = totalDistance/idealPeriod - (startSpeed + endSpeed)/2;
+			if (proposedTopSpeed > startSpeed && proposedTopSpeed > endSpeed)
 			{
-				// Change it into an accelerate-decelerate move with equal acceleration and deceleration times
-				acceleration = (twiceTotalDistance - (3 * startSpeed + endSpeed) * idealPeriod)/(2 * fsquare(idealPeriod));
-				deceleration = (twiceTotalDistance - (startSpeed + 3 * endSpeed) * idealPeriod)/(2 * fsquare(idealPeriod));
-				topSpeed = (totalDistance/idealPeriod) - (startSpeed + endSpeed)/2;
+				proposedAcceleration = (twiceTotalDistance - ((3 * startSpeed + endSpeed) * idealPeriod))/(2 * fsquare(idealPeriod));
+				proposedDeceleration = (twiceTotalDistance - ((startSpeed + 3 * endSpeed) * idealPeriod))/(2 * fsquare(idealPeriod));
+				if (   proposedAcceleration < drcMinimumAcceleration || proposedDeceleration < drcMinimumAcceleration
+					|| proposedAcceleration > acceleration || proposedDeceleration > deceleration
+				   )
+				{
+					return;
+				}
+				topSpeed = proposedTopSpeed;
+				acceleration = proposedAcceleration;
+				deceleration = proposedDeceleration;
 				accelDistance = startSpeed * idealPeriod + (acceleration * fsquare(idealPeriod))/2;
 				decelDistance = endSpeed * idealPeriod + (deceleration * fsquare(idealPeriod))/2;
 			}
 			else if (startSpeed < endSpeed)
 			{
 				// Change it into an accelerate-only move, accelerating as slowly as we can
-				acceleration = (fsquare(endSpeed) - fsquare(startSpeed))/twiceTotalDistance;
+				proposedAcceleration = (fsquare(endSpeed) - fsquare(startSpeed))/twiceTotalDistance;
+				if (proposedAcceleration < drcMinimumAcceleration)
+				{
+					return;		// avoid very small accelerations because they can be problematic
+				}
+				acceleration = proposedAcceleration;
 				topSpeed = endSpeed;
 				accelDistance = totalDistance;
 				decelDistance = 0.0;
 			}
-			else
+			else if (startSpeed > endSpeed)
 			{
 				// Change it into a decelerate-only move, decelerating as slowly as we can
-				deceleration = (fsquare(startSpeed) - fsquare(endSpeed))/twiceTotalDistance;
+				proposedDeceleration = (fsquare(startSpeed) - fsquare(endSpeed))/twiceTotalDistance;
+				if (proposedDeceleration < drcMinimumAcceleration)
+				{
+					return;		// avoid very small accelerations because they can be problematic
+				}
+				deceleration = proposedDeceleration;
 				topSpeed = startSpeed;
 				accelDistance = 0.0;
 				decelDistance = totalDistance;
 			}
+			else
+			{
+				// Start and end speeds are exactly the same, possibly zero, so give up trying to adjust this move
+				return;
+			}
 		}
-		const float totalTime = (topSpeed - startSpeed)/acceleration + (topSpeed - endSpeed)/deceleration + (totalDistance - accelDistance - decelDistance)/topSpeed;
+
+		const float totalTime =   (topSpeed - startSpeed)/acceleration
+								+ (topSpeed - endSpeed)/deceleration
+								+ (totalDistance - accelDistance - decelDistance)/topSpeed;
 		clocksNeeded = (uint32_t)(totalTime * StepClockRate);
 		if (reprap.Debug(moduleMove))
 		{
-			debugPrintf("New a=%f d=%f\n", (double)acceleration, (double)deceleration);
+			debugPrintf("New a=%.1f d=%.1f\n", (double)acceleration, (double)deceleration);
 		}
 	}
 }
@@ -1171,7 +1224,7 @@ void DDA::Prepare(uint8_t simMode)
 		const size_t numAxes = reprap.GetGCodes().GetTotalAxes();
 		for (size_t drive = 0; drive < DRIVES; ++drive)
 		{
-			DriveMovement* const pdm = pddm[drive];
+			DriveMovement* const pdm = FindDM(drive);
 			if (pdm != nullptr && pdm->state == DMState::moving)
 			{
 				if (isLeadscrewAdjustmentMove)
@@ -1209,7 +1262,8 @@ void DDA::Prepare(uint8_t simMode)
 							&& (   pdm->totalSteps > 1000000
 								|| pdm->reverseStartStep < pdm->mp.cart.decelStartStep
 								|| (pdm->reverseStartStep <= pdm->totalSteps
-									&& pdm->mp.cart.fourMaxStepDistanceMinusTwoDistanceToStopTimesCsquaredDivD > (int64_t)(pdm->mp.cart.twoCsquaredTimesMmPerStepDivD * pdm->reverseStartStep))
+									&& pdm->mp.cart.fourMaxStepDistanceMinusTwoDistanceToStopTimesCsquaredDivD > (int64_t)(pdm->mp.cart.twoCsquaredTimesMmPerStepDivD * pdm->reverseStartStep)
+								   )
 							   )
 						   )
 						{
@@ -1587,7 +1641,7 @@ pre(state == frozen)
 	return true;	// schedule another interrupt immediately
 }
 
-uint32_t DDA::numHiccups = 0;
+unsigned int DDA::numHiccups = 0;
 uint32_t DDA::lastStepLowTime = 0;
 uint32_t DDA::lastDirChangeTime = 0;
 
@@ -1695,6 +1749,7 @@ bool DDA::Step()
 			moveStartTime += delayClocks;
 			nextStepDue += delayClocks;
 			++numHiccups;
+			hadHiccup = true;
 		}
 
 		// 8. Schedule next interrupt, or if it would be too soon, generate more steps immediately
@@ -1819,6 +1874,13 @@ void DDA::ReduceHomingSpeed()
 
 bool DDA::HasStepError() const
 {
+#if 0	//debug
+	if (hadHiccup)
+	{
+		return true;			// temporary for debugging DAA
+	}
+#endif
+
 	for (size_t drive = 0; drive < DRIVES; ++drive)
 	{
 		const DriveMovement* const pdm = FindDM(drive);
diff --git a/src/Movement/DDA.h b/src/Movement/DDA.h
index caaa7918..2e465436 100644
--- a/src/Movement/DDA.h
+++ b/src/Movement/DDA.h
@@ -130,7 +130,7 @@ public:
 	static int32_t loggedProbePositions[XYZ_AXES * MaxLoggedProbePositions];
 #endif
 
-	static uint32_t numHiccups;										// how many times we delayed an interrupt to avoid using too much CPU time in interrupts
+	static unsigned int numHiccups;									// how many times we delayed an interrupt to avoid using too much CPU time in interrupts
 	static uint32_t lastStepLowTime;								// when we last completed a step pulse to a slow driver
 	static uint32_t lastDirChangeTime;								// when we last change the DIR signal to a slow driver
 
@@ -177,6 +177,7 @@ private:
 			uint8_t goingSlow : 1;					// True if we have slowed the movement because the Z probe is approaching its threshold
 			uint8_t isLeadscrewAdjustmentMove : 1;	// True if this is a leadscrews adjustment move
 			uint8_t usingStandardFeedrate : 1;		// True if this move uses the standard feed rate
+			uint8_t hadHiccup : 1;					// True if we had a hiccup while executing this move
 		};
 		uint16_t flags;								// so that we can print all the flags at once for debugging
 	};
diff --git a/src/Movement/DriveMovement.cpp b/src/Movement/DriveMovement.cpp
index 7ebbc6c5..189b0d7c 100644
--- a/src/Movement/DriveMovement.cpp
+++ b/src/Movement/DriveMovement.cpp
@@ -78,8 +78,7 @@ void DriveMovement::PrepareCartesianAxis(const DDA& dda, const PrepParams& param
 	else
 	{
 		mp.cart.decelStartStep = (uint32_t)(params.decelStartDistance * stepsPerMm) + 1;
-		const uint64_t initialDecelSpeedTimesCdivDSquared = isquare64(params.topSpeedTimesCdivD);
-		twoDistanceToStopTimesCsquaredDivD = initialDecelSpeedTimesCdivDSquared + roundU64((params.decelStartDistance * (StepClockRateSquared * 2))/dda.deceleration);
+		twoDistanceToStopTimesCsquaredDivD = isquare64(params.topSpeedTimesCdivD) + roundU64((params.decelStartDistance * (StepClockRateSquared * 2))/dda.deceleration);
 	}
 
 	// No reverse phase
diff --git a/src/Movement/Kinematics/RotaryDeltaKinematics.cpp b/src/Movement/Kinematics/RotaryDeltaKinematics.cpp
index 3b59d215..8323bbb8 100644
--- a/src/Movement/Kinematics/RotaryDeltaKinematics.cpp
+++ b/src/Movement/Kinematics/RotaryDeltaKinematics.cpp
@@ -77,14 +77,14 @@ bool RotaryDeltaKinematics::Configure(unsigned int mCode, GCodeBuffer& gb, const
 		{
 			bool seen = false;
 			size_t numValues = 3;
-			if (gb.TryGetFloatArray('L', numValues, armLengths, reply, seen, true))
+			if (gb.TryGetFloatArray('U', numValues, armLengths, reply, seen, true))
 			{
 				error = true;
 				return true;
 			}
 
 			numValues = 3;
-			if (gb.TryGetFloatArray('R', numValues, rodLengths, reply, seen, true))
+			if (gb.TryGetFloatArray('L', numValues, rodLengths, reply, seen, true))
 			{
 				error = true;
 				return true;
@@ -127,7 +127,7 @@ bool RotaryDeltaKinematics::Configure(unsigned int mCode, GCodeBuffer& gb, const
 			}
 			else
 			{
-				reply.printf("Arms (%.3f,%.2f,%.3f)mm, rods (%.3f,%.3f,%.3f)mm, bearingHeights (%.3f,%.2f,%.3f)mm"
+				reply.printf("Kinematics is rotary delta, arms (%.3f,%.2f,%.3f)mm, rods (%.3f,%.3f,%.3f)mm, bearingHeights (%.3f,%.2f,%.3f)mm"
 							 ", arm movement %.1f to %.1f" DEGREE_SYMBOL
 							 ", delta radius %.3f, bed radius %.1f"
 							 ", angle corrections (%.3f,%.3f,%.3f)" DEGREE_SYMBOL ,
@@ -186,9 +186,10 @@ bool RotaryDeltaKinematics::CartesianToMotorSteps(const float machinePos[], cons
 	// TEMP DEBUG
 	if (reprap.Debug(moduleMove))
 	{
-		debugPrintf("Transformed %.2f,%.2f,%.2f mm to %" PRIi32 ",%" PRIi32 ",%" PRIi32 "steps\n",
+		debugPrintf("Transformed %.2f,%.2f,%.2f mm to %" PRIi32 ",%" PRIi32 ",%" PRIi32 " steps %s\n",
 			(double)machinePos[0], (double)machinePos[1], (double)machinePos[2],
-			motorPos[0], motorPos[1], motorPos[2]);
+			motorPos[0], motorPos[1], motorPos[2],
+			(ok) ? "ok" : "fail");
 	}
 
 	// Transform any additional axes linearly
@@ -424,69 +425,43 @@ float RotaryDeltaKinematics::Transform(const float machinePos[], size_t axis) co
 void RotaryDeltaKinematics::ForwardTransform(float Ha, float Hb, float Hc, float machinePos[DELTA_AXES]) const
 {
 	// Calculate the Cartesian coordinates of the joints at the moving ends of the arms
-	const float angleA = Ha  * DegreesToRadians;
+	const float angleA = Ha * DegreesToRadians;
 	const float posAX = (radius + (armLengths[DELTA_A_AXIS] * cosf(angleA))) * armAngleCosines[DELTA_A_AXIS];
 	const float posAY = (radius + (armLengths[DELTA_A_AXIS] * cosf(angleA))) * armAngleSines[DELTA_A_AXIS];
-	const float posAZ = bearingHeights[DELTA_A_AXIS] + sinf(angleA);
+	const float posAZ = bearingHeights[DELTA_A_AXIS] + (armLengths[DELTA_A_AXIS] * sinf(angleA));
 
 	const float angleB = Hb * DegreesToRadians;
 	const float posBX = (radius + (armLengths[DELTA_B_AXIS] * cosf(angleB))) * armAngleCosines[DELTA_B_AXIS];
 	const float posBY = (radius + (armLengths[DELTA_B_AXIS] * cosf(angleB))) * armAngleSines[DELTA_B_AXIS];
-	const float posBZ = bearingHeights[DELTA_B_AXIS] + sinf(angleB);
+	const float posBZ = bearingHeights[DELTA_B_AXIS] + (armLengths[DELTA_A_AXIS] * sinf(angleB));
 
 	const float angleC = Hc * DegreesToRadians;
 	const float posCX = (radius + (armLengths[DELTA_C_AXIS] * cosf(angleC))) * armAngleCosines[DELTA_C_AXIS];
 	const float posCY = (radius + (armLengths[DELTA_C_AXIS] * cosf(angleC))) * armAngleSines[DELTA_C_AXIS];
-	const float posCZ = bearingHeights[DELTA_C_AXIS] + sinf(angleC);
+	const float posCZ = bearingHeights[DELTA_C_AXIS] + (armLengths[DELTA_A_AXIS] * sinf(angleC));
 
 	// Calculate some intermediate values that we will use more than once
 	const float Da2 = fsquare(posAX) + fsquare(posAY) + fsquare(posAZ);
 	const float Db2 = fsquare(posBX) + fsquare(posBY) + fsquare(posBZ);
 	const float Dc2 = fsquare(posCX) + fsquare(posCY) + fsquare(posCZ);
 
-	const float Xab = posAX - posBX;
-	const float Xbc = posBX - posCX;
-	const float Xca = posCX - posAX;
-
-	const float Yab = posAY - posBY;
-	const float Ybc = posBY - posCY;
-	const float Yca = posCY - posAY;
-
-	const float Zca = posCZ - posAZ;
-
 	// Calculate PQRST such that x = (Qz + S)/P, y = (Rz + T)/P.
-	const float P = (  posBX * Yca
-					 - posAX * posCY
-					 + posAY * posCX
-					 - posBY * Xca
-					) * 2;
-
-	const float Q = (  posBY * Zca
-					 - posAY * posCZ
-					 + posAZ * posCY
-					 - posBZ * Yca
-					) * 2;
-
-	const float R = - (  posBX * Zca
-					   + posAX * posCZ
-					   + posAZ * posCX
-					   - posBZ * Xca
-					  ) * 2;
+	const float P = (posBX * posCY - posAX * posCY - posCX * posBY + posAX * posBY + posCX * posAY - posBX * posAY) * 2;
+	const float Q = ((posBY - posAY) * posCZ + (posAY - posCY) * posBZ + (posCY - posBY) * posAZ) * 2;
+	const float R = ((posBX - posAX) * posCZ + (posAX - posCX) * posBZ + (posCX - posBX) * posAZ) * 2;
 
-	const float P2 = fsquare(P);
-	const float U = (posAZ * P2) + (posAX * Q * P) + (posAY * R * P);
-	const float A = (P2 + fsquare(Q) + fsquare(R)) * 2;
-
-	const float S = - Yab * (rodSquared[DELTA_C_AXIS] - Dc2)
-	                - Yca * (rodSquared[DELTA_B_AXIS] - Db2)
-	                - Ybc * (rodSquared[DELTA_A_AXIS] - Da2);
-	const float T = - Xab * (rodSquared[DELTA_C_AXIS] - Dc2)
-	                + Xca * (rodSquared[DELTA_B_AXIS] - Db2)
-	                + Xbc * (rodSquared[DELTA_A_AXIS] - Da2);
+	const float S =   (rodSquared[DELTA_A_AXIS] - rodSquared[DELTA_B_AXIS] + Db2 - Da2) * posCY
+					+ (rodSquared[DELTA_C_AXIS] - rodSquared[DELTA_A_AXIS] + Da2 - Dc2) * posBY
+					+ (rodSquared[DELTA_B_AXIS] - rodSquared[DELTA_C_AXIS] + Dc2 - Db2) * posAY;
+	const float T =   (rodSquared[DELTA_A_AXIS] - rodSquared[DELTA_B_AXIS] + Db2 - Da2) * posCX
+					+ (rodSquared[DELTA_C_AXIS] - rodSquared[DELTA_A_AXIS] + Da2 - Dc2) * posBX
+					+ (rodSquared[DELTA_B_AXIS] - rodSquared[DELTA_C_AXIS] + Dc2 - Db2) * posAX;
 
 	// Calculate quadratic equation coefficients
-	const float halfB = (S * Q) - (R * T) - U;
-	const float C = fsquare(S) + fsquare(T) + (posAY * T - posAX * S) * P * 2 + (Da2 - rodSquared[DELTA_A_AXIS]) * P2;
+	const float P2 = fsquare(P);
+	const float A = P2 + fsquare(Q) + fsquare(R);
+	const float halfB = (P * R * posAY) - (P2 * posAZ) - (P * Q * posAX) + (R * T) + (Q * S);
+	const float C = fsquare(S) + fsquare(T) + (T * posAY - S * posAX) * P * 2 + (Da2 - rodSquared[DELTA_A_AXIS]) * P2;
 
 	// Solve the quadratic equation for z
 	const float z = (- halfB - sqrtf(fsquare(halfB) - A * C))/A;
diff --git a/src/Movement/Move.cpp b/src/Movement/Move.cpp
index b1553de5..37d5a771 100644
--- a/src/Movement/Move.cpp
+++ b/src/Movement/Move.cpp
@@ -35,6 +35,7 @@
 
 #include "Move.h"
 #include "Platform.h"
+#include "GCodes/GCodeBuffer.h"
 #include "Tools/Tool.h"
 
 constexpr uint32_t UsualMinimumPreparedTime = StepClockRate/10;			// 100ms
@@ -75,6 +76,7 @@ void Move::Init()
 	numLookaheadUnderruns = numPrepareUnderruns = numLookaheadErrors = 0;
 	maxPrintingAcceleration = maxTravelAcceleration = 10000.0;
 	drcEnabled = false;											// disable dynamic ringing cancellation
+	drcMinimumAcceleration = 10.0;
 
 	// Clear the transforms
 	SetIdentityTransform();
@@ -598,9 +600,10 @@ void Move::Diagnostics(MessageType mtype)
 {
 	Platform& p = reprap.GetPlatform();
 	p.Message(mtype, "=== Move ===\n");
-	p.MessageF(mtype, "Hiccups: %" PRIu32 ", StepErrors: %u, LaErrors: %u, FreeDm: %d, MinFreeDm: %d, MaxWait: %" PRIu32 "ms, Underruns: %u, %u\n",
+	p.MessageF(mtype, "Hiccups: %u, StepErrors: %u, LaErrors: %u, FreeDm: %d, MinFreeDm: %d, MaxWait: %" PRIu32 "ms, Underruns: %u, %u\n",
 						DDA::numHiccups, stepErrors, numLookaheadErrors, DriveMovement::NumFree(), DriveMovement::MinFree(), longestGcodeWaitInterval, numLookaheadUnderruns, numPrepareUnderruns);
 	DDA::numHiccups = 0;
+	stepErrors = 0;
 	numLookaheadUnderruns = numPrepareUnderruns = numLookaheadErrors = 0;
 	longestGcodeWaitInterval = 0;
 	DriveMovement::ResetMinFree();
@@ -1292,18 +1295,69 @@ bool Move::WriteResumeSettings(FileStore *f) const
 	return kinematics->WriteResumeSettings(f) && (!usingMesh || f->Write("G29 S1\n"));
 }
 
-// Set the DRC frequency or disable DRC
-void Move::SetDRCfreq(float f)
+// Process M204
+GCodeResult Move::ConfigureAccelerations(GCodeBuffer&gb, const StringRef& reply)
 {
-	if (f >= 4.0 && f <= 10000.0)
+	bool seen = false;
+	if (gb.Seen('S'))
 	{
-		drcPeriod = 1.0/f;
-		drcEnabled = true;
+		// For backwards compatibility with old versions of Marlin (e.g. for Cura and the Prusa fork of slic3r), set both accelerations
+		seen = true;
+		maxTravelAcceleration = maxPrintingAcceleration = gb.GetFValue();
 	}
-	else
+	if (gb.Seen('P'))
+	{
+		seen = true;
+		maxPrintingAcceleration = gb.GetFValue();
+	}
+	if (gb.Seen('T'))
+	{
+		seen = true;
+		maxTravelAcceleration = gb.GetFValue();
+	}
+	if (!seen)
+	{
+		reply.printf("Maximum printing acceleration %.1f, maximum travel acceleration %.1f", (double)maxPrintingAcceleration, (double)maxTravelAcceleration);
+	}
+	return GCodeResult::ok;
+}
+
+// Process M593
+GCodeResult Move::ConfigureDynamicAcceleration(GCodeBuffer& gb, const StringRef& reply)
+{
+	bool seen = false;
+	if (gb.Seen('F'))
+	{
+		seen = true;
+		const float f = gb.GetFValue();
+		if (f >= 4.0 && f <= 10000.0)
+		{
+			drcPeriod = 1.0/f;
+			drcEnabled = true;
+		}
+		else
+		{
+			drcEnabled = false;
+		}
+	}
+	if (gb.Seen('L'))
 	{
-		drcEnabled = false;
+		seen = true;
+		drcMinimumAcceleration = max<float>(gb.GetFValue(), 1.0);		// very low accelerations cause problems with the maths
+	}
+
+	if (!seen)
+	{
+		if (reprap.GetMove().IsDRCenabled())
+		{
+			reply.printf("Dynamic ringing cancellation at %.1fHz, min. acceleration %.1f", (double)(1.0/drcPeriod), (double)drcMinimumAcceleration);
+		}
+		else
+		{
+			reply.copy("Dynamic ringing cancellation is disabled");
+		}
 	}
+	return GCodeResult::ok;
 }
 
 // For debugging
diff --git a/src/Movement/Move.h b/src/Movement/Move.h
index e8a6bb32..87070b4b 100644
--- a/src/Movement/Move.h
+++ b/src/Movement/Move.h
@@ -71,14 +71,15 @@ public:
 	unsigned int GetNumProbePoints() const;							// Return the number of currently used probe points
 	float PushBabyStepping(float amount);							// Try to push some babystepping through the lookahead queue
 
+	GCodeResult ConfigureAccelerations(GCodeBuffer&gb, const StringRef& reply);			// process M204
+	GCodeResult ConfigureDynamicAcceleration(GCodeBuffer& gb, const StringRef& reply);	// process M593
+
 	float GetMaxPrintingAcceleration() const { return maxPrintingAcceleration; }
-	void SetMaxPrintingAcceleration(float acc) { maxPrintingAcceleration = acc; }
 	float GetMaxTravelAcceleration() const { return maxTravelAcceleration; }
-	void SetMaxTravelAcceleration(float acc) { maxTravelAcceleration = acc; }
 	float GetDRCfreq() const { return 1.0/drcPeriod; }
 	float GetDRCperiod() const { return drcPeriod; }
+	float GetDRCminimumAcceleration() const { return drcMinimumAcceleration; }
 	float IsDRCenabled() const { return drcEnabled; }
-	void SetDRCfreq(float f);
 
 	void Diagnostics(MessageType mtype);							// Report useful stuff
 	void RecordLookaheadError() { ++numLookaheadErrors; }			// Record a lookahead error
@@ -179,6 +180,7 @@ private:
 	float maxPrintingAcceleration;
 	float maxTravelAcceleration;
 	float drcPeriod;									// the period of ringing that we don't want to excite
+	float drcMinimumAcceleration;						// the minimum value that we reduce acceleration to
 
 	unsigned int numLookaheadUnderruns;					// How many times we have run out of moves to adjust during lookahead
 	unsigned int numPrepareUnderruns;					// How many times we wanted a new move but there were only un-prepared moves in the queue
diff --git a/src/Version.h b/src/Version.h
index 10f4fb28..326e2994 100644
--- a/src/Version.h
+++ b/src/Version.h
@@ -22,7 +22,7 @@
 #endif
 
 #ifndef DATE
-# define DATE "2018-08-11b4"
+# define DATE "2018-08-12b5"
 #endif
 
 #define AUTHORS "reprappro, dc42, chrishamm, t3p3, dnewman"