2424
2525#if ENABLED(PROBE_TEMP_COMPENSATION)
2626
27+ // #define DEBUG_PTC // Print extra debug output with 'M871'
28+
2729#include " probe_temp_comp.h"
2830#include < math.h>
2931
@@ -79,9 +81,17 @@ void ProbeTempComp::print_offsets() {
7981 " temp: " , temp,
8082 " C; Offset: " , i < 0 ? 0 .0f : sensor_z_offsets[s][i], " um"
8183 );
82- temp += cali_info[s].temp_res ;
84+ temp += cali_info[s].temp_resolution ;
8385 }
8486 }
87+ #if ENABLED(DEBUG_PTC)
88+ float meas[4 ] = { 0 , 0 , 0 , 0 };
89+ compensate_measurement (TSI_PROBE , 27.5 , meas[0 ]);
90+ compensate_measurement (TSI_PROBE , 32.5 , meas[1 ]);
91+ compensate_measurement (TSI_PROBE , 77.5 , meas[2 ]);
92+ compensate_measurement (TSI_PROBE , 82.5 , meas[3 ]);
93+ SERIAL_ECHOLNPGM (" DEBUG_PTC 27.5:" , meas[0 ], " 32.5:" , meas[1 ], " 77.5:" , meas[2 ], " 82.5:" , meas[3 ]);
94+ #endif
8595}
8696
8797void ProbeTempComp::prepare_new_calibration (const_float_t init_meas_z) {
@@ -111,7 +121,7 @@ bool ProbeTempComp::finish_calibration(const TempSensorID tsi) {
111121
112122 const uint8_t measurements = cali_info[tsi].measurements ;
113123 const celsius_t start_temp = cali_info[tsi].start_temp ,
114- res_temp = cali_info[tsi].temp_res ;
124+ res_temp = cali_info[tsi].temp_resolution ;
115125 int16_t * const data = sensor_z_offsets[tsi];
116126
117127 // Extrapolate
@@ -156,46 +166,45 @@ bool ProbeTempComp::finish_calibration(const TempSensorID tsi) {
156166}
157167
158168void ProbeTempComp::compensate_measurement (const TempSensorID tsi, const celsius_t temp, float &meas_z) {
159- if (WITHIN (temp, cali_info[tsi].start_temp , cali_info[tsi].end_temp ))
160- meas_z -= get_offset_for_temperature (tsi, temp);
161- }
162-
163- float ProbeTempComp::get_offset_for_temperature (const TempSensorID tsi, const celsius_t temp) {
164169 const uint8_t measurements = cali_info[tsi].measurements ;
165170 const celsius_t start_temp = cali_info[tsi].start_temp ,
166- res_temp = cali_info[tsi].temp_res ;
171+ end_temp = cali_info[tsi].end_temp ,
172+ res_temp = cali_info[tsi].temp_resolution ;
167173 const int16_t * const data = sensor_z_offsets[tsi];
168174
169- auto point = [&](uint8_t i) -> xy_float_t {
170- return xy_float_t ({ static_cast <float >(start_temp) + i * res_temp, static_cast <float >(data[i]) });
175+ // Given a data index, return { celsius, zoffset } in the form { x, y }
176+ auto tpoint = [&](uint8_t i) -> xy_float_t {
177+ return xy_float_t ({ static_cast <float >(start_temp) + i * res_temp, i ? static_cast <float >(data[i - 1 ]) : 0 .0f });
171178 };
172179
180+ // Interpolate Z based on a temperature being within a given range
173181 auto linear_interp = [](const_float_t x, xy_float_t p1, xy_float_t p2) {
174- return (p2.y - p1.y ) / (p2.x - p2.y ) * (x - p1.x ) + p1.y ;
182+ // zoffs1 + zoffset_per_toffset * toffset
183+ return p1.y + (p2.y - p1.y ) / (p2.x - p1.x ) * (x - p1.x );
175184 };
176185
177- // Linear interpolation
178- uint8_t idx = static_cast <uint8_t >((temp - start_temp) / res_temp);
179-
180186 // offset in µm
181187 float offset = 0 .0f ;
182188
183- #if !defined(PTC_LINEAR_EXTRAPOLATION) || PTC_LINEAR_EXTRAPOLATION <= 0
184- if (idx < 0 )
189+ #if PTC_LINEAR_EXTRAPOLATION
190+ if (temp < start_temp)
191+ offset = linear_interp (temp, tpoint (0 ), tpoint (PTC_LINEAR_EXTRAPOLATION ));
192+ else if (temp >= end_temp)
193+ offset = linear_interp (temp, tpoint (measurements - PTC_LINEAR_EXTRAPOLATION ), tpoint (measurements));
194+ #else
195+ if (temp < start_temp)
185196 offset = 0 .0f ;
186- else if (idx > measurements - 2 )
197+ else if (temp >= end_temp )
187198 offset = static_cast <float >(data[measurements - 1 ]);
188- #else
189- if (idx < 0 )
190- offset = linear_interp (temp, point (0 ), point (PTC_LINEAR_EXTRAPOLATION ));
191- else if (idx > measurements - 2 )
192- offset = linear_interp (temp, point (measurements - PTC_LINEAR_EXTRAPOLATION - 1 ), point (measurements - 1 ));
193199 #endif
194- else
195- offset = linear_interp (temp, point (idx), point (idx + 1 ));
200+ else {
201+ // Linear interpolation
202+ const int8_t idx = static_cast <int8_t >((temp - start_temp) / res_temp);
203+ offset = linear_interp (temp, tpoint (idx), tpoint (idx + 1 ));
204+ }
196205
197- // return offset in mm
198- return offset / 1000 .0f ;
206+ // convert offset to mm and apply it
207+ meas_z -= offset / 1000 .0f ;
199208}
200209
201210bool 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
204213 if (!WITHIN (calib_idx, 2 , cali_info[tsi].measurements )) return false ;
205214
206215 const celsius_t start_temp = cali_info[tsi].start_temp ,
207- res_temp = cali_info[tsi].temp_res ;
216+ res_temp = cali_info[tsi].temp_resolution ;
208217 const int16_t * const data = sensor_z_offsets[tsi];
209218
210219 float sum_x = start_temp,
0 commit comments