Z probe and trigger improvements

M558 with P parameter always creates a new probe (fix for a bug when P
was used with the same type as before)
Added Z probe trigger height 2nd order temperaure coefficient
Added Z probe last stop height to object model, per Z probe
Added R2 option to M581 command to trigger only if not printing from SD
card

3 files changed, 64 insertions, 49 deletions

diff --git a/src/Endstops/EndstopsManager.cpp b/src/Endstops/EndstopsManager.cpp
index 253961c6..7e128895 100644
--- a/src/Endstops/EndstopsManager.cpp
+++ b/src/Endstops/EndstopsManager.cpp
@@ -614,21 +614,11 @@ GCodeResult EndstopsManager::HandleM558(GCodeBuffer& gb, const StringRef &reply)
 	}
 
 	const bool seenPort = gb.Seen('C');
-	const bool needNewProbe =  (existingProbe == nullptr)
-							|| (   seenType
-								&& probeType != (uint32_t)existingProbe->GetProbeType()
-								&& (   probeType == (uint32_t)ZProbeType::zMotorStall
-									|| probeType == (uint32_t)ZProbeType::none
-									|| existingProbe->GetProbeType() == ZProbeType::zMotorStall
-									|| existingProbe->GetProbeType() == ZProbeType::none
-								   )
-								)
-							|| (seenPort && existingProbe->GetProbeType() != ZProbeType::zMotorStall && existingProbe->GetProbeType() != ZProbeType::none);
-
 	bool seen = seenType || seenPort;
-	if (needNewProbe)
+
+	if (seen)									// we need a new probe if we have seen either P or C
 	{
-		if (!seenType)
+		if (!seenType)							// if a port is specified then the type must be specified too
 		{
 			reply.copy("Missing Z probe type number");
 			return GCodeResult::error;
@@ -688,7 +678,7 @@ GCodeResult EndstopsManager::HandleM558(GCodeBuffer& gb, const StringRef &reply)
 		return rslt;
 	}
 
-	// If we get get then there is an existing probe and we just need to change its configuration
+	// If we get here then there is an existing probe of the correct type and we just need to change its configuration
 	return zProbes[probeNumber]->Configure(gb, reply, seen);
 }
 
diff --git a/src/Endstops/ZProbe.cpp b/src/Endstops/ZProbe.cpp
index 557b32f2..bd9eaf79 100644
--- a/src/Endstops/ZProbe.cpp
+++ b/src/Endstops/ZProbe.cpp
@@ -44,28 +44,38 @@ constexpr ObjectModelArrayDescriptor ZProbe::valueArrayDescriptor =
 				}
 };
 
