![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Fertig
Fork of RT2_P3_students by
Diff: GPA.cpp
- Revision:
- 6:2cc56521aa16
- Parent:
- 3:769ce5f06d3e
--- a/GPA.cpp Mon Apr 09 09:26:58 2018 +0000 +++ b/GPA.cpp Tue Apr 10 11:44:36 2018 +0000 @@ -10,14 +10,15 @@ | P: plant v exc(1) --> o ->| C |--->o------->| P |----------> out - ^ | | + ^ - | | | --> inp | exc: excitation signal (E) | | inp: input plant (U) -------------------------------- out: output plant (Y) - instantiate option 1: + instantiate option 1: (logarithmic equaly spaced frequency points) + GPA(float fMin, float fMax, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1) - + fMin: minimal desired frequency that should be measured in Hz fMax: maximal desired frequency that should be measured in Hz NfexcDes: number of logarithmic equaly spaced frequency points @@ -26,11 +27,26 @@ Ts: sampling time Aexc0: excitation amplitude at fMin Aexc1: excitation amplitude at fMax + + instantiate option 2: (for a second, refined frequency grid measurement) + + GPA(float f0, float f1, float *fexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1) + + f0: frequency point for the calculation of Aexc0 in Hz (should be chosen like in the first measurement) + f1: frequency point for the calculation of Aexc1 in Hz (should be chosen like in the first measurement) + *fexcDes: sorted frequency point array in Hz + + instantiate option 3: (for an arbitary but sorted frequency grid measurement) + + GPA(float *fexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1) + + *fexcDes: sorted frequency point array in Hz + Aexc0: excitation amplitude at fexcDes[0] + Aexc1: excitation amplitude at fexcDes[NfexcDes-1] - hints: the amplitude drops with 1/fexc, if you're using - Axc1 = Aexc0/fMax then 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. + hints: the amplitude drops with 1/fexc, if you're using Axc1 = Aexc0/fMax then 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: @@ -82,7 +98,7 @@ #include "GPA.h" #include "mbed.h" #include "math.h" -#define pi 3.1415927f +#define pi 3.141592653589793 using namespace std; @@ -91,23 +107,75 @@ 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 = ((double)Aexc1 - (double)Aexc0)/(1.0/fexcDes[NfexcDes-1] - 1.0/fexcDes[0]); + this->bAexcDes = (double)Aexc0 - aAexcDes/fexcDes[0]; + + fnyq = 1.0/2.0/(double)Ts; + pi2 = 2.0*pi; + pi2Ts = pi2*(double)Ts; + piDiv2 = pi/2.0; + + sU = (double*)malloc(3*sizeof(double)); + sY = (double*)malloc(3*sizeof(double)); + reset(); +} + +GPA::GPA(float f0, float f1, float *fexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1) +{ + // convert fexcDes from float to double, it is assumed that it is sorted + NfexcDes = sizeof(fexcDes)/sizeof(fexcDes[0]); + this->fexcDes = (double*)malloc(NfexcDes*sizeof(double)); + for(int i = 0; i < NfexcDes; i++) { + this->fexcDes[i] = (double)fexcDes[i]; + } + this->NperMin = NperMin; + this->NmeasMin = NmeasMin; + this->Ts = (double)Ts; // 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/(double)f1 - 1.0/(double)f0); + this->bAexcDes = (double)Aexc0 - aAexcDes/fexcDes[0]; + + fnyq = 1.0/2.0/(double)Ts; + pi2 = 2.0*pi; + pi2Ts = pi2*(double)Ts; + piDiv2 = pi/2.0; + + sU = (double*)malloc(3*sizeof(double)); + sY = (double*)malloc(3*sizeof(double)); + reset(); +} - fnyq = 1/2.0f/Ts; - pi2 = 2.0f*pi; - pi2Ts = pi2*Ts; - piDiv2 = pi/2.0f; +GPA::GPA(float *fexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1) +{ + // convert fexcDes from float to double, it is assumed that it is sorted + NfexcDes = sizeof(fexcDes)/sizeof(fexcDes[0]); + this->fexcDes = (double*)malloc(NfexcDes*sizeof(double)); + for(int i = 0; i < NfexcDes; i++) { + this->fexcDes[i] = (double)fexcDes[i]; + } + this->NperMin = NperMin; + this->NmeasMin = NmeasMin; + this->Ts = (double)Ts; - sU = (float*)malloc(3*sizeof(float)); - sY = (float*)malloc(3*sizeof(float)); + // calculate coefficients for decreasing amplitude (1/fexc) + this->aAexcDes = ((double)Aexc1 - (double)Aexc0)/(1.0/this->fexcDes[NfexcDes-1] - 1.0/this->fexcDes[0]); + this->bAexcDes = (double)Aexc0 - aAexcDes/fexcDes[0]; + + fnyq = 1.0/2.0/(double)Ts; + pi2 = 2.0*pi; + pi2Ts = pi2*(double)Ts; + piDiv2 = pi/2.0; + + sU = (double*)malloc(3*sizeof(double)); + sY = (double*)malloc(3*sizeof(double)); reset(); } @@ -117,24 +185,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 +226,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 +252,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", (float)fexc, absGyu, angGyu, absGye, angGye, (float)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 +313,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 +325,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 +344,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