Version 1.04d

Tidied up delta auto-calibration code. We now use least squares for 3,
4, 6 or 7-factor calibration.
When doing delta calibration, if a probe offset was used to adjust the
head position when probing, use the actual head coordinates instead the
probed coordinates.
Bug fix: newline was missing at end of SD card file list sent to USB
when in Marlin emulation mode.
Bug fix: Heater average PWM report (M573) sometimes gave inaccurate
values when the S parameter was used.

20 files changed, 415 insertions, 299 deletions

diff --git a/.cproject b/.cproject
index 0bbbfb69..e1b32879 100644
--- a/.cproject
+++ b/.cproject
@@ -29,9 +29,9 @@
 									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/RepRapFirmware/Libraries/SD_HSMCI}&quot;"/>
 									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/RepRapFirmware/Libraries/SD_HSMCI/utility}&quot;"/>
 									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/RepRapFirmware/Libraries/Wire}&quot;"/>
-									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/RepRapFirmware/Flash}&quot;"/>
 									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/RepRapFirmware/arduino/variant}&quot;"/>
 									<listOptionValue builtIn="false" value="&quot;C:\Arduino-1.5.8\hardware\tools\gcc-arm-none-eabi-4.8.3-2014q1\lib\gcc\arm-none-eabi\4.8.3\include&quot;"/>
+									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/RepRapFirmware/Libraries/Flash}&quot;"/>
 								</option>
 								<inputType id="it.baeyens.arduino.compiler.cpp.sketch.input.1531738210" name="CPP source files" superClass="it.baeyens.arduino.compiler.cpp.sketch.input"/>
 							</tool>
diff --git a/.settings/org.eclipse.cdt.core.prefs b/.settings/org.eclipse.cdt.core.prefs
index d4474108..6d0b64af 100644
--- a/.settings/org.eclipse.cdt.core.prefs
+++ b/.settings/org.eclipse.cdt.core.prefs
@@ -91,7 +91,7 @@ environment/project/it.baeyens.arduino.core.toolChain.release.674980254/A.COMPIL
 environment/project/it.baeyens.arduino.core.toolChain.release.674980254/A.COMPILER.C.EXTRA_FLAGS/value=
 environment/project/it.baeyens.arduino.core.toolChain.release.674980254/A.COMPILER.C.FLAGS/delimiter=;
 environment/project/it.baeyens.arduino.core.toolChain.release.674980254/A.COMPILER.C.FLAGS/operation=replace
-environment/project/it.baeyens.arduino.core.toolChain.release.674980254/A.COMPILER.C.FLAGS/value=-c -g -Os -w -ffunction-sections -fdata-sections -nostdlib --param max-inline-insns-single\=500 -Dprintf\=iprintf
+environment/project/it.baeyens.arduino.core.toolChain.release.674980254/A.COMPILER.C.FLAGS/value=-c -g -O2 -w -ffunction-sections -fdata-sections -nostdlib --param max-inline-insns-single\=500 -Dprintf\=iprintf
 environment/project/it.baeyens.arduino.core.toolChain.release.674980254/A.COMPILER.CPP.CMD/delimiter=;
 environment/project/it.baeyens.arduino.core.toolChain.release.674980254/A.COMPILER.CPP.CMD/operation=replace
 environment/project/it.baeyens.arduino.core.toolChain.release.674980254/A.COMPILER.CPP.CMD/value=arm-none-eabi-g++
@@ -100,7 +100,7 @@ environment/project/it.baeyens.arduino.core.toolChain.release.674980254/A.COMPIL
 environment/project/it.baeyens.arduino.core.toolChain.release.674980254/A.COMPILER.CPP.EXTRA_FLAGS/value=
 environment/project/it.baeyens.arduino.core.toolChain.release.674980254/A.COMPILER.CPP.FLAGS/delimiter=;
 environment/project/it.baeyens.arduino.core.toolChain.release.674980254/A.COMPILER.CPP.FLAGS/operation=replace
-environment/project/it.baeyens.arduino.core.toolChain.release.674980254/A.COMPILER.CPP.FLAGS/value=-c -g -Os -w -ffunction-sections -fdata-sections -nostdlib -fno-threadsafe-statics --param max-inline-insns-single\=500 -fno-rtti -fno-exceptions -Dprintf\=iprintf
+environment/project/it.baeyens.arduino.core.toolChain.release.674980254/A.COMPILER.CPP.FLAGS/value=-c -g -O2 -save-temps -w -ffunction-sections -fdata-sections -nostdlib -fno-threadsafe-statics --param max-inline-insns-single\=500 -fno-rtti -fno-exceptions -Dprintf\=iprintf
 environment/project/it.baeyens.arduino.core.toolChain.release.674980254/A.COMPILER.DEFINE/delimiter=;
 environment/project/it.baeyens.arduino.core.toolChain.release.674980254/A.COMPILER.DEFINE/operation=replace
 environment/project/it.baeyens.arduino.core.toolChain.release.674980254/A.COMPILER.DEFINE/value=-DARDUINO\=
diff --git a/Configuration.h b/Configuration.h
index 2eb95f84..86b61b8d 100644
--- a/Configuration.h
+++ b/Configuration.h
@@ -24,8 +24,8 @@ Licence: GPL
 #define CONFIGURATION_H
 
 #define NAME "RepRapFirmware"
-#define VERSION "1.04c-dc42"
-#define DATE "2015-04-02"
+#define VERSION "1.04d-dc42"
+#define DATE "2015-04-05"
 #define AUTHORS "reprappro, dc42, zpl"
 
 #define FLASH_SAVE_ENABLED	(1)
diff --git a/DDA.cpp b/DDA.cpp
index a8848418..4a50b20f 100644
--- a/DDA.cpp
+++ b/DDA.cpp
@@ -617,7 +617,10 @@ void DDA::Prepare()
 			dm.nextStep = 0;
 			dm.nextStepTime = 0;
 			dm.stepError = false;							// clear any previous step error before we call CalcNextStep
