Bug fixes

Fixed memory corruption bug
Fixed delta trilateration when the rods are different lengths
M118 messages to telnet now have newline appended
Moved string compare nfunctions to RRFLibraries project
Changed the form of paths to syscalls.o so that they don't depend on the file being present

17 files changed, 114 insertions, 200 deletions

diff --git a/.cproject b/.cproject
index 1bc9513b..0858d309 100644
--- a/.cproject
+++ b/.cproject
@@ -57,7 +57,7 @@
 							</tool>
 							<tool id="cdt.managedbuild.tool.gnu.cross.c.linker.1692168928" name="Cross GCC Linker" superClass="cdt.managedbuild.tool.gnu.cross.c.linker"/>
 							<tool id="cdt.managedbuild.tool.gnu.cross.archiver.1755453550" name="Cross GCC Archiver" superClass="cdt.managedbuild.tool.gnu.cross.archiver"/>
-							<tool command="gcc" commandLinePattern="${COMMAND} ${FLAGS} ${OUTPUT_FLAG} ${OUTPUT_PREFIX}${OUTPUT} ${LinkFlags1} &quot;${workspace_loc:/${CoreName}/RADDS/cores/arduino/syscalls.o}&quot; ${INPUTS} ${LinkFlags2}" id="cdt.managedbuild.tool.gnu.cross.cpp.linker.1176271302" name="Cross G++ Linker" superClass="cdt.managedbuild.tool.gnu.cross.cpp.linker">
+							<tool command="gcc" commandLinePattern="${COMMAND} ${FLAGS} ${OUTPUT_FLAG} ${OUTPUT_PREFIX}${OUTPUT} ${LinkFlags1} &quot;${workspace_loc:/${CoreName}}/RADDS/cores/arduino/syscalls.o&quot; ${INPUTS} ${LinkFlags2}" id="cdt.managedbuild.tool.gnu.cross.cpp.linker.1176271302" name="Cross G++ Linker" superClass="cdt.managedbuild.tool.gnu.cross.cpp.linker">
 								<option id="gnu.cpp.link.option.nostdlibs.706270025" name="No startup or default libs (-nostdlib)" superClass="gnu.cpp.link.option.nostdlibs" useByScannerDiscovery="false" value="false" valueType="boolean"/>
 								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="gnu.cpp.link.option.paths.1160723414" name="Library search path (-L)" superClass="gnu.cpp.link.option.paths" useByScannerDiscovery="false" valueType="libPaths">
 									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${CoreName}/RADDS/}&quot;"/>
@@ -180,7 +180,7 @@
 							</tool>
 							<tool id="cdt.managedbuild.tool.gnu.cross.c.linker.1102044150" name="Cross GCC Linker" superClass="cdt.managedbuild.tool.gnu.cross.c.linker"/>
 							<tool id="cdt.managedbuild.tool.gnu.cross.archiver.1402882499" name="Cross GCC Archiver" superClass="cdt.managedbuild.tool.gnu.cross.archiver"/>
-							<tool command="gcc" commandLinePattern="${COMMAND} ${FLAGS} ${OUTPUT_FLAG} ${OUTPUT_PREFIX}${OUTPUT} ${LinkFlags1} &quot;${workspace_loc:/${CoreName}/SAM4E8E/cores/arduino/syscalls.o}&quot; ${INPUTS} ${LinkFlags2}" id="cdt.managedbuild.tool.gnu.cross.cpp.linker.1768134875" name="Cross G++ Linker" superClass="cdt.managedbuild.tool.gnu.cross.cpp.linker">
+							<tool command="gcc" commandLinePattern="${COMMAND} ${FLAGS} ${OUTPUT_FLAG} ${OUTPUT_PREFIX}${OUTPUT} ${LinkFlags1} &quot;${workspace_loc:/${CoreName}}/SAM4E8E/cores/arduino/syscalls.o&quot; ${INPUTS} ${LinkFlags2}" id="cdt.managedbuild.tool.gnu.cross.cpp.linker.1768134875" name="Cross G++ Linker" superClass="cdt.managedbuild.tool.gnu.cross.cpp.linker">
 								<option id="gnu.cpp.link.option.nostdlibs.399544265" name="No startup or default libs (-nostdlib)" superClass="gnu.cpp.link.option.nostdlibs" useByScannerDiscovery="false" value="false" valueType="boolean"/>
 								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="gnu.cpp.link.option.paths.605414111" name="Library search path (-L)" superClass="gnu.cpp.link.option.paths" useByScannerDiscovery="false" valueType="libPaths">
 									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${CoreName}/SAM4E8E/}&quot;"/>
@@ -305,7 +305,7 @@
 							</tool>
 							<tool id="cdt.managedbuild.tool.gnu.cross.c.linker.411798791" name="Cross GCC Linker" superClass="cdt.managedbuild.tool.gnu.cross.c.linker"/>
 							<tool id="cdt.managedbuild.tool.gnu.cross.archiver.2116733751" name="Cross GCC Archiver" superClass="cdt.managedbuild.tool.gnu.cross.archiver"/>
-							<tool command="gcc" commandLinePattern="${COMMAND} ${FLAGS} ${OUTPUT_FLAG} ${OUTPUT_PREFIX}${OUTPUT} ${LinkFlags1} &quot;${workspace_loc:/${CoreName}/SAM4S/cores/arduino/syscalls.o}&quot; ${INPUTS} ${LinkFlags2}" id="cdt.managedbuild.tool.gnu.cross.cpp.linker.1011842834" name="Cross G++ Linker" superClass="cdt.managedbuild.tool.gnu.cross.cpp.linker">
+							<tool command="gcc" commandLinePattern="${COMMAND} ${FLAGS} ${OUTPUT_FLAG} ${OUTPUT_PREFIX}${OUTPUT} ${LinkFlags1} &quot;${workspace_loc:/${CoreName}}/SAM4S/cores/arduino/syscalls.o&quot; ${INPUTS} ${LinkFlags2}" id="cdt.managedbuild.tool.gnu.cross.cpp.linker.1011842834" name="Cross G++ Linker" superClass="cdt.managedbuild.tool.gnu.cross.cpp.linker">
 								<option id="gnu.cpp.link.option.nostdlibs.91014132" name="No startup or default libs (-nostdlib)" superClass="gnu.cpp.link.option.nostdlibs" useByScannerDiscovery="false" value="false" valueType="boolean"/>
 								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="gnu.cpp.link.option.paths.2085138159" name="Library search path (-L)" superClass="gnu.cpp.link.option.paths" useByScannerDiscovery="false" valueType="libPaths">
 									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${CoreName}/SAM4S/}&quot;"/>
