Test of pmic GPA with filter
Dependencies: mbed
Fork of nucf446-cuboid-balance1_strong by
Diff: GPA.cpp
- Revision:
- 22:715d351d0be7
- Parent:
- 21:d9bda15377e2
- Child:
- 23:26a1ccd0a856
--- 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