-			uint32_t st = (isDeltaMovement && drive < AXES) ? dm.CalcNextStepTimeDelta(*this, drive) : dm.CalcNextStepTimeCartesian(drive);
+			dm.stepsTillRecalc = 1;
+			uint32_t st = (isDeltaMovement && drive < AXES)
+							? dm.CalcNextStepTimeDelta(*this, drive)
+							: dm.CalcNextStepTimeCartesian(drive);
 			if (st < firstStepTime)
 			{
 				firstStepTime = st;
diff --git a/DDA.h b/DDA.h
index 6228177c..2d217285 100644
--- a/DDA.h
+++ b/DDA.h
@@ -61,7 +61,14 @@ public:
 
 	static const uint32_t stepClockRate = VARIANT_MCK/32;			// the frequency of the clock used for stepper pulse timing (using TIMER_CLOCK3), about 0.38us resolution
 	static const uint64_t stepClockRateSquared = (uint64_t)stepClockRate * stepClockRate;
-	static const int32_t MinStepTime = (10000000/stepClockRate);	// the smallest sensible interval between steps (10us)
+	static const int32_t MinStepInterval = (4 * stepClockRate)/1000000; // the smallest sensible interval between steps (10us) in step timer clocks
+
+	// Note on the following constant:
+	// If we calculate the step interval on every clock, we reach a point where the calculation time exceeds the step interval.
+	// The worst case is pure Z movement on a delta. On a Mini Kossel with 80 steps/mm witt this formware runnig on a Duet (84MHx SAM3X8 processor),
+	// the calculation can just be managed in time at speeds of 15000mm/min (step interval 50us), but not at 20000mm/min (step interval 37.5us).
+	// Therefore, where the step interval falls below 70us, we don't calculate on every step.
+	static const uint32_t MinCalcInterval = (70 * stepClockRate)/1000000; // the smallest sensible interval between calculations (70us) in step timer clocks
 
 private:
 	static const uint32_t minInterruptInterval = 6;					// about 2us minimum interval between interrupts, in clocks
diff --git a/DriveMovement.cpp b/DriveMovement.cpp
index 63c7fe50..04fc2bf7 100644
--- a/DriveMovement.cpp
+++ b/DriveMovement.cpp
@@ -155,9 +155,9 @@ void DriveMovement::DebugPrint(char c, bool isDeltaMovement) const
 {
 	if (moving || stepError)
 	{
-		debugPrintf("DM%c%s dir=%c steps=%u next=%u sstcda=%u "
+		debugPrintf("DM%c%s dir=%c steps=%u next=%u interval=%u sstcda=%u "
 					"acmadtcdts=%d tstcdapdsc=%u tstdca2=%" PRIu64 "\n",
-					c, (stepError) ? " ERR:" : ":", (direction) ? 'F' : 'B', totalSteps, nextStep, startSpeedTimesCdivA,
+					c, (stepError) ? " ERR:" : ":", (direction) ? 'F' : 'B', totalSteps, nextStep, stepInterval, startSpeedTimesCdivA,
 					accelClocksMinusAccelDistanceTimesCdivTopSpeed, topSpeedTimesCdivAPlusDecelStartClocks, twoDistanceToStopTimesCsquaredDivA);
 
 		if (isDeltaMovement)
@@ -196,39 +196,64 @@ uint32_t DriveMovement::CalcNextStepTimeCartesian(size_t drive)
 		return NoStepTime;
 	}
 
-	uint32_t lastStepTime = nextStepTime;			// pick up the time of the last step
 	++nextStep;
-	if (nextStep < mp.cart.accelStopStep)
+	if (stepsTillRecalc > 1)
 	{
-		nextStepTime = isqrt64(isquare64(startSpeedTimesCdivA) + (mp.cart.twoCsquaredTimesMmPerStepDivA * nextStep)) - startSpeedTimesCdivA;
-	}
-	else if (nextStep < mp.cart.decelStartStep)
-	{
-		nextStepTime = (uint32_t)((int32_t)(((uint64_t)mp.cart.mmPerStepTimesCdivtopSpeed * nextStep)/K1) + accelClocksMinusAccelDistanceTimesCdivTopSpeed);
-	}
-	else if (nextStep < mp.cart.reverseStartStep)
-	{
-		uint64_t temp = mp.cart.twoCsquaredTimesMmPerStepDivA * nextStep;
-		// Allow for possible rounding error when the end speed is zero or very small
-		nextStepTime = (twoDistanceToStopTimesCsquaredDivA > temp)
-						? topSpeedTimesCdivAPlusDecelStartClocks - isqrt64(twoDistanceToStopTimesCsquaredDivA - temp)
-						: topSpeedTimesCdivAPlusDecelStartClocks;
+		--stepsTillRecalc;
+		nextStepTime += stepInterval;
 	}
 	else
 	{
-		if (nextStep == mp.cart.reverseStartStep)
+		uint32_t lastStepTime = nextStepTime;			// pick up the time of the last step
+		if (nextStep < mp.cart.accelStopStep)
+		{
+			nextStepTime = isqrt64(isquare64(startSpeedTimesCdivA) + (mp.cart.twoCsquaredTimesMmPerStepDivA * nextStep)) - startSpeedTimesCdivA;
+		}
+		else if (nextStep < mp.cart.decelStartStep)
+		{
+			nextStepTime = (uint32_t)((int32_t)(((uint64_t)mp.cart.mmPerStepTimesCdivtopSpeed * nextStep)/K1) + accelClocksMinusAccelDistanceTimesCdivTopSpeed);
+		}
+		else if (nextStep < mp.cart.reverseStartStep)
 		{
-			reprap.GetPlatform()->SetDirection(drive, !direction);
+			uint64_t temp = mp.cart.twoCsquaredTimesMmPerStepDivA * nextStep;
+			// Allow for possible rounding error when the end speed is zero or very small
+			nextStepTime = (twoDistanceToStopTimesCsquaredDivA > temp)
+							? topSpeedTimesCdivAPlusDecelStartClocks - isqrt64(twoDistanceToStopTimesCsquaredDivA - temp)
+							: topSpeedTimesCdivAPlusDecelStartClocks;
 		}
-		nextStepTime = topSpeedTimesCdivAPlusDecelStartClocks
-							+ isqrt64((int64_t)(mp.cart.twoCsquaredTimesMmPerStepDivA * nextStep) - mp.cart.fourMaxStepDistanceMinusTwoDistanceToStopTimesCsquaredDivA);
+		else
+		{
+			if (nextStep == mp.cart.reverseStartStep)
+			{
+				reprap.GetPlatform()->SetDirection(drive, !direction);
+			}
+			nextStepTime = topSpeedTimesCdivAPlusDecelStartClocks
+								+ isqrt64((int64_t)(mp.cart.twoCsquaredTimesMmPerStepDivA * nextStep) - mp.cart.fourMaxStepDistanceMinusTwoDistanceToStopTimesCsquaredDivA);
 
-	}
+		}
 
-	if ((int32_t)nextStepTime < (int32_t)(lastStepTime + DDA::MinStepTime) && nextStep > 1)
-	{
-		stepError = true;
-		return NoStepTime;
+		if (stepsTillRecalc == 1)
+		{
+			--stepsTillRecalc;			// we can't trust the interval
+		}
+		else
+		{
+			// Check for steps that are too fast, this normally indicates a problem with the calculation
+			int32_t interval = (int32_t)nextStepTime - (int32_t)lastStepTime;
+			if (interval < DDA::MinStepInterval)
+			{
+				stepInterval = (uint32_t)interval;
+				stepError = true;
+				return NoStepTime;
+			}
+
+			// If the step interval is very short, flag not to recalculate it next time
+			if (interval < DDA::MinCalcInterval)
+			{
+				stepInterval = (uint32_t)interval;
+				stepsTillRecalc = DDA::MinCalcInterval/stepInterval + 1;
+			}
+		}
 	}
 	return nextStepTime;
 }
@@ -242,58 +267,92 @@ uint32_t DriveMovement::CalcNextStepTimeDelta(const DDA &dda, size_t drive)
 		return NoStepTime;
 	}
 
-	uint32_t lastStepTime = nextStepTime;			// pick up the time of the last step
 	++nextStep;
-	if (nextStep == mp.delta.reverseStartStep)
+	if (stepsTillRecalc > 1 && nextStep != mp.delta.reverseStartStep)
 	{
-		direction = false;
-		reprap.GetPlatform()->SetDirection(drive, false);		// going down now
-	}
+		--stepsTillRecalc;
+		nextStepTime += stepInterval;
 
-	// Calculate d*s*K as an integer, where d = distance the head has travelled, s = steps/mm for this drive, K = a power of 2 to reduce the rounding errors
-	if (direction)
-	{
-		mp.delta.hmz0sK += (int32_t)K2;
+		// We can avoid most of the calculation, but we still need to update mp.delta.hmz0sk
+		if (direction)
+		{
+			mp.delta.hmz0sK += (int32_t)K2;
+		}
+		else
+		{
+			mp.delta.hmz0sK -= (int32_t)K2;
+		}
 	}
 	else
 	{
-		mp.delta.hmz0sK -= (int32_t)K2;
-	}
+		uint32_t lastStepTime = nextStepTime;			// pick up the time of the last step
+		if (nextStep == mp.delta.reverseStartStep)
+		{
+			direction = false;
+			reprap.GetPlatform()->SetDirection(drive, false);		// going down now
+		}
 
-	const int32_t hmz0scK = (int32_t)(((int64_t)mp.delta.hmz0sK * dda.cKc)/Kc);
-	const int32_t t1 = mp.delta.minusAaPlusBbTimesKs + hmz0scK;
-	const int32_t t2 = isqrt64(isquare64(t1) + mp.delta.dSquaredMinusAsquaredMinusBsquaredTimesKsquaredSsquared - isquare64(mp.delta.hmz0sK));
-	const int32_t dsK = (direction) ? t1 - t2 : t1 + t2;
+		// Calculate d*s*K as an integer, where d = distance the head has travelled, s = steps/mm for this drive, K = a power of 2 to reduce the rounding errors
+		if (direction)
+		{
+			mp.delta.hmz0sK += (int32_t)K2;
+		}
+		else
+		{
+			mp.delta.hmz0sK -= (int32_t)K2;
+		}
 
-	// Now feed dsK into a modified version of the step algorithm for Cartesian motion without elasticity compensation
-	if (dsK < 0)
-	{
-		stepError = true;
-		nextStep += 1000000;		// so that we can tell what happened in the debug print
-		return NoStepTime;
-	}
-	if ((uint32_t)dsK < mp.delta.accelStopDsK)
-	{
-		nextStepTime = isqrt64(isquare64(startSpeedTimesCdivA) + ((uint64_t)mp.delta.twoCsquaredTimesMmPerStepDivAK * (uint32_t)dsK)) - startSpeedTimesCdivA;
-	}
-	else if ((uint32_t)dsK < mp.delta.decelStartDsK)
-	{
-		nextStepTime = (uint32_t)((int32_t)(((uint64_t)mp.delta.mmPerStepTimesCdivtopSpeedK * (uint32_t)dsK)/(K1 * K2)) + accelClocksMinusAccelDistanceTimesCdivTopSpeed);
-	}
-	else
-	{
-		uint64_t temp = (uint64_t)mp.delta.twoCsquaredTimesMmPerStepDivAK * (uint32_t)dsK;
-		// Because of possible rounding error when the end speed is zero or very small, we need to check that the square root will work OK
-		nextStepTime = (temp < twoDistanceToStopTimesCsquaredDivA)
-						? topSpeedTimesCdivAPlusDecelStartClocks - isqrt64(twoDistanceToStopTimesCsquaredDivA - temp)
-						: topSpeedTimesCdivAPlusDecelStartClocks;
-	}
+		const int32_t hmz0scK = (int32_t)(((int64_t)mp.delta.hmz0sK * dda.cKc)/Kc);
+		const int32_t t1 = mp.delta.minusAaPlusBbTimesKs + hmz0scK;
+		const int32_t t2 = isqrt64(isquare64(t1) + mp.delta.dSquaredMinusAsquaredMinusBsquaredTimesKsquaredSsquared - isquare64(mp.delta.hmz0sK));
+		const int32_t dsK = (direction) ? t1 - t2 : t1 + t2;
 
-	if ((int32_t)nextStepTime < (int32_t)(lastStepTime + DDA::MinStepTime) && nextStep > 1)
-	{
-		stepError = true;
-//		debugPrintf("%u %u %u %d %d %d %d\n", nextStep, nextStepTime, lastStepTime, dsK, t1, t2, mp.delta.hmz0sK);
-		return NoStepTime;
+		// Now feed dsK into a modified version of the step algorithm for Cartesian motion without elasticity compensation
+		if (dsK < 0)
+		{
+			stepError = true;
+			nextStep += 1000000;		// so that we can tell what happened in the debug print
+			return NoStepTime;
+		}
+		if ((uint32_t)dsK < mp.delta.accelStopDsK)
+		{
+			nextStepTime = isqrt64(isquare64(startSpeedTimesCdivA) + ((uint64_t)mp.delta.twoCsquaredTimesMmPerStepDivAK * (uint32_t)dsK)) - startSpeedTimesCdivA;
+		}
+		else if ((uint32_t)dsK < mp.delta.decelStartDsK)
+		{
+			nextStepTime = (uint32_t)((int32_t)(((uint64_t)mp.delta.mmPerStepTimesCdivtopSpeedK * (uint32_t)dsK)/(K1 * K2)) + accelClocksMinusAccelDistanceTimesCdivTopSpeed);
+		}
+		else
+		{
+			uint64_t temp = (uint64_t)mp.delta.twoCsquaredTimesMmPerStepDivAK * (uint32_t)dsK;
+			// Because of possible rounding error when the end speed is zero or very small, we need to check that the square root will work OK
+			nextStepTime = (temp < twoDistanceToStopTimesCsquaredDivA)
+							? topSpeedTimesCdivAPlusDecelStartClocks - isqrt64(twoDistanceToStopTimesCsquaredDivA - temp)
+							: topSpeedTimesCdivAPlusDecelStartClocks;
+		}
+
+		if (stepsTillRecalc == 1)
+		{
+			--stepsTillRecalc;			// we can't trust the interval
+		}
+		else
+		{
+			// Check for steps that are too fast, this normally indicates a problem with the calculation
+			int32_t interval = (int32_t)nextStepTime - (int32_t)lastStepTime;
+			if (interval < DDA::MinStepInterval)
+			{
+				stepError = true;
+				stepInterval = (uint32_t)interval;
+				return NoStepTime;
+			}
+
+			// If the step interval is very short, flag not to recalculate it next time
+			if (interval < DDA::MinCalcInterval)
+			{
+				stepInterval = (uint32_t)interval;
+				stepsTillRecalc = DDA::MinCalcInterval/stepInterval + 1;
+			}
+		}
 	}
 	return nextStepTime;
 }
