Version 1.20beta10

Heater fault timeout to cancelling print to be configurable M570 S
parameter)
Filament monitor always updates calibration accumulators during a print
even if not in calbration mode
Added CoreXYUV
Aded Z probe types 7 and 8
Bug fix: semicolons inside quoted strings were still stipped out from
some gcode sources
Bug fix: giving silly numbers of microsteps in M350 messed up the
steps/mm
Bug fix: High microstepping and high acceleration on deltas led to poor
timing in deceleration phase and high MaxReps
Bug fix: The commands in tool change macro files were sometimes not
fully executed before the next action
Bug fix to speed limits in CoreXYU
Possible bug fix: avoid race condition in TMC SPI ISR

21 files changed, 425 insertions, 199 deletions

diff --git a/src/DuetNG/TMC2660.cpp b/src/DuetNG/TMC2660.cpp
index 30fb5c41..5f19da7d 100644
--- a/src/DuetNG/TMC2660.cpp
+++ b/src/DuetNG/TMC2660.cpp
@@ -30,8 +30,18 @@ const Pin DriversSclkPin = 23;								// PA23
 
 const int ChopperControlRegisterMode = 999;					// mode passed to get/set microstepping to indicate we want the chopper control register
 
-const uint32_t DriversSpiClockFrequency = 4000000;			// 4MHz SPI clock
-const int StallGuardThreshold = 1;							// Range is -64..63. Zero seems to be too sensitive. Higher values reduce sensitivity of stall detection.
+// The SPI clock speed is a compromise:
+// - too high and polling the driver chips take too much of the CPU time
+// - too low and we won't detect stalls quickly enough
+// With a 4MHz SPI clock:
+// - polling the drivers makes calculations take 13.5% longer, so it is taking about 12% of the CPU time
+// - we poll all 10 drivers in about 80us
+// With a 2MHz SPI clock:
+// - polling the drivers makes calculations take 8.3% longer, so it is taking about 7.7% of the CPU time
+// - we poll all 10 drivers in about 170us
+const uint32_t DriversSpiClockFrequency = 2000000;			// 2MHz SPI clock
+
+const int DefaultStallGuardThreshold = 1;					// Range is -64..63. Zero seems to be too sensitive. Higher values reduce sensitivity of stall detection.
 
 // TMC2660 register addresses
 const uint32_t TMC_REG_DRVCTRL = 0;
@@ -123,7 +133,7 @@ const uint32_t defaultChopConfReg =
 // StallGuard configuration register
 const uint32_t defaultSgscConfReg =
 	  TMC_REG_SGCSCONF
-	| TMC_SGCSCONF_SGT(StallGuardThreshold);
+	| TMC_SGCSCONF_SGT(DefaultStallGuardThreshold);
 
 // Driver configuration register
 const uint32_t defaultDrvConfReg =
@@ -419,37 +429,40 @@ inline void TmcDriverState::StartTransfer()
 	}
 
 	// Kick off a transfer for that register
+	irqflags_t flags = cpu_irq_save();					// avoid race condition
 	USART_TMC_DRV->US_CR = US_CR_RSTRX | US_CR_RSTTX;	// reset transmitter and receiver
 	fastDigitalWriteLow(pin);							// set CS low
 	SetupDMA(regVal);									// set up the PDC
 	USART_TMC_DRV->US_IER = US_IER_ENDRX;				// enable end-of-transfer interrupt
 	USART_TMC_DRV->US_CR = US_CR_RXEN | US_CR_TXEN;		// enable transmitter and receiver
+	cpu_irq_restore(flags);
 }
 
 // ISR for the USART
-void USART_TMC_DRV_Handler(void) __attribute__ ((hot));
+extern "C" void USART_TMC_DRV_Handler(void) __attribute__ ((hot));
 
 void USART_TMC_DRV_Handler(void)
 {
 	TmcDriverState *driver = currentDriver;				// capture volatile variable
-	driver->TransferDone();								// tidy up after the transfer we just completed
-
-	if (driversPowered)
+	if (driver != nullptr)
 	{
-		// Power is still good, so send/receive to/from the next driver
-		++driver;										// advance to the next driver
-		if (driver == driverStates + numTmc2660Drivers)
+		driver->TransferDone();							// tidy up after the transfer we just completed
+		if (driversPowered)
 		{
-			driver = driverStates;
+			// Power is still good, so send/receive to/from the next driver
+			++driver;									// advance to the next driver
+			if (driver == driverStates + numTmc2660Drivers)
+			{
+				driver = driverStates;
+			}
+			driver->StartTransfer();
+			return;
 		}
-		driver->StartTransfer();
-	}
-	else
-	{
-		// Driver power is down, so stop polling
-		USART_TMC_DRV->US_IDR = US_IDR_ENDRX;
-		currentDriver = nullptr;						// signal that we are not waiting for an interrupt
 	}
+
+	// Driver power is down or there is no current driver, so stop polling
+	USART_TMC_DRV->US_IDR = US_IDR_ENDRX;
+	currentDriver = nullptr;							// signal that we are not waiting for an interrupt
 }
 
 //--------------------------- Public interface ---------------------------------
@@ -546,12 +559,12 @@ namespace SmartDrivers
 				// Set the microstepping. We need to determine how many bits right to shift the desired microstepping to reach 1.
 				unsigned int shift = 0;
 				unsigned int uSteps = (unsigned int)microsteps;
-				while (uSteps > 1)
+				while ((uSteps & 1) == 0)
 				{
 					uSteps >>= 1;
 					++shift;
 				}
-				if (shift <= 8)
+				if (uSteps == 1 && shift <= 8)
 				{
 					driverStates[drive].SetMicrostepping(shift, mode != 0);
 					return true;
diff --git a/src/FilamentSensors/Duet3DFilamentSensor.cpp b/src/FilamentSensors/Duet3DFilamentSensor.cpp
index a4d70ee1..c1e5cf36 100644
--- a/src/FilamentSensors/Duet3DFilamentSensor.cpp
+++ b/src/FilamentSensors/Duet3DFilamentSensor.cpp
@@ -343,31 +343,30 @@ FilamentSensorStatus Duet3DFilamentSensor::CheckFilament(float amountCommanded,
 			}
 		}
 	}
-	else
+
+	// Update the calibration accumulators, even if the user hasn't asked to do calibration
+	const float ratio = accumulatedRevsMeasured/amountCommanded;
+	if (calibrationStarted)
 	{
-		// Tolerance < 0.0 means do calibration
-		const float ratio = accumulatedRevsMeasured/amountCommanded;
-		if (calibrationStarted)
+		if (ratio < minMovementRatio)
 		{
-			if (ratio < minMovementRatio)
-			{
-				minMovementRatio = ratio;
-			}
-			if (ratio > maxMovementRatio)
-			{
-				maxMovementRatio = ratio;
-			}
-			totalExtrusionCommanded += amountCommanded;
-			totalRevsMeasured += accumulatedRevsMeasured;
+			minMovementRatio = ratio;
 		}
-		else
+		if (ratio > maxMovementRatio)
 		{
-			minMovementRatio = maxMovementRatio = ratio;
-			totalExtrusionCommanded = amountCommanded;
-			totalRevsMeasured = accumulatedRevsMeasured;
-			calibrationStarted = true;
+			maxMovementRatio = ratio;
 		}
+		totalExtrusionCommanded += amountCommanded;
+		totalRevsMeasured += accumulatedRevsMeasured;
 	}
+	else
+	{
+		minMovementRatio = maxMovementRatio = ratio;
+		totalExtrusionCommanded = amountCommanded;
+		totalRevsMeasured = accumulatedRevsMeasured;
+		calibrationStarted = true;
+	}
+
 	accumulatedExtrusionCommanded -= amountCommanded;
 	extrusionCommandedAtLastMeasurement = accumulatedRevsMeasured = 0.0;
 
diff --git a/src/GCodes/GCodeInput.cpp b/src/GCodes/GCodeInput.cpp
index a0087345..2ba77c9b 100644
--- a/src/GCodes/GCodeInput.cpp
+++ b/src/GCodes/GCodeInput.cpp
@@ -63,8 +63,8 @@ size_t StreamGCodeInput::BytesCached() const
 
 // Dynamic G-code input class for caching codes from software-defined sources
 