@@ -438,7 +438,7 @@
 							</tool>
 							<tool id="cdt.managedbuild.tool.gnu.cross.c.linker.1392905935" name="Cross GCC Linker" superClass="cdt.managedbuild.tool.gnu.cross.c.linker"/>
 							<tool id="cdt.managedbuild.tool.gnu.cross.archiver.733829747" name="Cross GCC Archiver" superClass="cdt.managedbuild.tool.gnu.cross.archiver"/>
-							<tool command="gcc" commandLinePattern="${COMMAND} ${FLAGS} ${OUTPUT_FLAG} ${OUTPUT_PREFIX}${OUTPUT} ${LinkFlags1} &quot;${workspace_loc:/${CoreName}/SAME70/cores/arduino/syscalls.o}&quot; ${INPUTS} ${LinkFlags2}" id="cdt.managedbuild.tool.gnu.cross.cpp.linker.1019421301" name="Cross G++ Linker" superClass="cdt.managedbuild.tool.gnu.cross.cpp.linker">
+							<tool command="gcc" commandLinePattern="${COMMAND} ${FLAGS} ${OUTPUT_FLAG} ${OUTPUT_PREFIX}${OUTPUT} ${LinkFlags1} &quot;${workspace_loc:/${CoreName}}/SAME70/cores/arduino/syscalls.o&quot; ${INPUTS} ${LinkFlags2}" id="cdt.managedbuild.tool.gnu.cross.cpp.linker.1019421301" name="Cross G++ Linker" superClass="cdt.managedbuild.tool.gnu.cross.cpp.linker">
 								<option id="gnu.cpp.link.option.nostdlibs.1609488418" name="No startup or default libs (-nostdlib)" superClass="gnu.cpp.link.option.nostdlibs" useByScannerDiscovery="false" value="false" valueType="boolean"/>
 								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="gnu.cpp.link.option.paths.579521002" name="Library search path (-L)" superClass="gnu.cpp.link.option.paths" useByScannerDiscovery="false" valueType="libPaths">
 									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${CoreName}/SAME70/}&quot;"/>
@@ -570,7 +570,7 @@
 							</tool>
 							<tool id="cdt.managedbuild.tool.gnu.cross.c.linker.1448735774" name="Cross GCC Linker" superClass="cdt.managedbuild.tool.gnu.cross.c.linker"/>
 							<tool id="cdt.managedbuild.tool.gnu.cross.archiver.886333230" name="Cross GCC Archiver" superClass="cdt.managedbuild.tool.gnu.cross.archiver"/>
-							<tool command="gcc" commandLinePattern="${COMMAND} ${FLAGS} ${OUTPUT_FLAG} ${OUTPUT_PREFIX}${OUTPUT} ${LinkFlags1} &quot;${workspace_loc:/${CoreName}/SAM4S/cores/arduino/syscalls.o}&quot; ${INPUTS} ${LinkFlags2}" id="cdt.managedbuild.tool.gnu.cross.cpp.linker.687537254" name="Cross G++ Linker" superClass="cdt.managedbuild.tool.gnu.cross.cpp.linker">
+							<tool command="gcc" commandLinePattern="${COMMAND} ${FLAGS} ${OUTPUT_FLAG} ${OUTPUT_PREFIX}${OUTPUT} ${LinkFlags1} &quot;${workspace_loc:/${CoreName}}/SAM4S/cores/arduino/syscalls.o&quot; ${INPUTS} ${LinkFlags2}" id="cdt.managedbuild.tool.gnu.cross.cpp.linker.687537254" name="Cross G++ Linker" superClass="cdt.managedbuild.tool.gnu.cross.cpp.linker">
 								<option id="gnu.cpp.link.option.nostdlibs.1719677032" name="No startup or default libs (-nostdlib)" superClass="gnu.cpp.link.option.nostdlibs" useByScannerDiscovery="false" value="false" valueType="boolean"/>
 								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="gnu.cpp.link.option.paths.2028936348" name="Library search path (-L)" superClass="gnu.cpp.link.option.paths" useByScannerDiscovery="false" valueType="libPaths">
 									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${CoreName}/SAM4S_PCCB/}&quot;"/>
@@ -696,7 +696,7 @@
 							</tool>
 							<tool id="cdt.managedbuild.tool.gnu.cross.c.linker.1936542357" name="Cross GCC Linker" superClass="cdt.managedbuild.tool.gnu.cross.c.linker"/>
 							<tool id="cdt.managedbuild.tool.gnu.cross.archiver.1577761657" name="Cross GCC Archiver" superClass="cdt.managedbuild.tool.gnu.cross.archiver"/>