diff --git a/DriveMovement.h b/DriveMovement.h
index 03706407..e3d8c1a1 100644
--- a/DriveMovement.h
+++ b/DriveMovement.h
@@ -35,17 +35,24 @@ public:
 	void DebugPrint(char c, bool withDelta) const;
 
 	// Parameters common to Cartesian, delta and extruder moves
-	// These values don't depend on how the move is executed, so  are set by Init()
-	uint32_t totalSteps;								// total number of steps for this move
-	bool moving;										// true if this drive moves in this move, if false then all other values are don't cares
-	bool direction;										// true=forwards, false=backwards
-	bool stepError;										// for debugging
 
 	// The following only need to be stored per-drive if we are supporting elasticity compensation
+	uint64_t twoDistanceToStopTimesCsquaredDivA;
 	uint32_t startSpeedTimesCdivA;
 	int32_t accelClocksMinusAccelDistanceTimesCdivTopSpeed;		// this one can be negative
 	uint32_t topSpeedTimesCdivAPlusDecelStartClocks;
-	uint64_t twoDistanceToStopTimesCsquaredDivA;
+
+	// These values don't depend on how the move is executed, so are set by Init()
+	uint32_t totalSteps;								// total number of steps for this move
+	bool moving;										// true if this drive moves in this move, if false then all other values are don't cares
+	bool direction;										// true=forwards, false=backwards
+	bool stepError;										// for debugging
+	uint8_t stepsTillRecalc;							// how soon we need to recalculate
+
+	// These values change as the step is executed
+	uint32_t nextStep;									// number of steps already done
+	uint32_t nextStepTime;								// how many clocks after the start of this move the next step is due
+	uint32_t stepInterval;								// how many clocks between steps
 
 	// Parameters unique to a style of move (Cartesian, delta or extruder). Currently, extruders and Cartesian moves use the same parameters.
 	union MoveParams
@@ -68,10 +75,10 @@ public:
 		struct DeltaParameters							// Parameters for delta movement
 		{
 			// The following don't depend on how the move is executed, so they can be set up in Init
+			int64_t dSquaredMinusAsquaredMinusBsquaredTimesKsquaredSsquared;
 			uint32_t reverseStartStep;
 			int32_t hmz0sK;								// the starting step position less the starting Z height, multiplied by the Z movement fraction and K (can go negative)
 			int32_t minusAaPlusBbTimesKs;
-			int64_t dSquaredMinusAsquaredMinusBsquaredTimesKsquaredSsquared;
 			uint32_t twoCsquaredTimesMmPerStepDivAK;	// this could be stored in the DDA if all towers use the same steps/mm
 
 			// The following depend on how the move is executed, so they must be set up in Prepare()
@@ -81,10 +88,6 @@ public:
 		} delta;
 	} mp;
 