+constexpr ObjectModelArrayDescriptor ZProbe::temperatureCoefficientsArrayDescriptor =
+{
+	nullptr,
+	[] (const ObjectModel *self, const ObjectExplorationContext&) noexcept -> size_t { return ARRAY_SIZE(ZProbe::temperatureCoefficients); },
+	[] (const ObjectModel *self, ObjectExplorationContext& context) noexcept -> ExpressionValue
+				{ return ExpressionValue(((const ZProbe*)self)->temperatureCoefficients[context.GetLastIndex()], 5); }
+};
+
 constexpr ObjectModelTableEntry ZProbe::objectModelTable[] =
 {
 	// Within each group, these entries must be in alphabetical order
 	// 0. Probe members
-	{ "calibrationTemperature",	OBJECT_MODEL_FUNC(self->calibTemperature, 1), 				ObjectModelEntryFlags::none },
-	{ "deployedByUser",			OBJECT_MODEL_FUNC(self->isDeployedByUser), 					ObjectModelEntryFlags::none },
-	{ "disablesHeaters",		OBJECT_MODEL_FUNC((bool)self->misc.parts.turnHeatersOff), 	ObjectModelEntryFlags::none },
-	{ "diveHeight",				OBJECT_MODEL_FUNC(self->diveHeight, 1), 					ObjectModelEntryFlags::none },
-	{ "maxProbeCount",			OBJECT_MODEL_FUNC((int32_t)self->misc.parts.maxTaps), 		ObjectModelEntryFlags::none },
-	{ "offsets",				OBJECT_MODEL_FUNC_NOSELF(&offsetsArrayDescriptor), 			ObjectModelEntryFlags::none },
-	{ "recoveryTime",			OBJECT_MODEL_FUNC(self->recoveryTime, 1), 					ObjectModelEntryFlags::none },
-	{ "speed",					OBJECT_MODEL_FUNC(self->probeSpeed, 1), 					ObjectModelEntryFlags::none },
-	{ "temperatureCoefficient",	OBJECT_MODEL_FUNC(self->temperatureCoefficient, 3), 		ObjectModelEntryFlags::none },
-	{ "threshold",				OBJECT_MODEL_FUNC((int32_t)self->adcValue), 				ObjectModelEntryFlags::none },
-	{ "tolerance",				OBJECT_MODEL_FUNC(self->tolerance, 3), 						ObjectModelEntryFlags::none },
-	{ "travelSpeed",			OBJECT_MODEL_FUNC(self->travelSpeed, 1), 					ObjectModelEntryFlags::none },
-	{ "triggerHeight",			OBJECT_MODEL_FUNC(self->triggerHeight, 3), 					ObjectModelEntryFlags::none },
-	{ "type",					OBJECT_MODEL_FUNC((int32_t)self->type), 					ObjectModelEntryFlags::none },
-	{ "value",					OBJECT_MODEL_FUNC_NOSELF(&valueArrayDescriptor), 			ObjectModelEntryFlags::live },
+	{ "calibrationTemperature",		OBJECT_MODEL_FUNC(self->calibTemperature, 1), 						ObjectModelEntryFlags::none },
+	{ "deployedByUser",				OBJECT_MODEL_FUNC(self->isDeployedByUser), 							ObjectModelEntryFlags::none },
+	{ "disablesHeaters",			OBJECT_MODEL_FUNC((bool)self->misc.parts.turnHeatersOff), 			ObjectModelEntryFlags::none },
+	{ "diveHeight",					OBJECT_MODEL_FUNC(self->diveHeight, 1), 							ObjectModelEntryFlags::none },
+	{ "lastStopHeight",				OBJECT_MODEL_FUNC(self->lastStopHeight, 3), 						ObjectModelEntryFlags::none },
+	{ "maxProbeCount",				OBJECT_MODEL_FUNC((int32_t)self->misc.parts.maxTaps), 				ObjectModelEntryFlags::none },
+	{ "offsets",					OBJECT_MODEL_FUNC_NOSELF(&offsetsArrayDescriptor), 					ObjectModelEntryFlags::none },
+	{ "recoveryTime",				OBJECT_MODEL_FUNC(self->recoveryTime, 1), 							ObjectModelEntryFlags::none },
+	{ "speed",						OBJECT_MODEL_FUNC(self->probeSpeed, 1), 							ObjectModelEntryFlags::none },
+	{ "temperatureCoefficient",		OBJECT_MODEL_FUNC(self->temperatureCoefficients[0], 5), 			ObjectModelEntryFlags::none },
+	{ "temperatureCoefficients",	OBJECT_MODEL_FUNC_NOSELF(&temperatureCoefficientsArrayDescriptor), 	ObjectModelEntryFlags::none },
+	{ "threshold",					OBJECT_MODEL_FUNC((int32_t)self->adcValue), 						ObjectModelEntryFlags::none },
+	{ "tolerance",					OBJECT_MODEL_FUNC(self->tolerance, 3), 								ObjectModelEntryFlags::none },
+	{ "travelSpeed",				OBJECT_MODEL_FUNC(self->travelSpeed, 1), 							ObjectModelEntryFlags::none },
+	{ "triggerHeight",				OBJECT_MODEL_FUNC(self->triggerHeight, 3), 							ObjectModelEntryFlags::none },
+	{ "type",						OBJECT_MODEL_FUNC((int32_t)self->type), 							ObjectModelEntryFlags::none },
+	{ "value",						OBJECT_MODEL_FUNC_NOSELF(&valueArrayDescriptor), 					ObjectModelEntryFlags::live },
 };
 
