Started implementing PREALLOCATE_TOOL_AXES

10 files changed, 112 insertions, 43 deletions

diff --git a/src/GCodes/GCodeBuffer/GCodeBuffer.h b/src/GCodes/GCodeBuffer/GCodeBuffer.h
index 1d653a63..d18685cb 100644
--- a/src/GCodes/GCodeBuffer/GCodeBuffer.h
+++ b/src/GCodes/GCodeBuffer/GCodeBuffer.h
@@ -80,7 +80,7 @@ public:
 	bool SeenAny(ParameterLettersBitmap bm) const noexcept							// Return true if any of the parameter letters in the bitmap were seen
 		{ return AllParameters().Intersects(bm); }
 	bool SeenAny(const char *s) const noexcept										// Return true if any of the parameter letters in the string were seen
-		{ return SeenAny(ParameterLettersBitmap(ParametersToBitmap(s))); }
+		{ return SeenAny(ParameterLettersToBitmap(s)); }
 
 	float GetFValue() THROWS(GCodeException) SPEED_CRITICAL;						// Get a float after a key letter
 	float GetPositiveFValue() THROWS(GCodeException) SPEED_CRITICAL;				// Get a float after a key letter and check that it is greater than zero
@@ -302,13 +302,6 @@ protected:
 	DECLARE_OBJECT_MODEL
 
 private:
-	// Convert a string of uppercase parameter letters to a bit map
-	static inline constexpr uint32_t ParametersToBitmap(const char *s) noexcept
-	{
-		return (*s == 0) ? 0
-			: (*s >= 'A' && *s <= 'Z') ? ((uint32_t)1 << (*s - 'A')) | ParametersToBitmap(s + 1)
-				: ParametersToBitmap(s + 1);
-	}
 
 #if SUPPORT_OBJECT_MODEL
 	const char *GetStateText() const noexcept;
diff --git a/src/GCodes/GCodes.cpp b/src/GCodes/GCodes.cpp
index db68c372..93c4c96a 100644
--- a/src/GCodes/GCodes.cpp
+++ b/src/GCodes/GCodes.cpp
@@ -175,7 +175,7 @@ void GCodes::Init() noexcept
 	axisLetters[1] = 'Y';
 	axisLetters[2] = 'Z';
 #if SUPPORT_ASYNC_MOVES
-	allAxisLetters = ParameterLettersBitmap((1u << ('X'-'A')) | (1u << ('Y'-'A')) | (1u << ('Z'-'A')));
+	allAxisLetters = ParameterLettersBitmap(ParameterLetterToBitNumber('X') | ParameterLetterToBitNumber('Y') | ParameterLetterToBitNumber('Z'));
 #endif
 
 	numExtruders = 0;
@@ -1690,7 +1690,7 @@ void GCodes::LoadExtrusionAndFeedrateFromGCode(GCodeBuffer& gb, MovementState& m
 	ms.hasPositiveExtrusion = false;
 	ms.moveStartVirtualExtruderPosition = ms.latestVirtualExtruderPosition;	// save this before we update it
 	ExtrudersBitmap extrudersMoving;
-#if SUPPORT_ASYNC_MOVES
+#if SUPPORT_ASYNC_MOVES && !PREALLOCATE_TOOL_AXES
 	AxesBitmap logicalDrivesMoving;
 #endif
 
@@ -1764,7 +1764,7 @@ void GCodes::LoadExtrusionAndFeedrateFromGCode(GCodeBuffer& gb, MovementState& m
 																		? extrusionAmount * extrusionFactors[extruder]
 																		: extrusionAmount;
 						extrudersMoving.SetBit(extruder);
-#if SUPPORT_ASYNC_MOVES
+#if SUPPORT_ASYNC_MOVES && !PREALLOCATE_TOOL_AXES
 						logicalDrivesMoving.SetBit(ExtruderToLogicalDrive(extruder));
 #endif
 					}
@@ -1806,7 +1806,7 @@ void GCodes::LoadExtrusionAndFeedrateFromGCode(GCodeBuffer& gb, MovementState& m
 																			? extrusionAmount * extrusionFactors[extruder]
 																			: extrusionAmount;
 							extrudersMoving.SetBit(extruder);
-#if SUPPORT_ASYNC_MOVES
+#if SUPPORT_ASYNC_MOVES && !PREALLOCATE_TOOL_AXES
 							logicalDrivesMoving.SetBit(ExtruderToLogicalDrive(extruder));
 #endif
 						}
@@ -1820,7 +1820,7 @@ void GCodes::LoadExtrusionAndFeedrateFromGCode(GCodeBuffer& gb, MovementState& m
 		}
 	}
 
