From 1f8fe3af50f6abab7e5cb26b49fa1f1b2c0fda9f Mon Sep 17 00:00:00 2001
From: Uwe Jakobeit <info@2030.net>
Date: Mon, 6 Apr 2026 00:07:33 +0200
Subject: [PATCH] KS ( plant) taken from earlier example for a complete
 environment; some formatting on curly brackets and indentations

---
 app/TestModel.cpp | 196 ++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 196 insertions(+)
 create mode 100755 app/TestModel.cpp

diff --git a/app/TestModel.cpp b/app/TestModel.cpp
new file mode 100755
index 0000000..2630509
--- /dev/null
+++ b/app/TestModel.cpp
@@ -0,0 +1,196 @@
+#include <vector>
+#include <array>
+#include <iostream>
+#include <chrono>
+#include <thread>
+class HeatModel
+{
+public:
+    using State = std::array<double,3>;
+    HeatModel(State x0,int log_size=600)
+    {
+        T_hist_.reserve(log_size);
+        log_size_=log_size;
+        log_counter_= 0;
+        T_=x0;
+    };
+
+    void initialize(double T_start,double T_amb)
+    {
+        T_start_ = T_start;
+        T_amb_   = T_amb;
+        T_Air_ = T_amb;
+    }
+
+    // Log the Temperature. Return true if log is big enough, return false if log is already full
+    //you can use this as a timer (while (log(){}))
+    bool log()
+    {
+        if (log_counter_< log_size_-1)
+        {
+            T_hist_.push_back(T_Air_);
+            log_counter_++;
+            std::cout <<"T="<<T_Air_<<"\n";
+            return true;
+        }
+        else
+        {
+            T_hist_.push_back(T_Air_);
+            log_counter_++;
+            std::cout<<"Log exceeded Reservation Limit \n";
+            return false;
+        }
+
+    }
+
+    double get_Temp()
+    {
+        return T_Air_;
+    }
+
+    // You can call stepfunction with Bool Parameter(stepPWM) if you have PWM output, or with double parameter
+    // if you have normalized heating Power (0,1) (stepPower)
+    // Call this Function at least every 250ms (cycletime of Algortihm)
+    void stepPWM(double dt, bool u)
+    {
+        double u_double;
+        if (u)
+        {
+            u_double=1.0;
+        }
+        else
+        {
+            u_double=0.0;
+        }
+        u_double = u_double *1000.0;
+        const State k1 = Func(T_, u_double);
+        const State k2 = Func(addScaled(T_, k1, 0.5 * dt), u_double);
+        const State k3 = Func(addScaled(T_, k2, 0.5 * dt), u_double);
+        const State k4 = Func(addScaled(T_, k3, dt),        u_double);
+
+        for (int i = 0; i < 3; ++i)
+        {
+            T_[i] += (dt / 6.0) * (k1[i] + 2.0 * k2[i] + 2.0 * k3[i] + k4[i]);
+        }
+        T_Air_=T_[0];
+    }
+
+    void stepPower(double dt, double u)
+    {
+        u = u *1000.0;
+        const State k1 = Func(T_, u);
+        const State k2 = Func(addScaled(T_, k1, 0.5 * dt), u);
+        const State k3 = Func(addScaled(T_, k2, 0.5 * dt), u);
+        const State k4 = Func(addScaled(T_, k3, dt),        u);
+
+        for (int i = 0; i < 3; ++i)
+        {
+            T_[i] += (dt / 6.0) * (k1[i] + 2.0 * k2[i] + 2.0 * k3[i] + k4[i]);
+        }
+        T_Air_=T_[0];
+    }
+private:
+
+    double T_start_;
+    int log_size_;
+    double T_Air_;
+    double T_amb_;
+    int log_counter_;
+    std::vector<int> T_hist_;
+    State T_;
+
+    /*
+    LTI System Matrices for FP56
+     A =
+                  x1         x2         x3
+       x1  -0.007659   0.003217   0.004442
+       x2    0.08422   -0.08422          0
+       x3   0.005135          0  -0.005856
+
+      B =
+                 u1
+       x1         0
+       x2  0.008313
+       x3         0
+
+      C =
+           x1  x2  x3
+       y1   1   0   0
+
+      D =
+           u1
+       y1   0 */
+
+    const std::array<std::array<double, 3>, 3> A_{{
+            { -0.007659,  0.003217,  0.00442 },
+            {  0.08422, -0.08422,  0.0 },
+            {  0.005135,  0.0, -0.005856 }
+    }};
+    const std::vector<double> B_{0.0,0.008313, 0.00};
+    const std::vector<double> C_{1.0, 0.0, 0.0};
+
+
+
+    static State addScaled(const State& x, const State& dx, double scale)
+    {
+        State out{};
+        for (int i = 0; i < 3; ++i)
+        {
+            out[i] = x[i] + scale * dx[i];
+        }
+        return out;
+    }
+
+
+    State Func(const State& x,double u)const
+    {
+        State dx=matVec(A_,x);
+        for (int i=0;i<3;i++)
+        {
+            dx[i]+=B_[i]*u;
+        }
+        return dx;
+    }
+
+    static State matVec(const std::array<std::array<double, 3>, 3>& M, const State& x)
+    {
+        State y{};
+        for (int i= 0; i < 3; ++i)
+        {
+            double sum = 0.0;
+            for (int j = 0; j < 3; ++j)
+            {
+                sum += M[i][j] * x[j];
+            }
+            y[i] = sum;
+        }
+        return y;
+    }
+};
+
+
+#define SINGLE_APP 1
+
+#if SINGLE_APP
+int main(){
+    HeatModel::State x0={22.0,22.0,22.0};
+    int STEP_TIME=50; // dt in milliseconds
+    // Heatmodel(state starting_Values, size_of_log)
+    HeatModel TestModel(x0,5);
+    std::vector<double> temp;
+    temp.reserve(6000);
+    bool running = true;
+    int counter=0;
+    while (running){
+        TestModel.stepPower(STEP_TIME/1000.0,1.0);
+        temp.push_back(TestModel.get_Temp());
+        counter++;
+        //logg every Minute --> simulate for 5 minutes
+        if (counter >=10){
+            running=TestModel.log();
+            counter = 0;
+        }
+        std::this_thread::sleep_for(std::chrono::milliseconds(STEP_TIME));
+    }
+}
+#endif