2nd Library
Dependents: cuboid_balance_ros cuboid_balance mirror_actuator_V1
Diff: GPA.cpp
- Revision:
- 2:b54eb3e24d2d
- Parent:
- 0:a201a6cd4c0c
--- a/GPA.cpp Thu Mar 07 07:03:56 2019 +0000 +++ b/GPA.cpp Thu Feb 25 20:28:27 2021 +0000 @@ -1,31 +1,36 @@ /* - GPA: Frequency point wise gain and phase analyser to measure the frequency respone function (FRF) of a dynamical system, based on the one point DFT + GPA: Frequency point wise gain and phase analyser to measure a frequency respone function (FRF) of a dynamical system Hint: If the plant has a pole at zero, is unstable or weakly damped the measurement has to be perfomed in closed loop (this is NOT tfestimate, the algorithm is based on the one point DFT). - Assumption: The system is and remains at the desired steady state of interest when the measurment starts + Assumption: The system is and remains at the desired steady state when the measurment starts + + Note: 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 (could not be reached) try to increase Nmeas. + Instantiate option 0: ("Not a Jedi yet" users, for logarithmic equaly spaced frequency points) - + GPA(float fMin, float fMax, int NfexcDes, float Aexc0, float Aexc1, float Ts) - + 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 between fMin and fMax Aexc0: Excitation amplitude at fMin Aexc1: Excitation amplitude at fMax Ts: Sampling time in sec - + Default values are as follows: - int NperMin = 3; - int NmeasMin = (int)ceil(1.0f/Ts); - int NstartMin = (int)ceil(3.0f/Ts); - int NsweepMin = 0; + int NperMin = 3; + int NmeasMin = (int)ceil(1.0f/Ts); + int Nstart = (int)ceil(3.0f/Ts); + int Nsweep = (int)ceil(0.0f/Ts); Instantiate option 1: ("Jedi or Sith Lord", for logarithmic equaly spaced frequency points) - - GPA(float fMin, float fMax, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1, int NstartMin, int NsweepMin) - + + GPA(float fMin, float fMax, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1, int Nstart, int Nsweep) + 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 @@ -34,37 +39,33 @@ Ts: Sampling time in sec Aexc0: Excitation amplitude at fMin Aexc1: Excitation amplitude at fMax - NstartMin: Minimal number of samples to sweep to the first frequency point (can be equal 0) - NsweepMin: Minimal number of samples to sweep from freq. point to freq. point (can be equal 0) + Nstart: Minimal number of samples to sweep to the first frequency point (can be equal 0) + Nsweep: Minimal number of samples to sweep from freq. point to freq. point (can be equal 0) + - Instantiate option 2: (for a second, refined frequency grid measurement) - - GPA(float f0, float f1, float *fexcDes, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1, int NstartMin, int NsweepMin) - + + GPA(float f0, float f1, float *fexcDes, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1, int Nstart, int Nsweep) + 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 NfexcDes: Length of fexcDes - + For the other parameters see above. Instantiate option 3: (for an arbitary but sorted frequency grid measurement) - - GPA(float *fexcDes, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1, int NstartMin, int NsweepMin) - + + GPA(float *fexcDes, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1, int Nstart, int Nsweep) + *fexcDes: Sorted frequency point array in Hz Aexc0: Excitation amplitude at fexcDes[0] Aexc1: Excitation amplitude at fexcDes[NfexcDes-1] NfexcDes: Length of fexcDes - + For the other parameters see above. - Note: 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 (could not be reached) try to increase Nmeas. - - + Block diagram: w (const.) exc(2) C: controller @@ -75,33 +76,33 @@ | --> inp (u) | exc (R): excitation signal | | inp (U): input plant -------------------------------- out (Y): output plant - - + + Pseudo code for an open loop measurement: - Measuring the plant P = Gyu = Gyr: - + u = w + exc; ... write output u here! it follows exc(k+1) ... exc = Wobble(exc, y); - + Closed loop FRF calculus with a stabilizing controller C: - S = 1/(1 + C*P); % ( exc1 -> e , 1/(1 + C*P) ) contr. error rejection, robustness (1/max|S|) - T = 1 - S; % ( w -> y , C*P/(1 + C*P) ) tracking - CS = C*S; % ( exc1 -> u , C/(1 + C*P) ) disturbance plant output - PS = P*S; % ( exc2 -> y , P/(1 + C*P) ) disturbance plant input + S = 1/(1 + C*P); % ( exc1 -> e , 1/(1 + C*P) ) sensitivity, contr. error rejection, robustness (1/max|S|) + T = 1 - S; % ( w -> y , C*P/(1 + C*P) ) compl. sensitivity, tracking performance + CS = C*S; % ( exc1 -> u , C/(1 + C*P) ) control effort, disturbance plant output on plant input + PS = P*S; % ( exc2 -> y , P/(1 + C*P) ) compliance, disturbance plant input on plant output Pseudo code for a closed loop measurement with stabilizing controller C: - Excitation at excitation input (1): - + Excitation at excitation input (1): + - Measuring the plant P = Gyu and the closed loop tf T = PC/(1 + PC) = Gyr: - + u = C(w - y + exc); ... write output u here! it follows exc(k+1) ... exc = Wobble(u, y); - + Closed loop FRF calculus: S = 1 - T; PS = P*S; @@ -109,13 +110,13 @@ C = C/S; Excitation at excitation input (2): - + - Measuring the plant P = Gyu and the closed loop tf PS = P/(1 + PC) = Gyr: - + u = C(w - y) + exc; ... write output u here! it follows exc(k+1) ... exc = Wobble(u, y); - + Closed loop FRF calculus: S = PS/P; T = 1 - S; @@ -124,11 +125,11 @@ Usage: - exc(k+1) = myGPA(inp(k), out(k)) does update the internal states of the + 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 FRF data are plotted to a terminal (Putty) over a serial + k+1. The FRF data are plotted to a terminal (Putty) over a serial connection and look as follows: - + -------------------------------------------------------------------------------- fexc[Hz] |Gyu| deg(Gyu) |Gyr| deg(Gyr) |U| |Y| |R| -------------------------------------------------------------------------------- @@ -136,7 +137,7 @@ . . . . . . . . . . . . . . . . . . . . . . . . - + In Matlab you can use the editor as follows: data = [... insert measurement data here ...]; gyu = frd(data(:,2).*exp(1i*data(:,3)*pi/180), data(:,1), Ts, 'Units', 'Hz'); @@ -148,14 +149,14 @@ data = data(ind,:); - Autor and Copyrigth: 2018 / 2019 / M.E. Peter - + Autor and Copyrigth: 2018 / 2019 / 2020 / M.E. Peter + */ #include "GPA.h" #include "mbed.h" #include "math.h" -#define pi 3.141592653589793 +#define pi 3.14159265358979323846 // M_PI using namespace std; @@ -165,82 +166,90 @@ GPA::GPA(float fMin, float fMax, int NfexcDes, float Aexc0, float Aexc1, float Ts) { + doPrint = true; int NperMin = 3; int NmeasMin = (int)ceil(1.0f/Ts); - int NstartMin = (int)ceil(3.0f/Ts); - int NsweepMin = 0; - - assignParameters(NfexcDes, NperMin, NmeasMin, (double)Ts, NstartMin, NsweepMin); + int Nstart = (int)ceil(3.0f/Ts); + int Nsweep = (int)ceil(0.0f/Ts); - // calculate logarithmic spaced frequency points - fexcDes = (double*)malloc(NfexcDes*sizeof(double)); - fexcDesLogspace((double)fMin, (double)fMax, NfexcDes); + setParameters(fMin, fMax, NfexcDes, NperMin, NmeasMin, Ts, Aexc0, Aexc1, Nstart, Nsweep, doPrint); +} - calculateDecreasingAmplitudeCoefficients((double)Aexc0, (double)Aexc1); - initializeConstants((double)Ts); - assignFilterStorage(); - reset(); +GPA::GPA(float fMin, float fMax, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1, int Nstart, int Nsweep) +{ + doPrint = true; + setParameters(fMin, fMax, NfexcDes, NperMin, NmeasMin, Ts, Aexc0, Aexc1, Nstart, Nsweep, doPrint); } -GPA::GPA(float fMin, float fMax, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1, int NstartMin, int NsweepMin) +GPA::GPA(float f0, float f1, float *fexcDes, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1, int Nstart, int Nsweep) { - assignParameters(NfexcDes, NperMin, NmeasMin, (double)Ts, NstartMin, NsweepMin); + doPrint = true; + assignParameters(NfexcDes, NperMin, NmeasMin, Ts, Nstart, Nsweep); - // calculate logarithmic spaced frequency points - fexcDes = (double*)malloc(NfexcDes*sizeof(double)); - fexcDesLogspace((double)fMin, (double)fMax, NfexcDes); + // convert fexcDes from float to float, it is assumed that it is sorted + this->fexcDes = (float*)malloc(NfexcDes*sizeof(float)); + for(int i = 0; i < NfexcDes; i++) { + this->fexcDes[i] = (float)fexcDes[i]; + } - calculateDecreasingAmplitudeCoefficients((double)Aexc0, (double)Aexc1); - initializeConstants((double)Ts); + calculateDecreasingAmplitudeCoefficients(Aexc0, Aexc1); + initializeConstants(Ts); assignFilterStorage(); reset(); } -GPA::GPA(float f0, float f1, float *fexcDes, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1, int NstartMin, int NsweepMin) +GPA::GPA(float *fexcDes, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1, int Nstart, int Nsweep) { - assignParameters(NfexcDes, NperMin, NmeasMin, (double)Ts, NstartMin, NsweepMin); - - // convert fexcDes from float to double, it is assumed that it is sorted - this->fexcDes = (double*)malloc(NfexcDes*sizeof(double)); + doPrint = true; + assignParameters(NfexcDes, NperMin, NmeasMin, Ts, Nstart, Nsweep); + + // convert fexcDes from float to float, it is assumed that it is sorted + this->fexcDes = (float*)malloc(NfexcDes*sizeof(float)); for(int i = 0; i < NfexcDes; i++) { - this->fexcDes[i] = (double)fexcDes[i]; + this->fexcDes[i] = fexcDes[i]; } - - calculateDecreasingAmplitudeCoefficients((double)Aexc0, (double)Aexc1); - initializeConstants((double)Ts); + + calculateDecreasingAmplitudeCoefficients(Aexc0, Aexc1); + initializeConstants(Ts); assignFilterStorage(); reset(); } -GPA::GPA(float *fexcDes, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1, int NstartMin, int NsweepMin) +GPA::GPA(float fMin, float fMax, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1, int Nstart, int Nsweep, bool doPrint) { - assignParameters(NfexcDes, NperMin, NmeasMin, (double)Ts, NstartMin, NsweepMin); - - // convert fexcDes from float to double, it is assumed that it is sorted - this->fexcDes = (double*)malloc(NfexcDes*sizeof(double)); - for(int i = 0; i < NfexcDes; i++) { - this->fexcDes[i] = (double)fexcDes[i]; - } - - calculateDecreasingAmplitudeCoefficients((double)Aexc0, (double)Aexc1); - initializeConstants((double)Ts); - assignFilterStorage(); - reset(); + setParameters(fMin, fMax, NfexcDes, NperMin, NmeasMin, Ts, Aexc0, Aexc1, Nstart, Nsweep, doPrint); } // ----------------------------------------------------------------------------- -// virtual and reset +// virtual, reset and set // ----------------------------------------------------------------------------- GPA::~GPA() {} +void GPA::setParameters(float fMin, float fMax, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1, int Nstart, int Nsweep, bool doPrint) +{ + this->doPrint = doPrint; + assignParameters(NfexcDes, NperMin, NmeasMin, Ts, Nstart, Nsweep); + + // calculate logarithmic spaced frequency points + fexcDes = (float*)malloc(NfexcDes*sizeof(float)); + fexcDesLogspace(fMin, fMax, NfexcDes); + + calculateDecreasingAmplitudeCoefficients(Aexc0, Aexc1); + initializeConstants(Ts); + assignFilterStorage(); + reset(); + +} + void GPA::reset() { Nmeas = 0; Nper = 0; dfexc = 0.0; fexc = 0.0; - fexcPast = 0.0; + fexcPast = 0.0f; + dfexcj = 0.0f; i = 1; // iterating through desired frequency points j = 1; // iterating through measurement points w.r.t. reachable frequency scaleG = 0.0; @@ -251,27 +260,36 @@ sY[i] = 0.0; } sinarg = 0.0; + sinargR = 0.0f; NmeasTotal = 0; - Aexc = 0.0; + Aexc = 0.0f; pi2Tsfexc = 0.0; - Nsweep = NstartMin; - bfexc = 0.0; - afexc = 0.0; - aAexc = 0.0; - bAexc = 0.0; - AexcOut = 0.0; + Nsweep_i = Nstart; + AexcOut = 0.0f; + gpaData.fexc = 0.0f; + gpaData.absGyu = 0.0f; + gpaData.angGyu = 0.0f; + gpaData.absGyr = 0.0f; + gpaData.angGyr = 0.0f; + gpaData.Umag = 0.0f; + gpaData.Ymag = 0.0f; + gpaData.Rmag = 0.0f; + gpaData.MeasPointFinished = false; + gpaData.MeasFinished = false; + gpaData.ind = -1; + } // ----------------------------------------------------------------------------- // update (operator) -// ----------------------------------------------------------------------------- +// ----------------------------------------------------------------------------- -float GPA::update(double inp, double out) +float GPA::update(float inp, float out) { // a new frequency point has been reached if(j == 1) { // user info - if(i == 1) { + if(i == 1 && doPrint) { printLine(); printf(" fexc[Hz] |Gyu| deg(Gyu) |Gyr| deg(Gyr) |U| |Y| |R|\r\n"); printLine(); @@ -280,6 +298,8 @@ while(fexc == fexcPast) { // measurement finished if(i > NfexcDes) { + gpaData.MeasPointFinished = false; + gpaData.MeasFinished = true; return 0.0f; } calcGPAmeasPara(fexcDes[i - 1]); @@ -305,13 +325,14 @@ sU[i] = 0.0; sY[i] = 0.0; } - // calculate the parameters for the frequency sweep from fexcPast to fexc - if(Nsweep > 0) calcGPAsweepPara(); + gpaData.MeasPointFinished = false; } // perfomre the sweep or measure - if(j <= Nsweep) { - dfexc = afexc*(double)j + bfexc; - AexcOut = aAexc*(double)j + bAexc; + if(j <= Nsweep_i) { + dfexcj = ((float)j - 1.0f)/((float)Nsweep_i - 1.0f); + dfexcj = div12pi*sinf(pi4*dfexcj) - div812pi*sinf((float)pi2*dfexcj) + dfexcj; + dfexc = fexcPast + (fexc - fexcPast)*dfexcj; + AexcOut = AexcPast + (Aexc - AexcPast)*dfexcj; } else { dfexc = fexc; AexcOut = Aexc; @@ -324,27 +345,35 @@ sY[2] = sY[1]; sY[1] = sY[0]; } - // secure sinarg starts at 0 (numerically maybe not given) - if(j == 1 || j == Nsweep + 1) sinarg = 0.0; + // copy starting value for ang(R) + if(j == 1 || j == Nsweep_i + 1) sinargR = sinarg; // measurement of frequencypoint is finished - if(j == Nmeas + Nsweep) { + if(j == Nmeas + Nsweep_i) { fexcPast = fexc; AexcPast = Aexc; - Nsweep = NsweepMin; + Nsweep_i = Nsweep; // calculate the one point dft - 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]; + float Ureal = (float)(2.0*scaleG*(cr*sU[1] - sU[2])); + float Uimag = (float)(2.0*scaleG*ci*sU[1]); + float Yreal = (float)(2.0*scaleG*(cr*sY[1] - sY[2])); + float Yimag = (float)(2.0*scaleG*ci*sY[1]); // calculate magnitude and angle - float Umag = (float)(sqrt(Ureal*Ureal + Uimag*Uimag)); - float Ymag = (float)(sqrt(Yreal*Yreal + Yimag*Yimag)); - float absGyu = (float)(Ymag/Umag); - float angGyu = (float)wrapAngle(atan2(Yimag, Yreal) - atan2(Uimag, Ureal)); - float absGyr = (float)(Ymag/Aexc); - float angGyr = (float)wrapAngle(atan2(Yimag, Yreal) + piDiv2); + float Umag = sqrtf(Ureal*Ureal + Uimag*Uimag); + float Ymag = sqrtf(Yreal*Yreal + Yimag*Yimag); + gpaData.fexc = (float)fexc; + gpaData.absGyu = Ymag/Umag; + gpaData.angGyu = wrapAngle(atan2f(Yimag, Yreal) - atan2f(Uimag, Ureal))*rad2deg; + gpaData.absGyr = Ymag/Aexc; + gpaData.angGyr = wrapAngle(atan2f(Yimag, Yreal) + fmod(piDiv2 - sinargR, (float)pi2))*rad2deg; + gpaData.Umag = Umag; + gpaData.Ymag = Ymag; + gpaData.Rmag = Aexc; + gpaData.MeasPointFinished = true; + gpaData.ind++; // user info - printf("%11.4e %9.3e %8.3f %9.3e %8.3f %9.3e %9.3e %9.3e\r\n", (float)fexc, absGyu, angGyu*rad2deg, absGyr, angGyr*rad2deg, Umag, Ymag, (float)Aexc); + if(doPrint) { + printf("%11.4e %9.3e %8.3f %9.3e %8.3f %9.3e %9.3e %9.3e\r\n", gpaData.fexc, gpaData.absGyu, gpaData.angGyu, gpaData.absGyr, gpaData.angGyr, gpaData.Umag, gpaData.Ymag, gpaData.Rmag); + } i += 1; j = 1; } else { @@ -353,184 +382,121 @@ // calculate the excitation sinarg = fmod(sinarg + pi2Ts*dfexc, pi2); NmeasTotal += 1; - return (float)(AexcOut*sin(sinarg)); + return AexcOut*sinf(sinarg); } // ----------------------------------------------------------------------------- // private functions -// ----------------------------------------------------------------------------- +// ----------------------------------------------------------------------------- -void GPA::assignParameters(int NfexcDes, int NperMin, int NmeasMin, double Ts, int NstartMin, int NsweepMin) +void GPA::assignParameters(int NfexcDes, int NperMin, int NmeasMin, float Ts, int Nstart, int Nsweep) { this->NfexcDes = NfexcDes; this->NperMin = NperMin; this->NmeasMin = NmeasMin; this->Ts = Ts; - this->NstartMin = NstartMin; - this->NsweepMin = NsweepMin; + this->Nstart = Nstart; + this->Nsweep = Nsweep; } -void GPA::calculateDecreasingAmplitudeCoefficients(double Aexc0, double Aexc1) +void GPA::calculateDecreasingAmplitudeCoefficients(float Aexc0, float Aexc1) { // calculate coefficients for decreasing amplitude (1/fexc) - this->aAexcDes = (Aexc1 - Aexc0)/(1.0/fexcDes[NfexcDes-1] - 1.0/fexcDes[0]); - this->bAexcDes = Aexc0 - aAexcDes/fexcDes[0]; + this->aAexcDes = (Aexc1 - Aexc0)/(1.0f/fexcDes[NfexcDes-1] - 1.0f/fexcDes[0]); + this->bAexcDes = Aexc0 - aAexcDes/fexcDes[0]; } -void GPA::initializeConstants(double Ts) +void GPA::initializeConstants(float Ts) { - fnyq = 1.0/2.0/Ts; + fnyq = 1.0f/2.0f/Ts; pi2 = 2.0*pi; - pi2Ts = pi2*Ts; - piDiv2 = pi/2.0; + pi4 = 4.0f*(float)pi; + pi2Ts = 2.0*pi*(double)Ts;// 2.0*pi*Ts; + piDiv2 = (float)pi/2.0f; rad2deg = 180.0f/(float)pi; + div12pi = 1.0f/(12.0f*(float)pi); + div812pi = 8.0f/(12.0f*(float)pi); } void GPA::assignFilterStorage() { sU = (double*)malloc(3*sizeof(double)); - sY = (double*)malloc(3*sizeof(double)); + sY = (double*)malloc(3*sizeof(double)); } -void GPA::fexcDesLogspace(double fMin, double fMax, int NfexcDes) +void GPA::fexcDesLogspace(float fMin, float fMax, int NfexcDes) { // calculate logarithmic spaced frequency points - double Gain = log10(fMax/fMin)/((double)NfexcDes - 1.0); - double expon = 0.0; + float Gain = log10f(fMax/fMin)/((float)NfexcDes - 1.0f); + float expon = 0.0; for(int i = 0; i < NfexcDes; i++) { - fexcDes[i] = fMin*pow(10.0, expon); + fexcDes[i] = fMin*powf(10.0f, expon); expon += Gain; } } -void GPA::calcGPAmeasPara(double fexcDes_i) +void GPA::calcGPAmeasPara(float fexcDes_i) { // Nmeas has to be an integer Nper = NperMin; - Nmeas = (int)floor((double)Nper/fexcDes_i/Ts + 0.5); + 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((double)NmeasMin*fexcDes_i*Ts); - Nmeas = (int)floor((double)Nper/fexcDes_i/Ts + 0.5); + Nper = (int)ceil((float)NmeasMin*fexcDes_i*Ts); + Nmeas = (int)floor((float)Nper/fexcDes_i/Ts + 0.5f); } // evaluating reachable frequency - fexc = (double)Nper/(double)Nmeas/Ts; + fexc = (float)((double)Nper/(double)Nmeas/(double)Ts); } -void GPA::calcGPAsweepPara() -{ - // calculate linear frequency sweep parameters - double ksta = ceil(Ts*(double)Nsweep/2.0*(fexc + fexcPast)); - Nsweep = (int)floor(2.0*ksta/Ts/(fexc + fexcPast) + 0.5); - bfexc = 2.0*ksta/Ts/(double)Nsweep - fexc; - afexc = (fexc - bfexc)/((double)Nsweep + 1.0); - aAexc = (Aexc - AexcPast)/((double)Nsweep + 1.0); - bAexc = AexcPast; -} - -double GPA::wrapAngle(double angle) +float GPA::wrapAngle(float angle) { // wrap angle from (-2pi,2pi) into (-pi,pi) - if(abs(angle) > pi) angle -= copysign(-pi2, angle); // -1*sign(angle)*2*pi + angle; + if(abs(angle) > (float)pi) angle -= copysignf(-(float)pi2, angle); // -1*sign(angle)*2*pi + angle; return angle; } -void GPA::printLine() -{ - printf("--------------------------------------------------------------------------------\r\n"); -} - void GPA::printLongLine() { - printf("-------------------------------------------------------------------------------------------------------\r\n"); + printf("-------------------------------------------------------------------------------------------------------------------------------\r\n"); } // ----------------------------------------------------------------------------- -// public functions -// ----------------------------------------------------------------------------- +// public functions (mainly for debugging) +// ----------------------------------------------------------------------------- void GPA::printGPAfexcDes() { printLine(); for(int i = 0; i < NfexcDes; i++) { - printf("%9.4f\r\n", (float)fexcDes[i]); + printf("%9.4f\r\n", fexcDes[i]); } } void GPA::printGPAmeasPara() { printLine(); - printf(" fexcDes[Hz] fexc[Hz] Aexc Nmeas Nper Nsweep\r\n"); + printf(" fexcDes[Hz] fexc[Hz] Aexc Nmeas Nper Nsweep_i\r\n"); printLine(); for(int i = 0; i < NfexcDes; i++) { calcGPAmeasPara(fexcDes[i]); if(fexc == fexcPast || fexc >= fnyq) { fexc = 0.0; - Aexc = 0.0; + Aexc = 0.0f; Nmeas = 0; Nper = 0; - Nsweep = 0; - afexc = 0.0; - bfexc = 0.0; - aAexc = 0.0; - bAexc = 0.0; - + Nsweep_i = 0; } else { Aexc = aAexcDes/fexc + bAexcDes; - if(Nsweep > 0) calcGPAsweepPara(); - else{ - afexc = 0.0; - bfexc = 0.0; - aAexc = 0.0; - bAexc = 0.0; - } fexcPast = fexc; AexcPast = Aexc; } NmeasTotal += Nmeas; - NmeasTotal += Nsweep; - printf("%11.4e %12.4e %10.3e %7i %6i %7i\r\n", (float)fexcDes[i], (float)fexc, (float)Aexc, Nmeas, Nper, Nsweep); - Nsweep = NsweepMin; - } - printGPAmeasTime(); - reset(); -} - -void GPA::printFullGPAmeasPara() -{ - printLongLine(); - printf(" fexcDes[Hz] fexc[Hz] Aexc Nmeas Nper Nsweep afexc bfexc aAexc bAexc\r\n"); - printLongLine(); - for(int i = 0; i < NfexcDes; i++) { - calcGPAmeasPara(fexcDes[i]); - if(fexc == fexcPast || fexc >= fnyq) { - fexc = 0.0; - Aexc = 0.0; - Nmeas = 0; - Nper = 0; - Nsweep = 0; - afexc = 0.0; - bfexc = 0.0; - aAexc = 0.0; - bAexc = 0.0; - - } else { - Aexc = aAexcDes/fexc + bAexcDes; - if(Nsweep > 0) calcGPAsweepPara(); - else{ - afexc = 0.0; - bfexc = 0.0; - aAexc = 0.0; - bAexc = 0.0; - } - fexcPast = fexc; - AexcPast = Aexc; - } - NmeasTotal += Nmeas; - NmeasTotal += Nsweep; - printf("%11.4e %12.4e %10.3e %7i %6i %7i %10.3e %10.3e %10.3e %10.3e\r\n", (float)fexcDes[i], (float)fexc, (float)Aexc, Nmeas, Nper, Nsweep, (float)afexc, (float)bfexc, (float)aAexc, (float)bAexc); - Nsweep = NsweepMin; + NmeasTotal += Nsweep_i; + printf("%11.4e %12.4e %10.3e %7i %6i %7i\r\n", fexcDes[i], (float)fexc, Aexc, Nmeas, Nper, Nsweep_i); +// wait(0.01); + Nsweep_i = Nsweep; } printGPAmeasTime(); reset(); @@ -540,7 +506,7 @@ { printLine(); printf(" Number of data points : %11i\r\n", NmeasTotal); - printf(" Measurment time in sec: %12.2f\r\n", (float)((double)NmeasTotal*Ts)); + printf(" Measurment time in sec: %12.2f\r\n", (float)NmeasTotal*Ts); } void GPA::printNfexcDes() @@ -549,3 +515,12 @@ printf(" Number of frequancy points: %3i\r\n", NfexcDes); } +void GPA::printLine() +{ + printf("--------------------------------------------------------------------------------\r\n"); +} + +GPA::gpadata_t GPA::getGPAdata() +{ + return gpaData; +} \ No newline at end of file