From 69582e36accea9233a6686d49f2612a458adde09 Mon Sep 17 00:00:00 2001 From: dragm83 <69442263+dragm83@users.noreply.github.com> Date: Sun, 16 Nov 2025 14:27:27 -0500 Subject: [PATCH 1/2] Implement flow-based thermal feedforward for heater control - only enabled when 3wv is on - use calculated flow rate - use Kff parameter (combined thermal gain) - additional ff post-scaling to limit upside overshoot --- .../src/GaggiMateController.cpp | 20 ++++- .../src/peripherals/DimmedPump.h | 2 + .../src/peripherals/Heater.cpp | 84 ++++++++++++++++++- .../src/peripherals/Heater.h | 21 +++++ lib/NayrodPID/src/SimplePID/SimplePID.cpp | 16 +++- lib/NayrodPID/src/SimplePID/SimplePID.h | 12 +++ lib/NimBLEComm/src/NimBLEClientController.cpp | 8 +- lib/NimBLEComm/src/NimBLEComm.h | 2 +- lib/NimBLEComm/src/NimBLEServerController.cpp | 21 ++++- src/display/core/Controller.cpp | 9 +- src/display/core/constants.h | 2 +- web/src/pages/Settings/index.jsx | 35 ++++++++ 12 files changed, 216 insertions(+), 16 deletions(-) diff --git a/lib/GaggiMateController/src/GaggiMateController.cpp b/lib/GaggiMateController/src/GaggiMateController.cpp index f6300963e..1657693ff 100644 --- a/lib/GaggiMateController/src/GaggiMateController.cpp +++ b/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(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(pump); diff --git a/lib/GaggiMateController/src/peripherals/DimmedPump.h b/lib/GaggiMateController/src/peripherals/DimmedPump.h index 3ed5de53f..bada4c8dd 100644 --- a/lib/GaggiMateController/src/peripherals/DimmedPump.h +++ b/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(&_valveStatus); } // For thermal feedforward valve state void tare(); void setFlowTarget(float targetFlow, float pressureLimit); diff --git a/lib/GaggiMateController/src/peripherals/Heater.cpp b/lib/GaggiMateController/src/peripherals/Heater.cpp index a11c1b249..ee5c3d540 100644 --- a/lib/GaggiMateController/src/peripherals/Heater.cpp +++ b/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(arg); diff --git a/lib/GaggiMateController/src/peripherals/Heater.h b/lib/GaggiMateController/src/peripherals/Heater.h index 878920842..78b58fdc5 100644 --- a/lib/GaggiMateController/src/peripherals/Heater.h +++ b/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); }; diff --git a/lib/NayrodPID/src/SimplePID/SimplePID.cpp b/lib/NayrodPID/src/SimplePID/SimplePID.cpp index 871f49e67..1bb7a34d8 100644 --- a/lib/NayrodPID/src/SimplePID/SimplePID.cpp +++ b/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); +} diff --git a/lib/NayrodPID/src/SimplePID/SimplePID.h b/lib/NayrodPID/src/SimplePID/SimplePID.h index 26b7dce1d..ff2f37eff 100644 --- a/lib/NayrodPID/src/SimplePID/SimplePID.h +++ b/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 diff --git a/lib/NimBLEComm/src/NimBLEClientController.cpp b/lib/NimBLEComm/src/NimBLEClientController.cpp index 4c569006b..4d3cf0c11 100644 --- a/lib/NimBLEComm/src/NimBLEClientController.cpp +++ b/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))) { diff --git a/lib/NimBLEComm/src/NimBLEComm.h b/lib/NimBLEComm/src/NimBLEComm.h index 1575f238a..6603aca82 100644 --- a/lib/NimBLEComm/src/NimBLEComm.h +++ b/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; -using pid_control_callback_t = std::function; +using pid_control_callback_t = std::function; using pump_model_coeffs_callback_t = std::function; using ping_callback_t = std::function; using remote_err_callback_t = std::function; diff --git a/lib/NimBLEComm/src/NimBLEServerController.cpp b/lib/NimBLEComm/src/NimBLEServerController.cpp index 8ae8c86a3..697f317cb 100644 --- a/lib/NimBLEComm/src/NimBLEServerController.cpp +++ b/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()); diff --git a/src/display/core/Controller.cpp b/src/display/core/Controller.cpp index 4aeabd2a1..6b68375d4 100644 --- a/src/display/core/Controller.cpp +++ b/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; diff --git a/src/display/core/constants.h b/src/display/core/constants.h index 4b870aa99..9ee6cb35a 100644 --- a/src/display/core/constants.h +++ b/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" diff --git a/web/src/pages/Settings/index.jsx b/web/src/pages/Settings/index.jsx index 96c19269f..a8522a261 100644 --- a/web/src/pages/Settings/index.jsx +++ b/web/src/pages/Settings/index.jsx @@ -46,6 +46,19 @@ export function Settings() { dashboardLayout: fetchedSettings.dashboardLayout || DASHBOARD_LAYOUTS.ORDER_FIRST, }; + // Extract Kf from PID string and separate them + if (fetchedSettings.pid) { + const pidParts = fetchedSettings.pid.split(','); + if (pidParts.length >= 4) { + // PID string has Kf as 4th parameter + settingsWithToggle.pid = pidParts.slice(0, 3).join(','); // First 3 params + settingsWithToggle.kf = pidParts[3]; // 4th parameter + } else { + // No Kf in PID string, use default + settingsWithToggle.kf = '0.000'; + } + } + // Initialize auto-wakeup schedules if (fetchedSettings.autowakeupSchedules) { // Parse new schedule format: "time1|days1;time2|days2" @@ -179,6 +192,12 @@ export function Settings() { formData.altRelayFunction !== undefined ? formData.altRelayFunction : 1, ); + // Combine PID and Kf into single PID string + if (formData.pid && formData.kf !== undefined) { + const combinedPid = `${formData.pid},${formData.kf}`; + formDataToSubmit.set('pid', combinedPid); + } + // Add auto-wakeup schedules const schedulesStr = autowakeupSchedules .map(schedule => `${schedule.time}|${schedule.days.map(d => (d ? '1' : '0')).join('')}`) @@ -546,6 +565,22 @@ export function Settings() { /> +
+ + +
+