-RegularGCodeInput::RegularGCodeInput(bool removeComments): stripComments(removeComments),
-	state(GCodeInputState::idle), buffer(reinterpret_cast<char * const>(buf32)), writingPointer(0), readingPointer(0)
+RegularGCodeInput::RegularGCodeInput()
+	: state(GCodeInputState::idle), buffer(reinterpret_cast<char * const>(buf32)), writingPointer(0), readingPointer(0)
 {
 }
 
@@ -115,17 +115,7 @@ void RegularGCodeInput::Put(MessageType mtype, const char c)
 			state = (c == 'M') ? GCodeInputState::doingMCode : GCodeInputState::doingCode;
 			break;
 
-
 		case GCodeInputState::doingCode:
-			if (stripComments && c == ';')
-			{
-				// ignore comments if possible
-				state = GCodeInputState::inComment;
-				break;
-			}
-			// no break
-
-		case GCodeInputState::inComment:
 			if (c == 0 || c == '\r' || c == '\n')
 			{
 				state = GCodeInputState::idle;
@@ -159,12 +149,12 @@ void RegularGCodeInput::Put(MessageType mtype, const char c)
 				Reset();
 				return;
 			}
-
 			state = GCodeInputState::doingCode;
 			break;
 
 		case GCodeInputState::doingMCode122:
-			if (c <= ' ' || c == ';')
+			// Only execute M122 here if there is no parameter
+			if (c < ' ' || c == ';')
 			{
 				// Diagnostics requested - report them now
 				// Only send the report to the appropriate channel, because if we send it as a generic message instead then it gets truncated.
@@ -174,17 +164,15 @@ void RegularGCodeInput::Put(MessageType mtype, const char c)
 				Reset();
 				return;
 			}
+			state = GCodeInputState::doingCode;
 			break;
 	}
 
 	// Feed another character into the buffer
-	if (state != GCodeInputState::inComment)
+	buffer[writingPointer++] = c;
+	if (writingPointer == GCodeInputBufferSize)
 	{
-		buffer[writingPointer++] = c;
-		if (writingPointer == GCodeInputBufferSize)
-		{
-			writingPointer = 0;
-		}
+		writingPointer = 0;
 	}
 }
 