-#if SUPPORT_ASYNC_MOVES
+#if SUPPORT_ASYNC_MOVES && !PREALLOCATE_TOOL_AXES
 	AllocateAxes(gb, ms, logicalDrivesMoving, ParameterLettersBitmap());
 #endif
 
@@ -1870,7 +1870,6 @@ bool GCodes::DoStraightMove(GCodeBuffer& gb, bool isCoordinated) THROWS(GCodeExc
 		}
 	}
 
-
 #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;
@@ -3917,17 +3916,37 @@ GCodeResult GCodes::RetractFilament(GCodeBuffer& gb, bool retract) THROWS(GCodeE
 				// Set up the retract move
 				if (currentTool != nullptr && currentTool->DriveCount() != 0)
 				{
+#if SUPPORT_ASYNC_MOVES && !PREALLOCATE_TOOL_AXES
 					AxesBitmap drivesMoving;
+#endif
 					for (size_t i = 0; i < currentTool->DriveCount(); ++i)
 					{
 						const size_t logicalDrive = ExtruderToLogicalDrive(currentTool->GetDrive(i));
 						ms.coords[logicalDrive] = -currentTool->GetRetractLength();
+#if SUPPORT_ASYNC_MOVES && !PREALLOCATE_TOOL_AXES
 						drivesMoving.SetBit(logicalDrive);
+#endif
 					}
 					ms.feedRate = currentTool->GetRetractSpeed() * currentTool->DriveCount();
 					ms.canPauseAfter = false;			// don't pause after a retraction because that could cause too much retraction
 #if SUPPORT_ASYNC_MOVES
-					AllocateAxes(gb, ms, drivesMoving, ParameterLettersBitmap());
+					// If there is any Z hop then make sure we own the Z axis too
+# if PREALLOCATE_TOOL_AXES
+					if (currentTool->GetRetractHop() > 0.0 && !ms.GetOwnedAxisLetters().IsBitSet(ParameterLetterToBitNumber('Z')))
+					{
+						AllocateAxes(gb, ms, AxesBitmap::MakeFromBits(Z_AXIS), ParameterLetterToBitmap('Z'));
+					}
+# else
+					if (currentTool->GetRetractHop() > 0.0 && !ms.GetOwnedAxisLetters().IsBitSet(ParameterLetterToBitNumber('Z')))
+					{
+						drivesMoving.SetBit(Z_AXIS);
+						AllocateAxes(gb, ms, drivesMoving, ParameterLetterToBitmap('Z'));
+					}
+					else
+					{
+						AllocateAxes(gb, ms, drivesMoving, ParameterLettersBitmap());
+					}
+# endif
 #endif
 					NewSingleSegmentMoveAvailable(ms);
 				}
@@ -3945,7 +3964,7 @@ GCodeResult GCodes::RetractFilament(GCodeBuffer& gb, bool retract) THROWS(GCodeE
 				ms.canPauseAfter = false;			// don't pause in the middle of a command
 				ms.linearAxesMentioned = true;
 #if SUPPORT_ASYNC_MOVES
-				AllocateAxes(gb, ms, AxesBitmap::MakeFromBits(Z_AXIS), ParameterLettersBitmap('Z'));
+				AllocateAxes(gb, ms, AxesBitmap::MakeFromBits(Z_AXIS), ParameterLetterToBitmap('Z'));
 #endif
 				NewSingleSegmentMoveAvailable(ms);
 				gb.SetState(GCodeState::doingFirmwareUnRetraction);
@@ -3955,16 +3974,20 @@ GCodeResult GCodes::RetractFilament(GCodeBuffer& gb, bool retract) THROWS(GCodeE
 				// No retract hop, so just un-retract
 				if (currentTool != nullptr && currentTool->DriveCount() != 0)
 				{
+#if SUPPORT_ASYNC_MOVES && !PREALLOCATE_TOOL_AXES
 					AxesBitmap drivesMoving;
+#endif
 					for (size_t i = 0; i < ms.currentTool->DriveCount(); ++i)
 					{
 						const size_t logicalDrive = ExtruderToLogicalDrive(currentTool->GetDrive(i));
 						ms.coords[logicalDrive] = currentTool->GetRetractLength() + currentTool->GetRetractExtra();
+#if SUPPORT_ASYNC_MOVES && !PREALLOCATE_TOOL_AXES
 						drivesMoving.SetBit(logicalDrive);
+#endif
 					}
 					ms.feedRate = currentTool->GetUnRetractSpeed() * currentTool->DriveCount();
 					ms.canPauseAfter = true;
-#if SUPPORT_ASYNC_MOVES
+#if SUPPORT_ASYNC_MOVES && !PREALLOCATE_TOOL_AXES
 					AllocateAxes(gb, ms, drivesMoving, ParameterLettersBitmap());
 #endif
 					NewSingleSegmentMoveAvailable(ms);
@@ -4695,9 +4718,8 @@ OutputBuffer *GCodes::GenerateJsonStatusResponse(int type, int seq, ResponseSour
 }
 
 // Initiate a tool change. Caller has already checked that the correct tool isn't loaded and set up ms.newToolNumber.
-void GCodes::StartToolChange(GCodeBuffer& gb, uint8_t param) noexcept
+void GCodes::StartToolChange(GCodeBuffer& gb, MovementState& ms, uint8_t param) noexcept
 {
-	MovementState& ms = GetMovementState(gb);
 	ms.toolChangeParam = (IsSimulating()) ? 0 : param;
 	gb.SetState(GCodeState::toolChange0);
 }
@@ -4928,11 +4950,11 @@ const MovementState& GCodes::GetCurrentMovementState(const ObjectExplorationCont
 	const GCodeBuffer *gb = context.GetGCodeBuffer();
 	if (gb == nullptr)
 	{
-#if HAS_NETWORKING
+# if HAS_NETWORKING
 		gb = HttpGCode();				// assume the request came from the network
-#else
+# else
 		return moveStates[0];
-#endif
+# endif
 	}
 	return GetConstMovementState(*gb);
 }
@@ -4953,7 +4975,7 @@ void GCodes::AllocateAxes(const GCodeBuffer& gb, MovementState& ms, AxesBitmap a
 // 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 SUPPORT_COORDINATE_ROTATION
 	// If we are rotating coordinates then X implies Y and vice versa
 	if (g68Angle != 0.0)
 	{
@@ -4966,7 +4988,7 @@ void GCodes::AllocateAxisLetters(const GCodeBuffer& gb, MovementState& ms, Param
 			axLetters.SetBit(ParameterLetterToBitNumber('X'));
 		}
 	}
-#endif
+# endif
 
 	AxesBitmap newAxes;
 	for (size_t axis = 0; axis < numVisibleAxes; ++axis)
@@ -4975,6 +4997,7 @@ void GCodes::AllocateAxisLetters(const GCodeBuffer& gb, MovementState& ms, Param
 		const unsigned int axisLetterBitNumber = ParameterLetterToBitNumber(c);
 		if (axLetters.IsBitSet(axisLetterBitNumber))
 		{
+# if !PREALLOCATE_TOOL_AXES			// if we pre-allocated the tool X and Y axes, then if we have X or Y here then no tool is selected
 			if (axis == 0)			// axis 0 is always X
 			{
 				newAxes |= Tool::GetXAxes(ms.currentTool);
@@ -4984,6 +5007,7 @@ void GCodes::AllocateAxisLetters(const GCodeBuffer& gb, MovementState& ms, Param
 				newAxes |= Tool::GetYAxes(ms.currentTool);
 			}
 			else
+# endif
 			{
 				newAxes.SetBit(axis);
 			}
diff --git a/src/GCodes/GCodes.h b/src/GCodes/GCodes.h
index 084fe9e1..606eb36e 100644
--- a/src/GCodes/GCodes.h
+++ b/src/GCodes/GCodes.h
@@ -226,7 +226,7 @@ public:
 
 	void SavePosition(const GCodeBuffer& gb, unsigned int restorePointNumber) noexcept
 		pre(restorePointNumber < NumTotalRestorePoints);							// Save position etc. to a restore point
-	void StartToolChange(GCodeBuffer& gb, uint8_t param) noexcept;
+	void StartToolChange(GCodeBuffer& gb, MovementState& ms, uint8_t param) noexcept;
 
 	unsigned int GetPrimaryWorkplaceCoordinateSystemNumber() const noexcept { return GetPrimaryMovementState().currentCoordinateSystem + 1; }
 
diff --git a/src/GCodes/GCodes2.cpp b/src/GCodes/GCodes2.cpp
index 7bdb66c4..d9fac220 100644
--- a/src/GCodes/GCodes2.cpp
+++ b/src/GCodes/GCodes2.cpp
@@ -4736,12 +4736,13 @@ bool GCodes::HandleTcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx
 	}
 
 #if SUPPORT_ASYNC_MOVES
-		if (!gb.Executing())
-		{
-			UnlockAll(gb);
-			HandleReply(gb, GCodeResult::ok, "");
-			return true;
-		}
+	if (!gb.Executing())
+	{
+		// This is not the active MS, so ignore the command
+		UnlockAll(gb);
+		HandleReply(gb, GCodeResult::ok, "");
+		return true;
+	}
 #endif
 
 	bool seen = false;
@@ -4794,15 +4795,15 @@ bool GCodes::HandleTcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx
 		{
 			// Don't do the tool change, just remember which one we are supposed to use in 'newToolNumber'
 		}
-		else if (ms.GetCurrentToolNumber() != toolNum)		// if old and new are the same we no longer follow the sequence. User can deselect and then reselect the tool if he wants the macros run.
+		else if (ms.GetCurrentToolNumber() == toolNum)		// if old and new are the same we no longer follow the sequence. User can deselect and then reselect the tool if he wants the macros run.
 		{
-			StartToolChange(gb, (gb.Seen('P')) ? gb.GetUIValue() : DefaultToolChangeParam);
-			return true;									// proceeding with state machine, so don't unlock or send a reply
+			// Even though the tool is selected, we may have turned it off e.g. when upgrading the WiFi firmware or following a heater fault that has been cleared. So make sure the tool heaters are on.
+			ms.SelectTool(toolNum, IsSimulating());
 		}
 		else
 		{
-			// Even though the tool is selected, we may have turned it off e.g. when upgrading the WiFi firmware or following a heater fault that has been cleared. So make sure the tool heaters are on.
-			ms.SelectTool(toolNum, IsSimulating());
+			StartToolChange(gb, ms, (gb.Seen('P')) ? gb.GetUIValue() : DefaultToolChangeParam);
+			return true;									// proceeding with state machine, so don't unlock or send a reply
 		}
 	}
 	else
diff --git a/src/GCodes/GCodes4.cpp b/src/GCodes/GCodes4.cpp
index db6be018..547fc3fb 100644
--- a/src/GCodes/GCodes4.cpp
+++ b/src/GCodes/GCodes4.cpp
@@ -1347,8 +1347,11 @@ void GCodes::RunStateMachine(GCodeBuffer& gb, const StringRef& reply) noexcept
 		// We just did the retraction part of a firmware retraction, now we need to do the Z hop
 		if (ms.segmentsLeft == 0)
 		{
-			if (ms.currentTool != nullptr)
+			if (ms.currentTool != nullptr)					// this should always be true
 			{
+#if SUPPORT_ASYNC_MOVES
+				// We already allocated the Z axis to this MS when we began the retraction, so no need to do it here
+#endif
 				SetMoveBufferDefaults(ms);
 				ms.movementTool = ms.currentTool;
 				reprap.GetMove().GetCurrentUserPosition(ms.coords, 0, ms.currentTool);
diff --git a/src/Movement/RawMove.cpp b/src/Movement/RawMove.cpp
index 91162dd7..2b30d5c0 100644
--- a/src/Movement/RawMove.cpp
+++ b/src/Movement/RawMove.cpp
@@ -216,7 +216,7 @@ void MovementState::ResumePrinting(GCodeBuffer& gb) noexcept
 	reprap.GetGCodes().SavePosition(gb, ResumeObjectRestorePointNumber);	// save the position we should be at for the start of the next move
 	if (GetCurrentToolNumber() != newToolNumber)							// if the wrong tool is loaded
 	{
-		reprap.GetGCodes().StartToolChange(gb, DefaultToolChangeParam);
+		reprap.GetGCodes().StartToolChange(gb, *this, DefaultToolChangeParam);
 	}
 }
 
@@ -226,6 +226,8 @@ void MovementState::InitObjectCancellation() noexcept
 	currentObjectCancelled = printingJustResumed = false;
 }
 
+#if SUPPORT_ASYNC_MOVES
+
 // When releasing axes we must also release the corresponding axis letters, because they serve as a cache
 void MovementState::ReleaseOwnedAxesAndExtruders() noexcept
 {
@@ -233,8 +235,6 @@ void MovementState::ReleaseOwnedAxesAndExtruders() noexcept
 	ownedAxisLetters.Clear();
 }
 
-#if SUPPORT_ASYNC_MOVES
-
 void AsyncMove::SetDefaults() noexcept
 {
 	for (float& f : movements)
diff --git a/src/Movement/RawMove.h b/src/Movement/RawMove.h
index e3f72656..429d6b35 100644
--- a/src/Movement/RawMove.h
+++ b/src/Movement/RawMove.h
@@ -79,11 +79,14 @@ constexpr size_t ResumeObjectRestorePointNumber = NumVisibleRestorePoints + 1;
 class MovementState : public RawMove
 {
 public:
+
+#if SUPPORT_ASYNC_MOVES
 	AxesBitmap GetAxesAndExtrudersOwned() const noexcept { return axesAndExtrudersOwned; }		// Get the axes and extruders that this movement system owns
 	ParameterLettersBitmap GetOwnedAxisLetters() const noexcept { return ownedAxisLetters; }	// Get the letters denoting axes that this movement system owns
 	void AllocateAxes(AxesBitmap axes, ParameterLettersBitmap axisLetters) noexcept;
 	void ReleaseOwnedAxesAndExtruders() noexcept;
 	void ReleaseAxisLetter(char letter) noexcept;											// stop claiming that we own an axis letter (if we do) but don't release the associated axis
+#endif
 
 	float GetProportionDone() const noexcept;												// get the proportion of this whole move that has been completed, based on segmentsLeft and totalSegments
 	void Reset() noexcept;
@@ -178,10 +181,14 @@ public:
 	bool printingJustResumed;										// true if we have just restarted printing
 
 private:
+#if SUPPORT_ASYNC_MOVES
 	AxesBitmap axesAndExtrudersOwned;								// axes and extruders that this movement system has moved since the last sync
 	ParameterLettersBitmap ownedAxisLetters;						// letters denoting axes that this movement system owns
+#endif
 };
 
+#if SUPPORT_ASYNC_MOVES
+
 inline void MovementState::AllocateAxes(AxesBitmap axes, ParameterLettersBitmap axisLetters) noexcept
 {
 	axesAndExtrudersOwned |= axes;
@@ -194,8 +201,6 @@ inline void MovementState::ReleaseAxisLetter(char letter) noexcept
 	ownedAxisLetters.ClearBit(ParameterLetterToBitNumber(letter));
 }
 
-#if SUPPORT_ASYNC_MOVES
-
 struct AsyncMove
 {
 	float movements[MaxAxesPlusExtruders];
diff --git a/src/RepRapFirmware.h b/src/RepRapFirmware.h
index 1f1e1a9d..2c0133f4 100644
--- a/src/RepRapFirmware.h
+++ b/src/RepRapFirmware.h
@@ -111,6 +111,11 @@ enum class PinUsedBy : uint8_t
 static_assert(MinVisibleAxes <= MinAxes);
 static_assert(NumNamedPins <= 255 || sizeof(LogicalPin) > 1, "Need 16-bit logical pin numbers");
 
+// Motion system choices, temporary until we finalise the choices of behaviour
+#if SUPPORT_ASYNC_MOVES
+# define PREALLOCATE_TOOL_AXES		1		// if set, when a tool is selected we preallocate and hang on to its X/Y axes and extruders
+#endif
+
 #if SUPPORT_CAN_EXPANSION
 
 # include <CanId.h>
@@ -383,10 +388,30 @@ union LaserPwmOrIoBits
 #endif
 
 // Find the bit number corresponding to a parameter letter
-constexpr unsigned int ParameterLetterToBitNumber(char c) noexcept
+inline constexpr unsigned int ParameterLetterToBitNumber(char c) noexcept
 {
 	return (c <= 'Z') ? c - 'A' : c - ('a' - 26);
 }
+
+// Make a ParameterLettersBitmap representing a single letter
+inline constexpr ParameterLettersBitmap ParameterLetterToBitmap(char c) noexcept
+{
+	return ParameterLettersBitmap::MakeFromBits(ParameterLetterToBitNumber(c));
+}
+
+// Convert a string of parameter letters to a collection of bits. Normally used with constant strings, so recursive is OK.
+inline constexpr uint32_t ParameterLettersToBits(const char *s) noexcept
+{
+	return (*s == 0) ? 0
+		: (1u << ParameterLetterToBitNumber(*s)) | ParameterLettersToBits(s + 1);
+}
+
+// Convert a string of parameter letters to a bitmap
+inline constexpr ParameterLettersBitmap ParameterLettersToBitmap(const char *s) noexcept
+{
+	return ParameterLettersBitmap(ParameterLettersToBits(s));
+}
+
 // 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)
diff --git a/src/Tools/Tool.cpp b/src/Tools/Tool.cpp
index 244cd06c..1399b73a 100644
--- a/src/Tools/Tool.cpp
+++ b/src/Tools/Tool.cpp
@@ -958,4 +958,18 @@ void Tool::StopFeedForward() const noexcept
 	}
 }
 
+#if SUPPORT_ASYNC_MOVES && PREALLOCATE_TOOL_AXES
+
+AxesBitmap Tool::GetXYAxesAndExtruders() const noexcept
+{
+	AxesBitmap ret = axisMapping[0] | axisMapping[1];
+	for (size_t i = 0; i < driveCount; ++i)
+	{
+		ret.SetBit(ExtruderToLogicalDrive(drives[i]));
+	}
+	return ret;
+}
+
+#endif
+
 // End
diff --git a/src/Tools/Tool.h b/src/Tools/Tool.h
index fb79b733..4f3d30bf 100644
--- a/src/Tools/Tool.h
+++ b/src/Tools/Tool.h
@@ -146,6 +146,10 @@ public:
 	void Standby() noexcept;
 	void UpdateExtruderAndHeaterCount(uint16_t &numExtruders, uint16_t &numHeaters) const noexcept;
 
+#if SUPPORT_ASYNC_MOVES && PREALLOCATE_TOOL_AXES
+	AxesBitmap GetXYAxesAndExtruders() const noexcept;
+#endif
+
 	static ReadWriteLock toolListLock;
 
 protected: