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 | 63 +++++++++++--------- Marlin/src/feature/probe_temp_comp.h | 8 +-- Marlin/src/gcode/calibrate/G76_M192_M871.cpp | 2 +- 3 files changed, 40 insertions(+), 33 deletions(-) diff --git a/Marlin/src/feature/probe_temp_comp.cpp b/Marlin/src/feature/probe_temp_comp.cpp index 68984fe7566f..deae44756811 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 @@ -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(start_temp) + i * res_temp, static_cast(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(start_temp) + i * res_temp, i ? static_cast(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((temp - start_temp) / res_temp); - // offset in µm float offset = 0.0f; - #if !defined(PTC_LINEAR_EXTRAPOLATION) || PTC_LINEAR_EXTRAPOLATION <= 0 - if (idx < 0) + #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 (temp < start_temp) offset = 0.0f; - else if (idx > measurements - 2) + else if (temp >= end_temp) offset = static_cast(data[measurements - 1]); - #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)); #endif - else - offset = linear_interp(temp, point(idx), point(idx + 1)); + else { + // Linear interpolation + const int8_t idx = static_cast((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 f5f922410c17..f24b9acd9b18 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 170958cab407..946701050e70 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; };