🐛 Fix probe temp compensation maths (#23004)

Marlin/src/feature/probe_temp_comp.cpp

@@ -24,6 +24,8 @@
 
 #if ENABLED(PROBE_TEMP_COMPENSATION)
 
+//#define DEBUG_PTC   // Print extra debug output with 'M871'
+
 #include "probe_temp_comp.h"
 #include <math.h>
 
@@ -79,9 +81,17 @@ void ProbeTempComp::print_offsets() {
         " temp: ", temp,
         "C; Offset: ", i < 0 ? 0.0f : sensor_z_offsets[s][i], " um"
       );
-      temp += cali_info[s].temp_res;
+      temp += cali_info[s].temp_resolution;
     }
   }
+  #if ENABLED(DEBUG_PTC)
+    float meas[4] = { 0, 0, 0, 0 };
+    compensate_measurement(TSI_PROBE, 27.5, meas[0]);
+    compensate_measurement(TSI_PROBE, 32.5, meas[1]);
+    compensate_measurement(TSI_PROBE, 77.5, meas[2]);
+    compensate_measurement(TSI_PROBE, 82.5, meas[3]);
+    SERIAL_ECHOLNPGM("DEBUG_PTC 27.5:", meas[0], " 32.5:", meas[1], " 77.5:", meas[2], " 82.5:", meas[3]);
+  #endif
 }
 
 void ProbeTempComp::prepare_new_calibration(const_float_t init_meas_z) {
@@ -111,7 +121,7 @@ bool ProbeTempComp::finish_calibration(const TempSensorID tsi) {
 
   const uint8_t measurements = cali_info[tsi].measurements;
   const celsius_t start_temp = cali_info[tsi].start_temp,
-                    res_temp = cali_info[tsi].temp_res;
+                    res_temp = cali_info[tsi].temp_resolution;
   int16_t * const data = sensor_z_offsets[tsi];
 
   // Extrapolate
@@ -156,46 +166,45 @@ bool ProbeTempComp::finish_calibration(const TempSensorID tsi) {
 }
 
 void ProbeTempComp::compensate_measurement(const TempSensorID tsi, const celsius_t temp, float &meas_z) {
-  if (WITHIN(temp, cali_info[tsi].start_temp, cali_info[tsi].end_temp))
-    meas_z -= get_offset_for_temperature(tsi, temp);
-}
-
-float ProbeTempComp::get_offset_for_temperature(const TempSensorID tsi, const celsius_t temp) {
   const uint8_t measurements = cali_info[tsi].measurements;
   const celsius_t start_temp = cali_info[tsi].start_temp,
-                    res_temp = cali_info[tsi].temp_res;
+                    end_temp = cali_info[tsi].end_temp,
+                    res_temp = cali_info[tsi].temp_resolution;
   const int16_t * const data = sensor_z_offsets[tsi];
 
-  auto point = [&](uint8_t i) -> xy_float_t {
+  // Given a data index, return { celsius, zoffset } in the form { x, y }
-    return xy_float_t({ static_cast<float>(start_temp) + i * res_temp, static_cast<float>(data[i]) });
+  auto tpoint = [&](uint8_t i) -> xy_float_t {
+    return xy_float_t({ static_cast<float>(start_temp) + i * res_temp, i ? static_cast<float>(data[i - 1]) : 0.0f });
   };
 
+  // Interpolate Z based on a temperature being within a given range
   auto linear_interp = [](const_float_t x, xy_float_t p1, xy_float_t p2) {
-    return (p2.y - p1.y) / (p2.x - p2.y) * (x - p1.x) + p1.y;
+    //   zoffs1 +      zoffset_per_toffset      *  toffset
+    return p1.y + (p2.y - p1.y) / (p2.x - p1.x) * (x - p1.x);
   };
 
-  // Linear interpolation
-  uint8_t idx = static_cast<uint8_t>((temp - start_temp) / res_temp);
-
   // offset in µm
   float offset = 0.0f;
 
-  #if !defined(PTC_LINEAR_EXTRAPOLATION) || PTC_LINEAR_EXTRAPOLATION <= 0
+  #if PTC_LINEAR_EXTRAPOLATION
-    if (idx < 0)
+    if (temp < start_temp)
-      offset = 0.0f;
+      offset = linear_interp(temp, tpoint(0), tpoint(PTC_LINEAR_EXTRAPOLATION));
-    else if (idx > measurements - 2)
+    else if (temp >= end_temp)
-      offset = static_cast<float>(data[measurements - 1]);
+      offset = linear_interp(temp, tpoint(measurements - PTC_LINEAR_EXTRAPOLATION), tpoint(measurements));
   #else
-    if (idx < 0)
+    if (temp < start_temp)
-      offset = linear_interp(temp, point(0), point(PTC_LINEAR_EXTRAPOLATION));
+      offset = 0.0f;
-    else if (idx > measurements - 2)
+    else if (temp >= end_temp)
-      offset = linear_interp(temp, point(measurements - PTC_LINEAR_EXTRAPOLATION - 1), point(measurements - 1));
+      offset = static_cast<float>(data[measurements - 1]);
   #endif
-    else
+    else {
-      offset = linear_interp(temp, point(idx), point(idx + 1));
+      // Linear interpolation
+      const int8_t idx = static_cast<int8_t>((temp - start_temp) / res_temp);
+      offset = linear_interp(temp, tpoint(idx), tpoint(idx + 1));
+    }
 
-  // return offset in mm
+  // convert offset to mm and apply it
-  return offset / 1000.0f;
+  meas_z -= offset / 1000.0f;
 }
 
 bool ProbeTempComp::linear_regression(const TempSensorID tsi, float &k, float &d) {
@@ -204,7 +213,7 @@ bool ProbeTempComp::linear_regression(const TempSensorID tsi, float &k, float &d
   if (!WITHIN(calib_idx, 2, cali_info[tsi].measurements)) return false;
 
   const celsius_t start_temp = cali_info[tsi].start_temp,
-                    res_temp = cali_info[tsi].temp_res;
+                    res_temp = cali_info[tsi].temp_resolution;
   const int16_t * const data = sensor_z_offsets[tsi];
 
   float sum_x = start_temp,

Marlin/src/feature/probe_temp_comp.h

@@ -34,7 +34,7 @@ enum TempSensorID : uint8_t {
 
 typedef struct {
   uint8_t measurements;       // Max. number of measurements to be stored (35 - 80°C)
-  celsius_t temp_res,   // Resolution in °C between measurements
+  celsius_t temp_resolution,  // Resolution in °C between measurements
             start_temp,       // Base measurement; z-offset == 0
             end_temp;
 } temp_calib_t;
@@ -135,8 +135,6 @@ class ProbeTempComp {
      */
     static float init_measurement;
 
-    static float get_offset_for_temperature(const TempSensorID tsi, const celsius_t temp);
-
     /**
      * Fit a linear function in measured temperature offsets
      * to allow generating values of higher temperatures.

Marlin/src/gcode/calibrate/G76_M192_M871.cpp

@@ -121,7 +121,7 @@ void GcodeSuite::G76() {
         temp_comp.prepare_new_calibration(measured_z);
       else
         temp_comp.push_back_new_measurement(sid, measured_z);
-      targ += cali_info_init[sid].temp_res;
+      targ += cali_info_init[sid].temp_resolution;
     }
     return measured_z;
   };