Implement flow-based thermal feedforward for heater control

lib/GaggiMateController/src/GaggiMateController.cpp

@@ -73,7 +73,17 @@ void GaggiMateController::setup() {
         pressureSensor->setup();
         _ble.registerPressureScaleCallback([this](float scale) { this->pressureSensor->setScale(scale); });
     }
-
+   // Set up thermal feedforward for main heater if pressure/dimming capability exists
+    if (heater && _config.capabilites.dimming && _config.capabilites.pressure) {
+        auto dimmedPump = static_cast<DimmedPump *>(pump);
+        float* pumpFlowPtr = dimmedPump->getPumpFlowPtr();
+        bool* valveStatusPtr = dimmedPump->getValveStatusPtr();
+
+        heater->setThermalFeedforward(pumpFlowPtr, 23.0f, valveStatusPtr);
+        heater->setFeedforwardScale(0.0f);
+
+
+    } 
     // Initialize last ping time
     lastPingTime = millis();
 
@@ -111,7 +121,13 @@ void GaggiMateController::setup() {
             dimmedPump->setValveState(valve);
         });
     _ble.registerAltControlCallback([this](bool state) { this->alt->set(state); });
-    _ble.registerPidControlCallback([this](float Kp, float Ki, float Kd) { this->heater->setTunings(Kp, Ki, Kd); });
+    _ble.registerPidControlCallback([this](float Kp, float Ki, float Kd, float Kf) { 
+        this->heater->setTunings(Kp, Ki, Kd); 
+
+        // Apply thermal feedforward parameters if available
+        this->heater->setFeedforwardScale(Kf);
+
+    });
     _ble.registerPumpModelCoeffsCallback([this](float a, float b, float c, float d) {
         if (_config.capabilites.dimming) {
             auto dimmedPump = static_cast<DimmedPump *>(pump);

lib/GaggiMateController/src/peripherals/DimmedPump.h

@@ -21,6 +21,8 @@ class DimmedPump : public Pump {
     float getPumpFlow();
     float getPuckFlow();
     float getPuckResistance();
+    float* getPumpFlowPtr() { return &_currentFlow; }  // For thermal feedforward
+    bool* getValveStatusPtr() { return reinterpret_cast<bool*>(&_valveStatus); }  // For thermal feedforward valve state
     void tare();
 
     void setFlowTarget(float targetFlow, float pressureLimit);

lib/GaggiMateController/src/peripherals/Heater.cpp

@@ -65,6 +65,23 @@ void Heater::setTunings(float Kp, float Ki, float Kd) {
     }
 }
 
+
+void Heater::setThermalFeedforward(float *pumpFlowPtr, float incomingWaterTemp, bool *valveStatusPtr) {
+    pumpFlowRate = pumpFlowPtr;
+    valveStatus = valveStatusPtr;
+    this->incomingWaterTemp = incomingWaterTemp;
+
+    ESP_LOGI(LOG_TAG, "Thermal feedforward setup - incoming water temp: %.1f°C, valve tracking: %s", 
+             incomingWaterTemp, valveStatusPtr ? "enabled" : "disabled");
+    ESP_LOGI(LOG_TAG, "Feedforward will be %s based on Kff value (currently %.3f)", 
+             combinedKff > 0.0f ? "ENABLED" : "DISABLED", combinedKff);
+}
+
+void Heater::setFeedforwardScale(float combinedKff) {
+    this->combinedKff = combinedKff;
+    ESP_LOGI(LOG_TAG, "Combined feedforward gain (Kff) set to: %.3f output units per watt", combinedKff);
+}
+
 void Heater::autotune(int goal, int windowSize) {
     setupAutotune(goal, windowSize);
     autotuning = true;
@@ -73,7 +90,35 @@ void Heater::autotune(int goal, int windowSize) {
 void Heater::loopPid() {
     softPwm(TUNER_OUTPUT_SPAN);
     temperature = sensor->read();
-    if (simplePid->update()) {
+
+    // Calculate and set disturbance feedforward BEFORE PID update
+    // Only apply thermal feedforward when Kf>0, valve is open, and water is flowing
+    if (combinedKff > 0.0f && pumpFlowRate && *pumpFlowRate > 0.01f && valveStatus && *valveStatus) {
+        float currentFlowRate = *pumpFlowRate; // Use raw flow rate for fast response
+        float disturbanceGain = calculateDisturbanceFeedforwardGain();
+
+        // Apply smoothed temperature-based safety scaling
+        float tempError = temperature - setpoint;
+        float rawSafetyFactor = calculateSafetyScaling(tempError);
+
+        // Smooth safety factor transitions to reduce oscillations
+        const float safetyAlpha = 0.85f; // Faster response for quicker feedforward
+        float safetyFactor = safetyAlpha * rawSafetyFactor + (1.0f - safetyAlpha) * lastSafetyFactor;
+        lastSafetyFactor = safetyFactor;
+
+        disturbanceGain *= safetyFactor;
+
+        // Set the disturbance feedforward in SimplePID
+        simplePid->setDisturbanceFeedforward(currentFlowRate, disturbanceGain);
+
+    } else {
+        simplePid->setDisturbanceFeedforward(0.0f, 0.0f);
+    }
+
+    // Now run PID with proper feedforward integrated
+    bool pidUpdated = simplePid->update();
+
+    if (pidUpdated) {
         plot(output, 1.0f, 1);
     }
 }
@@ -151,6 +196,43 @@ void Heater::plot(float optimumOutput, float outputScale, uint8_t everyNth) {
         plotCount++;
 }
 
+float Heater::calculateDisturbanceFeedforwardGain() {
+    if (combinedKff <= 0.0f || !pumpFlowRate || *pumpFlowRate <= 0.01f) {
+        return 0.0f;
+    }
+
+    float currentFlowRate = *pumpFlowRate; // Use raw flow rate for fast response
+
+    // Calculate temperature difference (target - incoming water temperature)
+    float tempDelta = setpoint - incomingWaterTemp;
+    if (tempDelta <= 0.0f) return 0.0f;
+
+    // Calculate thermal power needed per ml/s of flow (Watts per ml/s)
+    float powerPerFlowRate = WATER_DENSITY * WATER_SPECIFIC_HEAT * tempDelta + (heatLossWatts / currentFlowRate);
+    powerPerFlowRate /= heaterEfficiency;
+
+    // Apply combined Kff directly (output units per watt)
+    float gainPerFlowRate = powerPerFlowRate * combinedKff;
+
+    return gainPerFlowRate;
+}
+
+float Heater::calculateSafetyScaling(float tempError) {
+    // tempError = temperature - setpoint
+    // Use smoother, less aggressive safety scaling to reduce oscillations
+    if (tempError > 1.0f) {
+        return 0.0f; // No FF if more than 1.0°C above setpoint
+    } else if (tempError > 0.0f) {
+        // Gradual reduction: 100% at 0°C error, 70% at +1.0°C error
+        return 0.7f + 0.3f * (1.0f - tempError / 1.0f);
+    } else if (tempError > -1.0f) {
+        // Scale from 70% to 100% as temperature drops below setpoint
+        return 0.7f + 0.3f * std::abs(tempError) / 1.0f;
+    } else {
+        return 1.0f; // Full FF when more than 1.0°C below setpoint
+    }
+}
+
 void Heater::loopTask(void *arg) {
     TickType_t lastWake = xTaskGetTickCount();
     auto *heater = static_cast<Heater *>(arg);

lib/GaggiMateController/src/peripherals/Heater.h

@@ -25,6 +25,12 @@ class Heater {
     void setSetpoint(float setpoint);
     void setTunings(float Kp, float Ki, float Kd);
     void autotune(int goal, int windowSize);
+
+
+    // Thermal feedforward control
+    void setThermalFeedforward(float *pumpFlowPtr = nullptr, float incomingWaterTemp = 23.0f, bool *valveStatusPtr = nullptr);
+    void setFeedforwardScale(float combinedKff);  // Set combined Kff value (output units per watt)
+
 
   private:
     void setupPid();
@@ -34,6 +40,8 @@ class Heater {
     float softPwm(uint32_t windowSize);
     void plot(float optimumOutput, float outputScale, uint8_t everyNth);
     void setTuningGoal(float percent);
+    float calculateDisturbanceFeedforwardGain();
+    float calculateSafetyScaling(float tempError);
     TemperatureSensor *sensor;
     uint8_t heaterPin;
     xTaskHandle taskHandle;
@@ -59,6 +67,19 @@ class Heater {
     bool startup = true;
     bool autotuning = false;
 
+    // Thermal feedforward variables
+    float *pumpFlowRate = nullptr;
+    bool *valveStatus = nullptr;
+    float lastSafetyFactor = 1.0f;  // For smooth safety scaling transitions
+    float incomingWaterTemp = 23.0f;
+    float heaterEfficiency = 0.95f;  // 95% efficiency (immersion heater)
+    float heatLossWatts = 5.0f;      // 5W heat loss (well-insulated boiler)
+    float combinedKff = 0.0f;        // Combined feedforward gain (output units per watt) - disabled by default
+
+    // Thermal model constants
+    static constexpr float WATER_DENSITY = 1.0f;        // g/ml
+    static constexpr float WATER_SPECIFIC_HEAT = 4.18f; // J/(g·°C)
+
     const char *LOG_TAG = "Heater";
     static void loopTask(void *arg);
 };

lib/NayrodPID/src/SimplePID/SimplePID.cpp

@@ -33,6 +33,8 @@ bool SimplePID::update() {
 
     // Compute the filtered setpoint values
     float FFOut = 0.0f;
+    float DistFFOut = 0.0f;
+
     if (isfilterSetpointActive) {
         setpointFiltering(setpointFilterFreq);
     } else {
@@ -41,6 +43,10 @@ bool SimplePID::update() {
 
     if (isFeedForwardActive)
         FFOut = setpointDerivative * gainFF;
+
+    if (isDisturbanceFeedForwardActive)
+        DistFFOut = currentDisturbance * gainDistFF;
+
     Serial.printf("%.2f\t %.2f\t %.2f\t %.2f\n", *setpointTarget, setpointFiltered, setpointDerivative, *sensorOutput);
 
     float deltaTime = 1.0f / ctrl_freq_sampling; // Time step in seconds
@@ -57,7 +63,7 @@ bool SimplePID::update() {
     float Dout = gainKd * derivative;
 
     // Calculate the output before antiwindup clamping
-    float sumPID = Pout + Iout + Dout + FFOut;
+    float sumPID = Pout + Iout + Dout + FFOut + DistFFOut;
     float sumPIDsat = constrain(sumPID, ctrlOutputLimits[0], ctrlOutputLimits[1]);
 
     // Antiwindup clamping
@@ -71,7 +77,7 @@ bool SimplePID::update() {
             error * deltaTime; // Forbide the integration to happen when the output is saturated and the error is in the same
                                // direction as the output (i.e. the system is not able to follow the setpoint)
         Iout = gainKi * feedback_integralState; // Recompute the integral term with the new state
-        sumPID = Pout + Iout + Dout + FFOut;    // Recompute the output with the new integral state
+        sumPID = Pout + Iout + Dout + FFOut + DistFFOut;    // Recompute the output with the new integral state
         sumPIDsat = constrain(sumPID, ctrlOutputLimits[0], ctrlOutputLimits[1]);
     }
 
@@ -183,3 +189,9 @@ void SimplePID::activateFeedForward(bool flag) {
         isFeedForwardActive = flag;
     }
 }
+
+void SimplePID::setDisturbanceFeedforward(float disturbance, float gainDFF) {
+    currentDisturbance = disturbance;
+    gainDistFF = gainDFF;
+    isDisturbanceFeedForwardActive = (gainDFF != 0.0f);
+}

lib/NayrodPID/src/SimplePID/SimplePID.h

@@ -43,6 +43,12 @@ class SimplePID {
     void setKi(float val) { gainKi = val; };
     void setKd(float val) { gainKd = val; };
     void setKFF(float val) { gainFF = val; };
+
+    // Disturbance feedforward methods
+    void setDisturbanceFeedforward(float disturbance, float gainDFF);
+    void setDisturbanceGain(float gainDFF) { gainDistFF = gainDFF; };
+    float getDisturbanceGain() { return gainDistFF; };
+    void activateDisturbanceFeedForward(bool flag) { isDisturbanceFeedForwardActive = flag; };
 
   private:
     // setpoint filtering
@@ -66,6 +72,12 @@ class SimplePID {
     float gainKi = 0.0f; // Integral gain (multiplies by Kp if Kp,Ki,Kd are strictly parallèle (no factoring by Kp))
     float gainKd = 0.0f; // Derivative gain (by default no derivative term)
     float gainFF = 0.5 * 1000.0f / 2.5f; // Feedforward gain
+
+    // Disturbance feedforward variables
+    float gainDistFF = 0.0f;                    // Disturbance feedforward gain
+    float currentDisturbance = 0.0f;            // Current disturbance value
+    bool isDisturbanceFeedForwardActive = false; // Flag to activate disturbance feedforward
+
     float feedback_integralState = 0.0f; // Integral state
     float prevError = 0.0f;              // Previous error for derivative calculation
     float prevOutput = 0.0f;             // Previous output for derivative calculation

lib/NimBLEComm/src/NimBLEClientController.cpp

@@ -285,7 +285,13 @@ void NimBLEClientController::notifyCallback(NimBLERemoteCharacteristic *pRemoteC
             float Kp = get_token(settings, 0, ',').toFloat();
             float Ki = get_token(settings, 1, ',').toFloat();
             float Kd = get_token(settings, 2, ',').toFloat();
-            autotuneResultCallback(Kp, Ki, Kd);
+
+            // Handle optional Kf parameter with default
+            float Kf = 0.0f;  // Default combined Kff
+            String kfToken = get_token(settings, 3, ',');
+            if (kfToken.length() > 0) Kf = kfToken.toFloat();
+
+            autotuneResultCallback(Kp, Ki, Kd, Kf);
         }
     }
     if (pRemoteCharacteristic->getUUID().equals(NimBLEUUID(VOLUMETRIC_MEASUREMENT_UUID))) {

lib/NimBLEComm/src/NimBLEComm.h

@@ -33,7 +33,7 @@ constexpr size_t ERROR_CODE_RUNAWAY = 4;
 constexpr size_t ERROR_CODE_TIMEOUT = 5;
 
 using pin_control_callback_t = std::function<void(bool isActive)>;
-using pid_control_callback_t = std::function<void(float Kp, float Ki, float Kd)>;
+using pid_control_callback_t = std::function<void(float Kp, float Ki, float Kd, float Kf)>;
 using pump_model_coeffs_callback_t = std::function<void(float a, float b, float c, float d)>;
 using ping_callback_t = std::function<void()>;
 using remote_err_callback_t = std::function<void(int errorCode)>;

lib/NimBLEComm/src/NimBLEServerController.cpp

@@ -121,8 +121,9 @@ void NimBLEServerController::sendSteamBtnState(bool steamButtonStatus) {
 
 void NimBLEServerController::sendAutotuneResult(float Kp, float Ki, float Kd) {
     if (deviceConnected) {
-        char pidStr[30];
-        snprintf(pidStr, sizeof(pidStr), "%.3f,%.3f,%.3f", Kp, Ki, Kd);
+        char pidStr[64];
+        // Send with default Kf=0.0 (disabled)
+        snprintf(pidStr, sizeof(pidStr), "%.3f,%.3f,%.3f,0.0", Kp, Ki, Kd);
         autotuneResultChar->setValue(pidStr);
         autotuneResultChar->notify();
     }
@@ -236,9 +237,21 @@ void NimBLEServerController::onWrite(NimBLECharacteristic *pCharacteristic) {
         float Kp = get_token(pid, 0, ',').toFloat();
         float Ki = get_token(pid, 1, ',').toFloat();
         float Kd = get_token(pid, 2, ',').toFloat();
-        ESP_LOGV(LOG_TAG, "Received PID settings: %.2f, %.2f, %.2f", Kp, Ki, Kd);
+
+        // Optional thermal feedforward parameter (default value if not provided)
+        float Kf = 0.0f;    // Default combined feedforward gain
+
+        String kfToken = get_token(pid, 3, ',');
+
+        if (kfToken.length() > 0 && kfToken.toFloat() > 0.0f) {
+            Kf = kfToken.toFloat();
+        }
+
+        ESP_LOGI(LOG_TAG, "BLE received PID string: '%s'", pid.c_str());
+        ESP_LOGI(LOG_TAG, "Parsed PID: Kp=%.2f, Ki=%.2f, Kd=%.2f, Kf=%.3f (combined)", 
+                 Kp, Ki, Kd, Kf);
         if (pidControlCallback != nullptr) {
-            pidControlCallback(Kp, Ki, Kd);
+            pidControlCallback(Kp, Ki, Kd, Kf);
         }
     } else if (pCharacteristic->getUUID().equals(NimBLEUUID(PUMP_MODEL_COEFFS_CHAR_UUID))) {
         auto pumpModelCoeffs = String(pCharacteristic->getValue().c_str());

src/display/core/Controller.cpp

@@ -160,10 +160,11 @@ void Controller::setupBluetooth() {
             ESP_LOGE(LOG_TAG, "Received error %d", error);
         }
     });
-    clientController.registerAutotuneResultCallback([this](const float Kp, const float Ki, const float Kd) {
-        ESP_LOGI(LOG_TAG, "Received new autotune values: %.3f, %.3f, %.3f", Kp, Ki, Kd);
-        char pid[30];
-        snprintf(pid, sizeof(pid), "%.3f,%.3f,%.3f", Kp, Ki, Kd);
+    clientController.registerAutotuneResultCallback([this](const float Kp, const float Ki, const float Kd, const float Kf) {
+        ESP_LOGI(LOG_TAG, "Received autotune values: Kp=%.3f, Ki=%.3f, Kd=%.3f, Kf=%.3f (combined)", Kp, Ki, Kd, Kf);
+        char pid[64];
+        // Store in simplified format with combined Kf
+        snprintf(pid, sizeof(pid), "%.3f,%.3f,%.3f,%.3f", Kp, Ki, Kd, Kf);
         settings.setPid(String(pid));
         pluginManager->trigger("controller:autotune:result");
         autotuning = false;

src/display/core/constants.h

@@ -16,7 +16,7 @@
 #define MAX_TEMP 160
 #define DEFAULT_TEMPERATURE_OFFSET 0
 #define DEFAULT_PRESSURE_SCALING 16.0f
-#define DEFAULT_PID "58.397,1.027,249.055"
+#define DEFAULT_PID "58.397,1.027,249.055,0.0"
 #define DEFAULT_PUMP_MODEL_COEFFS "10.205,5.521"
 #define DEFAULT_MDNS_NAME "gaggimate"
 #define DEFAULT_OTA_CHANNEL "latest"