diff --git a/src/GCodes/GCodeInput.h b/src/GCodes/GCodeInput.h
index 4bd106d4..11c900d3 100644
--- a/src/GCodes/GCodeInput.h
+++ b/src/GCodes/GCodeInput.h
@@ -53,7 +53,6 @@ enum class GCodeInputState
 {
 	idle,
 	doingCode,
-	inComment,
 	doingMCode,
 	doingMCode1,
 	doingMCode11,
@@ -68,7 +67,7 @@ enum class GCodeInputState
 class RegularGCodeInput : public GCodeInput
 {
 public:
-	RegularGCodeInput(bool removeComments);
+	RegularGCodeInput();
 
 	void Reset() override;
 	size_t BytesCached() const override;				// How many bytes have been cached?
@@ -81,7 +80,6 @@ public:
 	size_t BufferSpaceLeft() const;						// How much space do we have left?
 
 private:
-	bool stripComments;
 	GCodeInputState state;
 
 protected:
@@ -95,7 +93,7 @@ protected:
 class FileGCodeInput : public RegularGCodeInput
 {
 public:
-	FileGCodeInput() : RegularGCodeInput(false), lastFile(nullptr) { }
+	FileGCodeInput() : RegularGCodeInput(), lastFile(nullptr) { }
 
 	void Reset() override;								// This should be called when the associated file is being closed
 	void Reset(const FileData &file);					// Should be called when a specific G-code or macro file is closed outside the reading context
diff --git a/src/GCodes/GCodes.cpp b/src/GCodes/GCodes.cpp
index e5a25f75..eaef5ef9 100644
--- a/src/GCodes/GCodes.cpp
+++ b/src/GCodes/GCodes.cpp
@@ -48,8 +48,8 @@ GCodes::GCodes(Platform& p) :
 #endif
 	isFlashing(false), fileBeingHashed(nullptr), lastWarningMillis(0)
 {
-	httpInput = new RegularGCodeInput(true);
-	telnetInput = new RegularGCodeInput(true);
+	httpInput = new RegularGCodeInput;
+	telnetInput = new RegularGCodeInput;
 	fileInput = new FileGCodeInput();
 	serialInput = new StreamGCodeInput(SERIAL_MAIN_DEVICE);
 	auxInput = new StreamGCodeInput(SERIAL_AUX_DEVICE);
@@ -462,61 +462,68 @@ void GCodes::RunStateMachine(GCodeBuffer& gb, StringRef& reply)
 
 	case GCodeState::toolChange1:		// Release the old tool (if any), then run tpre for the new tool
 	case GCodeState::m109ToolChange1:	// Release the old tool (if any), then run tpre for the new tool
+		if (LockMovementAndWaitForStandstill(gb))		// wait for tfree.g to finish executing
 		{
 			const Tool * const oldTool = reprap.GetCurrentTool();
 			if (oldTool != nullptr)
 			{
 				reprap.StandbyTool(oldTool->Number());
 			}
-		}
-		gb.AdvanceState();
-		if (reprap.GetTool(gb.MachineState().newToolNumber) != nullptr && AllAxesAreHomed() && (gb.MachineState().toolChangeParam & TPreBit) != 0)
-		{
-			scratchString.printf("tpre%d.g", gb.MachineState().newToolNumber);
-			DoFileMacro(gb, scratchString.Pointer(), false);
+			gb.AdvanceState();
+			if (reprap.GetTool(gb.MachineState().newToolNumber) != nullptr && AllAxesAreHomed() && (gb.MachineState().toolChangeParam & TPreBit) != 0)
+			{
+				scratchString.printf("tpre%d.g", gb.MachineState().newToolNumber);
+				DoFileMacro(gb, scratchString.Pointer(), false);
+			}
 		}
 		break;
 
 	case GCodeState::toolChange2:		// Select the new tool (even if it doesn't exist - that just deselects all tools) and run tpost
 	case GCodeState::m109ToolChange2:	// Select the new tool (even if it doesn't exist - that just deselects all tools) and run tpost
-		reprap.SelectTool(gb.MachineState().newToolNumber, simulationMode != 0);
-		GetCurrentUserPosition();									// get the actual position of the new tool
-
-		gb.AdvanceState();
-		if (AllAxesAreHomed())
+		if (LockMovementAndWaitForStandstill(gb))		// wait for tpre.g to finish executing
 		{
-			if (reprap.GetTool(gb.MachineState().newToolNumber) != nullptr && (gb.MachineState().toolChangeParam & TPostBit) != 0)
+			reprap.SelectTool(gb.MachineState().newToolNumber, simulationMode != 0);
+			GetCurrentUserPosition();									// get the actual position of the new tool
+
+			gb.AdvanceState();
+			if (AllAxesAreHomed())
 			{
-				scratchString.printf("tpost%d.g", gb.MachineState().newToolNumber);
-				DoFileMacro(gb, scratchString.Pointer(), false);
+				if (reprap.GetTool(gb.MachineState().newToolNumber) != nullptr && (gb.MachineState().toolChangeParam & TPostBit) != 0)
+				{
+					scratchString.printf("tpost%d.g", gb.MachineState().newToolNumber);
+					DoFileMacro(gb, scratchString.Pointer(), false);
+				}
 			}
 		}
 		break;
 
 	case GCodeState::toolChangeComplete:
 	case GCodeState::m109ToolChangeComplete:
-		// Restore the original Z axis user position, so that different tool Z offsets work even if the first move after the tool change doesn't have a Z coordinate
-		currentUserPosition[Z_AXIS] = toolChangeRestorePoint.moveCoords[Z_AXIS];
-		gb.MachineState().feedrate = toolChangeRestorePoint.feedRate;
-		// We don't restore the default fan speed in case the user wants to use a different one for the new tool
-		doingToolChange = false;
-
-		if (gb.GetState() == GCodeState::toolChangeComplete)
+		if (LockMovementAndWaitForStandstill(gb))		// wait for tpost.g to finish executing
 		{
-			gb.SetState(GCodeState::normal);
-		}
-		else
-		{
-			UnlockAll(gb);									// allow movement again
-			if (cancelWait || ToolHeatersAtSetTemperatures(reprap.GetCurrentTool(), gb.MachineState().waitWhileCooling))
+			// Restore the original Z axis user position, so that different tool Z offsets work even if the first move after the tool change doesn't have a Z coordinate
+			currentUserPosition[Z_AXIS] = toolChangeRestorePoint.moveCoords[Z_AXIS];
+			gb.MachineState().feedrate = toolChangeRestorePoint.feedRate;
+			// We don't restore the default fan speed in case the user wants to use a different one for the new tool
+			doingToolChange = false;
+
+			if (gb.GetState() == GCodeState::toolChangeComplete)
 			{
-				cancelWait = isWaiting = false;
 				gb.SetState(GCodeState::normal);
 			}
 			else
 			{
-				CheckReportDue(gb, reply);
-				isWaiting = true;
+				UnlockAll(gb);									// allow movement again
+				if (cancelWait || ToolHeatersAtSetTemperatures(reprap.GetCurrentTool(), gb.MachineState().waitWhileCooling))
+				{
+					cancelWait = isWaiting = false;
+					gb.SetState(GCodeState::normal);
+				}
+				else
+				{
+					CheckReportDue(gb, reply);
+					isWaiting = true;
+				}
 			}
 		}
 		break;
@@ -3468,16 +3475,12 @@ void GCodes::DisableDrives()
 bool GCodes::ChangeMicrostepping(size_t drive, int microsteps, int mode) const
 {
 	bool dummy;
-	unsigned int oldSteps = platform.GetMicrostepping(drive, mode, dummy);
-	bool success = platform.SetMicrostepping(drive, microsteps, mode);
+	const unsigned int oldSteps = platform.GetMicrostepping(drive, mode, dummy);
+	const bool success = platform.SetMicrostepping(drive, microsteps, mode);
 	if (success && mode <= 1)							// modes higher than 1 are used for special functions
 	{
 		// We changed the microstepping, so adjust the steps/mm to compensate
-		float stepsPerMm = platform.DriveStepsPerUnit(drive);
-		if (stepsPerMm > 0)
-		{
-			platform.SetDriveStepsPerUnit(drive, stepsPerMm * (float)microsteps / (float)oldSteps);
-		}
+		platform.SetDriveStepsPerUnit(drive, platform.DriveStepsPerUnit(drive) * (float)microsteps / (float)oldSteps);
 	}
 	return success;
 }
diff --git a/src/GCodes/GCodes2.cpp b/src/GCodes/GCodes2.cpp
index c7167a4e..3c588f3a 100644
--- a/src/GCodes/GCodes2.cpp
+++ b/src/GCodes/GCodes2.cpp
@@ -2860,30 +2860,39 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply)
 		}
 		break;
 
-	case 570: // Set/report heater timeout
-		if (gb.Seen('H'))
+	case 570: // Set/report heater monitoring
 		{
-			const int heater = gb.GetIValue();
 			bool seen = false;
-			if (heater >= 0 && heater < (int)Heaters)
+			if (gb.Seen('S'))
 			{
-				float maxTempExcursion, maxFaultTime;
-				reprap.GetHeat().GetHeaterProtection(heater, maxTempExcursion, maxFaultTime);
-				gb.TryGetFValue('P', maxFaultTime, seen);
-				gb.TryGetFValue('T', maxTempExcursion, seen);
-				if (seen)
-				{
-					reprap.GetHeat().SetHeaterProtection(heater, maxTempExcursion, maxFaultTime);
-				}
-				else
+				seen = true;
+				heaterFaultTimeout = gb.GetUIValue() * (60 * 1000);
+			}
+			if (gb.Seen('H'))
+			{
+				const int heater = gb.GetIValue();
+				if (heater >= 0 && heater < (int)Heaters)
 				{
-					reply.printf("Heater %u allowed excursion %.1f" DEGREE_SYMBOL "C, fault trigger time %.1f seconds", heater, (double)maxTempExcursion, (double)maxFaultTime);
+					bool seenValue = false;
+					float maxTempExcursion, maxFaultTime;
+					reprap.GetHeat().GetHeaterProtection(heater, maxTempExcursion, maxFaultTime);
+					gb.TryGetFValue('P', maxFaultTime, seenValue);
+					gb.TryGetFValue('T', maxTempExcursion, seenValue);
+					if (seenValue)
+					{
+						reprap.GetHeat().SetHeaterProtection(heater, maxTempExcursion, maxFaultTime);
+					}
+					else if (!seen)
+					{
+						reply.printf("Heater %u allowed excursion %.1f" DEGREE_SYMBOL "C, fault trigger time %.1f seconds", heater, (double)maxTempExcursion, (double)maxFaultTime);
+					}
 				}
+				seen = true;
+			}
+			if (!seen)
+			{
+				reply.printf("Print will be terminated if a heater fault is not reset within %" PRIu32 " minutes", heaterFaultTimeout/(60 * 1000));
 			}
-		}
-		else if (gb.Seen('S'))
-		{
-			reply.copy("M570 S parameter is no longer required or supported");
 		}
 		break;
 
diff --git a/src/Movement/DDA.cpp b/src/Movement/DDA.cpp
index 037f5af1..ce13d974 100644
--- a/src/Movement/DDA.cpp
+++ b/src/Movement/DDA.cpp
@@ -977,7 +977,7 @@ void DDA::Prepare(uint8_t simMode)
 			// This code assumes that the previous move in the DDA ring is the previously-executed move, because it fetches the X and Y end coordinates from that move.
 			// Therefore the Move code must not store a new move in that entry until this one has been prepared! (It took me ages to track this down.)
 			// Ideally we would store the initial X and Y coordinates in the DDA, but we need to be economical with memory in the Duet 06/085 build.
-			cKc = (int32_t)(directionVector[Z_AXIS] * DriveMovement::Kc);
+			cKc = lrintf(directionVector[Z_AXIS] * DriveMovement::Kc);
 			params.a2plusb2 = fsquare(directionVector[X_AXIS]) + fsquare(directionVector[Y_AXIS]);
 			params.initialX = prev->GetEndCoordinate(X_AXIS, false);
 			params.initialY = prev->GetEndCoordinate(Y_AXIS, false);
@@ -990,11 +990,11 @@ void DDA::Prepare(uint8_t simMode)
 		const float accelStopTime = (topSpeed - startSpeed)/acceleration;
 		const float decelStartTime = accelStopTime + (params.decelStartDistance - accelDistance)/topSpeed;
 
-		params.startSpeedTimesCdivA = (uint32_t)((startSpeed * stepClockRate)/acceleration);
-		params.topSpeedTimesCdivA = (uint32_t)((topSpeed * stepClockRate)/acceleration);
-		params.decelStartClocks = (uint32_t)(decelStartTime * stepClockRate);
+		params.startSpeedTimesCdivA = (uint32_t)lrintf((startSpeed * stepClockRate)/acceleration);
+		params.topSpeedTimesCdivA = (uint32_t)lrintf((topSpeed * stepClockRate)/acceleration);
+		params.decelStartClocks = (uint32_t)lrintf(decelStartTime * stepClockRate);
 		params.topSpeedTimesCdivAPlusDecelStartClocks = params.topSpeedTimesCdivA + params.decelStartClocks;
-		params.accelClocksMinusAccelDistanceTimesCdivTopSpeed = (uint32_t)((accelStopTime - (accelDistance/topSpeed)) * stepClockRate);
+		params.accelClocksMinusAccelDistanceTimesCdivTopSpeed = (uint32_t)lrintf((accelStopTime - (accelDistance/topSpeed)) * stepClockRate);
 		params.compFactor = 1.0 - startSpeed/topSpeed;
 
 		firstDM = nullptr;
diff --git a/src/Movement/DriveMovement.cpp b/src/Movement/DriveMovement.cpp
index 04b98c05..7b89c8c0 100644
--- a/src/Movement/DriveMovement.cpp
+++ b/src/Movement/DriveMovement.cpp
@@ -65,7 +65,7 @@ void DriveMovement::PrepareCartesianAxis(const DDA& dda, const PrepParams& param
 	startSpeedTimesCdivA = params.startSpeedTimesCdivA;
 
 	// Constant speed phase parameters
-	mp.cart.mmPerStepTimesCdivtopSpeed = (uint32_t)(((float)DDA::stepClockRate * K1)/(stepsPerMm * dda.topSpeed));
+	mp.cart.mmPerStepTimesCdivtopSpeed = lrintf(((float)DDA::stepClockRate * K1)/(stepsPerMm * dda.topSpeed));
 	accelClocksMinusAccelDistanceTimesCdivTopSpeed = params.accelClocksMinusAccelDistanceTimesCdivTopSpeed;
 
 	// Deceleration phase parameters
@@ -98,10 +98,9 @@ void DriveMovement::PrepareDeltaAxis(const DDA& dda, const PrepParams& params)
 	const float aAplusbB = A * dda.directionVector[X_AXIS] + B * dda.directionVector[Y_AXIS];
 	const float dSquaredMinusAsquaredMinusBsquared = params.diagonalSquared - fsquare(A) - fsquare(B);
 	const float h0MinusZ0 = sqrtf(dSquaredMinusAsquaredMinusBsquared);
-	mp.delta.hmz0sK = (int32_t)(h0MinusZ0 * stepsPerMm * DriveMovement::K2);
-	mp.delta.minusAaPlusBbTimesKs = -(int32_t)(aAplusbB * stepsPerMm * DriveMovement::K2);
-	mp.delta.dSquaredMinusAsquaredMinusBsquaredTimesKsquaredSsquared =
-			(int64_t)(dSquaredMinusAsquaredMinusBsquared * fsquare(stepsPerMm * DriveMovement::K2));
+	mp.delta.hmz0sK = lrintf(h0MinusZ0 * stepsPerMm * DriveMovement::K2);
+	mp.delta.minusAaPlusBbTimesKs = -lrintf(aAplusbB * stepsPerMm * DriveMovement::K2);
+	mp.delta.dSquaredMinusAsquaredMinusBsquaredTimesKsquaredSsquared = llrintf(dSquaredMinusAsquaredMinusBsquared * fsquare(stepsPerMm * DriveMovement::K2));
 
 	// Calculate the distance at which we need to reverse direction.
 	if (params.a2plusb2 <= 0.0)
@@ -151,14 +150,14 @@ void DriveMovement::PrepareDeltaAxis(const DDA& dda, const PrepParams& params)
 		}
 	}
 
-	mp.delta.twoCsquaredTimesMmPerStepDivAK = (uint32_t)((float)DDA::stepClockRateSquared/(stepsPerMm * dda.acceleration * (K2/2)));
+	mp.delta.twoCsquaredTimesMmPerStepDivA = llrintf((double)(2 * DDA::stepClockRateSquared)/((double)stepsPerMm * (double)dda.acceleration));
 
 	// Acceleration phase parameters
-	mp.delta.accelStopDsK = (uint32_t)(dda.accelDistance * stepsPerMm * K2);
+	mp.delta.accelStopDsK = lrintf(dda.accelDistance * stepsPerMm * K2);
 	startSpeedTimesCdivA = params.startSpeedTimesCdivA;
 
 	// Constant speed phase parameters
-	mp.delta.mmPerStepTimesCdivtopSpeedK = (uint32_t)(((float)DDA::stepClockRate * K1)/(stepsPerMm * dda.topSpeed));
+	mp.delta.mmPerStepTimesCdivtopSpeedK = lrintf(((float)DDA::stepClockRate * K1)/(stepsPerMm * dda.topSpeed));
 	accelClocksMinusAccelDistanceTimesCdivTopSpeed = params.accelClocksMinusAccelDistanceTimesCdivTopSpeed;
 
 	// Deceleration phase parameters
@@ -171,10 +170,9 @@ void DriveMovement::PrepareDeltaAxis(const DDA& dda, const PrepParams& params)
 	}
 	else
 	{
-		mp.delta.decelStartDsK = (uint32_t)(params.decelStartDistance * stepsPerMm * K2);
+		mp.delta.decelStartDsK = lrintf(params.decelStartDistance * stepsPerMm * K2);
 		topSpeedTimesCdivAPlusDecelStartClocks = params.topSpeedTimesCdivAPlusDecelStartClocks;
-		const uint64_t initialDecelSpeedTimesCdivASquared = isquare64(params.topSpeedTimesCdivA);
-		twoDistanceToStopTimesCsquaredDivA = initialDecelSpeedTimesCdivASquared + (uint64_t)((params.decelStartDistance * (DDA::stepClockRateSquared * 2))/dda.acceleration);
+		twoDistanceToStopTimesCsquaredDivA = isquare64(params.topSpeedTimesCdivA) + llrintf((params.decelStartDistance * (DDA::stepClockRateSquared * 2))/dda.acceleration);
 	}
 }
 