-							<tool command="gcc" commandLinePattern="${COMMAND} ${FLAGS} ${OUTPUT_FLAG} ${OUTPUT_PREFIX}${OUTPUT} ${LinkFlags1} &quot;${workspace_loc:/${CoreName}/SAM4S/cores/arduino/syscalls.o}&quot; ${INPUTS} ${LinkFlags2}" id="cdt.managedbuild.tool.gnu.cross.cpp.linker.523461887" name="Cross G++ Linker" superClass="cdt.managedbuild.tool.gnu.cross.cpp.linker">
+							<tool command="gcc" commandLinePattern="${COMMAND} ${FLAGS} ${OUTPUT_FLAG} ${OUTPUT_PREFIX}${OUTPUT} ${LinkFlags1} &quot;${workspace_loc:/${CoreName}}/SAM4S/cores/arduino/syscalls.o&quot; ${INPUTS} ${LinkFlags2}" id="cdt.managedbuild.tool.gnu.cross.cpp.linker.523461887" name="Cross G++ Linker" superClass="cdt.managedbuild.tool.gnu.cross.cpp.linker">
 								<option id="gnu.cpp.link.option.nostdlibs.1674515330" name="No startup or default libs (-nostdlib)" superClass="gnu.cpp.link.option.nostdlibs" useByScannerDiscovery="false" value="false" valueType="boolean"/>
 								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="gnu.cpp.link.option.paths.144825348" name="Library search path (-L)" superClass="gnu.cpp.link.option.paths" useByScannerDiscovery="false" valueType="libPaths">
 									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${CoreName}/SAM4S_PCCB/}&quot;"/>
@@ -829,7 +829,7 @@
 							</tool>
 							<tool id="cdt.managedbuild.tool.gnu.cross.c.linker.642836635" name="Cross GCC Linker" superClass="cdt.managedbuild.tool.gnu.cross.c.linker"/>
 							<tool id="cdt.managedbuild.tool.gnu.cross.archiver.1474356143" name="Cross GCC Archiver" superClass="cdt.managedbuild.tool.gnu.cross.archiver"/>
-							<tool command="gcc" commandLinePattern="${COMMAND} ${FLAGS} ${OUTPUT_FLAG} ${OUTPUT_PREFIX}${OUTPUT} ${LinkFlags1} &quot;${workspace_loc:/${CoreName}/SAME70/cores/arduino/syscalls.o}&quot; ${INPUTS} ${LinkFlags2}" id="cdt.managedbuild.tool.gnu.cross.cpp.linker.8484819" name="Cross G++ Linker" superClass="cdt.managedbuild.tool.gnu.cross.cpp.linker">
+							<tool command="gcc" commandLinePattern="${COMMAND} ${FLAGS} ${OUTPUT_FLAG} ${OUTPUT_PREFIX}${OUTPUT} ${LinkFlags1} &quot;${workspace_loc:/${CoreName}}/SAME70/cores/arduino/syscalls.o&quot; ${INPUTS} ${LinkFlags2}" id="cdt.managedbuild.tool.gnu.cross.cpp.linker.8484819" name="Cross G++ Linker" superClass="cdt.managedbuild.tool.gnu.cross.cpp.linker">
 								<option id="gnu.cpp.link.option.nostdlibs.1515694027" name="No startup or default libs (-nostdlib)" superClass="gnu.cpp.link.option.nostdlibs" useByScannerDiscovery="false" value="false" valueType="boolean"/>
 								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="gnu.cpp.link.option.paths.78806302" name="Library search path (-L)" superClass="gnu.cpp.link.option.paths" useByScannerDiscovery="false" valueType="libPaths">
 									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${CoreName}/SAME70XPLD/}&quot;"/>
@@ -984,7 +984,7 @@
 							</tool>
 							<tool id="cdt.managedbuild.tool.gnu.cross.c.linker.121136768" name="Cross GCC Linker" superClass="cdt.managedbuild.tool.gnu.cross.c.linker"/>
 							<tool id="cdt.managedbuild.tool.gnu.cross.archiver.1125960435" name="Cross GCC Archiver" superClass="cdt.managedbuild.tool.gnu.cross.archiver"/>
-							<tool command="gcc" commandLinePattern="${COMMAND} ${FLAGS} ${OUTPUT_FLAG} ${OUTPUT_PREFIX}${OUTPUT} ${LinkFlags1} &quot;${workspace_loc:/${CoreName}/SAME70/cores/arduino/syscalls.o}&quot; ${INPUTS} ${LinkFlags2}" id="cdt.managedbuild.tool.gnu.cross.cpp.linker.500971768" name="Cross G++ Linker" superClass="cdt.managedbuild.tool.gnu.cross.cpp.linker">
+							<tool command="gcc" commandLinePattern="${COMMAND} ${FLAGS} ${OUTPUT_FLAG} ${OUTPUT_PREFIX}${OUTPUT} ${LinkFlags1} &quot;${workspace_loc:/${CoreName}}/SAME70/cores/arduino/syscalls.o&quot; ${INPUTS} ${LinkFlags2}" id="cdt.managedbuild.tool.gnu.cross.cpp.linker.500971768" name="Cross G++ Linker" superClass="cdt.managedbuild.tool.gnu.cross.cpp.linker">
 								<option id="gnu.cpp.link.option.nostdlibs.1767130579" name="No startup or default libs (-nostdlib)" superClass="gnu.cpp.link.option.nostdlibs" useByScannerDiscovery="false" value="false" valueType="boolean"/>
 								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="gnu.cpp.link.option.paths.788743762" name="Library search path (-L)" superClass="gnu.cpp.link.option.paths" useByScannerDiscovery="false" valueType="libPaths">
 									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${CoreName}/SAME70/}&quot;"/>
@@ -1116,7 +1116,7 @@
 							</tool>
 							<tool id="cdt.managedbuild.tool.gnu.cross.c.linker.178696318" name="Cross GCC Linker" superClass="cdt.managedbuild.tool.gnu.cross.c.linker"/>
 							<tool id="cdt.managedbuild.tool.gnu.cross.archiver.1343498785" name="Cross GCC Archiver" superClass="cdt.managedbuild.tool.gnu.cross.archiver"/>