-constexpr uint8_t ZProbe::objectModelTableDescriptor[] = { 1, 15 };
+constexpr uint8_t ZProbe::objectModelTableDescriptor[] = { 1, 17 };
 
 DEFINE_GET_OBJECT_MODEL_TABLE(ZProbe)
 
@@ -83,7 +93,10 @@ void ZProbe::SetDefaults() noexcept
 	xOffset = yOffset = 0.0;
 	triggerHeight = DefaultZProbeTriggerHeight;
 	calibTemperature = DefaultZProbeTemperature;
-	temperatureCoefficient = 0.0;
+	for (float& tc : temperatureCoefficients)
+	{
+		tc = 0.0;
+	}
 	diveHeight = DefaultZDive;
 	probeSpeed = DefaultProbingSpeed;
 	travelSpeed = DefaultZProbeTravelSpeed;
@@ -103,7 +116,8 @@ float ZProbe::GetActualTriggerHeight() const noexcept
 		const float temperature = reprap.GetHeat().GetSensorTemperature(sensor, err);
 		if (err == TemperatureError::success)
 		{
-			return ((temperature - calibTemperature) * temperatureCoefficient) + triggerHeight;
+			const float dt = temperature - calibTemperature;
+			return (dt * temperatureCoefficients[0]) + (fsquare(dt) * temperatureCoefficients[1]) + triggerHeight;
 		}
 	}
 	return triggerHeight;
@@ -250,8 +264,10 @@ GCodeResult ZProbe::HandleG31(GCodeBuffer& gb, const StringRef& reply) THROWS(GC
 	if (gb.Seen('C'))
 	{
 		seen = true;
-		float newTemperatureCoefficient = gb.GetFValue();
-		float newCalibTemperature = DefaultZProbeTemperature;
+		for (float& tc : temperatureCoefficients)
+		{
+			tc = 0.0;
+		}
 
 		TemperatureError terr;
 		const float currentTemperature = reprap.GetHeat().GetSensorTemperature(newSensor, terr);
@@ -260,7 +276,12 @@ GCodeResult ZProbe::HandleG31(GCodeBuffer& gb, const StringRef& reply) THROWS(GC
 			reply.printf("Cannot set a temperature coefficient with invalid sensor number %d", newSensor);
 			return GCodeResult::error;
 		}
-		else if (gb.Seen('S'))
+
+		size_t numValues = ARRAY_SIZE(temperatureCoefficients);
+		gb.GetFloatArray(temperatureCoefficients, numValues, false);
+		float newCalibTemperature = DefaultZProbeTemperature;
+
+		if (gb.Seen('S'))
 		{
 			newCalibTemperature = gb.GetFValue();
 		}
@@ -284,7 +305,6 @@ GCodeResult ZProbe::HandleG31(GCodeBuffer& gb, const StringRef& reply) THROWS(GC
 			err = GCodeResult::error;
 		}
 		gb.StopTimer();
-		temperatureCoefficient = newTemperatureCoefficient;
 		calibTemperature = newCalibTemperature;
 	}
 
@@ -319,9 +339,10 @@ GCodeResult ZProbe::HandleG31(GCodeBuffer& gb, const StringRef& reply) THROWS(GC
 			reply.catf(" (%d)", v1);
 		}
 		reply.catf(", threshold %d, trigger height %.3f", adcValue, (double)triggerHeight);
-		if (temperatureCoefficient != 0.0)
+		if (temperatureCoefficients[0] != 0.0)
 		{
-			reply.catf(" at %.1f" DEGREE_SYMBOL "C, temperature coefficient %.1f/" DEGREE_SYMBOL "C", (double)calibTemperature, (double)temperatureCoefficient);
+			reply.catf(" at %.1f" DEGREE_SYMBOL "C, temperature coefficients [%.1f/" DEGREE_SYMBOL "C, %.1f/" DEGREE_SYMBOL "C^2]",
+						(double)calibTemperature, (double)temperatureCoefficients[0], (double)temperatureCoefficients[1]);
 		}
 		reply.catf(", offsets X%.1f Y%.1f", (double)xOffset, (double)yOffset);
 	}
