forked from MarlinFirmware/Marlin
-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
PINDA v2 temperature sensor / compensation (MarlinFirmware#16293)
- Loading branch information
1 parent
4108c5d
commit a338dce
Showing
16 changed files
with
1,017 additions
and
42 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,223 @@ | ||
/** | ||
* Marlin 3D Printer Firmware | ||
* Copyright (c) 2019 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] | ||
* | ||
* Based on Sprinter and grbl. | ||
* Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm | ||
* | ||
* This program is free software: you can redistribute it and/or modify | ||
* it under the terms of the GNU General Public License as published by | ||
* the Free Software Foundation, either version 3 of the License, or | ||
* (at your option) any later version. | ||
* | ||
* This program is distributed in the hope that it will be useful, | ||
* but WITHOUT ANY WARRANTY; without even the implied warranty of | ||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the | ||
* GNU General Public License for more details. | ||
* | ||
* You should have received a copy of the GNU General Public License | ||
* along with this program. If not, see <http://www.gnu.org/licenses/>. | ||
* | ||
*/ | ||
|
||
#include "../inc/MarlinConfigPre.h" | ||
|
||
#if ENABLED(PROBE_TEMP_COMPENSATION) | ||
|
||
#include "probe_temp_compensation.h" | ||
#include <math.h> | ||
|
||
ProbeTempComp temp_comp; | ||
|
||
int16_t ProbeTempComp::z_offsets_probe[ProbeTempComp::cali_info_init[TSI_PROBE].measurements], // = {0} | ||
ProbeTempComp::z_offsets_bed[ProbeTempComp::cali_info_init[TSI_BED].measurements]; // = {0} | ||
|
||
#if ENABLED(USE_TEMP_EXT_COMPENSATION) | ||
int16_t ProbeTempComp::z_offsets_ext[ProbeTempComp::cali_info_init[TSI_EXT].measurements]; // = {0} | ||
#endif | ||
|
||
int16_t *ProbeTempComp::sensor_z_offsets[TSI_COUNT] = { | ||
ProbeTempComp::z_offsets_probe, ProbeTempComp::z_offsets_bed | ||
#if ENABLED(USE_TEMP_EXT_COMPENSATION) | ||
, ProbeTempComp::z_offsets_ext | ||
#endif | ||
}; | ||
|
||
const temp_calib_t ProbeTempComp::cali_info[TSI_COUNT] = { | ||
ProbeTempComp::cali_info_init[TSI_PROBE], ProbeTempComp::cali_info_init[TSI_BED] | ||
#if ENABLED(USE_TEMP_EXT_COMPENSATION) | ||
, ProbeTempComp::cali_info_init[TSI_EXT] | ||
#endif | ||
}; | ||
|
||
uint8_t ProbeTempComp::calib_idx; // = 0 | ||
float ProbeTempComp::init_measurement; // = 0.0 | ||
|
||
void ProbeTempComp::clear_offsets(const TempSensorID tsi) { | ||
for (uint8_t i = 0; i < cali_info[tsi].measurements; ++i) | ||
sensor_z_offsets[tsi][i] = 0; | ||
calib_idx = 0; | ||
} | ||
|
||
bool ProbeTempComp::set_offset(const TempSensorID tsi, const uint8_t idx, const int16_t offset) { | ||
if (idx >= cali_info[tsi].measurements) return false; | ||
sensor_z_offsets[tsi][idx] = offset; | ||
return true; | ||
} | ||
|
||
void ProbeTempComp::print_offsets() { | ||
for (uint8_t s = 0; s < TSI_COUNT; s++) { | ||
float temp = cali_info[s].start_temp; | ||
for (int16_t i = -1; i < cali_info[s].measurements; ++i) { | ||
serialprintPGM(s == TSI_BED ? PSTR("Bed") : | ||
#if ENABLED(USE_TEMP_EXT_COMPENSATION) | ||
s == TSI_EXT ? PSTR("Extruder") : | ||
#endif | ||
PSTR("Probe") | ||
); | ||
SERIAL_ECHOLNPAIR( | ||
" temp: ", temp, | ||
"C; Offset: ", i < 0 ? 0.0f : sensor_z_offsets[s][i], " um" | ||
); | ||
temp += cali_info[s].temp_res; | ||
} | ||
} | ||
} | ||
|
||
void ProbeTempComp::prepare_new_calibration(const float &init_meas_z) { | ||
calib_idx = 0; | ||
init_measurement = init_meas_z; | ||
} | ||
|
||
void ProbeTempComp::push_back_new_measurement(const TempSensorID tsi, const float &meas_z) { | ||
switch (tsi) { | ||
case TSI_PROBE: | ||
case TSI_BED: | ||
//case TSI_EXT: | ||
if (calib_idx >= cali_info[tsi].measurements) return; | ||
sensor_z_offsets[tsi][calib_idx++] = static_cast<int16_t>(meas_z * 1000.0f - init_measurement * 1000.0f); | ||
default: break; | ||
} | ||
} | ||
|
||
bool ProbeTempComp::finish_calibration(const TempSensorID tsi) { | ||
if (tsi != TSI_PROBE && tsi != TSI_BED) return false; | ||
|
||
if (calib_idx < 3) { | ||
SERIAL_ECHOLNPGM("!Insufficient measurements (min. 3)."); | ||
clear_offsets(tsi); | ||
return false; | ||
} | ||
|
||
const uint8_t measurements = cali_info[tsi].measurements; | ||
const float start_temp = cali_info[tsi].start_temp, | ||
res_temp = cali_info[tsi].temp_res; | ||
int16_t * const data = sensor_z_offsets[tsi]; | ||
|
||
// Extrapolate | ||
float k, d; | ||
if (calib_idx < measurements) { | ||
SERIAL_ECHOLNPAIR("Got ", calib_idx, " measurements. "); | ||
if (linear_regression(tsi, k, d)) { | ||
SERIAL_ECHOPGM("Applying linear extrapolation"); | ||
calib_idx--; | ||
for (; calib_idx < measurements; ++calib_idx) { | ||
const float temp = start_temp + float(calib_idx) * res_temp; | ||
data[calib_idx] = static_cast<int16_t>(k * temp + d); | ||
} | ||
} | ||
else { | ||
// Simply use the last measured value for higher temperatures | ||
SERIAL_ECHOPGM("Failed to extrapolate"); | ||
const int16_t last_val = data[calib_idx]; | ||
for (; calib_idx < measurements; ++calib_idx) | ||
data[calib_idx] = last_val; | ||
} | ||
SERIAL_ECHOLNPGM(" for higher temperatures."); | ||
} | ||
|
||
// Sanity check | ||
for (calib_idx = 0; calib_idx < measurements; ++calib_idx) { | ||
// Restrict the max. offset | ||
if (abs(data[calib_idx]) > 2000) { | ||
SERIAL_ECHOLNPGM("!Invalid Z-offset detected (0-2)."); | ||
clear_offsets(tsi); | ||
return false; | ||
} | ||
// Restrict the max. offset difference between two probings | ||
if (calib_idx > 0 && abs(data[calib_idx - 1] - data[calib_idx]) > 800) { | ||
SERIAL_ECHOLNPGM("!Invalid Z-offset between two probings detected (0-0.8)."); | ||
clear_offsets(TSI_PROBE); | ||
return false; | ||
} | ||
} | ||
|
||
return true; | ||
} | ||
|
||
void ProbeTempComp::compensate_measurement(const TempSensorID tsi, const float &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 float &temp) { | ||
|
||
const uint8_t measurements = cali_info[tsi].measurements; | ||
const float start_temp = cali_info[tsi].start_temp, | ||
end_temp = cali_info[tsi].end_temp, | ||
res_temp = cali_info[tsi].temp_res; | ||
const int16_t * const data = sensor_z_offsets[tsi]; | ||
|
||
if (temp <= start_temp) return 0.0f; | ||
if (temp >= end_temp) return static_cast<float>(data[measurements - 1]) / 1000.0f; | ||
|
||
// Linear interpolation | ||
int16_t val1 = 0, val2 = data[0]; | ||
uint8_t idx = 0; | ||
float meas_temp = start_temp + res_temp; | ||
while (meas_temp < temp) { | ||
if (++idx >= measurements) return static_cast<float>(val2) / 1000.0f; | ||
meas_temp += res_temp; | ||
val1 = val2; | ||
val2 = data[idx]; | ||
} | ||
const float factor = (meas_temp - temp) / static_cast<float>(res_temp); | ||
return (static_cast<float>(val2) - static_cast<float>(val2 - val1) * factor) / 1000.0f; | ||
} | ||
|
||
bool ProbeTempComp::linear_regression(const TempSensorID tsi, float &k, float &d) { | ||
if (tsi != TSI_PROBE && tsi != TSI_BED) return false; | ||
|
||
if (!WITHIN(calib_idx, 2, cali_info[tsi].measurements)) return false; | ||
|
||
const float start_temp = cali_info[tsi].start_temp, | ||
res_temp = cali_info[tsi].temp_res; | ||
const int16_t * const data = sensor_z_offsets[tsi]; | ||
|
||
float sum_x = start_temp, | ||
sum_x2 = sq(start_temp), | ||
sum_xy = 0, sum_y = 0; | ||
|
||
for (uint8_t i = 0; i < calib_idx; ++i) { | ||
const float xi = start_temp + (i + 1) * res_temp, | ||
yi = static_cast<float>(data[i]); | ||
sum_x += xi; | ||
sum_x2 += sq(xi); | ||
sum_xy += xi * yi; | ||
sum_y += yi; | ||
} | ||
|
||
const float denom = static_cast<float>(calib_idx + 1) * sum_x2 - sq(sum_x); | ||
if (fabs(denom) <= 10e-5) { | ||
// Singularity - unable to solve | ||
k = d = 0.0; | ||
return false; | ||
} | ||
|
||
k = (static_cast<float>(calib_idx + 1) * sum_xy - sum_x * sum_y) / denom; | ||
d = (sum_y - k * sum_x) / static_cast<float>(calib_idx + 1); | ||
|
||
return true; | ||
} | ||
|
||
#endif // PROBE_TEMP_COMPENSATION |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,116 @@ | ||
/** | ||
* Marlin 3D Printer Firmware | ||
* Copyright (c) 2019 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] | ||
* | ||
* Based on Sprinter and grbl. | ||
* Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm | ||
* | ||
* This program is free software: you can redistribute it and/or modify | ||
* it under the terms of the GNU General Public License as published by | ||
* the Free Software Foundation, either version 3 of the License, or | ||
* (at your option) any later version. | ||
* | ||
* This program is distributed in the hope that it will be useful, | ||
* but WITHOUT ANY WARRANTY; without even the implied warranty of | ||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the | ||
* GNU General Public License for more details. | ||
* | ||
* You should have received a copy of the GNU General Public License | ||
* along with this program. If not, see <http://www.gnu.org/licenses/>. | ||
* | ||
*/ | ||
#pragma once | ||
|
||
#include "../inc/MarlinConfig.h" | ||
|
||
enum TempSensorID : uint8_t { | ||
TSI_PROBE, | ||
TSI_BED, | ||
#if ENABLED(USE_TEMP_EXT_COMPENSATION) | ||
TSI_EXT, | ||
#endif | ||
TSI_COUNT | ||
}; | ||
|
||
typedef struct { | ||
uint8_t measurements; // Max. number of measurements to be stored (35 - 80°C) | ||
float temp_res, // Resolution in °C between measurements | ||
start_temp, // Base measurement; z-offset == 0 | ||
end_temp; | ||
} temp_calib_t; | ||
|
||
/** | ||
* Probe temperature compensation implementation. | ||
* Z-probes like the P.I.N.D.A V2 allow for compensation of | ||
* measurement errors/shifts due to changed temperature. | ||
*/ | ||
class ProbeTempComp { | ||
public: | ||
|
||
static constexpr temp_calib_t cali_info_init[TSI_COUNT] = { | ||
{ 30, 10, 5, 30 + 10 * 5 }, // Probe | ||
{ 60, 10, 5, 60 + 10 * 5 }, // Bed | ||
#if ENABLED(USE_TEMP_EXT_COMPENSATION) | ||
{ 180, 5, 20, 180 + 5 * 20 } // Extruder | ||
#endif | ||
}; | ||
static const temp_calib_t cali_info[TSI_COUNT]; | ||
|
||
// Where to park nozzle to wait for probe cooldown | ||
static constexpr xyz_pos_t park_point = { PTC_PARK_POS_X, PTC_PARK_POS_Y, PTC_PARK_POS_Z }; | ||
|
||
static constexpr int max_bed_temp = PTC_MAX_BED_TEMP, // Max temperature to avoid heating errors | ||
|
||
// XY coordinates of nozzle for probing the bed | ||
measure_point_x = PTC_PROBE_POS_X, // X-coordinate to probe | ||
measure_point_y = PTC_PROBE_POS_Y, // Y-coordinate to probe | ||
//measure_point_x = 12.0f, // X-coordinate to probe on MK52 magnetic heatbed | ||
//measure_point_y = 7.3f, // Y-coordinate to probe on MK52 magnetic heatbed | ||
|
||
probe_calib_bed_temp = max_bed_temp, // Bed temperature while calibrating probe | ||
bed_calib_probe_temp = 30; // Probe temperature while calibrating bed | ||
|
||
static int16_t *sensor_z_offsets[TSI_COUNT], | ||
z_offsets_probe[cali_info_init[TSI_PROBE].measurements], // (µm) | ||
z_offsets_bed[cali_info_init[TSI_BED].measurements]; // (µm) | ||
|
||
#if ENABLED(USE_TEMP_EXT_COMPENSATION) | ||
static int16_t z_offsets_ext[cali_info_init[TSI_EXT].measurements]; // (µm) | ||
#endif | ||
|
||
static inline void reset_index() { calib_idx = 0; }; | ||
static inline uint8_t get_index() { return calib_idx; } | ||
static void clear_offsets(const TempSensorID tsi); | ||
static inline void clear_all_offsets() { | ||
clear_offsets(TSI_BED); | ||
clear_offsets(TSI_PROBE); | ||
#if ENABLED(USE_TEMP_EXT_COMPENSATION) | ||
clear_offsets(TSI_EXT); | ||
#endif | ||
} | ||
static bool set_offset(const TempSensorID tsi, const uint8_t idx, const int16_t offset); | ||
static void print_offsets(); | ||
static void prepare_new_calibration(const float &init_meas_z); | ||
static void push_back_new_measurement(const TempSensorID tsi, const float &meas_z); | ||
static bool finish_calibration(const TempSensorID tsi); | ||
static void compensate_measurement(const TempSensorID tsi, const float &temp, float &meas_z); | ||
|
||
private: | ||
static uint8_t calib_idx; | ||
|
||
/** | ||
* Base value. Temperature compensation values will be deltas | ||
* to this value, set at first probe. | ||
*/ | ||
static float init_measurement; | ||
|
||
static float get_offset_for_temperature(const TempSensorID tsi, const float &temp); | ||
|
||
/** | ||
* Fit a linear function in measured temperature offsets | ||
* to allow generating values of higher temperatures. | ||
*/ | ||
static bool linear_regression(const TempSensorID tsi, float &k, float &d); | ||
}; | ||
|
||
extern ProbeTempComp temp_comp; |
Oops, something went wrong.