-							<tool command="gcc" commandLinePattern="${COMMAND} ${FLAGS} ${OUTPUT_FLAG} ${OUTPUT_PREFIX}${OUTPUT} ${LinkFlags1} &quot;${workspace_loc:/${CoreName}/SAM4S/cores/arduino/syscalls.o}&quot; ${INPUTS} ${LinkFlags2}" id="cdt.managedbuild.tool.gnu.cross.cpp.linker.547420771" name="Cross G++ Linker" superClass="cdt.managedbuild.tool.gnu.cross.cpp.linker">
+							<tool command="gcc" commandLinePattern="${COMMAND} ${FLAGS} ${OUTPUT_FLAG} ${OUTPUT_PREFIX}${OUTPUT} ${LinkFlags1} &quot;${workspace_loc:/${CoreName}}/SAM4S/cores/arduino/syscalls.o&quot; ${INPUTS} ${LinkFlags2}" id="cdt.managedbuild.tool.gnu.cross.cpp.linker.547420771" name="Cross G++ Linker" superClass="cdt.managedbuild.tool.gnu.cross.cpp.linker">
 								<option id="gnu.cpp.link.option.nostdlibs.295391099" name="No startup or default libs (-nostdlib)" superClass="gnu.cpp.link.option.nostdlibs" useByScannerDiscovery="false" value="false" valueType="boolean"/>
 								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="gnu.cpp.link.option.paths.93177435" name="Library search path (-L)" superClass="gnu.cpp.link.option.paths" useByScannerDiscovery="false" valueType="libPaths">
 									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${CoreName}/SAM4S_PCCB/}&quot;"/>
diff --git a/Maths/trilateration-triple.wxmx b/Maths/trilateration-triple.wxmx
new file mode 100644
index 00000000..d76a7bac
--- /dev/null
+++ b/Maths/trilateration-triple.wxmx
diff --git a/Maths/trilateration.wxmx b/Maths/trilateration.wxmx
index 3cf85c59..ab6c1e35 100644
--- a/Maths/trilateration.wxmx
+++ b/Maths/trilateration.wxmx
diff --git a/src/CAN/CanInterface.cpp b/src/CAN/CanInterface.cpp
index d0a8e1e0..0cb68195 100644
--- a/src/CAN/CanInterface.cpp
+++ b/src/CAN/CanInterface.cpp
@@ -80,10 +80,10 @@ void CanInterface::AddMovement(const DDA& dda, const PrepParams& params, size_t
 		move->decelClocks = lrintf(params.decelTime * StepTimer::StepClockRate);
 		move->initialSpeedFraction = params.initialSpeedFraction;
 		move->finalSpeedFraction = params.finalSpeedFraction;
-		move->hdr.u.deltaDrives = 0;							//TODO
-		move->hdr.u.endStopsToCheck = 0;						//TODO
-		move->hdr.u.pressureAdvanceDrives = 0;					//TODO
-		move->hdr.u.stopAllDrivesOnEndstopHit = false;			//TODO
+		move->deltaDrives = 0;								//TODO
+		move->endStopsToCheck = 0;							//TODO
+		move->pressureAdvanceDrives = 0;					//TODO
+		move->stopAllDrivesOnEndstopHit = false;			//TODO
 		// Additional parameters for delta movements
 		move->initialX = params.initialX;
 		move->finalX = params.finalX;
diff --git a/src/Configuration.h b/src/Configuration.h
index 0c974854..2ccb1ee2 100644
--- a/src/Configuration.h
+++ b/src/Configuration.h
@@ -314,7 +314,7 @@ constexpr float DefaultIdleCurrentFactor = 0.3;			// Proportion of normal motor
 constexpr float DefaultNonlinearExtrusionLimit = 0.2;	// Maximum additional commanded extrusion to compensate for nonlinearity
 constexpr size_t NumRestorePoints = 6;					// Number of restore points, must be at least 3
 
-constexpr float AxisRoundingError = 0.05;				// Maximum possible error when we round trip a machine position to motor coordinates and back
+constexpr float AxisRoundingError = 0.02;				// Maximum possible error when we round trip a machine position to motor coordinates and back
 
 // Triggers
 constexpr unsigned int MaxTriggers = 16;				// Must be <= 32 because we store a bitmap of pending triggers in a uint32_t
diff --git a/src/GCodes/GCodes2.cpp b/src/GCodes/GCodes2.cpp
index 06e65540..fcb7e432 100644
--- a/src/GCodes/GCodes2.cpp
+++ b/src/GCodes/GCodes2.cpp
@@ -1611,7 +1611,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply)
 					String<GCODE_LENGTH> message;
 					gb.GetQuotedString(message.GetRef());
 					platform.Message(type, message.c_str());
-					if (type != HttpMessage && type != TelnetMessage)
+					if (type != HttpMessage)
 					{
 						platform.Message(type, "\n");
 					}
