Diff: GPA.cpp

Revision:

6:da0c9587ecae

Child:

7:87b823282bf0

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/GPA.cpp	Tue Mar 13 20:26:17 2018 +0000
@@ -0,0 +1,196 @@
+#include "GPA.h"
+#include "mbed.h"
+#include "math.h"
+#define   pi 3.1415927f
+
+using namespace std;
+
+GPA::GPA(float fMin, float fMax, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1)
+{
+    this->NfexcDes = NfexcDes;
+    this->NperMin = NperMin;
+    this->NmeasMin = NmeasMin;
+    this->Ts = Ts;
+
+    // calculate logarithmic spaced frequency points
+    fexcDes = (float*)malloc(NfexcDes*sizeof(float));
+    fexcDesLogspace(fMin, 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];
+
+    fnyq = 1/2.0f/Ts;
+    pi2 = 2.0f*pi;
+    pi2Ts = pi2*Ts;
+    piDiv2 = pi/2.0f;
+
+    sU = (float*)malloc(3*sizeof(float));
+    sY = (float*)malloc(3*sizeof(float));
+    reset();
+}
+
+GPA::~GPA() {}
+
+void GPA::reset()
+{
+    Nmeas = 0;
+    Nper = 0;
+    fexc = 0.0f;
+    fexcPast = 0.0f;
+    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;
+    for(int i = 0; i < 3; i++) {
+        sU[i] = 0.0f;
+        sY[i] = 0.0f;
+    }
+    sinarg = 0.0f;
+    NmeasTotal = 0;
+    Aexc = 0.0f;
+    pi2Tsfexc = 0.0f;
+}
+
+float GPA::update(float inp, float out)
+{
+    // a new frequency point has been reached
+    if(jj == 1) {
+        // get a new unique frequency point
+        while(fexc == fexcPast) {
+            // measurement finished
+            if(ii > NfexcDes) {
+                return 0.0f;
+            }
+            calcGPAmeasPara(fexcDes[ii - 1]);
+            // secure fexc is not higher or equal to nyquist frequency
+            if(fexc >= fnyq) {
+                fexc = fexcPast;
+            }
+            // no frequency found
+            if(fexc == fexcPast) {
+                ii += 1;
+            } else {
+                Aexc = aAexcDes/fexc + bAexcDes;
+                pi2Tsfexc = pi2Ts*fexc;
+            }
+        }
+        fexcPast = fexc;
+        // filter scaling
+        scaleG = 1.0f/sqrt((float)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;
+        }
+    }
+    // filter step for signal su
+    sU[0] = scaleG*inp + 2.0f*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[2] = sY[1];
+    sY[1] = sY[0];
+    // measurement of frequencypoint is finished
+    if(jj == Nmeas) {
+        jj = 1;
+        // 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];
+        // 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);
+        // user info
+        if(ii == 1) {
+            printLine();
+            printf("   fexc[Hz]    |Gyu|     ang(Gyu)    |Gye|     ang(Gye)     |E|        |U|        |Y|\r\n");
+            printLine();
+        }
+        printf("%11.2e %10.2e %10.2e %10.2e %10.2e %10.2e %10.2e %10.2e\r\n", fexc, absGyu, angGyu, absGye, angGye, Aexc, Umag, Ymag);
+    } else {
+        jj += 1;
+    }
+    sinarg = fmod(sinarg + pi2Tsfexc, pi2);
+    NmeasTotal += 1;
+    return Aexc*sin(sinarg);
+}
+
+void GPA::fexcDesLogspace(float fMin, float fMax, int NfexcDes)
+{
+    // calculate logarithmic spaced frequency points
+    float Gain = log10(fMax/fMin)/((float)NfexcDes - 1.0f);
+    float expon = 0.0f;
+    for(int i = 0; i < NfexcDes; i++) {
+        fexcDes[i] = fMin*pow(10.0f, expon);
+        expon += Gain;
+    }
+}
+
+void GPA::calcGPAmeasPara(float fexcDes_i)
+{
+    // Nmeas has to be an integer
+    Nper = NperMin;
+    Nmeas = (int)floor((float)Nper/fexcDes_i/Ts + 0.5f);
+    //  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);
+    }
+    // evaluating reachable frequency
+    fexc = (float)Nper/(float)Nmeas/Ts;
+}
+
+void GPA::printLine()
+{
+    printf("-----------------------------------------------------------------------------------------\r\n");
+}
+
+void GPA::printGPAfexcDes()
+{
+    printLine();
+    for(int i = 0; i < NfexcDes; i++) {
+        printf("%9.4f\r\n", fexcDes[i]);
+    }
+}
+
+void GPA::printGPAmeasPara()
+{
+    printLine();
+    printf(" fexcDes[Hz]  fexc[Hz]       Aexc   Nmeas   Nper\r\n");
+    printLine();
+    for(int i = 0; i < NfexcDes; i++) {
+        calcGPAmeasPara(fexcDes[i]);
+        if(fexc == fexcPast || fexc >= fnyq) {
+            fexc = 0.0f;
+            Nmeas = 0;
+            Nper = 0;
+            Aexc = 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);
+    }
+    printGPAmeasTime();
+    reset();
+}
+
+void GPA::printGPAmeasTime()
+{
+    printLine();
+    printf(" number of data points:  %9i\r\n", NmeasTotal);
+    printf(" measurment time in sec: %9.2f\r\n", (float)NmeasTotal*Ts);
+}
\ No newline at end of file