Diff: GPA.cpp

Revision:

22:715d351d0be7

Parent:

21:d9bda15377e2

Child:

23:26a1ccd0a856

diff -r d9bda15377e2 -r 715d351d0be7 GPA.cpp
--- a/GPA.cpp	Mon Apr 09 07:06:04 2018 +0000
+++ b/GPA.cpp	Mon Apr 09 14:48:55 2018 +0000
@@ -1,6 +1,6 @@
 /*
     GPA: frequency point wise gain and phase analyser to measure the frequency
-         respone of a dynamical system
+         respone of dynamical system
 
         hint:       the measurements should only be perfomed in closed loop
         assumption: the system is at the desired steady state of interest when
@@ -22,49 +22,38 @@
         fMax:     maximal desired frequency that should be measured in Hz
         NfexcDes: number of logarithmic equaly spaced frequency points
         NperMin:  minimal number of periods that are used for each frequency point
-        NmeasMin: minimal number of samples that are used for each frequency point
+        NmeasMin: maximal number of samples that are used for each frequency point
         Ts:       sampling time
         Aexc0:    excitation amplitude at fMin
         Aexc1:    excitation amplitude at fMax
 
         hints:    the amplitude drops with 1/fexc, if you're using
-                  Axc1 = Aexc0/fMax then d/dt exc = const., this is recommended
+                  Axc1 = Aexc0/fMax the d/dt exc = const., this is recommended
                   if your controller does not have a rolloff.
                   if a desired frequency point is not measured try to increase Nmeas.
 
-    pseudo code for a closed loop measurement with a controller C:
+    pseudo code for a closed loop measurement with a proportional controller Kp:
+
+        float inp = "measurement of inputsignal";
+        float out = "mesurement of outputsignal";
+        float exc = myGPA(inp, out);
+        float off = "desired steady state off the system";
 
         excitation input at (1):
-       
-        - measuring the plant P and the closed loop tf T = PC/(1 + PC):
-            desTorque = pi_w(omega_desired - omega + excWobble);
-            inpWobble = desTorque;
-            outWobble = omega;
-            excWobble = Wobble(excWobble, outWobble);
-                        
-        - measuring the controller C and the closed loop tf SC = C/(1 + PC)
-            desTorque = pi_w(omega_desired - omega + excWobble);
-            inpWobble = n_soll + excWobble - omega;
-            outWobble = desTorque;
-            excWobble = Wobble(inpWobble, outWobble);
+        inp = Kp*(exc + off - out);
 
         excitation input at (2):
-        
-        - measuring the plant P and the closed loop tf SP = P/(1 + PC):
-            desTorque = pi_w(omega_desired - omega) + excWobble;
-            inpWobble = desTorque;
-            outWobble = omega;
-            excWobble = Wobble(excWobble, outWobble);    
+        inp = Kp*(off - out) + exc;
 
     usage:
-        exc(k+1) = myGPA(inp(k), out(k)) does update the internal states of the 
-        gpa at the timestep k and returns the excitation signal for the timestep
-        k+1. the results are plotted to a terminal (putty) over a serial 
-        connection and look as follows:
+        exc = myGPA(inp, out) does update the internal states of the gpa at the
+        timestep k and returns the excitation signal for the timestep k+1. the
+        results are plotted to a terminal (putty) over serial cennection and look
+        as follows:
  -----------------------------------------------------------------------------------------
    fexc[Hz]    |Gyu|     ang(Gyu)    |Gye|     ang(Gye)     |E|        |U|        |Y|
  -----------------------------------------------------------------------------------------
-   1.000e+00  2.924e+01 -1.572e+00  1.017e+00 -4.983e-02  5.000e+00  1.739e-01  5.084e+00
+   7.01e-01   1.08e+01  -7.45e-02   1.08e+01  -7.38e-02   9.99e-01   9.99e-01   1.08e+01
 
     in matlab you can use:
         dataNucleo = [... insert measurement data here ...];
@@ -82,7 +71,7 @@
 #include "GPA.h"
 #include "mbed.h"
 #include "math.h"
-#define   pi 3.1415927f
+#define   pi 3.141592653589793
 
 using namespace std;
 
@@ -91,23 +80,23 @@
     this->NfexcDes = NfexcDes;
     this->NperMin = NperMin;
     this->NmeasMin = NmeasMin;
-    this->Ts = Ts;
+    this->Ts = (double)Ts;
 
     // calculate logarithmic spaced frequency points
-    fexcDes = (float*)malloc(NfexcDes*sizeof(float));
-    fexcDesLogspace(fMin, fMax, NfexcDes);
+    fexcDes = (double*)malloc(NfexcDes*sizeof(double));
+    fexcDesLogspace((double)fMin, (double)fMax, NfexcDes);
 
     // calculate coefficients for decreasing amplitude (1/fexc)
-    this->aAexcDes = (Aexc1 - Aexc0)/(1.0f/fexcDes[NfexcDes-1] - 1.0f/fexcDes[0]);
-    this->bAexcDes = Aexc0 - aAexcDes/fexcDes[0];
+    this->aAexcDes = ((double)Aexc1 - (double)Aexc0)/(1.0/fexcDes[NfexcDes-1] - 1.0/fexcDes[0]);
+    this->bAexcDes = (double)Aexc0 - aAexcDes/fexcDes[0];
 
-    fnyq = 1/2.0f/Ts;
-    pi2 = 2.0f*pi;
-    pi2Ts = pi2*Ts;
-    piDiv2 = pi/2.0f;
+    fnyq = 1.0/2.0/(double)Ts;
+    pi2 = 2.0*pi;
+    pi2Ts = pi2*(double)Ts;
+    piDiv2 = pi/2.0;
 
-    sU = (float*)malloc(3*sizeof(float));
-    sY = (float*)malloc(3*sizeof(float));
+    sU = (double*)malloc(3*sizeof(double));
+    sY = (double*)malloc(3*sizeof(double));
     reset();
 }
 
@@ -117,24 +106,24 @@
 {
     Nmeas = 0;
     Nper = 0;
-    fexc = 0.0f;
-    fexcPast = 0.0f;
+    fexc = 0.0;
+    fexcPast = 0.0;
     ii = 1; // iterating through desired frequency points
     jj = 1; // iterating through measurement points w.r.t. reachable frequency
-    scaleG = 0.0f;
-    cr = 0.0f;
-    ci = 0.0f;
+    scaleG = 0.0;
+    cr = 0.0;
+    ci = 0.0;
     for(int i = 0; i < 3; i++) {
-        sU[i] = 0.0f;
-        sY[i] = 0.0f;
+        sU[i] = 0.0;
+        sY[i] = 0.0;
     }
-    sinarg = 0.0f;
+    sinarg = 0.0;
     NmeasTotal = 0;
-    Aexc = 0.0f;
-    pi2Tsfexc = 0.0f;
+    Aexc = 0.0;
+    pi2Tsfexc = 0.0;
 }
 
-float GPA::update(float inp, float out)
+float GPA::update(double inp, double out)
 {
     // a new frequency point has been reached
     if(jj == 1) {
@@ -158,24 +147,24 @@
             }
         }
         // secure sinarg starts at 0 (numerically maybe not given)
-        sinarg = 0.0f;
+        sinarg = 0.0;
         // filter scaling
-        scaleG = 1.0f/sqrt((float)Nmeas);
+        scaleG = 1.0/sqrt((double)Nmeas);
         // filter coefficients
         cr = cos(pi2Tsfexc);
         ci = sin(pi2Tsfexc);
         // filter storage
         for(int i = 0; i < 3; i++) {
-            sU[i] = 0.0f;
-            sY[i] = 0.0f;
+            sU[i] = 0.0;
+            sY[i] = 0.0;
         }
     }
     // filter step for signal su
-    sU[0] = scaleG*inp + 2.0f*cr*sU[1] - sU[2];
+    sU[0] = scaleG*inp + 2.0*cr*sU[1] - sU[2];
     sU[2] = sU[1];
     sU[1] = sU[0];
     // filter step for signal sy
-    sY[0] = scaleG*out + 2.0f*cr*sY[1] - sY[2];
+    sY[0] = scaleG*out + 2.0*cr*sY[1] - sY[2];
     sY[2] = sY[1];
     sY[1] = sY[0];
     // measurement of frequencypoint is finished
@@ -184,56 +173,56 @@
         ii += 1;
         fexcPast = fexc;
         // calculate the one point dft
-        float Ureal = 2.0f*scaleG*(cr*sU[1] - sU[2]);
-        float Uimag = 2.0f*scaleG*ci*sU[1];
-        float Yreal = 2.0f*scaleG*(cr*sY[1] - sY[2]);
-        float Yimag = 2.0f*scaleG*ci*sY[1];
+        double Ureal = 2.0*scaleG*(cr*sU[1] - sU[2]);
+        double Uimag = 2.0*scaleG*ci*sU[1];
+        double Yreal = 2.0*scaleG*(cr*sY[1] - sY[2]);
+        double Yimag = 2.0*scaleG*ci*sY[1];
         // calculate magnitude and angle
-        float Umag = sqrt(Ureal*Ureal + Uimag*Uimag);
-        float Ymag = sqrt(Yreal*Yreal + Yimag*Yimag);
-        float absGyu = Ymag/Umag;
-        float angGyu = atan2(Yimag, Yreal) - atan2(Uimag, Ureal);
-        float absGye = Ymag/Aexc;
-        float angGye = (atan2(Yimag, Yreal) + piDiv2);
+        float Umag = (float)(sqrt(Ureal*Ureal + Uimag*Uimag));
+        float Ymag = (float)(sqrt(Yreal*Yreal + Yimag*Yimag));
+        float absGyu = (float)(Ymag/Umag);
+        float angGyu = (float)(atan2(Yimag, Yreal) - atan2(Uimag, Ureal));
+        float absGye = (float)(Ymag/Aexc);
+        float angGye = (float)(atan2(Yimag, Yreal) + piDiv2);
         // user info
         if(ii == 2) {
             printLine();
             printf("   fexc[Hz]    |Gyu|     ang(Gyu)    |Gye|     ang(Gye)     |E|        |U|        |Y|\r\n");
             printLine();
         }
-        printf("%11.3e %10.3e %10.3e %10.3e %10.3e %10.3e %10.3e %10.3e\r\n", fexc, absGyu, angGyu, absGye, angGye, Aexc, Umag, Ymag);
+         printf("%11.3e %10.3e %10.3e %10.3e %10.3e %10.3e %10.3e %10.3e\r\n", fexc, absGyu, angGyu, absGye, angGye, Aexc, Umag, Ymag);
     } else {
         jj += 1;
     }
     sinarg = fmod(sinarg + pi2Tsfexc, pi2);
     NmeasTotal += 1;
-    return Aexc*sin(sinarg);
+    return (float)(Aexc*sin(sinarg));
 }
 
-void GPA::fexcDesLogspace(float fMin, float fMax, int NfexcDes)
+void GPA::fexcDesLogspace(double fMin, double fMax, int NfexcDes)
 {
     // calculate logarithmic spaced frequency points
-    float Gain = log10(fMax/fMin)/((float)NfexcDes - 1.0f);
-    float expon = 0.0f;
+    double Gain = log10(fMax/fMin)/((double)NfexcDes - 1.0);
+    double expon = 0.0;
     for(int i = 0; i < NfexcDes; i++) {
-        fexcDes[i] = fMin*pow(10.0f, expon);
+        fexcDes[i] = fMin*pow(10.0, expon);
         expon += Gain;
     }
 }
 
-void GPA::calcGPAmeasPara(float fexcDes_i)
+void GPA::calcGPAmeasPara(double fexcDes_i)
 {
     // Nmeas has to be an integer
     Nper = NperMin;
-    Nmeas = (int)floor((float)Nper/fexcDes_i/Ts + 0.5f);
+    Nmeas = (int)floor((double)Nper/fexcDes_i/Ts + 0.5);
     //  secure that the minimal number of measurements is fullfilled
     int Ndelta = NmeasMin - Nmeas;
     if(Ndelta > 0) {
-        Nper = (int)ceil((float)NmeasMin*fexcDes_i*Ts);
-        Nmeas = (int)floor((float)Nper/fexcDes_i/Ts + 0.5f);
+        Nper = (int)ceil((double)NmeasMin*fexcDes_i*Ts);
+        Nmeas = (int)floor((double)Nper/fexcDes_i/Ts + 0.5);
     }
     // evaluating reachable frequency
-    fexc = (float)Nper/(float)Nmeas/Ts;
+    fexc = (double)Nper/(double)Nmeas/Ts;
 }
 
 void GPA::printLine()
@@ -245,7 +234,7 @@
 {
     printLine();
     for(int i = 0; i < NfexcDes; i++) {
-        printf("%9.4f\r\n", fexcDes[i]);
+        printf("%9.4f\r\n", (float)fexcDes[i]);
     }
 }
 
@@ -257,16 +246,16 @@
     for(int i = 0; i < NfexcDes; i++) {
         calcGPAmeasPara(fexcDes[i]);
         if(fexc == fexcPast || fexc >= fnyq) {
-            fexc = 0.0f;
+            fexc = 0.0;
             Nmeas = 0;
             Nper = 0;
-            Aexc = 0;
+            Aexc = 0.0;
         } else {
             Aexc = aAexcDes/fexc + bAexcDes;
             fexcPast = fexc;
         }
         NmeasTotal += Nmeas;
-        printf("%12.2e %9.2e %10.2e %7i %6i \r\n", fexcDes[i], fexc, Aexc, Nmeas, Nper);
+        printf("%12.2e %9.2e %10.2e %7i %6i \r\n", (float)fexcDes[i], (float)fexc, (float)Aexc, Nmeas, Nper);
     }
     printGPAmeasTime();
     reset();
@@ -276,5 +265,5 @@
 {
     printLine();
     printf(" number of data points:  %9i\r\n", NmeasTotal);
-    printf(" measurment time in sec: %9.2f\r\n", (float)NmeasTotal*Ts);
+    printf(" measurment time in sec: %9.2f\r\n", (float)((double)NmeasTotal*Ts));
 }
\ No newline at end of file