gpa in double prescision

Dependencies:   mbed

Revision:
12:e54941459353
Parent:
7:b3c5116e9fab
--- a/PI_Cntrl.cpp	Wed Apr 04 09:32:06 2018 +0000
+++ b/PI_Cntrl.cpp	Wed Apr 04 14:17:31 2018 +0000
@@ -1,48 +1,61 @@
+/*  
+    PI Controller class with anti windup reset in biquad transposed direct form 2
+    see e.g.: https://www.dsprelated.com/freebooks/filters/Four_Direct_Forms.html
+    everything is calculated in double
+    
+                  Tn*s + 1                      
+      G(s) = Kp -------------  with s ~ (1 - z^-1)/Ts
+                    Ts*s                     
+*/
+
 #include "PI_Cntrl.h"
 
 using namespace std;
 
-PI_Cntrl::PI_Cntrl(float Kp_, float Tn_){
-    Kp = Kp_;
-    Tn = Tn_;
-    i_part_old = 0;
-    del = 0;
-    out_max =1e10;
-    out_min=-1e10;
-    }
+PI_Cntrl::PI_Cntrl(float Kp, float Tn, float Ts)
+{
+    setCoefficients(Kp, Tn, Ts);
+    uMax = 10000000000.0;
+    uMin = -uMax;
+    reset(0.0f);
+}
 
-PI_Cntrl::PI_Cntrl(float Kp_, float Tn_,float ma){
-    Kp = Kp_;
-    Tn = Tn_;
-    i_part_old = 0;
-    del = 0;
-    out_max = ma;
-    out_min = -ma;
-    }
+PI_Cntrl::PI_Cntrl(float Kp, float Tn, float Ts, float uMax)
+{
+    setCoefficients(Kp, Tn, Ts);
+    this->uMax = (double)uMax;
+    uMin = -(double)uMax;
+    reset(0.0f);
+}
 
-PI_Cntrl::~PI_Cntrl() {} 
-    
-void PI_Cntrl::reset(float initValue) {
-    i_part_old = initValue;
-    del = 0;
+PI_Cntrl::PI_Cntrl(float Kp, float Tn, float Ts, float uMax, float uMin)
+{
+    setCoefficients(Kp, Tn, Ts);
+    this->uMax = (double)uMax;
+    this->uMin = (double)uMin;
+    reset(0.0f);
 }
 
-float PI_Cntrl::doStep(float error){
-    float kpe = Kp * error;
-    float i_part = Ts/Tn*(kpe-1.0f*del) + i_part_old; // I with windup subtraction
-    i_part_old = i_part; 
-    float y_out_temp = (kpe + i_part);                   // unconstrained output
-    float y_out;
-    // -------- limit output --------
-    if(y_out_temp > out_max)
-        y_out = out_max;
-    else if(y_out_temp < out_min)
-        y_out = out_min;
-    else
-        y_out = y_out_temp;
-    // -------- Anti-Windup -------- 
-    del = (y_out_temp - y_out);
-    // -------- Timeshift --------
-    
-    return y_out;
-    }
\ No newline at end of file
+PI_Cntrl::~PI_Cntrl() {}
+
+void PI_Cntrl::reset(float initValue)
+{
+    s = (double)initValue;
+}
+
+void PI_Cntrl::setCoefficients(float Kp, float Tn, float Ts)
+{
+    b0 = (double)Kp*(1.0 + (double)Ts/(double)Tn);
+    b1 = -(double)Kp;
+    b2 = (double)Ts/(double)Tn;    
+}
+
+float PI_Cntrl::doStep(double e)
+{
+    double u = b0*e + s;          // unconstrained output
+    double uc = u;                // constrained output
+    if(u > uMax) uc = uMax;
+    else if(u < uMin) uc = uMin;
+    s = b1*e + u - b2*(u - uc);
+    return (float)uc;
+}
\ No newline at end of file