Work on multiple motion systems

11 files changed, 134 insertions, 62 deletions

diff --git a/src/GCodes/GCodes.cpp b/src/GCodes/GCodes.cpp
index 6d5d58e0..1d60b9a6 100644
--- a/src/GCodes/GCodes.cpp
+++ b/src/GCodes/GCodes.cpp
@@ -1555,7 +1555,6 @@ bool GCodes::LockAllMovementSystemsAndWaitForStandstill(GCodeBuffer& gb) noexcep
 // Lock movement and wait for pending moves to finish.
 // Return true if successful, false if we need to try again later.
 // As a side-effect it updates the user coordinates from the machine coordinates.
-
 bool GCodes::LockMovementSystemAndWaitForStandstill(GCodeBuffer& gb, unsigned int msNumber) noexcept
 {
 	// Lock movement to stop another source adding moves to the queue
@@ -1597,7 +1596,11 @@ bool GCodes::LockMovementSystemAndWaitForStandstill(GCodeBuffer& gb, unsigned in
 		// Get the position of all axes by combining positions from the queues
 		Move& move = reprap.GetMove();
 		const AxesBitmap ownedAxes = moveStates[msNumber].axesAndExtrudersOwned;
+
+		// Whenever we release axes, we must update lastKnownMachinePositions for those axes first so that whoever allocated them next gets the correct positions
 		move.GetPartialMachinePosition(lastKnownMachinePositions, ownedAxes, msNumber);
+		moveStates[msNumber].axesAndExtrudersOwned.Clear();
+
 		memcpyf(moveStates[msNumber].coords, lastKnownMachinePositions, MaxAxes);
 		move.InverseAxisAndBedTransform(lastKnownMachinePositions, moveStates[msNumber].currentTool);
 		UpdateUserPositionFromMachinePosition(gb, moveStates[msNumber]);
@@ -1605,7 +1608,6 @@ bool GCodes::LockMovementSystemAndWaitForStandstill(GCodeBuffer& gb, unsigned in
 
 		// Release the axes and extruders that this movement system owns
 		axesAndExtrudersMoved.ClearBits(ownedAxes);
-		moveStates[msNumber].axesAndExtrudersOwned.Clear();
 		ms.ownedAxisLetters.Clear();
 #else
 		UpdateCurrentUserPosition(gb);
@@ -1844,7 +1846,6 @@ bool GCodes::DoStraightMove(GCodeBuffer& gb, bool isCoordinated) THROWS(GCodeExc
 	axisLettersMentioned.ClearBits(ms.ownedAxisLetters);
 	if (axisLettersMentioned.IsNonEmpty())
 	{
-		//TODO problem! this ignores axis mapping and coordinate rotation!!!
 		AllocateAxisLetters(gb, ms, axisLettersMentioned);
 	}
 #endif
@@ -2259,8 +2260,20 @@ bool GCodes::DoArcMove(GCodeBuffer& gb, bool clockwise)
 	const unsigned int selectedPlane = gb.LatestMachineState().selectedPlane;
 	const unsigned int axis0 = (unsigned int[]){ X_AXIS, Z_AXIS, Y_AXIS }[selectedPlane];
 	const unsigned int axis1 = (axis0 + 1) % 3;
-
 	MovementState& ms = GetMovementState(gb);
+
+#if SUPPORT_ASYNC_MOVES
+	// We need to check for moving unowned axes right at the start in case we need to fetch axis positions before processing the command
+	ParameterLettersBitmap axisLettersMentioned = gb.AllParameters() & allAxisLetters;
+	axisLettersMentioned.SetBit(ParameterLetterToBitNumber('X') + axis0);		// add in the implicit axes
+	axisLettersMentioned.SetBit(ParameterLetterToBitNumber('X') + axis1);
+	axisLettersMentioned.ClearBits(ms.ownedAxisLetters);
+	if (axisLettersMentioned.IsNonEmpty())
+	{
+		AllocateAxisLetters(gb, ms, axisLettersMentioned);
+	}
+#endif
+
 	if (ms.moveFractionToSkip > 0.0)
 	{
 		ms.initialUserC0 = ms.restartInitialUserC0;
@@ -4696,8 +4709,6 @@ void GCodes:: SetMoveBufferDefaults(MovementState& ms) noexcept
 // Locking the same resource more than once only locks it once, there is no lock count held.
 bool GCodes::LockResource(const GCodeBuffer& gb, Resource r) noexcept
 {
-	TaskCriticalSectionLocker lock;
-
 	if (resourceOwners[r] == &gb)
 	{
 		return true;
@@ -4714,8 +4725,6 @@ bool GCodes::LockResource(const GCodeBuffer& gb, Resource r) noexcept
 // Grab the movement lock even if another GCode source has it
 void GCodes::GrabResource(const GCodeBuffer& gb, Resource r) noexcept
 {
-	TaskCriticalSectionLocker lock;
-
 	if (resourceOwners[r] != &gb)
 	{
 		// Note, we now leave the resource bit set in the original owning GCodeBuffer machine state
@@ -4784,8 +4793,6 @@ void GCodes::GrabMovement(const GCodeBuffer& gb) noexcept
 // Unlock the resource if we own it
 void GCodes::UnlockResource(const GCodeBuffer& gb, Resource r) noexcept
 {
-	TaskCriticalSectionLocker lock;
-
 	if (resourceOwners[r] == &gb)
 	{
 		// Note, we leave the bit set in previous stack levels! This is needed e.g. to allow M291 blocking messages to be used in homing files.
@@ -4797,17 +4804,16 @@ void GCodes::UnlockResource(const GCodeBuffer& gb, Resource r) noexcept
 // Release all locks, except those that were owned when the current macro was started
 void GCodes::UnlockAll(const GCodeBuffer& gb) noexcept
 {
-	TaskCriticalSectionLocker lock;
-
 	const GCodeMachineState * const mc = gb.LatestMachineState().GetPrevious();
 	const GCodeMachineState::ResourceBitmap resourcesToKeep = (mc == nullptr) ? GCodeMachineState::ResourceBitmap() : mc->lockedResources;
 	for (size_t i = 0; i < NumResources; ++i)
 	{
 		if (resourceOwners[i] == &gb && !resourcesToKeep.IsBitSet(i))
 		{
-			if (i >= MoveResourceBase && i < MoveResourceBase + NumResources)
+			if (i >= MoveResourceBase && i < MoveResourceBase + NumMovementSystems && mc == nullptr)
 			{
 				// In case homing was aborted because of an exception, we need to clear toBeHomed when releasing the movement lock
+				//debugPrintf("tbh %04x clearing\n", (unsigned int)toBeHomed.GetRaw());
 				toBeHomed.Clear();
 			}
 			resourceOwners[i] = nullptr;
@@ -4938,21 +4944,50 @@ void GCodes::AllocateAxes(const GCodeBuffer& gb, MovementState& ms, AxesBitmap a
 	ms.axesAndExtrudersOwned |= axes;
 }
 
-// Allocate additional axes by letter
+// Allocate additional axes by letter. The axis letters are as in the GCode, before we account for coordinate rotation and axis mapping.
+// We must clear out owned axis letters on a tool change, or when coordinate rotation is changed from zero to nonzero
 void GCodes::AllocateAxisLetters(const GCodeBuffer& gb, MovementState& ms, ParameterLettersBitmap axLetters) THROWS(GCodeException)
 {
+#if SUPPORT_COORDINATE_ROTATION
+	// If we are rotating coordinates then X implies Y and vice versa
+	if (g68Angle != 0.0)
+	{
+		if (axLetters.IsBitSet(ParameterLetterToBitNumber('X')))
+		{
+			axLetters.SetBit(ParameterLetterToBitNumber('Y'));
+		}
+		if (axLetters.IsBitSet(ParameterLetterToBitNumber('Y')))
+		{
+			axLetters.SetBit(ParameterLetterToBitNumber('X'));
+		}
+	}
+#endif
+
 	AxesBitmap newAxes;
 	for (size_t axis = 0; axis < numVisibleAxes; ++axis)
 	{
 		const char c = axisLetters[axis];
-		const unsigned int axisLetterBit = (c >= 'a') ? c - ('a' - 26) : c - 'A';
-		if (axLetters.IsBitSet(axisLetterBit))
+		const unsigned int axisLetterBitNumber = ParameterLetterToBitNumber(c);
+		if (axLetters.IsBitSet(axisLetterBitNumber))
 		{
-			newAxes.SetBit(axis);
+			if (axis == 0)			// axis 0 is always X
+			{
+				newAxes |= Tool::GetXAxes(ms.currentTool);
+			}
+			else if (axis == 1)		// axis 1 is always Y
+			{
+				newAxes |= Tool::GetYAxes(ms.currentTool);
+			}
+			else
+			{
+				newAxes.SetBit(axis);
+			}
 		}
 	}
+	newAxes &= ~ms.axesAndExtrudersOwned;
 	AllocateAxes(gb, ms, newAxes);
 	ms.ownedAxisLetters |= axLetters;
+	UpdateAllCoordinates(gb);
 }
 
 // Synchronise motion systems and update user coordinates.
@@ -5027,7 +5062,7 @@ bool GCodes::DoSync(GCodeBuffer& gb) noexcept
 	return rslt;
 }
 
-void GCodes::UpdateAllCoordinates(GCodeBuffer& gb) noexcept
+void GCodes::UpdateAllCoordinates(const GCodeBuffer& gb) noexcept
 {
 	const unsigned int msNumber = gb.GetOwnQueueNumber();
 	memcpyf(moveStates[msNumber].coords, lastKnownMachinePositions, MaxAxes);
diff --git a/src/GCodes/GCodes.h b/src/GCodes/GCodes.h
index c5118440..29df59ca 100644
--- a/src/GCodes/GCodes.h
+++ b/src/GCodes/GCodes.h
@@ -559,7 +559,7 @@ private:
 																											// allocate axes by letter
 	bool DoSync(GCodeBuffer& gb) noexcept;																	// sync with the other stream returning true if done, false if we need to wait for it
 	bool SyncWith(GCodeBuffer& thisGb, const GCodeBuffer& otherGb) noexcept;								// synchronise motion systems
-	void UpdateAllCoordinates(GCodeBuffer& gb) noexcept;
+	void UpdateAllCoordinates(const GCodeBuffer& gb) noexcept;
 #endif
 
 #if SUPPORT_COORDINATE_ROTATION
diff --git a/src/GCodes/GCodes2.cpp b/src/GCodes/GCodes2.cpp
index 8b0f75ed..7bdb66c4 100644
--- a/src/GCodes/GCodes2.cpp
+++ b/src/GCodes/GCodes2.cpp
@@ -2390,7 +2390,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx
 					}
 					else
 					{
-						reply.printf("Max speeds (%s)): ", (usingMmPerSec) ? "mm/sec" : "mm/min");
+						reply.printf("Max speeds (%s): ", (usingMmPerSec) ? "mm/sec" : "mm/min");
 						for (size_t axis = 0; axis < numTotalAxes; ++axis)
 						{
 							reply.catf("%c: %.1f, ", axisLetters[axis], (double)InverseConvertSpeedToMm(platform.MaxFeedrate(axis), usingMmPerSec));
diff --git a/src/GCodes/GCodes3.cpp b/src/GCodes/GCodes3.cpp
index 779043fb..f65dedb5 100644
--- a/src/GCodes/GCodes3.cpp
+++ b/src/GCodes/GCodes3.cpp
@@ -71,10 +71,21 @@ GCodeResult GCodes::SetPositions(GCodeBuffer& gb, const StringRef& reply) THROWS
 	}
 #endif
 
+	MovementState& ms = GetMovementState(gb);
+
+#if SUPPORT_ASYNC_MOVES
+	// Check for setting unowned axes before processing the command
+	ParameterLettersBitmap axisLettersMentioned = gb.AllParameters() & allAxisLetters;
+	axisLettersMentioned.ClearBits(ms.ownedAxisLetters);
+	if (axisLettersMentioned.IsNonEmpty())
+	{
+		AllocateAxisLetters(gb, ms, axisLettersMentioned);
+	}
+#endif
+
 	// Don't wait for the machine to stop if only extruder drives are being reset.
 	// This avoids blobs and seams when the gcode uses absolute E coordinates and periodically includes G92 E0.
 	AxesBitmap axesIncluded;
-	MovementState& ms = GetMovementState(gb);
 	for (size_t axis = 0; axis < numVisibleAxes; ++axis)
 	{
 		if (gb.Seen(axisLetters[axis]))
@@ -82,7 +93,7 @@ GCodeResult GCodes::SetPositions(GCodeBuffer& gb, const StringRef& reply) THROWS
 			const float axisValue = gb.GetFValue();
 			if (axesIncluded.IsEmpty())
 			{
-				if (!LockAllMovementSystemsAndWaitForStandstill(gb))	// lock movement and get current coordinates
+				if (!LockCurrentMovementSystemAndWaitForStandstill(gb))	// lock movement and get current coordinates
 				{
 					return GCodeResult::notFinished;
 				}
@@ -1537,10 +1548,13 @@ GCodeResult GCodes::HandleG68(GCodeBuffer& gb, const StringRef& reply) THROWS(GC
 		gb.MustSee('A', 'X');
 		centreX = gb.GetFValue();
 		gb.MustSee('B', 'Y');
-		centreY= gb.GetFValue();
+		centreY = gb.GetFValue();
 
 		g68Centre[0] = centreX + GetWorkplaceOffset(gb, 0);
 		g68Centre[1] = centreY + GetWorkplaceOffset(gb, 1);
+#if SUPPORT_ASYNC_MOVES
+		const float oldG68Angle = g68Angle;
+#endif
 		if (gb.Seen('I'))
 		{
 			g68Angle += angle;
@@ -1549,6 +1563,16 @@ GCodeResult GCodes::HandleG68(GCodeBuffer& gb, const StringRef& reply) THROWS(GC
 		{
 			g68Angle = angle;
 		}
+#if SUPPORT_ASYNC_MOVES
+		if (g68Angle != 0.0 && oldG68Angle == 0.0)
+		{
+			// We have just started doing coordinate rotation, so if we own axis letter X we need to own Y and vice versa
+			// Simplest is just to say we don't own either in the axis letters bitmap
+			MovementState& ms = GetMovementState(gb);
+			ms.ownedAxisLetters.ClearBit('X' - 'A');
+			ms.ownedAxisLetters.ClearBit('Y' - 'A');
+		}
+#endif
 		UpdateCurrentUserPosition(gb);
 	}
 	return GCodeResult::ok;
diff --git a/src/GCodes/GCodes4.cpp b/src/GCodes/GCodes4.cpp
index 4a9e169f..db6be018 100644
--- a/src/GCodes/GCodes4.cpp
+++ b/src/GCodes/GCodes4.cpp
@@ -310,6 +310,7 @@ void GCodes::RunStateMachine(GCodeBuffer& gb, const StringRef& reply) noexcept
 			// Test whether the previous homing move homed any axes
 			if (toBeHomed.Disjoint(axesHomed))
 			{
+				//debugPrintf("tbh %04x, ah %04x\n", (unsigned int)toBeHomed.GetRaw(), (unsigned int)axesHomed.GetRaw());
 				reply.copy("Failed to home axes ");
 				AppendAxes(reply, toBeHomed);
 				stateMachineResult = GCodeResult::error;
diff --git a/src/Movement/Move.cpp b/src/Movement/Move.cpp
index 26277d12..53ffd9b1 100644
--- a/src/Movement/Move.cpp
+++ b/src/Movement/Move.cpp
@@ -507,7 +507,7 @@ void Move::Diagnostics(MessageType mtype) noexcept
 #if 0	// debug only
 	String<StringLength256> scratchString;
 #else
-	String<StringLength50> scratchString;
+	String<StringLength100> scratchString;
 #endif
 	scratchString.copy(GetCompensationTypeString());
 
@@ -549,6 +549,10 @@ void Move::Diagnostics(MessageType mtype) noexcept
 	maxDelay = maxDelayIncrease = 0;
 #endif
 
+	scratchString.Clear();
+	StepTimer::Diagnostics(scratchString.GetRef());
+	p.MessageF(mtype, "%s\n", scratchString.c_str());
+
 	for (size_t i = 0; i < ARRAY_SIZE(rings); ++i)
 	{
 		rings[i].Diagnostics(mtype, i);
diff --git a/src/Movement/RawMove.cpp b/src/Movement/RawMove.cpp
index 9d78fa10..0df696f8 100644
--- a/src/Movement/RawMove.cpp
+++ b/src/Movement/RawMove.cpp
@@ -106,7 +106,7 @@ void MovementState::Diagnostics(MessageType mtype, unsigned int moveSystemNumber
 {
 	reprap.GetPlatform().MessageF(mtype, "Q%u segments left %u"
 #if SUPPORT_ASYNC_MOVES
-											", axes/extruders owned %03x"
+											", axes/extruders owned 0x%07x"
 #endif
 												"\n",
 													moveSystemNumber,
diff --git a/src/Movement/StepTimer.cpp b/src/Movement/StepTimer.cpp
index 5edb2b00..e14a8859 100644
--- a/src/Movement/StepTimer.cpp
+++ b/src/Movement/StepTimer.cpp
@@ -220,9 +220,6 @@ bool StepTimer::ScheduleTimerInterrupt(uint32_t tim) noexcept
 	while (StepTc->SYNCBUSY.reg & TC_SYNCBUSY_CC0) { }
 	StepTc->INTFLAG.reg = TC_INTFLAG_MC0;							// clear any existing compare match
 	StepTc->INTENSET.reg = TC_INTFLAG_MC0;
-#elif defined(__LPC17xx__)
-	STEP_TC->MR[0] = tim;											// set MR0 compare register
-	STEP_TC->MCR |= (1u<<SBIT_MR0I);									// enable interrupt on MR0 match
 #else
 	STEP_TC->TC_CHANNEL[STEP_TC_CHAN].TC_RB = tim;					// set up the compare register
 	(void)STEP_TC->TC_CHANNEL[STEP_TC_CHAN].TC_SR;					// read the status register, which clears the status bits and any pending interrupt
@@ -353,7 +350,7 @@ void StepTimer::DisableTimerInterrupt() noexcept
 #endif
 
 // The guts of the ISR
-/*static*/ void StepTimer::Interrupt() noexcept
+/*static*/ inline void StepTimer::Interrupt() noexcept
 {
 	StepTimer * tmr = pendingList;
 	if (tmr != nullptr)
@@ -442,7 +439,8 @@ bool StepTimer::ScheduleCallbackFromIsr() noexcept
 	}
 
 	// Optimise the common case i.e. no other timer is pending
-	if (pendingList == nullptr)
+	StepTimer *tmr = pendingList;			// capture volatile variable
+	if (tmr == nullptr)
 	{
 		if (ScheduleTimerInterrupt(whenDue))
 		{
@@ -450,32 +448,33 @@ bool StepTimer::ScheduleCallbackFromIsr() noexcept
 		}
 		next = nullptr;
 		pendingList = this;
-		active = true;
-		return false;
 	}
-
-	// Another timer is already pending
-	const Ticks now = GetTimerTicks();
-	const int32_t howSoon = (int32_t)(whenDue - now);
-	StepTimer** ppst = const_cast<StepTimer**>(&pendingList);
-	if (howSoon < (int32_t)((*ppst)->whenDue - now))
+	else
 	{
-		// This one is due earlier than the first existing one
-		if (ScheduleTimerInterrupt(whenDue))
+		// Another timer is already pending
+		const Ticks now = GetTimerTicks();
+		const int32_t howSoon = (int32_t)(whenDue - now);
+		if (howSoon < (int32_t)(tmr->whenDue - now))
 		{
-			return true;
+			// This one is due earlier than the first existing one
+			if (ScheduleTimerInterrupt(whenDue))
+			{
+				return true;
+			}
+			next = tmr;
+			pendingList = this;
 		}
-	}
-	else
-	{
-		while (*ppst != nullptr && (int32_t)((*ppst)->whenDue - now) < howSoon)
+		else
 		{
-			ppst = &((*ppst)->next);
+			while (tmr->next != nullptr && (int32_t)(tmr->next->whenDue - now) < howSoon)
+			{
+				tmr = tmr->next;
+			}
+			next = tmr->next;
+			tmr->next = this;
 		}
 	}
 
-	next = *ppst;
-	*ppst = this;
 	active = true;
 	return false;
 }
@@ -515,16 +514,17 @@ extern "C" uint32_t StepTimerGetTimerTicks() noexcept
 	return StepTimer::GetTimerTicks();
 }
 
-#if SUPPORT_REMOTE_COMMANDS
-
-// Remote diagnostics
 /*static*/ void StepTimer::Diagnostics(const StringRef& reply) noexcept
 {
-	reply.lcatf("Peak sync jitter %" PRIi32 "/%" PRIi32 ", peak Rx sync delay %" PRIu32 ", resyncs %u/%u, ", peakNegJitter, peakPosJitter, peakReceiveDelay, numTimeoutResyncs, numJitterResyncs);
-	gotJitter = false;
-	numTimeoutResyncs = numJitterResyncs = 0;
-	peakReceiveDelay = 0;
-
+#if SUPPORT_REMOTE_COMMANDS
+	if (CanInterface::InExpansionMode())
+	{
+		reply.lcatf("Peak sync jitter %" PRIi32 "/%" PRIi32 ", peak Rx sync delay %" PRIu32 ", resyncs %u/%u, ", peakNegJitter, peakPosJitter, peakReceiveDelay, numTimeoutResyncs, numJitterResyncs);
+		gotJitter = false;
+		numTimeoutResyncs = numJitterResyncs = 0;
+		peakReceiveDelay = 0;
+	}
+#endif
 	StepTimer *pst = pendingList;
 	if (pst == nullptr)
 	{
@@ -536,13 +536,15 @@ extern "C" uint32_t StepTimerGetTimerTicks() noexcept
 					pst->whenDue - GetTimerTicks(),
 # if SAME5x
 					((StepTc->INTENSET.reg & TC_INTFLAG_MC0) == 0)
-# elif SAME70
+# elif SAME70 || SAM4E || SAM4S
 					((STEP_TC->TC_CHANNEL[STEP_TC_CHAN].TC_IER & TC_IER_CPBS) == 0)
 # endif
 						? "disabled" : "enabled");
 # if SAME5x
 		if (StepTc->CC[0].reg != pst->whenDue)
-# elif SAME70
+# elif SAM4E
+		if (STEP_TC->TC_CHANNEL[STEP_TC_CHAN].TC_RB != pst->whenDue)
+# elif SAME70 || SAM4S
 		if (STEP_TC->TC_CHANNEL[STEP_TC_CHAN].TC_RB != (uint16_t)pst->whenDue)
 # endif
 		{
@@ -551,6 +553,4 @@ extern "C" uint32_t StepTimerGetTimerTicks() noexcept
 	}
 }
 
-#endif
-
 // End
diff --git a/src/Movement/StepTimer.h b/src/Movement/StepTimer.h
index f0080cd7..ae36797e 100644
--- a/src/Movement/StepTimer.h
+++ b/src/Movement/StepTimer.h
@@ -63,6 +63,9 @@ public:
 	// ISR called from StepTimer
 	static void Interrupt() noexcept;
 
+	// Append diagnostics to reply string
+	static void Diagnostics(const StringRef& reply) noexcept;
+
 	static constexpr uint32_t MinInterruptInterval = 6;							// Minimum interval between step timer interrupts, in step clocks; about 6us
 
 #if SUPPORT_REMOTE_COMMANDS
@@ -71,9 +74,7 @@ public:
 	static uint32_t ConvertToLocalTime(uint32_t masterTime) noexcept { return masterTime + localTimeOffset; }
 	static uint32_t ConvertToMasterTime(uint32_t localTime) noexcept { return localTime - localTimeOffset; }
 	static uint32_t GetMasterTime() noexcept { return ConvertToMasterTime(GetTimerTicks()); }
-
 	static bool IsSynced() noexcept;
-	static void Diagnostics(const StringRef& reply) noexcept;
 
 	static constexpr uint32_t MinSyncInterval = 2000;							// maximum interval in milliseconds between sync messages for us to remain synced
 																				// increased from 1000 because of workaround we added for bad Tx time stamps on SAME70
diff --git a/src/Platform/Platform.cpp b/src/Platform/Platform.cpp
index 5dc74632..72ebaa4f 100644
--- a/src/Platform/Platform.cpp
+++ b/src/Platform/Platform.cpp
@@ -5143,6 +5143,8 @@ GCodeResult Platform::EutProcessM569Point7(const CanMessageGeneric& msg, const S
 		String<StringLength20> portName;
 		parser.GetStringParam('C', portName.GetRef());
 		return GetGCodeResultFromSuccess(brakePorts[drive].AssignPort(portName.c_str(), reply, PinUsedBy::gpout, PinAccess::write0));
+		//TODO use the following instead when we track the enable state of each driver individually
+		//return GetGCodeResultFromSuccess(brakePorts[drive].AssignPort(portName.c_str(), reply, PinUsedBy::gpout, (driverDisabled[drive]) ? PinAccess::write0 : PinAccess::write1));
 	}
 
 	reply.printf("Driver %u.%u uses brake port ", CanInterface::GetCanAddress(), drive);
diff --git a/src/RepRapFirmware.h b/src/RepRapFirmware.h
index a994f734..1f1e1a9d 100644
--- a/src/RepRapFirmware.h
+++ b/src/RepRapFirmware.h
@@ -382,6 +382,11 @@ union LaserPwmOrIoBits
 };
 #endif
 
+// Find the bit number corresponding to a parameter letter
+constexpr unsigned int ParameterLetterToBitNumber(char c) noexcept
+{
+	return (c <= 'Z') ? c - 'A' : c - ('a' - 26);
+}
 // Debugging support
 extern "C" void debugPrintf(const char* fmt, ...) noexcept __attribute__ ((format (printf, 1, 2)));
 #define DEBUG_HERE do { debugPrintf("At " __FILE__ " line %d\n", __LINE__); delay(50); } while (false)