diff --git a/src/Hardware/IoPorts.cpp b/src/Hardware/IoPorts.cpp
index 9484ada0..a56f6b1d 100644
--- a/src/Hardware/IoPorts.cpp
+++ b/src/Hardware/IoPorts.cpp
@@ -271,8 +271,19 @@ bool IoPort::SetMode(PinAccess access)
 		const AnalogChannelNumber chan = PinToAdcChannel(PinTable[logicalPin].pin);
 		if (chan != NO_ADC)
 		{
-			AnalogInEnableChannel(chan, access == PinAccess::readAnalog);
-			analogChannel = (access == PinAccess::readAnalog) ? chan : NO_ADC;
+			if (access == PinAccess::readAnalog)
+			{
+				IoPort::SetPinMode(PinTable[logicalPin].pin, AIN);				// SAME70 errata says we must disable the pullup resistor before enabling the AFEC channel
+				AnalogInEnableChannel(chan, access == PinAccess::readAnalog);
+				analogChannel = chan;
+				logicalPinModes[logicalPin] = (int8_t)desiredMode;
+				return true;
+			}
+			else
+			{
+				AnalogInEnableChannel(chan, false);
+				analogChannel = NO_ADC;
+			}
 		}
 		else if (access == PinAccess::readAnalog)
 		{
diff --git a/src/Movement/Kinematics/LinearDeltaKinematics.cpp b/src/Movement/Kinematics/LinearDeltaKinematics.cpp
index 431d8a8d..a2c225ac 100644
--- a/src/Movement/Kinematics/LinearDeltaKinematics.cpp
+++ b/src/Movement/Kinematics/LinearDeltaKinematics.cpp
@@ -62,13 +62,12 @@ void LinearDeltaKinematics::Recalc()
 	Ybc = towerY[DELTA_C_AXIS] - towerY[DELTA_B_AXIS];
 	Yca = towerY[DELTA_A_AXIS] - towerY[DELTA_C_AXIS];
 	Yab = towerY[DELTA_B_AXIS] - towerY[DELTA_A_AXIS];
-	coreFa = fsquare(towerX[DELTA_A_AXIS]) + fsquare(towerY[DELTA_A_AXIS]);
-	coreFb = fsquare(towerX[DELTA_B_AXIS]) + fsquare(towerY[DELTA_B_AXIS]);
-	coreFc = fsquare(towerX[DELTA_C_AXIS]) + fsquare(towerY[DELTA_C_AXIS]);
-	Q = (Xca * Yab - Xab * Yca) * 2;
+
+	Q = (Xab * towerY[DELTA_C_AXIS] + Xca * towerY[DELTA_B_AXIS] + Xbc * towerY[DELTA_A_AXIS]) * 2;
 	Q2 = fsquare(Q);
 
-	// Calculate the base carriage heights when the printer is homed, i.e. the carriages are at the endstops, and the always-reachable height
+	// Calculate the squares of the diagonals and the base carriage heights when the printer is homed, i.e. the carriages are at the endstops
+	// Also calculate the always-reachable height
 	alwaysReachableHeight = homedHeight;
 	for (size_t axis = 0; axis < numTowers; ++axis)
 	{
@@ -83,6 +82,14 @@ void LinearDeltaKinematics::Recalc()
 		}
 	}
 
+	// Calculate coreKa, corrKb and coreKc which are used by the forward transform
+	const float coreFa = fsquare(towerX[DELTA_A_AXIS]) + fsquare(towerY[DELTA_A_AXIS]);
+	const float coreFb = fsquare(towerX[DELTA_B_AXIS]) + fsquare(towerY[DELTA_B_AXIS]);
+	const float coreFc = fsquare(towerX[DELTA_C_AXIS]) + fsquare(towerY[DELTA_C_AXIS]);
+	coreKa = (D2[DELTA_B_AXIS] - D2[DELTA_C_AXIS]) + (coreFc - coreFb);
+	coreKb = (D2[DELTA_C_AXIS] - D2[DELTA_A_AXIS]) + (coreFa - coreFc);
+	coreKc = (D2[DELTA_A_AXIS] - D2[DELTA_B_AXIS]) + (coreFb - coreFa);
+
 	printRadiusSquared = fsquare(printRadius);
 
 	if (reprap.Debug(moduleMove))
@@ -126,26 +133,31 @@ float LinearDeltaKinematics::Transform(const float machinePos[], size_t axis) co
 // Calculate the Cartesian coordinates from the motor coordinates
 void LinearDeltaKinematics::ForwardTransform(float Ha, float Hb, float Hc, float machinePos[XYZ_AXES]) const
 {
-	const float Fa = coreFa + fsquare(Ha);
-	const float Fb = coreFb + fsquare(Hb);
-	const float Fc = coreFc + fsquare(Hc);
-
-	// Calculate PQRSU such that x = -(S - Uz)/Q, y = (P - Rz)/Q
-	const float P = (Xbc * Fa) + (Xca * Fb) + (Xab * Fc);
-	const float S = (Ybc * Fa) + (Yca * Fb) + (Yab * Fc);
+	// Calculate RSUT such that x = (Uz + S)/Q, y = -(Rz + T)/Q
 	const float R = ((Xbc * Ha) + (Xca * Hb) + (Xab * Hc)) * 2;
 	const float U = ((Ybc * Ha) + (Yca * Hb) + (Yab * Hc)) * 2;
 
-	const float R2 = fsquare(R), U2 = fsquare(U);
+	// Note, Ka + Kb + Kc = 0 so we could calculate just two of them
+	const float Ka = coreKa + (fsquare(Hc) - fsquare(Hb)),
+				Kb = coreKb + (fsquare(Ha) - fsquare(Hc)),
+				Kc = coreKc + (fsquare(Hb) - fsquare(Ha));
 
-	const float A = U2 + R2 + Q2;
-	const float minusHalfB = S * U + P * R + Ha * Q2 + towerX[DELTA_A_AXIS] * U * Q - towerY[DELTA_A_AXIS] * R * Q;
-	const float C = fsquare(S + towerX[DELTA_A_AXIS] * Q) + fsquare(P - towerY[DELTA_A_AXIS] * Q) + (fsquare(Ha) - D2[DELTA_A_AXIS]) * Q2;
+	const float S = Ka * towerY[DELTA_A_AXIS] + Kb * towerY[DELTA_B_AXIS] + Kc * towerY[DELTA_C_AXIS];
+	const float T = Ka * towerX[DELTA_A_AXIS] + Kb * towerX[DELTA_B_AXIS] + Kc * towerX[DELTA_C_AXIS];
+
+	const float A = fsquare(U) + fsquare(R) + Q2;
+	const float minusHalfB =  Q2 * Ha
+							+ Q * (U * towerX[DELTA_A_AXIS] - R * towerY[DELTA_A_AXIS])
+							- (R * T + U * S);
+	const float C = fsquare(towerX[DELTA_A_AXIS] * Q - S) + fsquare(towerY[DELTA_A_AXIS] * Q + T) + (fsquare(Ha) - D2[DELTA_A_AXIS]) * Q2;
 
 	const float z = (minusHalfB - sqrtf(fsquare(minusHalfB) - A * C)) / A;
-	machinePos[X_AXIS] = (U * z - S) / Q;
-	machinePos[Y_AXIS] = (P - R * z) / Q;
+	machinePos[X_AXIS] = (U * z + S) / Q;
+	machinePos[Y_AXIS] = -(R * z + T) / Q;
 	machinePos[Z_AXIS] = z - ((machinePos[X_AXIS] * xTilt) + (machinePos[Y_AXIS] * yTilt));
+
+//	debugPrintf("Ha=%f Hb=%f Hc=%f Ka=%f Kb=%f Kc=%f\n", (double)Ha, (double)Hb, (double)Hc, (double)Ka, (double)Kb, (double)Kc);
+//	debugPrintf("Q=%f R=%f U=%f S=%f T=%f A=%f B=%f C=%f\n", (double)Q, (double)R, (double)U, (double)S, (double)T, (double)A, (double)minusHalfB, (double)C);
 }
 
 // Convert Cartesian coordinates to motor steps