@@ -183,11 +181,11 @@ void DriveMovement::PrepareExtruder(const DDA& dda, const PrepParams& params, bo
 {
 	const float dv = dda.directionVector[drive];
 	const float stepsPerMm = reprap.GetPlatform().DriveStepsPerUnit(drive) * fabsf(dv);
-	mp.cart.twoCsquaredTimesMmPerStepDivA = (uint64_t)(((float)DDA::stepClockRate * (float)DDA::stepClockRate)/(stepsPerMm * dda.acceleration)) * 2;
+	mp.cart.twoCsquaredTimesMmPerStepDivA = llrintf(((float)DDA::stepClockRate * (float)DDA::stepClockRate)/(stepsPerMm * dda.acceleration)) * 2;
 
 	// Calculate the pressure advance parameter
 	const float compensationTime = (doCompensation && dv > 0.0) ? reprap.GetPlatform().GetPressureAdvance(drive - reprap.GetGCodes().GetTotalAxes()) : 0.0;
-	const uint32_t compensationClocks = (uint32_t)(compensationTime * DDA::stepClockRate);
+	const uint32_t compensationClocks = lrintf(compensationTime * DDA::stepClockRate);
 
 	// Calculate the net total step count to allow for compensation. It may be negative.
 	// Note that we add totalSteps in floating point mode, to round the number of steps down consistently
@@ -222,7 +220,7 @@ void DriveMovement::PrepareExtruder(const DDA& dda, const PrepParams& params, bo
 		const uint64_t initialDecelSpeedTimesCdivASquared = isquare64(initialDecelSpeedTimesCdivA);
 		topSpeedTimesCdivAPlusDecelStartClocks = params.topSpeedTimesCdivAPlusDecelStartClocks - compensationClocks;
 		twoDistanceToStopTimesCsquaredDivA =
-			initialDecelSpeedTimesCdivASquared + (uint64_t)(((params.decelStartDistance + accelCompensationDistance) * (DDA::stepClockRateSquared * 2))/dda.acceleration);
+			initialDecelSpeedTimesCdivASquared + llrintf(((params.decelStartDistance + accelCompensationDistance) * (DDA::stepClockRateSquared * 2))/dda.acceleration);
 
 		// Calculate the move distance to the point of zero speed, where reverse motion starts
 		const float initialDecelSpeed = dda.topSpeed - dda.acceleration * compensationTime;
@@ -273,9 +271,9 @@ void DriveMovement::DebugPrint(char c, bool isDeltaMovement) const
 		if (isDeltaMovement)
 		{
 			debugPrintf("hmz0sK=%" PRIi32 " minusAaPlusBbTimesKs=%" PRIi32 " dSquaredMinusAsquaredMinusBsquared=%" PRId64 "\n"
-						"2c2mmsdak=%" PRIu32 " asdsk=%" PRIu32 " dsdsk=%" PRIu32 " mmstcdts=%" PRIu32 "\n",
+						"2c2mmsda=%" PRIu64 " asdsk=%" PRIu32 " dsdsk=%" PRIu32 " mmstcdts=%" PRIu32 "\n",
 						mp.delta.hmz0sK, mp.delta.minusAaPlusBbTimesKs, mp.delta.dSquaredMinusAsquaredMinusBsquaredTimesKsquaredSsquared,
-						mp.delta.twoCsquaredTimesMmPerStepDivAK, mp.delta.accelStopDsK, mp.delta.decelStartDsK, mp.delta.mmPerStepTimesCdivtopSpeedK
+						mp.delta.twoCsquaredTimesMmPerStepDivA, mp.delta.accelStopDsK, mp.delta.decelStartDsK, mp.delta.mmPerStepTimesCdivtopSpeedK
 						);
 		}
 		else
@@ -456,7 +454,7 @@ pre(nextStep < totalSteps; stepsTillRecalc == 0)
 	if ((uint32_t)dsK < mp.delta.accelStopDsK)
 	{
 		// Acceleration phase
-		nextStepTime = isqrt64(isquare64(startSpeedTimesCdivA) + ((uint64_t)mp.delta.twoCsquaredTimesMmPerStepDivAK * (uint32_t)dsK)) - startSpeedTimesCdivA;
+		nextStepTime = isqrt64(isquare64(startSpeedTimesCdivA) + (mp.delta.twoCsquaredTimesMmPerStepDivA * (uint32_t)dsK)/K2) - startSpeedTimesCdivA;
 	}
 	else if ((uint32_t)dsK < mp.delta.decelStartDsK)
 	{
@@ -465,7 +463,7 @@ pre(nextStep < totalSteps; stepsTillRecalc == 0)
 	}
 	else
 	{
-		const uint64_t temp = (uint64_t)mp.delta.twoCsquaredTimesMmPerStepDivAK * (uint32_t)dsK;
+		const uint64_t temp = (mp.delta.twoCsquaredTimesMmPerStepDivA * (uint32_t)dsK)/K2;
 		// 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)
diff --git a/src/Movement/DriveMovement.h b/src/Movement/DriveMovement.h
index 19a600c6..8a1a2999 100644
--- a/src/Movement/DriveMovement.h
+++ b/src/Movement/DriveMovement.h
@@ -127,7 +127,7 @@ private:
 			int64_t dSquaredMinusAsquaredMinusBsquaredTimesKsquaredSsquared;
 			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;
-			uint32_t twoCsquaredTimesMmPerStepDivAK;	// this could be stored in the DDA if all towers use the same steps/mm
+			uint64_t twoCsquaredTimesMmPerStepDivA;		// 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()
 			uint32_t accelStopDsK;
@@ -136,10 +136,10 @@ private:
 		} delta;
 	} mp;
 
-	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)
-	static const int32_t Kc = 1024 * 1024;				// a power of 2 for scaling the Z movement fraction
+	static constexpr uint32_t NoStepTime = 0xFFFFFFFF;	// value to indicate that no further steps are needed when calculating the next step time
+	static constexpr uint32_t K1 = 1024;				// a power of 2 used to multiply the value mmPerStepTimesCdivtopSpeed to reduce rounding errors
+	static constexpr uint32_t K2 = 512;					// a power of 2 used in delta calculations to reduce rounding errors (but too large makes things worse)
+	static constexpr int32_t Kc = 1024 * 1024;			// a power of 2 for scaling the Z movement fraction
 };
 
 // Calculate and store the time since the start of the move when the next step for the specified DriveMovement is due.
