Implemented fast-then-slow G30 probing

3 files changed, 20 insertions, 19 deletions

diff --git a/src/GCodes/GCodes.cpp b/src/GCodes/GCodes.cpp
index 30dd0825..8191e510 100644
--- a/src/GCodes/GCodes.cpp
+++ b/src/GCodes/GCodes.cpp
@@ -406,9 +406,9 @@ void GCodes::CheckFinishedRunningConfigFile(GCodeBuffer& gb) noexcept
 }
 
 // Set up to do the first of a possibly multi-tap probe
-void GCodes::InitialiseTaps() noexcept
+void GCodes::InitialiseTaps(bool fastThenSlow) noexcept
 {
-	tapsDone = 0;
+	tapsDone = (fastThenSlow) ? -1 : 0;
 	g30zHeightErrorSum = 0.0;
 	g30zHeightErrorLowestDiff = 1000.0;
 }
@@ -2883,7 +2883,7 @@ GCodeResult GCodes::ExecuteG30(GCodeBuffer& gb, const StringRef& reply)
 		// G30 without P parameter. This probes the current location starting from the current position.
 		// If S=-1 it just reports the stopped height, else it resets the Z origin.
 		const auto zp = SetZProbeNumber(gb);					// may throw, so do this before changing the state
-		InitialiseTaps();
+		InitialiseTaps(zp->HasTwoProbingSpeeds());
 		gb.SetState(GCodeState::probingAtPoint2a);
 		if (zp->GetProbeType() != ZProbeType::blTouch)
 		{
diff --git a/src/GCodes/GCodes.h b/src/GCodes/GCodes.h
index fc01d839..7be1107d 100644
--- a/src/GCodes/GCodes.h
+++ b/src/GCodes/GCodes.h
@@ -421,7 +421,7 @@ private:
 	GCodeResult ProbeGrid(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeException);	// Start probing the grid, returning true if we didn't because of an error
 	ReadLockedPointer<ZProbe> SetZProbeNumber(GCodeBuffer& gb) THROWS(GCodeException);		// Set up currentZProbeNumber and return the probe
 	GCodeResult ExecuteG30(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeException);	// Probes at a given position - see the comment at the head of the function itself
-	void InitialiseTaps() noexcept;												// Set up to do the first of a possibly multi-tap probe
+	void InitialiseTaps(bool fastThenSlow) noexcept;								// Set up to do the first of a possibly multi-tap probe
 	void SetBedEquationWithProbe(int sParam, const StringRef& reply);			// Probes a series of points and sets the bed equation
 
 	GCodeResult ConfigureTrigger(GCodeBuffer& gb, const StringRef& reply);		// Handle M581
@@ -440,7 +440,7 @@ private:
 	bool WriteConfigOverrideHeader(FileStore *f) const noexcept;				// Write the config-override header
 #endif
 
-	void CheckFinishedRunningConfigFile(GCodeBuffer& gb) noexcept;						// Copy the feed rate etc. from the daemon to the input channels
+	void CheckFinishedRunningConfigFile(GCodeBuffer& gb) noexcept;				// Copy the feed rate etc. from the daemon to the input channels
 
 	MessageType GetMessageBoxDevice(GCodeBuffer& gb) const;						// Decide which device to display a message box on
 	void DoManualProbe(GCodeBuffer&, const char *message, const char *title, const AxesBitmap); // Do manual probe in arbitrary direction
@@ -601,7 +601,7 @@ private:
 	bool doingManualBedProbe;					// true if we are waiting for the user to jog the nozzle until it touches the bed
 	bool hadProbingError;						// true if there was an error probing the last point
 	bool zDatumSetByProbing;					// true if the Z position was last set by probing, not by an endstop switch or by G92
-	uint8_t tapsDone;							// how many times we tapped the current point
+	int8_t tapsDone;							// how many times we tapped the current point
 	uint8_t currentZProbeNumber;				// which Z probe a G29 or G30 command is using
 
 	// Simulation and print time
diff --git a/src/GCodes/GCodes4.cpp b/src/GCodes/GCodes4.cpp
index 3d1f0455..a3ac2c51 100644
--- a/src/GCodes/GCodes4.cpp
+++ b/src/GCodes/GCodes4.cpp
@@ -566,10 +566,7 @@ void GCodes::RunStateMachine(GCodeBuffer& gb, const StringRef& reply) noexcept
 					moveBuffer.feedRate = zp->GetTravelSpeed();
 					NewMoveAvailable(1);
 
-					tapsDone = 0;
-					g30zHeightErrorSum = 0.0;
-					g30zHeightErrorLowestDiff = 1000.0;
-
+					InitialiseTaps(false);
 					gb.AdvanceState();
 				}
 				else