diff --git a/src/Movement/Kinematics/LinearDeltaKinematics.h b/src/Movement/Kinematics/LinearDeltaKinematics.h
index a3c289c6..1e203d6c 100644
--- a/src/Movement/Kinematics/LinearDeltaKinematics.h
+++ b/src/Movement/Kinematics/LinearDeltaKinematics.h
@@ -98,7 +98,7 @@ private:
 	float printRadiusSquared;
 	float homedCarriageHeights[MaxTowers];
 	float Xbc, Xca, Xab, Ybc, Yca, Yab;
-	float coreFa, coreFb, coreFc;
+	float coreKa, coreKb, coreKc;
 	float Q, Q2;
 	float D2[MaxTowers];
 	float alwaysReachableHeight;
diff --git a/src/Movement/Move.cpp b/src/Movement/Move.cpp
index 0200cae5..806e2449 100644
--- a/src/Movement/Move.cpp
+++ b/src/Movement/Move.cpp
@@ -1004,16 +1004,22 @@ void Move::CreateLaserTask()
 	}
 }
 
-// Wake up the laser task. Call this at the start of a new move from standstill (not from an ISR)
+// Wake up the laser task, if there is one (must check!). Call this at the start of a new move from standstill (not from an ISR)
 void Move::WakeLaserTask()
 {
-	laserTask->Give();
+	if (laserTask != nullptr)
+	{
+		laserTask->Give();
+	}
 }
 
-// Wake up the laser task. Call this at the start of a new move from standstill (not from an ISR)
+// Wake up the laser task if there is one (must check!) from an ISR
 void Move::WakeLaserTaskFromISR()
 {
-	laserTask->GiveFromISR();
+	if (laserTask != nullptr)
+	{
+		laserTask->GiveFromISR();
+	}
 }
 
 void Move::LaserTaskRun()
diff --git a/src/Networking/ESP8266WiFi/WiFiInterface.cpp b/src/Networking/ESP8266WiFi/WiFiInterface.cpp
index 9970d632..299a1268 100644
--- a/src/Networking/ESP8266WiFi/WiFiInterface.cpp
+++ b/src/Networking/ESP8266WiFi/WiFiInterface.cpp
@@ -1539,10 +1539,10 @@ void WiFiInterface::SetupSpi()
 #endif
 
 	// Set up the SPI pins
-	ConfigurePin(g_APinDescription[APIN_ESP_SPI_SCK]);
-	ConfigurePin(g_APinDescription[APIN_ESP_SPI_MOSI]);
-	ConfigurePin(g_APinDescription[APIN_ESP_SPI_MISO]);
-	ConfigurePin(g_APinDescription[APIN_ESP_SPI_SS0]);
+	ConfigurePin(APIN_ESP_SPI_SCK);
+	ConfigurePin(APIN_ESP_SPI_MOSI);
+	ConfigurePin(APIN_ESP_SPI_MISO);
+	ConfigurePin(APIN_ESP_SPI_SS0);
 
 	pmc_enable_periph_clk(ESP_SPI_INTERFACE_ID);
 	spi_dma_disable();
diff --git a/src/Platform.cpp b/src/Platform.cpp
index dd10e60b..00931628 100644
--- a/src/Platform.cpp
+++ b/src/Platform.cpp
@@ -1844,6 +1844,9 @@ void Platform::PrintUniqueId(MessageType mtype)
 
 #endif
 