diff --git a/src/Movement/Kinematics/CoreBaseKinematics.cpp b/src/Movement/Kinematics/CoreBaseKinematics.cpp
index 0ba9046c..9dbc9c68 100644
--- a/src/Movement/Kinematics/CoreBaseKinematics.cpp
+++ b/src/Movement/Kinematics/CoreBaseKinematics.cpp
@@ -33,12 +33,12 @@ bool CoreBaseKinematics::Configure(unsigned int mCode, GCodeBuffer& gb, StringRe
 				seen = true;
 			}
 		}
-		if (!seen && !gb.Seen('S'))
+		if (!seen && !gb.Seen('K'))
 		{
-			reply.printf("Printer mode is %s with axis factors", GetName());
+			reply.printf("Kinematics is %s with axis factors", GetName());
 			for (size_t axis = 0; axis < XYZ_AXES; ++axis)
 			{
-				reply.catf(" %c:%f", reprap.GetGCodes().GetAxisLetters()[axis], (double)axisFactors[axis]);
+				reply.catf(" %c:%.3f", reprap.GetGCodes().GetAxisLetters()[axis], (double)axisFactors[axis]);
 			}
 		}
 		return seen;
diff --git a/src/Movement/Kinematics/CoreXYUKinematics.cpp b/src/Movement/Kinematics/CoreXYUKinematics.cpp
index 060fa5dc..943a0e35 100644
--- a/src/Movement/Kinematics/CoreXYUKinematics.cpp
+++ b/src/Movement/Kinematics/CoreXYUKinematics.cpp
@@ -21,7 +21,6 @@ const char *CoreXYUKinematics::GetName(bool forStatusReport) const
 
 // Set the parameters from a M665, M666 or M669 command
 // Return true if we changed any parameters. Set 'error' true if there was an error, otherwise leave it alone.
-// This function is used for CoreXY and CoreXZ kinematics, but it overridden for CoreXYU kinematics
 bool CoreXYUKinematics::Configure(unsigned int mCode, GCodeBuffer& gb, StringRef& reply, bool& error) /*override*/
 {
 	if (mCode == 669)
@@ -35,20 +34,18 @@ bool CoreXYUKinematics::Configure(unsigned int mCode, GCodeBuffer& gb, StringRef
 				seen = true;
 			}
 		}
-		if (!seen && !gb.Seen('S'))
+		if (!seen && !gb.Seen('K'))
 		{
-			reply.printf("Printer mode is %s with axis factors", GetName(false));
+			reply.printf("Kinematics is %s with axis factors", GetName(false));
 			for (size_t axis = 0; axis < CoreXYU_AXES; ++axis)
 			{
-				reply.catf(" %c:%f", reprap.GetGCodes().GetAxisLetters()[axis], (double)axisFactors[axis]);
+				reply.catf(" %c:%3f", reprap.GetGCodes().GetAxisLetters()[axis], (double)axisFactors[axis]);
 			}
 		}
 		return seen;
 	}
-	else
-	{
-		return CoreBaseKinematics::Configure(mCode, gb, reply, error);
-	}
+
+	return CoreBaseKinematics::Configure(mCode, gb, reply, error);
 }
 
 // Convert Cartesian coordinates to motor coordinates
