From 5b478cd5f6b6eae0343acbf169976f97b1ba5609 Mon Sep 17 00:00:00 2001
From: tombrazier <68918209+tombrazier@users.noreply.github.com>
Date: Fri, 22 Oct 2021 21:56:05 +0100
Subject: [PATCH] =?UTF-8?q?=F0=9F=90=9B=20Fix=20probe=20temp=20compensatio?=
=?UTF-8?q?n=20maths=20(#23004)?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
Marlin/src/feature/probe_temp_comp.cpp | 65 +++++++++++---------
Marlin/src/feature/probe_temp_comp.h | 8 +--
Marlin/src/gcode/calibrate/G76_M192_M871.cpp | 2 +-
3 files changed, 41 insertions(+), 34 deletions(-)
diff --git a/Marlin/src/feature/probe_temp_comp.cpp b/Marlin/src/feature/probe_temp_comp.cpp
index 68984fe7566..deae4475681 100644
--- a/Marlin/src/feature/probe_temp_comp.cpp
+++ b/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 {
- return xy_float_t({ static_cast<float>(start_temp) + i * res_temp, static_cast<float>(data[i]) });
+ // Given a data index, return { celsius, zoffset } in the form { x, y }
+ 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 (idx < 0)
- offset = 0.0f;
- else if (idx > measurements - 2)
- offset = static_cast<float>(data[measurements - 1]);
+ #if PTC_LINEAR_EXTRAPOLATION
+ if (temp < start_temp)
+ offset = linear_interp(temp, tpoint(0), tpoint(PTC_LINEAR_EXTRAPOLATION));
+ else if (temp >= end_temp)
+ offset = linear_interp(temp, tpoint(measurements - PTC_LINEAR_EXTRAPOLATION), tpoint(measurements));
#else
- if (idx < 0)
- offset = linear_interp(temp, point(0), point(PTC_LINEAR_EXTRAPOLATION));
- else if (idx > measurements - 2)
- offset = linear_interp(temp, point(measurements - PTC_LINEAR_EXTRAPOLATION - 1), point(measurements - 1));
+ if (temp < start_temp)
+ offset = 0.0f;
+ else if (temp >= end_temp)
+ offset = static_cast<float>(data[measurements - 1]);
#endif
- else
- offset = linear_interp(temp, point(idx), point(idx + 1));
+ else {
+ // 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
- return offset / 1000.0f;
+ // convert offset to mm and apply it
+ 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,
diff --git a/Marlin/src/feature/probe_temp_comp.h b/Marlin/src/feature/probe_temp_comp.h
index f5f922410c1..f24b9acd9b1 100644
--- a/Marlin/src/feature/probe_temp_comp.h
+++ b/Marlin/src/feature/probe_temp_comp.h
@@ -33,9 +33,9 @@ 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
- start_temp, // Base measurement; z-offset == 0
+ uint8_t measurements; // Max. number of measurements to be stored (35 - 80°C)
+ 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.
diff --git a/Marlin/src/gcode/calibrate/G76_M192_M871.cpp b/Marlin/src/gcode/calibrate/G76_M192_M871.cpp
index 170958cab40..946701050e7 100644
--- a/Marlin/src/gcode/calibrate/G76_M192_M871.cpp
+++ b/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;
};