+// Global variable for debugging in tricky situations e.g. within ISRs
+int debugLine = 0;
+
 // Return diagnostic information
 void Platform::Diagnostics(MessageType mtype)
 {
@@ -1854,6 +1857,12 @@ void Platform::Diagnostics(MessageType mtype)
 
 	Message(mtype, "=== Platform ===\n");
 
+	// Debugging support
+	if (debugLine != 0)
+	{
+		MessageF(mtype, "Debug line %d\n", debugLine);
+	}
+
 #ifndef __LPC17xx__
 	// Show the up time and reason for the last reset
 	const uint32_t now = (uint32_t)(millis64()/1000u);		// get up time in seconds
@@ -1895,7 +1904,7 @@ void Platform::Diagnostics(MessageType mtype)
 		memset(srdBuf, 0, sizeof(srdBuf));
 		int slot = -1;
 
-#if SAM4E || SAM4S || SAME70
+# if SAM4E || SAM4S || SAME70
 		// Work around bug in ASF flash library: flash_read_user_signature calls a RAMFUNC without disabling interrupts first.
 		// This caused a crash (watchdog timeout) sometimes if we run M122 while a print is in progress
 		const irqflags_t flags = cpu_irq_save();
@@ -1905,9 +1914,9 @@ void Platform::Diagnostics(MessageType mtype)
 		cpu_irq_restore(flags);
 
 		if (rc == FLASH_RC_OK)
-#else
+# else
 		DueFlashStorage::read(SoftwareResetData::nvAddress, srdBuf, sizeof(srdBuf));
-#endif
+# endif
 		{
 			// Find the last slot written
 			slot = SoftwareResetData::numberOfSlots;
@@ -2034,14 +2043,6 @@ void Platform::Diagnostics(MessageType mtype)
 	MessageF(mtype, "Cache data hit count %" PRIu32 "\n", cacheCount);
 #endif
 
-// Debug
-//MessageF(mtype, "TC_FMR = %08x, PWM_FPE = %08x, PWM_FSR = %08x\n", TC2->TC_FMR, PWM->PWM_FPE, PWM->PWM_FSR);
-//MessageF(mtype, "PWM2 period %08x, duty %08x\n", PWM->PWM_CH_NUM[2].PWM_CPRD, PWM->PWM_CH_NUM[2].PWM_CDTY);
-//MessageF(mtype, "Shortest/longest times read %.1f/%.1f write %.1f/%.1f ms, %u/%u\n",
-//		(float)shortestReadWaitTime/1000, (float)longestReadWaitTime/1000, (float)shortestWriteWaitTime/1000, (float)longestWriteWaitTime/1000,
-//		maxRead, maxWrite);
-//longestWriteWaitTime = longestReadWaitTime = 0; shortestReadWaitTime = shortestWriteWaitTime = 1000000;
-
 	reprap.Timing(mtype);
 
 #ifdef MOVE_DEBUG
diff --git a/src/Platform.h b/src/Platform.h
index d97efcd1..a0c87b5b 100644
--- a/src/Platform.h
+++ b/src/Platform.h
@@ -293,7 +293,7 @@ public:
 	GCodeResult DiagnosticTest(GCodeBuffer& gb, const StringRef& reply, int d);
 	void LogError(ErrorCode e) { errorCodeBits |= (uint32_t)e; }
 
-	void SoftwareReset(uint16_t reason, const uint32_t *stk = nullptr) __attribute((noreturn));
+	[[noreturn]] void SoftwareReset(uint16_t reason, const uint32_t *stk = nullptr);
 	bool AtxPower() const;
 	void AtxPowerOn();
 	void AtxPowerOff(bool defer);
diff --git a/src/RepRapFirmware.cpp b/src/RepRapFirmware.cpp
index 81d693dc..670fd1c1 100644
--- a/src/RepRapFirmware.cpp
+++ b/src/RepRapFirmware.cpp
@@ -243,110 +243,6 @@ void delay(uint32_t ms)
 	vTaskDelay(ms);
 }
 
-// String testing
-
-bool StringEndsWithIgnoreCase(const char* string, const char* ending)
-{
-	const size_t j = strlen(string);
-	const size_t k = strlen(ending);
-	return k <= j && StringEqualsIgnoreCase(&string[j - k], ending);
-}
-
-bool StringEqualsIgnoreCase(const char* s1, const char* s2)
-{
-	size_t i = 0;
-	while (s1[i] != 0 && s2[i] != 0)
-	{
-		if (tolower(s1[i]) != tolower(s2[i]))
-		{
-			return false;
-		}
-		i++;
-	}
-
-	return s1[i] == 0 && s2[i] == 0;
-}
-
-bool StringStartsWith(const char* string, const char* starting)
-{
-	const size_t j = strlen(string);
-	const size_t k = strlen(starting);
-	if (k > j)
-	{
-		return false;
-	}
-
-	for (size_t i = 0; i < k; i++)
-	{
-		if (string[i] != starting[i])
-		{
-			return false;
-		}
-	}
-
-	return true;
-}
-
-bool StringStartsWithIgnoreCase(const char* string, const char* starting)
-{
-	const size_t j = strlen(string);
-	const size_t k = strlen(starting);
-	if (k > j)
-	{
-		return false;
-	}
-
-	for (size_t i = 0; i < k; i++)
-	{
-		if (tolower(string[i]) != tolower(starting[i]))
-		{
-			return false;
-		}
-	}
-
-	return true;
-}
-
-int StringContains(const char* string, const char* match)
-{
-	int i = 0;
-	int count = 0;
-
-	while (string[i] != 0)
-	{
-		if (string[i++] == match[count])
-		{
-			count++;
-			if (match[count] == 0)
-			{
-				return i - count;
-			}
-		}
-		else
-		{
-			count = 0;
-		}
-	}
-
-	return -1;
-}
-
-// Version of strncpy that ensures the result is null terminated
-void SafeStrncpy(char *dst, const char *src, size_t length)
-{
-	strncpy(dst, src, length);
-	dst[length - 1] = 0;
-}
-
-// Version of strcat that takes the original buffer size as the limit and ensures the result is null terminated
-void SafeStrncat(char *dst, const char *src, size_t length)
-{
-	dst[length - 1] = 0;
-	const size_t index = strlen(dst);
-	strncat(dst + index, src, length - index);
-	dst[length - 1] = 0;
-}
-
 // Convert a float to double for passing to printf etc. If it is a NaN or infinity, convert it to 9999.9 to avoid getting JSON parse errors.
 double HideNan(float val)
 {
diff --git a/src/RepRapFirmware.h b/src/RepRapFirmware.h
index 46499017..47223e88 100644
--- a/src/RepRapFirmware.h
+++ b/src/RepRapFirmware.h
@@ -32,6 +32,7 @@ Licence: GPL
 
 #include "Core.h"
 #include "General/StringRef.h"
+#include "General/StringFunctions.h"
 #include "General/BitMap.h"
 
 // Definitions needed by Pins.h and/or Configuration.h
@@ -186,14 +187,6 @@ extern "C" void debugPrintf(const char* fmt, ...) __attribute__ ((format (printf
 // Functions and globals not part of any class
 void delay(uint32_t ms);
 
-bool StringEndsWithIgnoreCase(const char* string, const char* ending);
-bool StringStartsWith(const char* string, const char* starting);
-bool StringStartsWithIgnoreCase(const char* string, const char* starting);
-bool StringEqualsIgnoreCase(const char* s1, const char* s2);
-int StringContains(const char* string, const char* match);
-void SafeStrncpy(char *dst, const char *src, size_t length) pre(length != 0);
-void SafeStrncat(char *dst, const char *src, size_t length) pre(length != 0);
-
 double HideNan(float val);
 
 void ListDrivers(const StringRef& str, DriversBitmap drivers);
diff --git a/src/Tasks.cpp b/src/Tasks.cpp
index ad8e4be7..90ca715c 100644
--- a/src/Tasks.cpp
+++ b/src/Tasks.cpp
@@ -125,9 +125,9 @@ extern "C" void AppMain()
 	// Add the FreeRTOS internal tasks to the task list
 	idleTask.AddToList();
 
-# if configUSE_TIMERS
+#if configUSE_TIMERS
 	timerTask.AddToList();
-# endif
+#endif
 
 	// Create the startup task
 	mainTask.Create(MainTask, "MAIN", nullptr, TaskPriority::SpinPriority);
@@ -187,7 +187,7 @@ namespace Tasks
 		return neverUsedRam;
 	}
 
-	// Write data about the current task (if RTOS) or the system
+	// Write data about the current task
 	void Diagnostics(MessageType mtype)
 	{
 		Platform& p = reprap.GetPlatform();
@@ -322,14 +322,13 @@ extern "C"
 	// Exception handlers
 	// By default the Usage Fault, Bus Fault and Memory Management fault handlers are not enabled,
 	// so they escalate to a Hard Fault and we don't need to provide separate exception handlers for them.
-	void hardFaultDispatcher(const uint32_t *pulFaultStackAddress) __attribute((noreturn));
-	void hardFaultDispatcher(const uint32_t *pulFaultStackAddress)
+	[[noreturn]] void hardFaultDispatcher(const uint32_t *pulFaultStackAddress)
 	{
 	    reprap.GetPlatform().SoftwareReset((uint16_t)SoftwareResetReason::hardFault, pulFaultStackAddress + 5);
 	}
 
 	// The fault handler implementation calls a function called hardFaultDispatcher()
-	void HardFault_Handler() __attribute__((naked, noreturn));
+    void HardFault_Handler() __attribute__((naked, noreturn));
 	void HardFault_Handler()
 	{
 	    __asm volatile
@@ -344,8 +343,7 @@ extern "C"
 	    );
 	}
 
-	void wdtFaultDispatcher(const uint32_t *pulFaultStackAddress) __attribute((noreturn));
-	void wdtFaultDispatcher(const uint32_t *pulFaultStackAddress)
+	[[noreturn]] void wdtFaultDispatcher(const uint32_t *pulFaultStackAddress)
 	{
 	    reprap.GetPlatform().SoftwareReset((uint16_t)SoftwareResetReason::wdtFault, pulFaultStackAddress + 5);
 	}
@@ -372,8 +370,7 @@ extern "C"
 	    );
 	}
 
-	void otherFaultDispatcher(const uint32_t *pulFaultStackAddress) __attribute((noreturn));
-	void otherFaultDispatcher(const uint32_t *pulFaultStackAddress)
+	[[noreturn]] void otherFaultDispatcher(const uint32_t *pulFaultStackAddress)
 	{
 	    reprap.GetPlatform().SoftwareReset((uint16_t)SoftwareResetReason::otherFault, pulFaultStackAddress + 5);
 	}
@@ -402,29 +399,27 @@ extern "C"
 
 	void DebugMon_Handler   () __attribute__ ((noreturn,alias("OtherFault_Handler")));
 
-// FreeRTOS hooks that we need to provide
-void stackOverflowDispatcher(const uint32_t *pulFaultStackAddress, char* pcTaskName) __attribute((noreturn));
-void stackOverflowDispatcher(const uint32_t *pulFaultStackAddress, char* pcTaskName)
-{
-	reprap.GetPlatform().SoftwareReset((uint16_t)SoftwareResetReason::stackOverflow, pulFaultStackAddress);
-}
+	// FreeRTOS hooks that we need to provide
+	[[noreturn]] void stackOverflowDispatcher(const uint32_t *pulFaultStackAddress, char* pcTaskName)
+	{
+		reprap.GetPlatform().SoftwareReset((uint16_t)SoftwareResetReason::stackOverflow, pulFaultStackAddress);
+	}
 
-void vApplicationStackOverflowHook(TaskHandle_t pxTask, char *pcTaskName) __attribute((naked, noreturn));
-void vApplicationStackOverflowHook(TaskHandle_t pxTask, char *pcTaskName)
-{
-	// r0 = pxTask, r1 = pxTaskName
-	__asm volatile
-	(
-		" push {r0, r1, lr}											\n"		/* save parameters and call address on the stack */
-		" mov r0, sp												\n"
-		" ldr r2, handler_sovf_address_const                        \n"
-		" bx r2                                                     \n"
-		" handler_sovf_address_const: .word stackOverflowDispatcher \n"
-	);
-}
+	void vApplicationStackOverflowHook(TaskHandle_t pxTask, char *pcTaskName) __attribute((naked, noreturn));
+	void vApplicationStackOverflowHook(TaskHandle_t pxTask, char *pcTaskName)
+	{
+		// r0 = pxTask, r1 = pxTaskName
+		__asm volatile
+		(
+			" push {r0, r1, lr}											\n"		/* save parameters and call address on the stack */
+			" mov r0, sp												\n"
+			" ldr r2, handler_sovf_address_const                        \n"
+			" bx r2                                                     \n"
+			" handler_sovf_address_const: .word stackOverflowDispatcher \n"
+		);
+	}
 
-	void assertCalledDispatcher(const uint32_t *pulFaultStackAddress) __attribute((noreturn));
-	void assertCalledDispatcher(const uint32_t *pulFaultStackAddress)
+	[[noreturn]] void assertCalledDispatcher(const uint32_t *pulFaultStackAddress)
 	{
 	    reprap.GetPlatform().SoftwareReset((uint16_t)SoftwareResetReason::assertCalled, pulFaultStackAddress);
 	}
diff --git a/src/Version.h b/src/Version.h
index 6d14fc4d..708c9550 100644
--- a/src/Version.h
+++ b/src/Version.h
@@ -15,7 +15,7 @@
 #endif
 
 #ifndef DATE
-# define DATE "2019-07-07b1"
+# define DATE "2019-07-14b2"
 #endif
 
 #define AUTHORS "reprappro, dc42, chrishamm, t3p3, dnewman, printm3d"