-	// These values change as the step is executed
-	uint32_t nextStep;									// number of steps already done
-	uint32_t nextStepTime;								// how many clocks after the start of this move the next step is due
-
 	static const uint32_t NoStepTime = 0xFFFFFFFF;		// value to indicate that no further steps are needed when calculating the next step time
 	static const uint32_t K1 = 1024;					// a power of 2 used to multiply the value mmPerStepTimesCdivtopSpeed to reduce rounding errors
 	static const uint32_t K2 = 512;						// a power of 2 used in delta calculations to reduce rounding errors (but too large makes things worse)
diff --git a/GCodes.cpp b/GCodes.cpp
index 63ed654a..d1bb46e5 100644
--- a/GCodes.cpp
+++ b/GCodes.cpp
@@ -1020,7 +1020,7 @@ bool GCodes::DoSingleZProbeAtPoint(int probePointIndex)
 		if (DoCannedCycleMove(0))
 		{
 			cannedCycleMoveCount = 0;
-			reprap.GetMove()->SetZBedProbePoint(probePointIndex, lastProbedZ);
+			reprap.GetMove()->SetZBedProbePoint(probePointIndex, lastProbedZ, true);
 			return true;
 		}
 		return false;
@@ -1080,10 +1080,8 @@ bool GCodes::SetSingleZProbeAtAPosition(GCodeBuffer *gb, StringRef& reply)
 		return true;
 	}
 
-	const ZProbeParameters& rp = platform->GetZProbeParameters();
-
-	float x = (gb->Seen(axisLetters[X_AXIS])) ? gb->GetFValue() : moveBuffer[X_AXIS] - rp.xOffset;
-	float y = (gb->Seen(axisLetters[Y_AXIS])) ? gb->GetFValue() : moveBuffer[Y_AXIS] - rp.yOffset;
+	float x = (gb->Seen(axisLetters[X_AXIS])) ? gb->GetFValue() : moveBuffer[X_AXIS];
+	float y = (gb->Seen(axisLetters[Y_AXIS])) ? gb->GetFValue() : moveBuffer[Y_AXIS];
 	float z = (gb->Seen(axisLetters[Z_AXIS])) ? gb->GetFValue() : moveBuffer[Z_AXIS];
 
 	reprap.GetMove()->SetXBedProbePoint(probePointIndex, x);