@@ -122,7 +119,7 @@ void CoreXYUKinematics::LimitSpeedAndAcceleration(DDA& dda, const float *normali
 
 	// Limit the UV motor accelerations
 	const float vecU = normalisedDirectionVector[3];
-	const float vecMaxUV = max<float>(fabs(vecU + vecY), fabs(vecY - vecY));		// pick the case for the motor that is working hardest
+	const float vecMaxUV = max<float>(fabs(vecU + vecY), fabs(vecU - vecY));		// pick the case for the motor that is working hardest
 	if (vecMaxUV > 0.01)															// avoid division by zero or near-zero
 	{
 		const Platform& platform = reprap.GetPlatform();
@@ -130,8 +127,8 @@ void CoreXYUKinematics::LimitSpeedAndAcceleration(DDA& dda, const float *normali
 		const float aY = platform.Acceleration(1);
 		const float vU = platform.MaxFeedrate(3);
 		const float vY = platform.MaxFeedrate(1);
-		const float aMax = (fabs(vecX) + fabs(vecY)) * aU * aY/(vecMaxUV * (fabs(vecX) * aY + fabs(vecY) * aU));
-		const float vMax = (fabs(vecX) + fabs(vecY)) * vU * vY/(vecMaxUV * (fabs(vecX) * vY + fabs(vecY) * vU));
+		const float aMax = (fabs(vecU) + fabs(vecY)) * aU * aY/(vecMaxUV * (fabs(vecU) * aY + fabs(vecY) * aU));
+		const float vMax = (fabs(vecU) + fabs(vecY)) * vU * vY/(vecMaxUV * (fabs(vecU) * vY + fabs(vecY) * vU));
 		dda.LimitSpeedAndAcceleration(vMax, aMax);
 	}
 }
diff --git a/src/Movement/Kinematics/CoreXYUVKinematics.cpp b/src/Movement/Kinematics/CoreXYUVKinematics.cpp
new file mode 100644
index 00000000..c12bbd44
--- /dev/null
+++ b/src/Movement/Kinematics/CoreXYUVKinematics.cpp
@@ -0,0 +1,137 @@
+/*
+ * CoreXYUVKinematics.cpp
+ *
+ *  Created on: 23 Nov 2017
+ *      Author: David
+ */
+
+#include "CoreXYUVKinematics.h"
+
+#include "GCodes/GCodes.h"
+#include "Movement/DDA.h"
+
+CoreXYUVKinematics::CoreXYUVKinematics() : CoreBaseKinematics(KinematicsType::coreXYUV)
+{
+}
+
+// Return the name of the current kinematics
+const char *CoreXYUVKinematics::GetName(bool forStatusReport) const
+{
+	return (forStatusReport) ? "coreXYUV" : "CoreXYUV";
+}
+
+// Set the parameters from a M665, M666 or M669 command
+// Return true if we changed any parameters. Set 'error' true if there was an error, otherwise leave it alone.
+bool CoreXYUVKinematics::Configure(unsigned int mCode, GCodeBuffer& gb, StringRef& reply, bool& error) /*override*/
+{
+	if (mCode == 669)
+	{
+		bool seen = false;
+		for (size_t axis = 0; axis < CoreXYUV_AXES; ++axis)
+		{
+			if (gb.Seen(reprap.GetGCodes().GetAxisLetters()[axis]))
+			{
+				axisFactors[axis] = gb.GetFValue();
+				seen = true;
+			}
+		}
+		if (!seen && !gb.Seen('K'))
+		{
+			reply.printf("Kinematics is %s with axis factors", GetName(false));
+			for (size_t axis = 0; axis < CoreXYUV_AXES; ++axis)
+			{
+				reply.catf(" %c:%.3f", reprap.GetGCodes().GetAxisLetters()[axis], (double)axisFactors[axis]);
+			}
+		}
+		return seen;
+	}
+
+	return CoreBaseKinematics::Configure(mCode, gb, reply, error);
+}
+
+// Convert Cartesian coordinates to motor coordinates
+bool CoreXYUVKinematics::CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool isCoordinated) const
+{
+	motorPos[X_AXIS] = lrintf(((machinePos[X_AXIS] * axisFactors[X_AXIS]) + (machinePos[Y_AXIS] * axisFactors[Y_AXIS])) * stepsPerMm[X_AXIS]);
+	motorPos[Y_AXIS] = lrintf(((machinePos[X_AXIS] * axisFactors[X_AXIS]) - (machinePos[Y_AXIS] * axisFactors[Y_AXIS])) * stepsPerMm[Y_AXIS]);
+	motorPos[Z_AXIS] = lrintf(machinePos[Z_AXIS] * stepsPerMm[Z_AXIS]);
+	motorPos[U_AXIS] = lrintf(((machinePos[U_AXIS] * axisFactors[U_AXIS]) + (machinePos[V_AXIS] * axisFactors[V_AXIS])) * stepsPerMm[U_AXIS]);
+	motorPos[V_AXIS] = lrintf(((machinePos[U_AXIS] * axisFactors[U_AXIS]) - (machinePos[V_AXIS] * axisFactors[V_AXIS])) * stepsPerMm[V_AXIS]);
+
+	for (size_t axis = CoreXYUV_AXES; axis < numVisibleAxes; ++axis)
+	{
+		motorPos[axis] = lrintf(machinePos[axis] * stepsPerMm[axis]);
+	}
+	return true;
+}
+
+// Convert motor coordinates to machine coordinates. Used after homing and after individual motor moves.
+void CoreXYUVKinematics::MotorStepsToCartesian(const int32_t motorPos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, float machinePos[]) const
+{
+	// Convert the main axes
+	const float xyStepsMm = stepsPerMm[X_AXIS] * stepsPerMm[Y_AXIS];
+	const float uvStepsMm = stepsPerMm[U_AXIS] * stepsPerMm[V_AXIS];
+	machinePos[X_AXIS] = ((motorPos[X_AXIS] * stepsPerMm[Y_AXIS]) + (motorPos[Y_AXIS] * stepsPerMm[X_AXIS]))
+								/(2 * axisFactors[X_AXIS] * xyStepsMm);
+	machinePos[Y_AXIS] = ((motorPos[X_AXIS] * stepsPerMm[Y_AXIS]) - (motorPos[Y_AXIS] * stepsPerMm[X_AXIS]))
+								/(2 * axisFactors[Y_AXIS] * xyStepsMm);
+	machinePos[U_AXIS] = ((motorPos[U_AXIS] * stepsPerMm[V_AXIS]) + (motorPos[V_AXIS] * stepsPerMm[U_AXIS]))
+								/(2 * axisFactors[V_AXIS] * uvStepsMm);
+	machinePos[V_AXIS] = ((motorPos[U_AXIS] * stepsPerMm[V_AXIS]) - (motorPos[V_AXIS] * stepsPerMm[U_AXIS]))
+								/(2 * axisFactors[V_AXIS] * uvStepsMm);
+
+	machinePos[Z_AXIS] = motorPos[Z_AXIS]/stepsPerMm[Z_AXIS];
+
+	// Convert any additional axes
+	for (size_t drive = CoreXYUV_AXES; drive < numVisibleAxes; ++drive)
+	{
+		machinePos[drive] = motorPos[drive]/stepsPerMm[drive];
+	}
+}
+
+// Return true if the specified endstop axis uses shared motors.
+// Used to determine whether to abort the whole move or just one motor when an endstop switch is triggered.
+bool CoreXYUVKinematics::DriveIsShared(size_t drive) const
+{
+	return drive == X_AXIS || drive == Y_AXIS || drive == U_AXIS || drive == V_AXIS;
+}
+
+// Limit the speed and acceleration of a move to values that the mechanics can handle.
+// The speeds along individual Cartesian axes have already been limited before this is called.
+void CoreXYUVKinematics::LimitSpeedAndAcceleration(DDA& dda, const float *normalisedDirectionVector) const
+{
+	const float vecX = normalisedDirectionVector[0];
+	const float vecY = normalisedDirectionVector[1];
+
+	// Limit the XY motor accelerations
+	const float vecMaxXY = max<float>(fabs(vecX + vecY), fabs(vecX - vecY));		// pick the case for the motor that is working hardest
+	if (vecMaxXY > 0.01)															// avoid division by zero or near-zero
+	{
+		const Platform& platform = reprap.GetPlatform();
+		const float aX = platform.Acceleration(0);
+		const float aY = platform.Acceleration(1);
+		const float vX = platform.MaxFeedrate(0);
+		const float vY = platform.MaxFeedrate(1);
+		const float aMax = (fabs(vecX) + fabs(vecY)) * aX * aY/(vecMaxXY * (fabs(vecX) * aY + fabs(vecY) * aX));
+		const float vMax = (fabs(vecX) + fabs(vecY)) * vX * vY/(vecMaxXY * (fabs(vecX) * vY + fabs(vecY) * vX));
+		dda.LimitSpeedAndAcceleration(vMax, aMax);
+	}
+
+	// Limit the UV motor accelerations
+	const float vecU = normalisedDirectionVector[3];
+	const float vecV = normalisedDirectionVector[4];
+	const float vecMaxUV = max<float>(fabs(vecU + vecV), fabs(vecU - vecV));		// pick the case for the motor that is working hardest
+	if (vecMaxUV > 0.01)															// avoid division by zero or near-zero
+	{
+		const Platform& platform = reprap.GetPlatform();
+		const float aU = platform.Acceleration(3);
+		const float aV = platform.Acceleration(4);
+		const float vU = platform.MaxFeedrate(3);
+		const float vV = platform.MaxFeedrate(4);
+		const float aMax = (fabs(vecU) + fabs(vecV)) * aU * aV/(vecMaxUV * (fabs(vecU) * aV + fabs(vecV) * aU));
+		const float vMax = (fabs(vecU) + fabs(vecV)) * vU * vV/(vecMaxUV * (fabs(vecU) * vV + fabs(vecV) * vU));
+		dda.LimitSpeedAndAcceleration(vMax, aMax);
+	}
+}
+
+// End
diff --git a/src/Movement/Kinematics/CoreXYUVKinematics.h b/src/Movement/Kinematics/CoreXYUVKinematics.h
new file mode 100644
index 00000000..181b1aa7
--- /dev/null
+++ b/src/Movement/Kinematics/CoreXYUVKinematics.h
@@ -0,0 +1,27 @@
+/*
+ * CoreXYUVKinematics.h
+ *
+ *  Created on: 23 Nov 2017
+ *      Author: David
+ */
+
+#ifndef SRC_MOVEMENT_KINEMATICS_COREXYUVKINEMATICS_H_
+#define SRC_MOVEMENT_KINEMATICS_COREXYUVKINEMATICS_H_
+
+#include "CoreBaseKinematics.h"
+
+class CoreXYUVKinematics : public CoreBaseKinematics
+{
+public:
+	CoreXYUVKinematics();
+
+	// Overridden base class functions. See Kinematics.h for descriptions.
+	const char *GetName(bool forStatusReport) const override;
+	bool Configure(unsigned int mCode, GCodeBuffer& gb, StringRef& reply, bool& error) override;
+	bool CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool isCoordinated) const override;
+	void MotorStepsToCartesian(const int32_t motorPos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, float machinePos[]) const override;
+	bool DriveIsShared(size_t drive) const override;
+	void LimitSpeedAndAcceleration(DDA& dda, const float *normalisedDirectionVector) const override;
+};
+
+#endif /* SRC_MOVEMENT_KINEMATICS_COREXYUVKINEMATICS_H_ */
diff --git a/src/Movement/Kinematics/Kinematics.cpp b/src/Movement/Kinematics/Kinematics.cpp
index d85f8d24..7bdd4fa4 100644
--- a/src/Movement/Kinematics/Kinematics.cpp
+++ b/src/Movement/Kinematics/Kinematics.cpp
@@ -5,7 +5,7 @@
  *      Author: David
  */
 