diff --git a/src/Endstops/ZProbe.h b/src/Endstops/ZProbe.h
index e36086c6..844975cf 100644
--- a/src/Endstops/ZProbe.h
+++ b/src/Endstops/ZProbe.h
@@ -45,6 +45,7 @@ public:
 	float GetTravelSpeed() const noexcept { return travelSpeed; }
 	float GetRecoveryTime() const noexcept { return recoveryTime; }
 	float GetTolerance() const noexcept { return tolerance; }
+	float GetLastStoppedHeight() const noexcept { return lastStopHeight; }
 	bool GetTurnHeatersOff() const noexcept { return misc.parts.turnHeatersOff; }
 	bool GetSaveToConfigOverride() const noexcept { return misc.parts.saveToConfigOverride; }
 	int GetAdcValue() const noexcept { return adcValue; }
@@ -58,6 +59,7 @@ public:
 	void SetTriggerHeight(float height) noexcept { triggerHeight = height; }
 	void SetSaveToConfigOverride() noexcept { misc.parts.saveToConfigOverride = true; }
 	void SetDeployedByUser(bool b) noexcept { isDeployedByUser = b; }
+	void SetLastStoppedHeight(float h) noexcept { lastStopHeight = h; }
 
 #if HAS_MASS_STORAGE
 	bool WriteParameters(FileStore *f, unsigned int probeNumber) const noexcept;
@@ -69,6 +71,7 @@ protected:
 	DECLARE_OBJECT_MODEL
 	OBJECT_MODEL_ARRAY(offsets)
 	OBJECT_MODEL_ARRAY(value)
+	OBJECT_MODEL_ARRAY(temperatureCoefficients)
 
 	uint8_t number;
 	ZProbeType type;
@@ -85,17 +88,18 @@ protected:
 		} parts;
 		uint16_t all;
 	} misc;
-	float xOffset, yOffset;			// the offset of the probe relative to the print head
-	float triggerHeight;			// the nozzle height at which the target ADC value is returned
-	float calibTemperature;			// the temperature at which we did the calibration
-	float temperatureCoefficient;	// the variation of height with bed temperature
-	float diveHeight;				// the dive height we use when probing
-	float probeSpeed;				// the initial speed of probing
-	float travelSpeed;				// the speed at which we travel to the probe point
-	float recoveryTime;				// Z probe recovery time
-	float tolerance;				// maximum difference between probe heights when doing >1 taps
-
-	bool isDeployedByUser;			// true if the user has used the M401 command to deploy this probe and not sent M402 to retract it
+	float xOffset, yOffset;				// the offset of the probe relative to the print head
+	float triggerHeight;				// the nozzle height at which the target ADC value is returned
+	float calibTemperature;				// the temperature at which we did the calibration
+	float temperatureCoefficients[2];	// the variation of height with bed temperature and with the square of temperature
+	float diveHeight;					// the dive height we use when probing
+	float probeSpeed;					// the initial speed of probing
+	float travelSpeed;					// the speed at which we travel to the probe point
+	float recoveryTime;					// Z probe recovery time
+	float tolerance;					// maximum difference between probe heights when doing >1 taps
+	float lastStopHeight;				// the height at which the last G30 probe move stopped
+
+	bool isDeployedByUser;				// true if the user has used the M401 command to deploy this probe and not sent M402 to retract it
 };
 
 // MotorStall Z probes have no port, also in a CAN environment the local and remote proxy versions are the same