RT2_Cuboid_demo
Cuboid
Dependencies: mbed
Revision 28:fc53b2d62a1e, committed 2019-02-07
- Comitter:
- pmic
- Date:
- Thu Feb 07 09:11:51 2019 +0000
- Parent:
- 27:8f1ab2a2267b
- Commit message:
- Add new default instantiate option for GPA
Changed in this revision
| GPA.cpp | Show annotated file Show diff for this revision Revisions of this file |
| GPA.h | Show annotated file Show diff for this revision Revisions of this file |
diff -r 8f1ab2a2267b -r fc53b2d62a1e GPA.cpp
--- a/GPA.cpp Tue Dec 25 11:40:08 2018 +0000
+++ b/GPA.cpp Thu Feb 07 09:11:51 2019 +0000
@@ -1,100 +1,155 @@
/*
- GPA: frequency point wise gain and phase analyser to measure the frequency
- respone of a dynamical system
+ 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
- hint: the measurements should only be perfomed in closed loop
- assumption: the system is at the desired steady state of interest when
- the measurment starts
+ 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
- exc(2) C: controller
- | 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 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;
- instantiate option 1: (logarithmic equaly spaced frequency points)
+ 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)
+ GPA(float fMin, float fMax, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1, int NstartMin, int NsweepMin)
+
+ 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
+ NperMin: Minimal number of periods that are used for each frequency point
+ NmeasMin: Minimal number of samples that are used for each frequency point
+ 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)
+
+
+ Instantiate option 2: (for a second, refined frequency grid measurement)
- 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
- NperMin: minimal number of periods that are used for each frequency point
- NmeasMin: minimal number of samples that are used for each frequency point
- 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 NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1, int NstartMin, int NsweepMin)
+
+ 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 f0, float f1, float *fexcDes, int NfexcDes, 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
- NfexcDes: length of fexcDes
+ GPA(float *fexcDes, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1, int NstartMin, int NsweepMin)
- 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)
-
- *fexcDes: sorted frequency point array in Hz
- Aexc0: excitation amplitude at fexcDes[0]
- Aexc1: excitation amplitude at fexcDes[NfexcDes-1]
- NfexcDes: length of fexcDes
+ *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.
- 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.
+ 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:
- pseudo code for a closed loop measurement with a controller C:
+ w (const.) exc(2) C: controller
+ | | P: plant
+ v e v
+ exc(1) --> o ->| C |--->o------->| P |----------> out (y)
+ ^ - | |
+ | --> inp (u) | exc (R): excitation signal
+ | | inp (U): input plant
+ -------------------------------- out (Y): output plant
+
+
+ Pseudo code for an open loop measurement:
- 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 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
+
+
+ Pseudo code for a closed loop measurement with stabilizing controller C:
+
+ 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);
- - 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);
+ Closed loop FRF calculus:
+ S = 1 - T;
+ PS = P*S;
+ CS = T/P;
+ C = C/S;
- excitation input at (2):
+ Excitation at excitation input (2):
+
+ - Measuring the plant P = Gyu and the closed loop tf PS = P/(1 + PC) = Gyr:
- - 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);
+ 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;
+ CS = T/P;
+ C = C/S;
- usage:
+
+ 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
+ k+1. The FRF data are plotted to a terminal (Putty) over a serial
connection 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
+
+--------------------------------------------------------------------------------
+ fexc[Hz] |Gyu| deg(Gyu) |Gyr| deg(Gyr) |U| |Y| |R|
+--------------------------------------------------------------------------------
+ 5.0000e-02 1.001e+00 -0.309 1.001e+00 -0.309 4.000e-01 4.000e-01 4.005e-01
+ . . . . . . . .
+ . . . . . . . .
+ . . . . . . . .
+
+ 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');
+ gyr = frd(data(:,4).*exp(1i*data(:,5)*pi/180), data(:,1), Ts, 'Units', 'Hz');
- in matlab you can use:
- dataNucleo = [... insert measurement data here ...];
- g = frd(dataNucleo(:,2).*exp(1i*dataNucleo(:,3)), dataNucleo(:,1), Ts, 'Units', 'Hz');
- gcl = frd(dataNucleo(:,4).*exp(1i*dataNucleo(:,5)), dataNucleo(:,1), Ts, 'Units', 'Hz');
+ If you're evaluating more than one measurement which contain equal frequency points use:
+ data = [data1; data2];
+ [~, ind] = unique(data(:,1), 'stable');
+ data = data(ind,:);
- if you're evaluating more than one measurement which contain equal frequency points try:
- dataNucleo = [dataNucleo1; dataNucleo2];
- [~, ind] = unique(dataNucleo(:,1),'stable');
- dataNucleo = dataNucleo(ind,:);
- autor: M.E. Peter
+ Autor and Copyrigth: 2018 / 2019 / M.E. Peter
+
*/
#include "GPA.h"
@@ -104,96 +159,91 @@
using namespace std;
-GPA::GPA(float fMin, float fMax, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1)
+// -----------------------------------------------------------------------------
+// instantiate
+// -----------------------------------------------------------------------------
+
+GPA::GPA(float fMin, float fMax, int NfexcDes, float Aexc0, float Aexc1, float Ts)
{
- this->NfexcDes = NfexcDes;
- this->NperMin = NperMin;
- this->NmeasMin = NmeasMin;
- this->Ts = (double)Ts;
+ 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);
+
+ // calculate logarithmic spaced frequency points
+ fexcDes = (double*)malloc(NfexcDes*sizeof(double));
+ fexcDesLogspace((double)fMin, (double)fMax, NfexcDes);
+
+ 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 NstartMin, int NsweepMin)
+{
+ assignParameters(NfexcDes, NperMin, NmeasMin, (double)Ts, NstartMin, NsweepMin);
// calculate logarithmic spaced frequency points
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));
+ calculateDecreasingAmplitudeCoefficients((double)Aexc0, (double)Aexc1);
+ initializeConstants((double)Ts);
+ assignFilterStorage();
reset();
}
-GPA::GPA(float f0, float f1, float *fexcDes, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1)
+GPA::GPA(float f0, float f1, float *fexcDes, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1, int NstartMin, int NsweepMin)
{
+ assignParameters(NfexcDes, NperMin, NmeasMin, (double)Ts, NstartMin, NsweepMin);
+
// convert fexcDes from float to double, it is assumed that it is sorted
- this->NfexcDes = NfexcDes;
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 = ((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));
+
+ calculateDecreasingAmplitudeCoefficients((double)Aexc0, (double)Aexc1);
+ initializeConstants((double)Ts);
+ assignFilterStorage();
reset();
}
-GPA::GPA(float *fexcDes, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1)
+GPA::GPA(float *fexcDes, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1, int NstartMin, int NsweepMin)
{
+ assignParameters(NfexcDes, NperMin, NmeasMin, (double)Ts, NstartMin, NsweepMin);
+
// convert fexcDes from float to double, it is assumed that it is sorted
- this->NfexcDes = NfexcDes;
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 = ((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));
+ calculateDecreasingAmplitudeCoefficients((double)Aexc0, (double)Aexc1);
+ initializeConstants((double)Ts);
+ assignFilterStorage();
reset();
}
+// -----------------------------------------------------------------------------
+// virtual and reset
+// -----------------------------------------------------------------------------
+
GPA::~GPA() {}
void GPA::reset()
{
Nmeas = 0;
Nper = 0;
+ dfexc = 0.0;
fexc = 0.0;
fexcPast = 0.0;
- ii = 1; // iterating through desired frequency points
- jj = 1; // iterating through measurement points w.r.t. reachable frequency
+ i = 1; // iterating through desired frequency points
+ j = 1; // iterating through measurement points w.r.t. reachable frequency
scaleG = 0.0;
- scaleH = 2.0;
- wk = 0.0;
cr = 0.0;
ci = 0.0;
for(int i = 0; i < 3; i++) {
@@ -204,84 +254,142 @@
NmeasTotal = 0;
Aexc = 0.0;
pi2Tsfexc = 0.0;
+ Nsweep = NstartMin;
+ bfexc = 0.0;
+ afexc = 0.0;
+ aAexc = 0.0;
+ bAexc = 0.0;
+ AexcOut = 0.0;
}
+// -----------------------------------------------------------------------------
+// update (operator)
+// -----------------------------------------------------------------------------
+
float GPA::update(double inp, double out)
{
// a new frequency point has been reached
- if(jj == 1) {
+ if(j == 1) {
+ // user info
+ if(i == 1) {
+ printLine();
+ printf(" fexc[Hz] |Gyu| deg(Gyu) |Gyr| deg(Gyr) |U| |Y| |R|\r\n");
+ printLine();
+ }
// get a new unique frequency point
while(fexc == fexcPast) {
// measurement finished
- if(ii > NfexcDes) {
+ if(i > NfexcDes) {
return 0.0f;
}
- calcGPAmeasPara(fexcDes[ii - 1]);
+ calcGPAmeasPara(fexcDes[i - 1]);
// secure fexc is not higher or equal to nyquist frequency
if(fexc >= fnyq) {
fexc = fexcPast;
}
// no frequency found
if(fexc == fexcPast) {
- ii += 1;
+ i += 1;
} else {
Aexc = aAexcDes/fexc + bAexcDes;
pi2Tsfexc = pi2Ts*fexc;
}
}
- // secure sinarg starts at 0 (numerically maybe not given)
- sinarg = 0.0;
// filter scaling
scaleG = 1.0/sqrt((double)Nmeas);
// filter coefficients
cr = cos(pi2Tsfexc);
ci = sin(pi2Tsfexc);
- // filter storage
+ // set filter storage zero
for(int i = 0; i < 3; i++) {
sU[i] = 0.0;
sY[i] = 0.0;
}
+ // calculate the parameters for the frequency sweep from fexcPast to fexc
+ if(Nsweep > 0) calcGPAsweepPara();
}
- // update hann window
- calcHann();
- // filter step for signal su
- sU[0] = wk*scaleG*inp + 2.0*cr*sU[1] - sU[2];
- sU[2] = sU[1];
- sU[1] = sU[0];
- // filter step for signal sy
- sY[0] = wk*scaleG*out + 2.0*cr*sY[1] - sY[2];
- sY[2] = sY[1];
- sY[1] = sY[0];
+ // perfomre the sweep or measure
+ if(j <= Nsweep) {
+ dfexc = afexc*(double)j + bfexc;
+ AexcOut = aAexc*(double)j + bAexc;
+ } else {
+ dfexc = fexc;
+ AexcOut = Aexc;
+ // one point DFT filter step for signal su
+ sU[0] = scaleG*inp + 2.0*cr*sU[1] - sU[2];
+ sU[2] = sU[1];
+ sU[1] = sU[0];
+ // one point DFT filter step for signal sy
+ sY[0] = scaleG*out + 2.0*cr*sY[1] - sY[2];
+ 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;
// measurement of frequencypoint is finished
- if(jj == Nmeas) {
- jj = 1;
- ii += 1;
+ if(j == Nmeas + Nsweep) {
fexcPast = fexc;
+ AexcPast = Aexc;
+ Nsweep = NsweepMin;
// calculate the one point dft
- double Ureal = 2.0*scaleH*scaleG*(cr*sU[1] - sU[2]);
- double Uimag = 2.0*scaleH*scaleG*ci*sU[1];
- double Yreal = 2.0*scaleH*scaleG*(cr*sY[1] - sY[2]);
- double Yimag = 2.0*scaleH*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 = (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);
+ float angGyu = (float)wrapAngle(atan2(Yimag, Yreal) - atan2(Uimag, Ureal));
+ float absGyr = (float)(Ymag/Aexc);
+ float angGyr = (float)wrapAngle(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", (float)fexc, absGyu, angGyu, absGye, angGye, (float)Aexc, Umag, Ymag);
+ 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);
+ i += 1;
+ j = 1;
} else {
- jj += 1;
+ j += 1;
}
- sinarg = fmod(sinarg + pi2Tsfexc, pi2);
+ // calculate the excitation
+ sinarg = fmod(sinarg + pi2Ts*dfexc, pi2);
NmeasTotal += 1;
- return (float)(Aexc*sin(sinarg));
+ return (float)(AexcOut*sin(sinarg));
+}
+
+// -----------------------------------------------------------------------------
+// private functions
+// -----------------------------------------------------------------------------
+
+void GPA::assignParameters(int NfexcDes, int NperMin, int NmeasMin, double Ts, int NstartMin, int NsweepMin)
+{
+ this->NfexcDes = NfexcDes;
+ this->NperMin = NperMin;
+ this->NmeasMin = NmeasMin;
+ this->Ts = Ts;
+ this->NstartMin = NstartMin;
+ this->NsweepMin = NsweepMin;
+}
+
+void GPA::calculateDecreasingAmplitudeCoefficients(double Aexc0, double 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];
+}
+
+void GPA::initializeConstants(double Ts)
+{
+ fnyq = 1.0/2.0/Ts;
+ pi2 = 2.0*pi;
+ pi2Ts = pi2*Ts;
+ piDiv2 = pi/2.0;
+ rad2deg = 180.0f/(float)pi;
+}
+
+void GPA::assignFilterStorage()
+{
+ sU = (double*)malloc(3*sizeof(double));
+ sY = (double*)malloc(3*sizeof(double));
}
void GPA::fexcDesLogspace(double fMin, double fMax, int NfexcDes)
@@ -310,16 +418,38 @@
fexc = (double)Nper/(double)Nmeas/Ts;
}
-void GPA::calcHann()
+void GPA::calcGPAsweepPara()
{
- wk = 0.5 - 0.5*cos(2.0*pi*((double)jj-1.0)/(double)Nmeas);
+ // 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)
+{
+ // wrap angle from (-2pi,2pi) into (-pi,pi)
+ if(abs(angle) > pi) angle -= copysign(-pi2, angle); // -1*sign(angle)*2*pi + angle;
+ return angle;
}
void GPA::printLine()
{
- printf("-----------------------------------------------------------------------------------------\r\n");
+ printf("--------------------------------------------------------------------------------\r\n");
}
+void GPA::printLongLine()
+{
+ printf("-------------------------------------------------------------------------------------------------------\r\n");
+}
+
+// -----------------------------------------------------------------------------
+// public functions
+// -----------------------------------------------------------------------------
+
void GPA::printGPAfexcDes()
{
printLine();
@@ -331,21 +461,76 @@
void GPA::printGPAmeasPara()
{
printLine();
- printf(" fexcDes[Hz] fexc[Hz] Aexc Nmeas Nper\r\n");
+ printf(" fexcDes[Hz] fexc[Hz] Aexc Nmeas Nper Nsweep\r\n");
printLine();
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;
- Aexc = 0.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;
- printf("%12.2e %9.2e %10.2e %7i %6i \r\n", (float)fexcDes[i], (float)fexc, (float)Aexc, Nmeas, Nper);
+ 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;
}
printGPAmeasTime();
reset();
@@ -354,12 +539,12 @@
void GPA::printGPAmeasTime()
{
printLine();
- printf(" number of data points: %9i\r\n", NmeasTotal);
- printf(" measurment time in sec: %9.2f\r\n", (float)((double)NmeasTotal*Ts));
+ printf(" Number of data points : %11i\r\n", NmeasTotal);
+ printf(" Measurment time in sec: %12.2f\r\n", (float)((double)NmeasTotal*Ts));
}
void GPA::printNfexcDes()
{
printLine();
- printf(" number of frequancy points: %2i\r\n", NfexcDes);
+ printf(" Number of frequancy points: %3i\r\n", NfexcDes);
}
diff -r 8f1ab2a2267b -r fc53b2d62a1e GPA.h
--- a/GPA.h Tue Dec 25 11:40:08 2018 +0000
+++ b/GPA.h Thu Feb 07 09:11:51 2019 +0000
@@ -1,10 +1,11 @@
class GPA
{
public:
-
- GPA(float fMin, float fMax, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1);
- GPA(float f0, float f1, float *fexcDes, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1);
- GPA(float *fexcDes, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1);
+
+ GPA(float fMin, float fMax, int NfexcDes, float Aexc0, float Aexc1, float Ts);
+ GPA(float fMin, float fMax, 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 NstartMin, int NsweepMin);
+ GPA(float *fexcDes, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1, int NstartMin, int NsweepMin);
float operator()(float inp, float out) {
return update((double)inp, (double)out);
@@ -17,6 +18,7 @@
void printGPAfexcDes();
void printGPAmeasPara();
+ void printFullGPAmeasPara();
void printGPAmeasTime();
void printNfexcDes();
@@ -34,16 +36,16 @@
double pi2;
double pi2Ts;
double piDiv2;
+ float rad2deg;
int Nmeas;
int Nper;
+ double dfexc;
double fexc;
double fexcPast;
- int ii;
- int jj;
+ int i;
+ int j;
double scaleG;
- double scaleH;
- double wk;
double cr;
double ci;
double *sU;
@@ -51,11 +53,26 @@
double sinarg;
int NmeasTotal;
double Aexc;
+ double AexcPast;
double pi2Tsfexc;
-
+ int NstartMin;
+ int NsweepMin;
+ int Nsweep;
+ double bfexc;
+ double afexc;
+ double aAexc;
+ double bAexc;
+ double AexcOut;
+
+ void assignParameters(int NfexcDes, int NperMin, int NmeasMin, double Ts, int NstartMin, int NsweepMin);
+ void calculateDecreasingAmplitudeCoefficients(double Aexc0, double Aexc1);
+ void initializeConstants(double Ts);
+ void assignFilterStorage();
void fexcDesLogspace(double fMin, double fMax, int NfexcDes);
void calcGPAmeasPara(double fexcDes_i);
+ void calcGPAsweepPara();
+ double wrapAngle(double angle);
+ void printLongLine();
void printLine();
- void calcHann();
};
\ No newline at end of file