-#include <Movement/Kinematics/LinearDeltaKinematics.h>
+#include "LinearDeltaKinematics.h"
 #include "Kinematics.h"
 #include "CartesianKinematics.h"
 #include "CoreXYKinematics.h"
@@ -13,8 +13,10 @@
 #include "ScaraKinematics.h"
 #include "CoreXYUKinematics.h"
 #include "PolarKinematics.h"
+#include "CoreXYUVKinematics.h"
 #include "RepRap.h"
 #include "Platform.h"
+#include "GCodes/GCodeBuffer.h"
 
 const char *Kinematics::HomeAllFileName = "homeall.g";
 const char * const Kinematics::StandardHomingFileNames[] = AXES_("homex.g", "homey.g", "homez.g", "homeu.g", "homev.g", "homew.g", "homea.g", "homeb.g", "homec.g");
@@ -31,7 +33,10 @@ bool Kinematics::Configure(unsigned int mCode, GCodeBuffer& gb, StringRef& reply
 {
 	if (mCode == 669)
 	{
-		reply.printf("Current kinematics is %s", GetName());
+		if (!gb.Seen('K'))
+		{
+			reply.printf("Kinematics is %s", GetName());
+		}
 	}
 	else
 	{
@@ -142,6 +147,8 @@ const char* Kinematics::GetHomingFileName(AxesBitmap toBeHomed, AxesBitmap alrea
 		return new CoreXYUKinematics();
 	case KinematicsType::polar:
 		return new PolarKinematics();
+	case KinematicsType::coreXYUV:
+		return new CoreXYUVKinematics();
 	}
 }
 
diff --git a/src/Movement/Kinematics/Kinematics.h b/src/Movement/Kinematics/Kinematics.h
index fa9b2e25..fe9d4e52 100644
--- a/src/Movement/Kinematics/Kinematics.h
+++ b/src/Movement/Kinematics/Kinematics.h
@@ -28,6 +28,7 @@ enum class KinematicsType : uint8_t
 	coreXYU,
 	hangprinter,
 	polar,
+	coreXYUV,
 
 	unknown				// this one must be last!
 };
diff --git a/src/Movement/Kinematics/PolarKinematics.cpp b/src/Movement/Kinematics/PolarKinematics.cpp
index 8d1eb8b8..9b6ab2c5 100644
--- a/src/Movement/Kinematics/PolarKinematics.cpp
+++ b/src/Movement/Kinematics/PolarKinematics.cpp
@@ -74,9 +74,9 @@ bool PolarKinematics::Configure(unsigned int mCode, GCodeBuffer& gb, StringRef&
 		{
 			Recalc();
 		}
-		else
+		else if (!gb.Seen('K'))
 		{
-			reply.printf("Printer mode is Polar with radius %.1f to %.1fmm, homed radius %.1fmm, segments/sec %d, min. segment length %.2f",
+			reply.printf("Kinematics is Polar with radius %.1f to %.1fmm, homed radius %.1fmm, segments/sec %d, min. segment length %.2f",
 							(double)minRadius, (double)maxRadius, (double)homedRadius,
 							(int)segmentsPerSecond, (double)minSegmentLength);
 		}
diff --git a/src/Movement/Kinematics/ScaraKinematics.cpp b/src/Movement/Kinematics/ScaraKinematics.cpp
index cfda4a5c..78e12db9 100644
--- a/src/Movement/Kinematics/ScaraKinematics.cpp
+++ b/src/Movement/Kinematics/ScaraKinematics.cpp
@@ -189,9 +189,9 @@ bool ScaraKinematics::Configure(unsigned int mCode, GCodeBuffer& gb, StringRef&
 		{
 			Recalc();
 		}
-		else
+		else if (!gb.Seen('K'))
 		{
-			reply.printf("Printer mode is Scara with proximal arm %.2fmm range %.1f to %.1f" DEGREE_SYMBOL
+			reply.printf("Kinematics is Scara with proximal arm %.2fmm range %.1f to %.1f" DEGREE_SYMBOL
 							", distal arm %.2fmm range %.1f to %.1f" DEGREE_SYMBOL ", crosstalk %.1f:%.1f:%.1f, bed origin (%.1f, %.1f), segments/sec %d, min. segment length %.2f",
 							(double)proximalArmLength, (double)thetaLimits[0], (double)thetaLimits[1],
 							(double)distalArmLength, (double)psiLimits[0], (double)psiLimits[1],
diff --git a/src/Platform.cpp b/src/Platform.cpp
index 068f8cbc..787bd437 100644
--- a/src/Platform.cpp
+++ b/src/Platform.cpp
@@ -735,6 +735,7 @@ void Platform::InitZProbe()
 		break;
 
 	case 5:
+	case 8:
 	default:
 		AnalogInEnableChannel(zProbeAdcChannel, false);
 		pinMode(zProbePin, INPUT_PULLUP);
@@ -747,6 +748,13 @@ void Platform::InitZProbe()
 		pinMode(endStopPins[E0_AXIS + 1], INPUT);
 		pinMode(zProbeModulationPin, OUTPUT_LOW);		// we now set the modulation output high during probing only when using probe types 4 and higher
 		break;
+
+	case 7:
+		AnalogInEnableChannel(zProbeAdcChannel, false);
+		pinMode(zProbePin, INPUT_PULLUP);
+		pinMode(endStopPins[Z_AXIS], INPUT);
+		pinMode(zProbeModulationPin, OUTPUT_LOW);		// we now set the modulation output high during probing only when using probe types 4 and higher
+		break;
 	}
 }
 
@@ -755,7 +763,7 @@ void Platform::InitZProbe()
 int Platform::GetZProbeReading() const
 {
 	int zProbeVal = 0;			// initialised to avoid spurious compiler warning
-	if (zProbeOnFilter.IsValid() && zProbeOffFilter.IsValid())
+	if (zProbeType == 8 || (zProbeOnFilter.IsValid() && zProbeOffFilter.IsValid()))
 	{
 		switch (zProbeType)
 		{
@@ -764,6 +772,7 @@ int Platform::GetZProbeReading() const
 		case 4:		// Switch connected to E0 endstop input
 		case 5:		// Switch connected to Z probe input
 		case 6:		// Switch connected to E1 endstop input
+		case 7:		// Switch connected to Z endstop input
 			zProbeVal = (int) ((zProbeOnFilter.GetSum() + zProbeOffFilter.GetSum()) / (8 * Z_PROBE_AVERAGE_READINGS));
 			break;
 
@@ -773,6 +782,10 @@ int Platform::GetZProbeReading() const
 			zProbeVal = (int) (((int32_t) zProbeOnFilter.GetSum() - (int32_t) zProbeOffFilter.GetSum()) / (int)(4 * Z_PROBE_AVERAGE_READINGS));
 			break;
 
+		case 8:		// Switch connected to Z probe input, no filtering
+			zProbeVal = GetRawZProbeReading()/4;
+			break;
+
 		default:
 			return 0;
 		}
@@ -837,7 +850,7 @@ float Platform::GetZProbeTravelSpeed() const
 
 void Platform::SetZProbeType(int pt)
 {
-	zProbeType = (pt >= 0 && pt <= 6) ? pt : 0;
+	zProbeType = (pt >= 0 && pt <= 8) ? pt : 0;
 	InitZProbe();
 }
 
@@ -857,11 +870,13 @@ const ZProbeParameters& Platform::GetZProbeParameters(int32_t probeType) const
 	case 1:
 	case 2:
 	case 5:
+	case 8:
 		return irZProbeParameters;
 	case 3:
 		return alternateZProbeParameters;
 	case 4:
 	case 6:
+	case 7:
 	default:
 		return switchZProbeParameters;
 	}
@@ -874,6 +889,7 @@ void Platform::SetZProbeParameters(int32_t probeType, const ZProbeParameters& pa
 	case 1:
 	case 2:
 	case 5:
+	case 8:
 		irZProbeParameters = params;
 		break;
 
@@ -883,6 +899,7 @@ void Platform::SetZProbeParameters(int32_t probeType, const ZProbeParameters& pa
 
 	case 4:
 	case 6:
+	case 7:
 	default:
 		switchZProbeParameters = params;
 		break;
@@ -2257,18 +2274,36 @@ void Platform::DiagnosticTest(int d)
 
 	case (int)DiagnosticTestType::TimeSquareRoot:		// Show the square root calculation time. The displayed value is subject to interrupts.
 		{
-			const uint32_t num1 = 0x7265ac3d;
-			const uint32_t now1 = Platform::GetInterruptClocks();
-			const uint32_t num1a = isqrt64((uint64_t)num1 * num1);
-			const uint32_t tim1 = Platform::GetInterruptClocks() - now1;
-
-			const uint32_t num2 = 0x0000a4c5;
-			const uint32_t now2 = Platform::GetInterruptClocks();
-			const uint32_t num2a = isqrt64((uint64_t)num2 * num2);
-			const uint32_t tim2 = Platform::GetInterruptClocks() - now2;
-			MessageF(GenericMessage, "Square roots: 64-bit %.1fus %s, 32-bit %.1fus %s\n",
-					(double)(tim1 * 1000000)/DDA::stepClockRate, (num1a == num1) ? "ok" : "ERROR",
-							(double)(tim2 * 1000000)/DDA::stepClockRate, (num2a == num2) ? "ok" : "ERROR");
+			uint32_t tim1 = 0;
+			bool ok1 = true;
+			for (uint32_t i = 0; i < 100; ++i)
+			{
+				const uint32_t num1 = 0x7265ac3d + i;
+				const uint32_t now1 = Platform::GetInterruptClocks();
+				const uint32_t num1a = isqrt64((uint64_t)num1 * num1);
+				tim1 += Platform::GetInterruptClocks() - now1;
+				if (num1a != num1)
+				{
+					ok1 = false;
+				}
+			}
+
+			uint32_t tim2 = 0;
+			bool ok2 = true;
+			for (uint32_t i = 0; i < 100; ++i)
+			{
+				const uint32_t num2 = 0x0000a4c5 + i;
+				const uint32_t now2 = Platform::GetInterruptClocks();
+				const uint32_t num2a = isqrt64((uint64_t)num2 * num2);
+				tim2 += Platform::GetInterruptClocks() - now2;
+				if (num2a != num2)
+				{
+					ok2 = false;
+				}
+			}
+			MessageF(GenericMessage, "Square roots: 62-bit %.2fus %s, 32-bit %.2fus %s\n",
+					(double)(tim1 * 10000)/DDA::stepClockRate, (ok1) ? "ok" : "ERROR",
+							(double)(tim2 * 10000)/DDA::stepClockRate, (ok2) ? "ok" : "ERROR");
 		}
 		break;
 
@@ -2853,6 +2888,7 @@ bool Platform::SetDriverMicrostepping(size_t driver, int microsteps, int mode)
 // Set the microstepping, returning true if successful. All drivers for the same axis must use the same microstepping.
 bool Platform::SetMicrostepping(size_t drive, int microsteps, int mode)
 {
+	// Check that it is a valid microstepping number
 	const size_t numAxes = reprap.GetGCodes().GetTotalAxes();
 	if (drive < numAxes)
 	{
@@ -2958,7 +2994,7 @@ void Platform::SetDriverStepTiming(size_t driver, float microseconds)
 	}
 	else
 	{
-		const uint32_t clocks = (uint32_t)(((float)DDA::stepClockRate * microseconds/1000000.0) + 0.99);	// convert microseconds to step clocks, rounding up
+		const uint32_t clocks = (uint32_t)(((float)DDA::stepClockRate * microseconds * 0.000001) + 0.99);	// convert microseconds to step clocks, rounding up
 		if (clocks > slowDriverStepPulseClocks)
 		{
 			slowDriverStepPulseClocks = clocks;
diff --git a/src/Platform.h b/src/Platform.h
index 8c10fec9..62635f97 100644
--- a/src/Platform.h
+++ b/src/Platform.h
@@ -1114,6 +1114,11 @@ inline uint16_t Platform::GetRawZProbeReading() const
 {
 	switch (zProbeType)
 	{
+	case 1:
+	case 2:
+	case 3:
+		return min<uint16_t>(AnalogInReadChannel(zProbeAdcChannel), 4000);
+
 	case 4:
 		{
 			const bool b = IoPort::ReadPin(endStopPins[E0_AXIS]);
@@ -1121,6 +1126,7 @@ inline uint16_t Platform::GetRawZProbeReading() const
 		}
 
 	case 5:
+	case 8:
 		return (IoPort::ReadPin(zProbePin)) ? 4000 : 0;
 
 	case 6:
@@ -1129,8 +1135,14 @@ inline uint16_t Platform::GetRawZProbeReading() const
 			return (b) ? 4000 : 0;
 		}
 
+	case 7:
+		{
+			const bool b = IoPort::ReadPin(endStopPins[Z_AXIS]);
+			return (b) ? 4000 : 0;
+		}
+
 	default:
-		return min<uint16_t>(AnalogInReadChannel(zProbeAdcChannel), 4000);
+		return 4000;
 	}
 }
 
diff --git a/src/RepRapFirmware.h b/src/RepRapFirmware.h
index 11f3a7b5..33e8023d 100644
--- a/src/RepRapFirmware.h
+++ b/src/RepRapFirmware.h
@@ -195,21 +195,22 @@ template<typename BitmapType> BitmapType LongArrayToBitMap(const long *arr, size
 extern StringRef scratchString;
 
 // Common definitions used by more than one module
-const size_t XYZ_AXES = 3;										// The number of Cartesian axes
-const size_t X_AXIS = 0, Y_AXIS = 1, Z_AXIS = 2, E0_AXIS = 3;	// The indices of the Cartesian axes in drive arrays
-const size_t CoreXYU_AXES = 5;									// The number of axes in a CoreXYU machine
-const size_t U_AXIS = 3, V_AXIS = 4;							// The indices of the U and V motors in a CoreXYU machine (needed by Platform)
+constexpr size_t XYZ_AXES = 3;										// The number of Cartesian axes
+constexpr size_t X_AXIS = 0, Y_AXIS = 1, Z_AXIS = 2, E0_AXIS = 3;	// The indices of the Cartesian axes in drive arrays
+constexpr size_t CoreXYU_AXES = 5;									// The number of axes in a CoreXYU machine (there is a hidden V axis)
+constexpr size_t CoreXYUV_AXES = 5;									// The number of axes in a CoreXYUV machine
+constexpr size_t U_AXIS = 3, V_AXIS = 4;							// The indices of the U and V motors in a CoreXYU machine (needed by Platform)
 
 // Common conversion factors
-const float MinutesToSeconds = 60.0;
-const float SecondsToMinutes = 1.0/MinutesToSeconds;
-const float SecondsToMillis = 1000.0;
-const float MillisToSeconds = 0.001;
-const float InchToMm = 25.4;
-const float DegreesToRadians = PI/180.0;
-const float RadiansToDegrees = 180.0/PI;
-
-#define DEGREE_SYMBOL	"\xC2\xB0"								// degree-symbol encoding in UTF8
+constexpr float MinutesToSeconds = 60.0;
+constexpr float SecondsToMinutes = 1.0/MinutesToSeconds;
+constexpr float SecondsToMillis = 1000.0;
+constexpr float MillisToSeconds = 0.001;
+constexpr float InchToMm = 25.4;
+constexpr float DegreesToRadians = PI/180.0;
+constexpr float RadiansToDegrees = 180.0/PI;
+
+#define DEGREE_SYMBOL	"\xC2\xB0"									// degree-symbol encoding in UTF8
 
 // Type of an offset in a file
 typedef uint32_t FilePosition;
diff --git a/src/Version.h b/src/Version.h
index 3ff72052..d88d8b27 100644
--- a/src/Version.h
+++ b/src/Version.h
@@ -9,11 +9,11 @@
 #define SRC_VERSION_H_
 
 #ifndef VERSION
-# define VERSION "1.20beta8+1"
+# define VERSION "1.20beta10"
 #endif
 
 #ifndef DATE
-# define DATE "2017-11-18"
+# define DATE "2017-11-23"
 #endif
 
 #define AUTHORS "reprappro, dc42, chrishamm, t3p3, dnewman"