Skip to content

Commit

Permalink
PINDA v2 temperature sensor / compensation (MarlinFirmware#16293)
Browse files Browse the repository at this point in the history
  • Loading branch information
tompe-proj authored and thinkyhead committed Jan 17, 2020
1 parent 4108c5d commit a338dce
Showing 16 changed files with 1,017 additions and 42 deletions.
1 change: 1 addition & 0 deletions Marlin/Configuration.h
Original file line number Diff line number Diff line change
@@ -412,6 +412,7 @@
#define TEMP_SENSOR_4 0
#define TEMP_SENSOR_5 0
#define TEMP_SENSOR_BED 0
#define TEMP_SENSOR_PROBE 0
#define TEMP_SENSOR_CHAMBER 0

// Dummy thermistor constant temperature readings, for use with 998 and 999
32 changes: 32 additions & 0 deletions Marlin/Configuration_adv.h
Original file line number Diff line number Diff line change
@@ -1529,6 +1529,38 @@

#endif

/**
* Thermal Probe Compensation
* Probe measurements are adjusted to compensate for temperature distortion.
* Use G76 to calibrate this feature. Use M871 to set values manually.
* For a more detailed explanation of the process see G76_M871.cpp.
*/
#if HAS_BED_PROBE && TEMP_SENSOR_PROBE && TEMP_SENSOR_BED
// Enable thermal first layer compensation using bed and probe temperatures
#define PROBE_TEMP_COMPENSATION

// Add additional compensation depending on hotend temperature
// Note: this values cannot be calibrated and have to be set manually
#ifdef PROBE_TEMP_COMPENSATION
// Max temperature that can be reached by heated bed.
// This is required only for the calibration process.
#define PTC_MAX_BED_TEMP 110

// Park position to wait for probe cooldown
#define PTC_PARK_POS_X 0.0F
#define PTC_PARK_POS_Y 0.0F
#define PTC_PARK_POS_Z 100.0F

// Probe position to probe and wait for probe to reach target temperature
#define PTC_PROBE_POS_X 90.0F
#define PTC_PROBE_POS_Y 100.0F

// Enable additional compensation using hotend temperature
// Note: this values cannot be calibrated automatically but have to be set manually
//#define USE_TEMP_EXT_COMPENSATION
#endif
#endif

// @section extras

//
223 changes: 223 additions & 0 deletions Marlin/src/feature/probe_temp_compensation.cpp
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
116 changes: 116 additions & 0 deletions Marlin/src/feature/probe_temp_compensation.h
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;
Loading

0 comments on commit a338dce

Please sign in to comment.