@@ -648,7 +645,7 @@ void GCodes::RunStateMachine(GCodeBuffer& gb, const StringRef& reply) noexcept
 					moveBuffer.checkEndstops = true;
 					moveBuffer.reduceAcceleration = true;
 					moveBuffer.coords[Z_AXIS] = -zp->GetDiveHeight() + zp->GetActualTriggerHeight();
-					moveBuffer.feedRate = zp->GetProbingSpeed();
+					moveBuffer.feedRate = zp->GetProbingSpeed(tapsDone);
 					NewMoveAvailable(1);
 					gb.AdvanceState();
 				}
@@ -680,6 +677,7 @@ void GCodes::RunStateMachine(GCodeBuffer& gb, const StringRef& reply) noexcept
 					break;
 				}
 
+				// Grid probing never does an additional fast tap, so we can always include this tap in the average
 				g30zHeightError = moveBuffer.coords[Z_AXIS] - zp->GetActualTriggerHeight();
 				g30zHeightErrorSum += g30zHeightError;
 			}
@@ -725,7 +723,7 @@ void GCodes::RunStateMachine(GCodeBuffer& gb, const StringRef& reply) noexcept
 						acceptReading = true;
 					}
 				}
-				else if (tapsDone == zp->GetMaxTaps())
+				else if (tapsDone == (int)zp->GetMaxTaps())
 				{
 					g30zHeightError = g30zHeightErrorSum/tapsDone;
 					acceptReading = true;
@@ -737,7 +735,7 @@ void GCodes::RunStateMachine(GCodeBuffer& gb, const StringRef& reply) noexcept
 				reprap.GetMove().AccessHeightMap().SetGridHeight(gridAxis0index, gridAxis1index, g30zHeightError);
 				gb.AdvanceState();
 			}
-			else if (tapsDone < zp->GetMaxTaps())
+			else if (tapsDone < (int)zp->GetMaxTaps())
 			{
 				// Tap again
 				lastProbedTime = millis();
@@ -863,7 +861,7 @@ void GCodes::RunStateMachine(GCodeBuffer& gb, const StringRef& reply) noexcept
 			moveBuffer.feedRate = zp->GetTravelSpeed();
 			NewMoveAvailable(1);
 
-			InitialiseTaps();
+			InitialiseTaps(false);
 			gb.AdvanceState();
 		}
 		break;
@@ -939,7 +937,7 @@ void GCodes::RunStateMachine(GCodeBuffer& gb, const StringRef& reply) noexcept
 					moveBuffer.coords[Z_AXIS] = (IsAxisHomed(Z_AXIS))
 												? platform.AxisMinimum(Z_AXIS) - zp->GetDiveHeight() + zp->GetActualTriggerHeight()	// Z axis has been homed, so no point in going very far
 												: -1.1 * platform.AxisTotalLength(Z_AXIS);	// Z axis not homed yet, so treat this as a homing move
-					moveBuffer.feedRate = zp->GetProbingSpeed();
+					moveBuffer.feedRate = zp->GetProbingSpeed(tapsDone);
 					NewMoveAvailable(1);
 					gb.AdvanceState();
 				}
@@ -979,8 +977,11 @@ void GCodes::RunStateMachine(GCodeBuffer& gb, const StringRef& reply) noexcept
 					reprap.GetMove().GetCurrentMachinePosition(m, false);		// get height without bed compensation
 					const float g30zStoppedHeight = m[Z_AXIS] - g30HValue;					// save for later
 					zp->SetLastStoppedHeight(g30zStoppedHeight);
-					g30zHeightError = g30zStoppedHeight - zp->GetActualTriggerHeight();
-					g30zHeightErrorSum += g30zHeightError;
+					if (tapsDone > 0)
+					{
+						g30zHeightError = g30zStoppedHeight - zp->GetActualTriggerHeight();
+						g30zHeightErrorSum += g30zHeightError;
+					}
 				}
 			}
 
@@ -1058,7 +1059,7 @@ void GCodes::RunStateMachine(GCodeBuffer& gb, const StringRef& reply) noexcept
 
 			if (!acceptReading)
 			{
-				if (tapsDone < zp->GetMaxTaps())
+				if (tapsDone < (int)zp->GetMaxTaps())
 				{
 					// Tap again
 					g30PrevHeightError = g30zHeightError;
@@ -1227,7 +1228,7 @@ void GCodes::RunStateMachine(GCodeBuffer& gb, const StringRef& reply) noexcept
 						moveBuffer.checkEndstops = true;
 						moveBuffer.reduceAcceleration = true;
 						sps.SetCoordsToTarget(moveBuffer.coords);
-						moveBuffer.feedRate = zp->GetProbingSpeed();
+						moveBuffer.feedRate = zp->GetProbingSpeed(0);
 						NewMoveAvailable(1);
 						gb.AdvanceState();
 					}