@@ -1091,7 +1089,7 @@ bool GCodes::SetSingleZProbeAtAPosition(GCodeBuffer *gb, StringRef& reply)
 
 	if (z > SILLY_Z_VALUE)
 	{
-		reprap.GetMove()->SetZBedProbePoint(probePointIndex, z);
+		reprap.GetMove()->SetZBedProbePoint(probePointIndex, z, false);
 		if (gb->Seen('S'))
 		{
 			zProbesSet = true;
@@ -2301,8 +2299,7 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 				{
 					if (encapsulate_list)
 					{
-						reply.catf("%c%s%c%c", FILE_LIST_BRACKET, file_info.fileName, FILE_LIST_BRACKET,
-								FILE_LIST_SEPARATOR);
+						reply.catf("%c%s%c%c", FILE_LIST_BRACKET, file_info.fileName, FILE_LIST_BRACKET, FILE_LIST_SEPARATOR);
 					}
 					else
 					{
@@ -2310,8 +2307,11 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 					}
 				} while (platform->GetMassStorage()->FindNext(file_info));
 
-				// remove the last separator
-				reply[reply.strlen() - 1] = 0;
+				if (encapsulate_list)
+				{
+					// remove the last separator and replace by newline
+					reply[reply.strlen() - 1] = '\n';
+				}
 			}
 			else
 			{
diff --git a/Heat.cpp b/Heat.cpp
index 646a1d68..8a8ac6e5 100644
--- a/Heat.cpp
+++ b/Heat.cpp
@@ -203,18 +203,11 @@ void PID::Spin()
   float error = targetTemperature - temperature;
   const PidParameters& pp = platform->GetPidParameters(heater);
   
-  if(!pp.UsePID())
+  if (!pp.UsePID())
   {
-	if(error > 0.0)
-	{
-		platform->SetHeater(heater, pp.kS);
-		averagePWM = averagePWM * (1.0 - invHeatPwmAverageCount) + pp.kS;
-	}
-	else
-	{
-		platform->SetHeater(heater, 0.0);
-		averagePWM *= (1.0 - invHeatPwmAverageCount);
-	}
+	float heaterValue = (error > 0.0) ? min<float>(pp.kS, 1.0) : 0.0;
+	platform->SetHeater(heater, heaterValue);
+	averagePWM = averagePWM * (1.0 - invHeatPwmAverageCount) + heaterValue;
 	return;
   }
   
@@ -231,8 +224,9 @@ void PID::Spin()
   {
 	  // actual temperature is well below target
 	  temp_iState = (targetTemperature - pp.fullBand - 25.0) * pp.kT;	// set the I term to our estimate of what will be needed ready for the switch to PID
-	  platform->SetHeater(heater, pp.kS);
-	  averagePWM = averagePWM * (1.0 - invHeatPwmAverageCount) + pp.kS;
+	  float heaterValue = min<float>(pp.kS, 1.0);
+	  platform->SetHeater(heater, heaterValue);
+	  averagePWM = averagePWM * (1.0 - invHeatPwmAverageCount) + heaterValue;
 	  lastTemperature = temperature;
 	  return;
   }  
@@ -250,26 +244,22 @@ void PID::Spin()
   }
    
   float temp_dState = pp.kD * (temperature - lastTemperature) / sampleInterval;
-  float result = pp.kP * error + temp_iState - temp_dState;
+  float result = (pp.kP * error + temp_iState - temp_dState) * pp.kS / 255.0;
 
   lastTemperature = temperature;
 
-  // Legacy - old RepRap PID parameters were set to give values in [0, 255] for 1 byte PWM control
-  // TODO - maybe change them to give [0.0, 1.0]?
-
   if (result < 0.0)
   {
     result = 0.0;
   }
-  else if (result > 255.0)
+  else if (result > 1.0)
   {
-    result = 255.0;
+    result = 1.0;
   }
-  result = result/255.0;
 
-  if(!temperatureFault)
+  if (!temperatureFault)
   {
-	  platform->SetHeater(heater, result * pp.kS);
+	  platform->SetHeater(heater, result);
   }
 
   averagePWM = averagePWM * (1.0 - invHeatPwmAverageCount) + result;
diff --git a/Isqrt.cpp b/Isqrt.cpp
index ad699798..f73932d7 100644
--- a/Isqrt.cpp
+++ b/Isqrt.cpp
@@ -8,16 +8,39 @@
 uint32_t isqrt64(uint64_t num)
 {
 	uint32_t numHigh = (uint32_t)(num >> 32);
-	uint32_t res;
+	if (numHigh == 0)
+	{
+		// 32-bit square root - thanks to Wilco Dijksra for this efficient ARM algorithm
+		uint32_t num32 = (uint32_t)num;
+		uint32_t res = 0;
 
-	if ((numHigh & (3 << 30)) != 0)
+		#define iter32(N)						\
+		{										\
+			uint32_t temp = res | (1 << N);		\
+			if (num32 >= temp << N)				\
+			{									\
+				num32 -= temp << N;				\
+				res |= 2 << N;					\
+			}									\
+		}
+
+		// We need to do 16 iterations
+		iter32(15); iter32(14); iter32(13); iter32(12);
+		iter32(11); iter32(10); iter32(9); iter32(8);
+		iter32(7); iter32(6); iter32(5); iter32(4);
+		iter32(3); iter32(2); iter32(1); iter32(0);
+
+		return res >> 1;
+	}
+	else if ((numHigh & (3u << 30)) != 0)
 	{
 		// Input out of range - probably negative, so return -1
-		res = 0xFFFFFFFF;
+		return 0xFFFFFFFF;
 	}
 	else
 	{
-		res = 0;
+		// 62-bit square root
+		uint32_t res = 0;
 
 #define iter64a(N) 								\
 		{										\
@@ -59,14 +82,12 @@ uint32_t isqrt64(uint64_t num)
 		iter64b(14) iter64b(12) iter64b(10) iter64b(8)
 		iter64b(6)  iter64b(4)  iter64b(2)  iter64b(0)
 
-		res >>= 1;
+		return res >> 1;
 
 #undef iter64a
 #undef iter64b
 
 	}
-
-	return res;
 }
 
 #if 0
diff --git a/Flash/DueFlashStorage.cpp b/Libraries/Flash/DueFlashStorage.cpp
index 6e671c1b..6e671c1b 100644
--- a/Flash/DueFlashStorage.cpp
+++ b/Libraries/Flash/DueFlashStorage.cpp
diff --git a/Flash/DueFlashStorage.h b/Libraries/Flash/DueFlashStorage.h
index 5a6b12e6..5a6b12e6 100644
--- a/Flash/DueFlashStorage.h
+++ b/Libraries/Flash/DueFlashStorage.h
diff --git a/Flash/efc.cpp b/Libraries/Flash/efc.cpp
index 74f0c8a1..74f0c8a1 100644
--- a/Flash/efc.cpp
+++ b/Libraries/Flash/efc.cpp
diff --git a/Flash/efc.h b/Libraries/Flash/efc.h
index 4481b985..4481b985 100644
--- a/Flash/efc.h
+++ b/Libraries/Flash/efc.h
diff --git a/Flash/flash_efc.cpp b/Libraries/Flash/flash_efc.cpp
index c7b3aa86..c7b3aa86 100644
--- a/Flash/flash_efc.cpp
+++ b/Libraries/Flash/flash_efc.cpp
diff --git a/Flash/flash_efc.h b/Libraries/Flash/flash_efc.h
index bd520334..bd520334 100644
--- a/Flash/flash_efc.h
+++ b/Libraries/Flash/flash_efc.h
diff --git a/Matrix.h b/Matrix.h
index d21e5e45..d88783a1 100644
--- a/Matrix.h
+++ b/Matrix.h
@@ -27,19 +27,22 @@ template<class T, size_t ROWS, size_t COLS> class FixedMatrix : public MathMatri
 public:
 	size_t rows() const override { return ROWS; }
 	size_t cols() const override { return COLS; }
+
+	// Indexing operator, non-const version
 	T& operator() (size_t r, size_t c) override
 	//pre(r < ROWS; c < COLS)
 	{
 		return data[r * COLS + c];
 	}
 
+	// Indexing operator, const version
 	const T& operator() (size_t r, size_t c) const override
 	//pre(r < ROWS; c < COLS)
 	{
 		return data[r * COLS + c];
 	}
 
-	void SwapRows(size_t i, size_t j)
+	void SwapRows(size_t i, size_t j, size_t numCols = COLS)
 	//pre(i < ROWS; j < ROWS)
 	;
 
@@ -47,16 +50,30 @@ public:
 	//pre(numRows <= ROWS; numRows + 1 <= COLS)
 	;
 
+	// Return a pointer to a specified row, non-const version
+	T* GetRow(size_t r)
+	//pre(r < ROWS)
+	{
+		return data + (r * COLS);
+	}
+
+	// Return a pointer to a specified row, const version
+	const T* GetRow(size_t r) const
+	//pre(r < ROWS)
+	{
+		return data + (r * COLS);
+	}
+
 private:
 	T data[ROWS * COLS];
 };
 
 // Swap 2 rows of a matrix
-template<class T, size_t ROWS, size_t COLS> inline void FixedMatrix<T, ROWS, COLS>::SwapRows(size_t i, size_t j)
+template<class T, size_t ROWS, size_t COLS> inline void FixedMatrix<T, ROWS, COLS>::SwapRows(size_t i, size_t j, size_t numCols)
 {
 	if (i != j)
 	{
-		for (size_t k = i; k < COLS; ++k)
+		for (size_t k = i; k < numCols; ++k)
 		{
 			T temp = (*this)(i, k);
 			(*this)(i, k) = (*this)(j, k);
diff --git a/Move.cpp b/Move.cpp
index 93105dd0..220d2b3b 100644
--- a/Move.cpp
+++ b/Move.cpp
@@ -124,6 +124,7 @@ void DeltaParameters::InverseTransform(float Ha, float Hb, float Hc, float machi
 // 3, 4 = X, Y tower X position
 // 5 = Z tower Y position
 // 6 = diagonal rod length
+// 7 = delta radius (only if isEquilateral is true)
 float DeltaParameters::ComputeDerivative(unsigned int deriv, float ha, float hb, float hc)
 {
 	const float perturb = 0.2;			// perturbation amount in mm
@@ -157,6 +158,11 @@ float DeltaParameters::ComputeDerivative(unsigned int deriv, float ha, float hb,
 		hiParams.diagonal += perturb;
 		loParams.diagonal -= perturb;
 		break;
+
+	case 7:
+		hiParams.SetRadius(radius + perturb);
+		loParams.SetRadius(radius - perturb);
+		break;
 	}
 
 	hiParams.Recalc();
@@ -171,46 +177,48 @@ float DeltaParameters::ComputeDerivative(unsigned int deriv, float ha, float hb,
 	return (zHi - zLo)/(2 * perturb);
 }
 
-// Perform 4-factor adjustment.
-// The input vector contains the following parameters in this order:
-// X, Y and Z endstop adjustments
-// Delta radius
-void DeltaParameters::AdjustFour(const float v[4])
-{
-	endstopAdjustments[A_AXIS] += v[0];
-	endstopAdjustments[B_AXIS] += v[1];
-	endstopAdjustments[C_AXIS] += v[2];
-	NormaliseEndstopAdjustments();
-	SetRadius(radius + v[3]);		// this sets isEquilateral true, recalculates tower positions, then calls Recalc()
-}
-
-// Perform 7-factor adjustment.
+// Perform 3, 4, 6 or 7-factor adjustment.
 // The input vector contains the following parameters in this order:
 //  X, Y and Z endstop adjustments
+//  If we are doing 4-factor adjustment, the next argument is the delta radius. Otherwise:
 //  X tower X position adjustment
 //  Y tower X position adjustment
 //  Z tower Y position adjustment
 //  Diagonal rod length adjustment
-void DeltaParameters::AdjustSeven(const float v[7])
+void DeltaParameters::Adjust(size_t numFactors, const float v[])
 {
 	const float oldCarriageHeightA = GetHomedCarriageHeight(A_AXIS);	// save for later
 
+	// Update endstop adjustments
 	endstopAdjustments[A_AXIS] += v[0];
 	endstopAdjustments[B_AXIS] += v[1];
 	endstopAdjustments[C_AXIS] += v[2];
 	NormaliseEndstopAdjustments();
 
-	towerX[A_AXIS] += v[3];
-	towerX[B_AXIS] += v[4];
+	if (numFactors == 4)
+	{
+		// 4-factor adjustment, so update delta radius
+		SetRadius(radius + v[3]);		// this sets isEquilateral true, recalculates tower positions, then calls Recalc()
+	}
+	else if (numFactors > 3)
+	{
+		// 6- or 7-factor adjustment
+		towerX[A_AXIS] += v[3];
+		towerX[B_AXIS] += v[4];
 
-	const float yAdj = v[5] * (1.0/3.0);
-	towerY[A_AXIS] -= yAdj;
-	towerY[B_AXIS] -= yAdj;
-	towerY[C_AXIS] += (v[5] - yAdj);
-	diagonal += v[6];
-	isEquilateral = false;
+		const float yAdj = v[5] * (1.0/3.0);
+		towerY[A_AXIS] -= yAdj;
+		towerY[B_AXIS] -= yAdj;
+		towerY[C_AXIS] += (v[5] - yAdj);
+		isEquilateral = false;
 
-	Recalc();
+		if (numFactors == 7)
+		{
+			diagonal += v[6];
+		}
+
+		Recalc();
+	}
 
 	// Adjusting the diagonal and the tower positions affects the homed carriage height.
 	// We need to adjust homedHeight to allow for this, to get the change that was requested in the endstop corrections.
@@ -374,7 +382,7 @@ void Move::Spin()
 				// segment it so that we can apply proper bed compensation
 				// Issues here:
 				// 1. Are there enough DDAs? need to make nextMove static and remember whether we have the remains of a move in there.
-				// 2. Restarting: if we restart a segmented move when we have already executed part of it, we will extrude too much.
+				// 2. Pause/restart: if we restart a segmented move when we have already executed part of it, we will extrude too much.
 				// Perhaps remember how much of the last move we executed? Or always insist on completing all the segments in a move?
 				bool isSegmented;
 				do
@@ -646,11 +654,14 @@ void Move::MachineToEndPoint(const int32_t motorPos[], float machinePos[], size_
 	{
 		deltaParams.InverseTransform(motorPos[A_AXIS]/stepsPerUnit[A_AXIS], motorPos[B_AXIS]/stepsPerUnit[B_AXIS], motorPos[C_AXIS]/stepsPerUnit[C_AXIS], machinePos);
 
+#if 0
 		// We don't do inverse transforms very often, so if debugging is enabled, print them
 		if (reprap.Debug(moduleMove))
 		{
 			debugPrintf("Inverse transformed %d %d %d to %f %f %f\n", motorPos[0], motorPos[1], motorPos[2], machinePos[0], machinePos[1], machinePos[2]);
 		}
+#endif
+
 	}
 	else
 	{
@@ -908,27 +919,31 @@ void Move::FinishedBedProbing(int sParam, StringRef& reply)
 	{
 		// A negative sParam just prints the probe heights
 		reply.copy("Bed probe heights:");
+		float sumOfSquares = 0.0;
 		for (size_t i = 0; i < NumberOfProbePoints(); ++i)
 		{
-			reply.catf(" %.2f", zBedProbePoints[i]);
+			reply.catf(" %.3f", zBedProbePoints[i]);
+			sumOfSquares += fsquare(zBedProbePoints[i]);
 		}
-		reply.cat("\n");
+		reply.catf(", RMS error: %.3f\n", sqrt(sumOfSquares/NumberOfProbePoints()));
 	}
 	else if (NumberOfProbePoints() < sParam)
 	{
 		reprap.GetPlatform()->Message(BOTH_ERROR_MESSAGE,
-				"Bed calibration error: %d points requested but only %s provided\n", sParam, NumberOfProbePoints());
+				"Bed calibration error: %d factor calibration requested but only %d points provided\n", sParam, NumberOfProbePoints());
 	}
 	else
 	{
 		if (reprap.Debug(moduleMove))
 		{
 			debugPrintf("Z probe offsets:");
+			float sumOfSquares = 0.0;
 			for (size_t i = 0; i < NumberOfProbePoints(); ++i)
 			{
-				debugPrintf(" %.2f", zBedProbePoints[i]);
+				debugPrintf(" %.3f", zBedProbePoints[i]);
+				sumOfSquares += fsquare(zBedProbePoints[i]);
 			}
-			debugPrintf("\n");
+			debugPrintf(", RMS error: %.3f\n", sqrt(sumOfSquares/NumberOfProbePoints()));
 		}
 
 		if (sParam == 0)
@@ -944,6 +959,13 @@ void Move::FinishedBedProbing(int sParam, StringRef& reply)
 		{
 			SetProbedBedEquation(sParam, reply);
 		}
+
+		// Clear out the Z heights so that we don't re-use old points.
+		// This allows us to use different numbers of probe point on different occasions.
+		for (size_t i = 0; i < MaxProbePoints; ++i)
+		{
+			probePointSet[i] &= ~zSet;
+		}
 	}
 }
 
@@ -1023,7 +1045,7 @@ void Move::SetProbedBedEquation(size_t numPoints, StringRef& reply)
 }
 
 // Perform 4- or 7-factor delta adjustment
-void Move::AdjustDeltaParameters(const float v[], bool allSeven)
+void Move::AdjustDeltaParameters(const float v[], size_t numFactors)
 {
 	// Save the old home carriage heights
 	float homedCarriageHeights[AXES];
@@ -1032,15 +1054,8 @@ void Move::AdjustDeltaParameters(const float v[], bool allSeven)
 		homedCarriageHeights[drive] = deltaParams.GetHomedCarriageHeight(drive);
 	}
 
-	// Adjust the delta parameters
-	if (allSeven)
-	{
-		deltaParams.AdjustSeven(v);
-	}
-	else
-	{
-		deltaParams.AdjustFour(v);
-	}
+
+	deltaParams.Adjust(numFactors, v);	// adjust the delta parameters
 
 	// Adjust the motor endpoints to allow for the change in endstop adjustments
 	DDA *lastQueuedMove = ddaRingAddPointer->GetPrevious();
@@ -1058,88 +1073,22 @@ void Move::AdjustDeltaParameters(const float v[], bool allSeven)
 	liveCoordinatesValid = false;		// force the live XYZ position to be recalculated
 }
 
-#if 0
-// Do 4-point delta calibration. We adjust the 3 endstop corrections and the delta radius.
-void Move::FourPointDeltaCalibration(StringRef& reply)
-{
-	const float averageEdgeHeight = (zBedProbePoints[0] + zBedProbePoints[1] + zBedProbePoints[2])/3.0;
-	const float averageEndstopOffset = (deltaParams.GetEndstopAdjustment(X_AXIS) + deltaParams.GetEndstopAdjustment(Y_AXIS) + deltaParams.GetEndstopAdjustment(Z_AXIS))/3.0;
-	float probeRadiusSquared = 0.0;
-
-	// Adjust the endstops to account for the differences in reading, while setting the average of the new values to zero
-	for (size_t axis = 0; axis < 3; ++axis)
-	{
-		deltaParams.SetEndstopAdjustment(axis, deltaParams.GetEndstopAdjustment(axis) + averageEdgeHeight - averageEndstopOffset - zBedProbePoints[axis]);
-		probeRadiusSquared += fsquare(xBedProbePoints[axis]) + fsquare(yBedProbePoints[axis]);
-	}
-
-	// Adjust the delta radius to make the bed appear flat
-	const float edgeDistance = deltaParams.GetRadius() - sqrt(probeRadiusSquared/3.0);
-	const float factor = edgeDistance/sqrt(fsquare(deltaParams.GetDiagonal()) - fsquare(edgeDistance))
-							- deltaParams.GetRadius()/sqrt(fsquare(deltaParams.GetDiagonal()) - fsquare(deltaParams.GetRadius()));
-	const float diff = zBedProbePoints[3] - averageEdgeHeight;
-	deltaParams.SetRadius(deltaParams.GetRadius() + diff/factor);
-
-	// Adjust the homed height to account for the error at the centre and the change in average endstop correction
-	deltaParams.SetHomedHeight(deltaParams.GetHomedHeight() + averageEndstopOffset - zBedProbePoints[3]);
-	liveCoordinatesValid = false;				// we've updated the delta parameters, so we need to recalculate the position
-
-	// Print the parameters so the user can see when they have converged
-	deltaParams.PrintParameters(reply, false);
-}
-
-// Do 6-point delta calibration. We adjust the X positions of the front two towers, the Y position of the rear tower, and the three endstop corrections.
-void Move::SixPointDeltaCalibration(StringRef& reply)
+// Do delta calibration. We adjust the three endstop corrections, and either the delta radius,
+// or the X positions of the front two towers, the Y position of the rear tower, and the diagonal rod length.
+void Move::DoDeltaCalibration(size_t numFactors, StringRef& reply)
 {
-	if (reprap.Debug(moduleMove))
-	{
-		deltaParams.PrintParameters(scratchString, true);
-		debugPrintf("%s\n", scratchString.Pointer());
-	}
-
-	// Build a 6x7 matrix of derivatives with respect to xa, xb, yc, za, zb, zc and the height errors.
-	FixedMatrix<float, 6, 7> matrix;
-	for (size_t i = 0; i < 6; ++i)
-	{
-		float machinePos[3];
-		machinePos[0] = xBedProbePoints[i];
-		machinePos[1] = yBedProbePoints[i];
-		machinePos[2] = 0.0;						// the height doesn't matter
-		for (size_t j = 0; j < 6; ++j)
-		{
-			matrix(i, j) = deltaParams.ComputeDerivative(j, machinePos);
-		}
-		matrix(i, 6) = -zBedProbePoints[i];
-	}
+	const size_t NumDeltaFactors = 7;		// number of delta machine factors we can adjust
+	const size_t numPoints = NumberOfProbePoints();
 
-	if (reprap.Debug(moduleMove))
+	if (numFactors != 3 && numFactors != 4 && numFactors != 6 && numFactors != 7)
 	{
-		PrintMatrix("Raw matrix", matrix);
+		reprap.GetPlatform()->Message(BOTH_ERROR_MESSAGE, "Delta calibration error: %d factors requested but only 3, 4, 6 and 7 supported\n", numFactors);
+		return;
 	}
 
-	float solution[6];
-	matrix.GaussJordan(solution);
-
-	if (reprap.Debug(moduleMove))
+	if (numFactors == 4 && !deltaParams.IsEquilateral())
 	{
-		PrintMatrix("Solved matrix", matrix);
-		PrintVector("Solution", solution, 6);
-	}
-
-	AdjustDeltaParameters(solution, true);
-	liveCoordinatesValid = false;					// we've updated the delta parameters, so we need to recalculate the position
-	deltaParams.PrintParameters(reply, true);
-}
-#endif
-
-// Do delta calibration. We adjust the three endstop corrections, and either the delta radius,
-// or the X positions of the front two towers, the Y position of the rear tower, and the diagonal rod length.
-void Move::DoDeltaCalibration(size_t numPoints, StringRef& reply)
-{
-	if (numPoints < 4 || numPoints > MaxDeltaCalibrationPoints)
-	{
-		reprap.GetPlatform()->Message(BOTH_ERROR_MESSAGE,
-				"Delta calibration error: %d probe points provided but must be between 4 and %d\n", MaxDeltaCalibrationPoints);
+		reprap.GetPlatform()->Message(BOTH_ERROR_MESSAGE, "Delta calibration error: 4 factor calibration not possible because tower positions have been adjusted\n");
 		return;
 	}
 
@@ -1149,80 +1098,138 @@ void Move::DoDeltaCalibration(size_t numPoints, StringRef& reply)
 		debugPrintf("%s\n", scratchString.Pointer());
 	}
 
-	size_t numFactors = (numPoints >= 7) ? 7 : 4;
+	// The following is for printing out the calculation time, see later
+	//uint32_t startTime = reprap.GetPlatform()->GetInterruptClocks();
 
-	// Build a Nx7 matrix of derivatives with respect to xa, xb, yc, za, zb, zc, diagonal.
-	FixedMatrix<float, MaxDeltaCalibrationPoints, 7> derivativeMatrix;
+	// Transform the probing points to motor endpoints and store them in a matrix, so that we can do multiple iterations using the same data
+	FixedMatrix<float, MaxDeltaCalibrationPoints, AXES> probeMotorPositions;
+	float corrections[MaxDeltaCalibrationPoints];
 	for (size_t i = 0; i < numPoints; ++i)
 	{
-		float machinePos[3];
-		machinePos[0] = xBedProbePoints[i];
-		machinePos[1] = yBedProbePoints[i];
-		machinePos[2] = 0.0;						// the height doesn't matter
-
-		float ha = deltaParams.Transform(machinePos, A_AXIS);
-		float hb = deltaParams.Transform(machinePos, B_AXIS);
-		float hc = deltaParams.Transform(machinePos, C_AXIS);
-		for (size_t j = 0; j < numFactors; ++j)
+		corrections[i] = 0.0;
+		float machinePos[AXES];
+		float xp = xBedProbePoints[i], yp = yBedProbePoints[i];
+		if (probePointSet[i] & xyCorrected)
 		{
-			derivativeMatrix(i, j) = deltaParams.ComputeDerivative(j, ha, hb, hc);
+			// The point was probed with the sensor at the specified XY coordinates, so subtract the sensor offset to get the head position
+			const ZProbeParameters& zparams = reprap.GetPlatform()->GetZProbeParameters();
+			xp -= zparams.xOffset;
+			yp -= zparams.yOffset;
 		}
-	}
+		machinePos[X_AXIS] = xp;
+		machinePos[Y_AXIS] = yp;
+		machinePos[Z_AXIS] = 0.0;
 
-	if (reprap.Debug(moduleMove))
-	{
-		PrintMatrix("Derivative matrix", derivativeMatrix, numPoints, numFactors);
+		probeMotorPositions(i, A_AXIS) = deltaParams.Transform(machinePos, A_AXIS);
+		probeMotorPositions(i, B_AXIS) = deltaParams.Transform(machinePos, B_AXIS);
+		probeMotorPositions(i, C_AXIS) = deltaParams.Transform(machinePos, C_AXIS);
 	}
 
-	// Now build the normal equations for least squares fitting
-	FixedMatrix<float, 7, 8> normalMatrix;
-	for (size_t i = 0; i < numFactors; ++i)
+	// Do 1 or more Newton-Raphson iterations
+	unsigned int iteration = 0;
+	for (;;)
 	{
-		for (size_t j = 0; j < numFactors; ++j)
+		// Build a Nx7 matrix of derivatives with respect to xa, xb, yc, za, zb, zc, diagonal.
+		FixedMatrix<float, MaxDeltaCalibrationPoints, NumDeltaFactors> derivativeMatrix;
+		for (size_t i = 0; i < numPoints; ++i)
+		{
+			for (size_t j = 0; j < numFactors; ++j)
+			{
+				derivativeMatrix(i, j) =
+					deltaParams.ComputeDerivative((numFactors == 4 && j == 3) ? 7 : j,
+													probeMotorPositions(i, A_AXIS), probeMotorPositions(i, B_AXIS), probeMotorPositions(i, C_AXIS));
+			}
+		}
+
+		if (reprap.Debug(moduleMove))
+		{
+			PrintMatrix("Derivative matrix", derivativeMatrix, numPoints, numFactors);
+		}
+
+		// Now build the normal equations for least squares fitting
+		FixedMatrix<float, NumDeltaFactors, NumDeltaFactors + 1> normalMatrix;
+		for (size_t i = 0; i < numFactors; ++i)
 		{
-			float temp = derivativeMatrix(0, i) * derivativeMatrix(0, j);
+			for (size_t j = 0; j < numFactors; ++j)
+			{
+				float temp = derivativeMatrix(0, i) * derivativeMatrix(0, j);
+				for (size_t k = 1; k < numPoints; ++k)
+				{
+					temp += derivativeMatrix(k, i) * derivativeMatrix(k, j);
+				}
+				normalMatrix(i, j) = temp;
+			}
+			float temp = derivativeMatrix(0, i) * -(zBedProbePoints[0] + corrections[0]);
 			for (size_t k = 1; k < numPoints; ++k)
 			{
-				temp += derivativeMatrix(k, i) * derivativeMatrix(k, j);
+				temp += derivativeMatrix(k, i) * -(zBedProbePoints[k] + corrections[k]);
 			}
-			normalMatrix(i, j) = temp;
+			normalMatrix(i, numFactors) = temp;
 		}
-		float temp = derivativeMatrix(0, i) * -zBedProbePoints[0];
-		for (size_t k = 1; k < numPoints; ++k)
+
+		if (reprap.Debug(moduleMove))
 		{
-			temp += derivativeMatrix(k, i) * -zBedProbePoints[k];
+			PrintMatrix("Normal matrix", normalMatrix, numFactors, numFactors + 1);
 		}
-		normalMatrix(i, numFactors) = temp;
-	}
 
-	if (reprap.Debug(moduleMove))
-	{
-		PrintMatrix("Normal matrix", normalMatrix, numFactors, numFactors + 1);
-	}
+		float solution[NumDeltaFactors];
+		normalMatrix.GaussJordan(solution, numFactors);
 
-	float solution[7];
-	normalMatrix.GaussJordan(solution, numFactors);
+		if (reprap.Debug(moduleMove))
+		{
+			PrintMatrix("Solved matrix", normalMatrix, numFactors, numFactors + 1);
+			PrintVector("Solution", solution, numFactors);
 
-	if (reprap.Debug(moduleMove))
-	{
-		PrintMatrix("Solved matrix", normalMatrix, numFactors, numFactors + 1);
-		PrintVector("Solution", solution, numFactors);
+			// Calculate and display the residuals
+			float residuals[MaxDeltaCalibrationPoints];
+			for (size_t i = 0; i < numPoints; ++i)
+			{
+				residuals[i] = zBedProbePoints[i];
+				for (size_t j = 0; j < numFactors; ++j)
+				{
+					residuals[i] += solution[j] * derivativeMatrix(i, j);
+				}
+			}
 
-		// Calculate and display the residuals
-		float residuals[MaxDeltaCalibrationPoints];
-		for (size_t i = 0; i < numPoints; ++i)
+			PrintVector("Residuals", residuals, numPoints);
+		}
+
+
+		AdjustDeltaParameters(solution, numFactors);
+
+		// Calculate the expected probe heights using the new parameters
 		{
-			residuals[i] = zBedProbePoints[i];
-			for (size_t j = 0; j < numFactors; ++j)
+			float expectedResiduals[MaxDeltaCalibrationPoints];
+			float sumOfSquares = 0.0;
+			for (size_t i = 0; i < numPoints; ++i)
+			{
+				for (size_t axis = 0; axis < AXES; ++axis)
+				{
+					probeMotorPositions(i, axis) += solution[axis];
+				}
+				float newPosition[AXES];
+				deltaParams.InverseTransform(probeMotorPositions(i, A_AXIS), probeMotorPositions(i, B_AXIS), probeMotorPositions(i, C_AXIS), newPosition);
+				corrections[i] = newPosition[Z_AXIS];
+				expectedResiduals[i] = zBedProbePoints[i] + newPosition[Z_AXIS];
+				sumOfSquares += fsquare(expectedResiduals[i]);
+			}
+
+			if (reprap.Debug(moduleMove))
 			{
-				residuals[i] += solution[j] * derivativeMatrix(i, j);
+				PrintVector("Expected probe error", expectedResiduals, numPoints);
+				debugPrintf("Expected RMS error %.3f\n", sqrt(sumOfSquares/numPoints));
 			}
 		}
 
-		PrintVector("Residuals", residuals, numPoints);
+		// Decide whether to do another iteration Two is slightly better than one, but three doesn't improve things.
+		// Alteratively, we could stop when the expected RMS error is only slightly worse than the RMS of the residuals.
+		++iteration;
+		if (iteration == 2) break;
 	}
 
-	AdjustDeltaParameters(solution, numFactors == 7);
+	// Print out the calculation time
+	//debugPrintf("Time taken %dms\n", (reprap.GetPlatform()->GetInterruptClocks() - startTime) * 1000 / DDA::stepClockRate);
+
 	deltaParams.PrintParameters(reply, true);
 }
 
@@ -1426,7 +1433,7 @@ void Move::SetYBedProbePoint(int index, float y)
 	probePointSet[index] |= ySet;
 }
 
-void Move::SetZBedProbePoint(int index, float z)
+void Move::SetZBedProbePoint(int index, float z, bool wasXyCorrected)
 {
 	if(index < 0 || index >= MaxProbePoints)
 	{
@@ -1435,6 +1442,14 @@ void Move::SetZBedProbePoint(int index, float z)
 	}
 	zBedProbePoints[index] = z;
 	probePointSet[index] |= zSet;
+	if (wasXyCorrected)
+	{
+		probePointSet[index] |= xyCorrected;
+	}
+	else
+	{
+		probePointSet[index] &= ~xyCorrected;
+	}
 }
 
 float Move::XBedProbePoint(int index) const
diff --git a/Move.h b/Move.h
index 3017c760..32d5d7f7 100644
--- a/Move.h
+++ b/Move.h
@@ -18,7 +18,8 @@ enum PointCoordinateSet
 	unset = 0,
 	xSet = 1,
 	ySet = 2,
-	zSet = 4
+	zSet = 4,
+	xyCorrected = 8
 };
 
 // Class to hold the parameter for a delta machine.
@@ -29,6 +30,7 @@ public:
 	DeltaParameters() { Init(); }
 
 	bool IsDeltaMode() const { return deltaMode; }
+	bool IsEquilateral() const { return isEquilateral; }
 	float GetDiagonal() const { return diagonal; }
 	float GetRadius() const { return radius; }
     float GetPrintRadius() const { return printRadius; }
@@ -50,13 +52,12 @@ public:
     void InverseTransform(float Ha, float Hb, float Hc, float machinePos[AXES]) const;	// Calculate the Cartesian position from the motor positions
 
     float ComputeDerivative(unsigned int deriv, float ha, float hb, float hc);		// Compute the derivative of height with respect to a parameter at a set of motor endpoints
-    void AdjustFour(const float v[4]);												// Perform 4-factor adjustment
-    void AdjustSeven(const float v[7]);												// Perform 7-factor adjustment
+    void Adjust(size_t numFactors, const float v[]);								// Perform 4-, 6- or 7-factor adjustment
     void PrintParameters(StringRef& reply, bool full);
 
 private:
 	void Recalc();
-	void NormaliseEndstopAdjustments();					// Make the average of the endstop adjustments zero
+	void NormaliseEndstopAdjustments();												// Make the average of the endstop adjustments zero
 
 	// Core parameters
     float diagonal;										// The diagonal rod length, all 3 are assumed to be the same length
@@ -103,7 +104,7 @@ public:
     void SetLiveCoordinates(const float coords[DRIVES]); // Force the live coordinates (see above) to be these
     void SetXBedProbePoint(int index, float x);			// Record the X coordinate of a probe point
     void SetYBedProbePoint(int index, float y);			// Record the Y coordinate of a probe point
-    void SetZBedProbePoint(int index, float z);			// Record the Z coordinate of a probe point
+    void SetZBedProbePoint(int index, float z, bool wasXyCorrected);	// Record the Z coordinate of a probe point
     float XBedProbePoint(int index) const;				// Get the X coordinate of a probe point
     float YBedProbePoint(int index) const;				// Get the Y coordinate of a probe point
     float ZBedProbePoint(int index)const ;				// Get the Z coordinate of a probe point
@@ -159,7 +160,7 @@ private:
     		size_t p2, float x, float y, float& l1,     // (see http://en.wikipedia.org/wiki/Barycentric_coordinate_system).
     		float& l2, float& l3) const;
     float TriangleZ(float x, float y) const;			// Interpolate onto a triangular grid
-    void AdjustDeltaParameters(const float v[], bool allSeven);		// Perform 4- or 7-factor delta adjustment
+    void AdjustDeltaParameters(const float v[], size_t numFactors);	// Perform delta adjustment
 
     static void PrintMatrix(const char* s, const MathMatrix<float>& m, size_t numRows = 0, size_t maxCols = 0);	// for debugging
     static void PrintVector(const char *s, const float *v, size_t numElems);	// for debugging
diff --git a/Release/RepRapFirmware-1.04d-dc42.bin b/Release/RepRapFirmware-1.04d-dc42.bin
new file mode 100644
index 00000000..f88e2d86
--- /dev/null
+++ b/Release/RepRapFirmware-1.04d-dc42.bin