Implement temperature model autotuning

Calibrate C/R values via univariate minimization using golden section.
This is done in several passes:

- Bootstrap C by setting an initial high R value
- Calibrate R at the requested working temperature
- Cooldown
- Refine C to the final value
- Estimate R losses for a subset of fan speeds
- Interpolate remaining values to speed-up the process

This results in robust values which are tailored to the current
filtering constants, and avoid having to sample for an extended
time to reach the required resolution.

The refining pass could avoid cooldown if the recording buffer was at
least twice as large, so that we could record both the heating and the
steady-state, saving _considerable_ time.

Firmware/Marlin.h

@@ -440,6 +440,7 @@ extern uint8_t calc_percent_done();
 
 #define KEEPALIVE_STATE(n) do { busy_state = n;} while (0)
 extern void host_keepalive();
+extern void host_autoreport();
 //extern MarlinBusyState busy_state;
 extern int8_t busy_state;
 

Firmware/Marlin_main.cpp

@@ -1779,7 +1779,7 @@ void serial_read_stream() {
  * Output autoreport values according to features requested in M155
  */
 #if defined(AUTO_REPORT)
-static void host_autoreport()
+void host_autoreport()
 {
     if (autoReportFeatures.TimerExpired())
     {
@@ -7785,8 +7785,8 @@ Sigma_Exit:
     case 310:
     {
         // parse all parameters
-        float P = NAN, C = NAN, R = NAN, E = NAN, W = NAN, T = NAN, A = NAN;
-        int8_t I = -1, S = -1, B = -1;
+        float P = NAN, C = NAN, R = NAN, E = NAN, W = NAN, T = NAN;
+        int8_t I = -1, S = -1, B = -1, A = -1;
         if(code_seen('C')) C = code_value();
         if(code_seen('P')) P = code_value();
         if(code_seen('I')) I = code_value_short();
@@ -7796,10 +7796,10 @@ Sigma_Exit:
         if(code_seen('E')) E = code_value();
         if(code_seen('W')) W = code_value();
         if(code_seen('T')) T = code_value();
-        if(code_seen('A')) A = code_value();
+        if(code_seen('A')) A = code_value_short();
 
         // report values if nothing has been requested
-        if(isnan(C) && isnan(P) && isnan(R) && isnan(E) && isnan(W) && isnan(T) && isnan(A) && I < 0 && S < 0 && B < 0) {
+        if(isnan(C) && isnan(P) && isnan(R) && isnan(E) && isnan(W) && isnan(T) && I < 0 && S < 0 && B < 0 && A < 0) {
             temp_model_report_settings();
             break;
         }
@@ -7811,7 +7811,7 @@ Sigma_Exit:
         if(I >= 0 && !isnan(R)) temp_model_set_resistance(I, R);
 
         // run autotune
-        if(!isnan(A)) temp_model_autotune(A != 0? A: NAN);
+        if(A >= 0) temp_model_autotune(A);
     }
     break;
     

Firmware/temp_model.h

@@ -5,9 +5,10 @@
 
 #include "planner.h"
 
-constexpr uint8_t TEMP_MODEL_CAL_S = 60; // Maximum recording lenght during calibration (s)
-constexpr float TEMP_MODEL_fS = 0.065;   // simulation filter (1st-order IIR factor)
-constexpr float TEMP_MODEL_fE = 0.05;    // error filter (1st-order IIR factor)
+constexpr uint8_t TEMP_MODEL_CAL_S = 60;     // Maximum recording lenght during calibration (s)
+constexpr uint8_t TEMP_MODEL_CAL_R_STEP = 4; // Fan interpolation steps during calibration
+constexpr float TEMP_MODEL_fS = 0.065;       // simulation filter (1st-order IIR factor)
+constexpr float TEMP_MODEL_fE = 0.05;        // error filter (1st-order IIR factor)
 
 // transport delay buffer size (samples)
 constexpr uint8_t TEMP_MODEL_LAG_SIZE = (TEMP_MODEL_LAG / TEMP_MGR_INTV + 0.5);
@@ -17,18 +18,6 @@ constexpr uint8_t TEMP_MODEL_R_SIZE = (1 << FAN_SOFT_PWM_BITS);
 
 namespace temp_model {
 
-// recording scratch buffer
-struct rec_entry
-{
-    float temp;  // heater temperature
-    uint8_t pwm; // heater PWM
-};
-
-constexpr uint16_t rec_buffer_size = TEMP_MODEL_CAL_S / TEMP_MGR_INTV;
-static rec_entry* const rec_buffer = (rec_entry*)block_buffer; // oh-hey, free memory!
-static_assert(sizeof(rec_entry[rec_buffer_size]) <= sizeof(block_buffer),
-    "recording length too long to fit within available buffer");
-
 struct model_data
 {
     // temporary buffers
@@ -108,3 +97,19 @@ static void log_isr(); // isr log handler
 #endif
 
 } // namespace temp_model
+
+namespace temp_model_cal {
+
+// recording scratch buffer
+struct rec_entry
+{
+    float temp;  // heater temperature
+    uint8_t pwm; // heater PWM
+};
+
+constexpr uint16_t REC_BUFFER_SIZE = TEMP_MODEL_CAL_S / TEMP_MGR_INTV;
+static rec_entry* const rec_buffer = (rec_entry*)block_buffer; // oh-hey, free memory!
+static_assert(sizeof(rec_entry[REC_BUFFER_SIZE]) <= sizeof(block_buffer),
+    "recording length too long to fit within available buffer");
+
+} // namespace temp_model_cal

Firmware/temperature.cpp

@@ -52,6 +52,8 @@
 #define TEMP_MGR_INTV   0.27 // seconds, ~3.7Hz
 #define TIMER5_PRESCALE 256
 #define TIMER5_OCRA_OVF (uint16_t)(TEMP_MGR_INTV / ((long double)TIMER5_PRESCALE / F_CPU))
+#define TEMP_MGR_INT_FLAG_STATE()    (TIFR5 & (1<<OCF5A))
+#define TEMP_MGR_INT_FLAG_CLEAR()    TIFR5 |= (1<<OCF5A)
 #define TEMP_MGR_INTERRUPT_STATE()   (TIMSK5 & (1<<OCIE5A))
 #define ENABLE_TEMP_MGR_INTERRUPT()  TIMSK5 |=  (1<<OCIE5A)
 #define DISABLE_TEMP_MGR_INTERRUPT() TIMSK5 &= ~(1<<OCIE5A)
@@ -1893,7 +1895,7 @@ void temp_mgr_init()
     OCR5A = TIMER5_OCRA_OVF;
 
     // clear pending interrupts, enable COMPA
-    TIFR5 |= (1<<OCF5A);
+    TEMP_MGR_INT_FLAG_CLEAR();
     ENABLE_TEMP_MGR_INTERRUPT();
 
     CRITICAL_SECTION_END;
@@ -2215,8 +2217,8 @@ ISR(TIMER5_COMPA_vect)
 {
     // immediately schedule a new conversion
     if(adc_values_ready != true) return;
-    adc_start_cycle();
     adc_values_ready = false;
+    adc_start_cycle();
 
     // run temperature management with interrupts enabled to reduce latency
     DISABLE_TEMP_MGR_INTERRUPT();
@@ -2618,9 +2620,202 @@ void temp_model_save_settings()
     eeprom_update_float((float*)EEPROM_TEMP_MODEL_E, temp_model::data.err);
 }
 
-void temp_model_autotune(float temp)
+namespace temp_model_cal {
+
+void waiting_handler()
+{
+    manage_heater();
+    host_keepalive();
+    host_autoreport();
+}
+
+void wait(unsigned ms)
+{
+    unsigned long mark = _millis() + ms;
+    while(_millis() < mark)
+        waiting_handler();
+}
+
+void wait_temp()
+{
+    while(current_temperature[0] < (target_temperature[0] - TEMP_HYSTERESIS))
+        waiting_handler();
+}
+
+void cooldown(float temp)
+{
+    float old_speed = fanSpeedSoftPwm;
+    fanSpeedSoftPwm = 255;
+    while((current_temperature[0] >= temp) &&
+        (current_temperature[0] >= (current_temperature_ambient + temp_model::data.Ta_corr + TEMP_HYSTERESIS)))
+        waiting_handler();
+    fanSpeedSoftPwm = old_speed;
+}
+
+uint16_t record(uint16_t samples = REC_BUFFER_SIZE) {
+    TempMgrGuard temp_mgr_guard;
+
+    uint16_t pos = 0;
+    while(pos < samples) {
+        if(!TEMP_MGR_INT_FLAG_STATE()) {
+            // temperatures not ready yet
+            waiting_handler();
+            continue;
+        }
+        TEMP_MGR_INT_FLAG_CLEAR();
+
+        // manually repeat what the regular isr would do
+        if(adc_values_ready != true) continue;
+        adc_values_ready = false;
+        adc_start_cycle();
+        temp_mgr_isr();
+
+        // record a new entry
+        rec_entry& entry = rec_buffer[pos];
+        entry.temp = current_temperature_isr[0];
+        entry.pwm = soft_pwm[0];
+        ++pos;
+    }
+
+    return pos;
+}
+
+float cost_fn(uint16_t samples, float* const var, float v, float ambient)
+{
+    *var = v;
+    uint8_t fan_pwm = soft_pwm_fan;
+    temp_model::data.reset(rec_buffer[0].pwm, fan_pwm, rec_buffer[0].temp, ambient);
+    float err = 0;
+    for(uint16_t i = 1; i < samples; ++i) {
+        temp_model::data.step(rec_buffer[i].pwm, fan_pwm, rec_buffer[i].temp, ambient);
+        err += fabsf(temp_model::data.dT_err_prev);
+    }
+    return (err / (samples - 1));
+}
+
+constexpr float GOLDEN_RATIO = 0.6180339887498949;
+
+void update_section(float points[2], const float bounds[2])
+{
+    float d = GOLDEN_RATIO * (bounds[1] - bounds[0]);
+    points[0] = bounds[0] + d;
+    points[1] = bounds[1] - d;
+}
+
+float estimate(uint16_t samples, float* const var, float min, float max, float thr, uint16_t max_itr, float ambient)
+{
+    float e = NAN;
+    float points[2];
+    float bounds[2] = {min, max};
+    update_section(points, bounds);
+
+    for(uint8_t it = 0; it != max_itr; ++it) {
+        float c1 = cost_fn(samples, var, points[0], ambient);
+        float c2 = cost_fn(samples, var, points[1], ambient);
+        bool dir = (c2 < c1);
+        bounds[dir] = points[!dir];
+        update_section(points, bounds);
+        float x = points[!dir];
+        e = (1-GOLDEN_RATIO) * fabsf((bounds[0]-bounds[1]) / x);
+
+        printf_P(PSTR("TM iter:%u v:%.2f e:%.3f\n"), it, x, e);
+        if(e < thr) return e;
+    }
+
+    SERIAL_ECHOLNPGM("TM estimation did not converge");
+    return e;
+}
+
+void autotune(int16_t cal_temp)
+{
+    SERIAL_ECHOLNPGM("TM: autotune start");
+    uint16_t samples;
+
+    // disable the model checking during self-calibration
+    bool was_enabled = temp_model::enabled;
+    temp_model_set_enabled(false);
+
+    // bootstrap C/R values without fan
+    fanSpeedSoftPwm = 0;
+
+    for(uint8_t i = 0; i != 2; ++i) {
+        const char* PROGMEM verb = (i == 0? PSTR("initial"): PSTR("refining"));
+
+        target_temperature[0] = 0;
+        if(current_temperature[0] >= TEMP_MODEL_CAL_Tl) {
+            printf_P(PSTR("TM: cooling down to %dC\n"), TEMP_MODEL_CAL_Tl);
+            cooldown(TEMP_MODEL_CAL_Tl);
+            wait(10000);
+        }
+
+        // we need a valid R value for the initial C guess
+        if(isnan(temp_model::data.R[0]))
+            temp_model::data.R[0] = TEMP_MODEL_Rh;
+
+        printf_P(PSTR("TM: %S C estimation\n"), verb);
+        target_temperature[0] = cal_temp;
+        samples = record();
+        estimate(samples, &temp_model::data.C,
+            TEMP_MODEL_Cl, TEMP_MODEL_Ch, TEMP_MODEL_C_thr, TEMP_MODEL_C_itr,
+            current_temperature_ambient);
+
+        wait_temp();
+        if(i) break; // we don't need to refine R
+        wait(30000); // settle PID regulation
+
+        printf_P(PSTR("TM: %S R estimation @ %dC\n"), verb, cal_temp);
+        samples = record();
+        estimate(samples, &temp_model::data.R[0],
+            TEMP_MODEL_Rl, TEMP_MODEL_Rh, TEMP_MODEL_R_thr, TEMP_MODEL_R_itr,
+            current_temperature_ambient);
+    }
+
+    // Estimate fan losses at regular intervals, starting from full speed to avoid low-speed
+    // kickstart issues, although this requires us to wait more for the PID stabilization.
+    // Normally exhibits logarithmic behavior with the stock fan+shroud, so the shorter interval
+    // at lower speeds is helpful to increase the resolution of the interpolation.
+    fanSpeedSoftPwm = 255;
+    wait(30000);
+
+    for(int8_t i = TEMP_MODEL_R_SIZE; i > 0; i -= TEMP_MODEL_CAL_R_STEP) {
+        fanSpeedSoftPwm = 256 / TEMP_MODEL_R_SIZE * i - 1;
+        wait(10000);
+
+        printf_P(PSTR("TM: R[%u] estimation\n"), (unsigned)soft_pwm_fan);
+        samples = record();
+        estimate(samples, &temp_model::data.R[soft_pwm_fan],
+            TEMP_MODEL_Rl, temp_model::data.R[0], TEMP_MODEL_R_thr, TEMP_MODEL_R_itr,
+            current_temperature_ambient);
+    }
+
+    // interpolate remaining steps to speed-up calibration
+    int8_t next = TEMP_MODEL_R_SIZE - 1;
+    for(uint8_t i = TEMP_MODEL_R_SIZE - 2; i != 0; --i) {
+        if(!((TEMP_MODEL_R_SIZE - i - 1) % TEMP_MODEL_CAL_R_STEP)) {
+            next = i;
+            continue;
+        }
+        int8_t prev = next - TEMP_MODEL_CAL_R_STEP;
+        if(prev < 0) prev = 0;
+        float f = (float)(i - prev) / TEMP_MODEL_CAL_R_STEP;
+        float d = (temp_model::data.R[next] - temp_model::data.R[prev]);
+        temp_model::data.R[i] = temp_model::data.R[prev] + d * f;
+    }
+
+    // restore the initial state
+    fanSpeedSoftPwm = 0;
+    target_temperature[0] = 0;
+    temp_model_set_enabled(was_enabled);
+}
+
+} // namespace temp_model_cal
+
+void temp_model_autotune(int16_t temp)
 {
-    // TODO
+    // TODO: ensure printer is idle/queue empty/buffer empty
+    KEEPALIVE_STATE(IN_PROCESS);
+    temp_model_cal::autotune(temp > 0 ? temp : TEMP_MODEL_CAL_Th);
+    temp_model_report_settings();
 }
 
 #ifdef TEMP_MODEL_DEBUG

Firmware/temperature.h

@@ -240,7 +240,7 @@ void temp_model_reset_settings();
 void temp_model_load_settings();
 void temp_model_save_settings();
 
-void temp_model_autotune(float temp = NAN);
+void temp_model_autotune(int16_t temp = 0);
 
 #ifdef TEMP_MODEL_DEBUG
 void temp_model_log_enable(bool enable);

Firmware/variants/1_75mm_MK3S-EINSy10a-E3Dv6full.h

@@ -435,7 +435,6 @@
 #define TEMP_MODEL_R_thr 0.01 // R estimation iteration threshold
 #define TEMP_MODEL_R_itr 30   // R estimation iteration limit
 
-#define TEMP_MODEL_Rf_D -15   // initial guess for resistance change with full-power fan
 #define TEMP_MODEL_Ta_corr -7 // Default ambient temperature correction
 #define TEMP_MODEL_LAG 2.1    // Temperature transport delay (s)