PINDA v2 temperature sensor / compensation (MarlinFirmware#16293)

Marlin/src/feature/probe_temp_compensation.cpp

@@ -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

Marlin/src/feature/probe_temp_compensation.h

@@ -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;

Marlin/src/gcode/bedlevel/abl/G29.cpp

@@ -43,6 +43,11 @@ static inline void MINDA_BROKEN_CABLE_DETECTION__MBL_END() {}
 #include "../../../module/probe.h"
 #include "../../queue.h"
 
+#if ENABLED(PROBE_TEMP_COMPENSATION)
+  #include "../../../feature/probe_temp_compensation.h"
+  #include "../../../module/temperature.h"
+#endif
+
 #if HAS_DISPLAY
   #include "../../../lcd/ultralcd.h"
 #endif
@@ -739,6 +744,14 @@ G29_TYPE GcodeSuite::G29() {
             break; // Breaks out of both loops
           }
 
+          #if ENABLED(PROBE_TEMP_COMPENSATION)
+            temp_comp.compensate_measurement(TSI_BED, thermalManager.degBed(), measured_z);
+            temp_comp.compensate_measurement(TSI_PROBE, thermalManager.degProbe(), measured_z);
+            #if ENABLED(USE_TEMP_EXT_COMPENSATION)
+              temp_comp.compensate_measurement(TSI_EXT, thermalManager.degHotend(), measured_z);
+            #endif
+          #endif
+
           #if ENABLED(AUTO_BED_LEVELING_LINEAR)
 
             mean += measured_z;