altb_pmic
Control Library by altb
Dependents: My_Libraries IndNav_QK3_T265
Diff: GPA.cpp
- Revision:
- 6:694fe8894215
- Parent:
- 0:d49418189c5c
diff -r d8c53cece01b -r 694fe8894215 GPA.cpp
--- a/GPA.cpp Mon Aug 26 07:07:02 2019 +0000
+++ b/GPA.cpp Mon Aug 26 11:48:24 2019 +0000
@@ -1,7 +1,7 @@
/*
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: If the plant has a pole at zero, is unstable or weakly damped the measurement has to be perfomed
+ 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
@@ -17,14 +17,14 @@
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
@@ -34,13 +34,13 @@
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)
@@ -51,7 +51,7 @@
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]
@@ -86,10 +86,10 @@
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:
@@ -167,10 +167,11 @@
{
int NperMin = 3;
int NmeasMin = (int)ceil(1.0f/Ts);
- int NstartMin = (int)ceil(3.0f/Ts);
- int NsweepMin = 0;
+ int Nstart = (int)ceil(3.0f/Ts);
+ int Nsweep = (int)ceil(0.0f/Ts);
- assignParameters(NfexcDes, NperMin, NmeasMin, (double)Ts, NstartMin, NsweepMin);
+ doPrint = true;
+ assignParameters(NfexcDes, NperMin, NmeasMin, (double)Ts, Nstart, Nsweep);
// calculate logarithmic spaced frequency points
fexcDes = (double*)malloc(NfexcDes*sizeof(double));
@@ -182,9 +183,10 @@
reset();
}
-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 fMin, float fMax, 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, (double)Ts, Nstart, Nsweep);
// calculate logarithmic spaced frequency points
fexcDes = (double*)malloc(NfexcDes*sizeof(double));
@@ -196,9 +198,10 @@
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 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, (double)Ts, Nstart, Nsweep);
// convert fexcDes from float to double, it is assumed that it is sorted
this->fexcDes = (double*)malloc(NfexcDes*sizeof(double));
@@ -212,9 +215,10 @@
reset();
}
-GPA::GPA(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);
+ doPrint = true;
+ assignParameters(NfexcDes, NperMin, NmeasMin, (double)Ts, Nstart, Nsweep);
// convert fexcDes from float to double, it is assumed that it is sorted
this->fexcDes = (double*)malloc(NfexcDes*sizeof(double));
@@ -228,6 +232,21 @@
reset();
}
+GPA::GPA(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, (double)Ts, Nstart, Nsweep);
+
+ // 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();
+}
+
// -----------------------------------------------------------------------------
// virtual and reset
// -----------------------------------------------------------------------------
@@ -241,6 +260,7 @@
dfexc = 0.0;
fexc = 0.0;
fexcPast = 0.0;
+ dfexcj = 0.0;
i = 1; // iterating through desired frequency points
j = 1; // iterating through measurement points w.r.t. reachable frequency
scaleG = 0.0;
@@ -251,15 +271,22 @@
sY[i] = 0.0;
}
sinarg = 0.0;
+ sinargR = 0.0;
NmeasTotal = 0;
Aexc = 0.0;
pi2Tsfexc = 0.0;
- Nsweep = NstartMin;
- bfexc = 0.0;
- afexc = 0.0;
- aAexc = 0.0;
- bAexc = 0.0;
+ Nsweep_i = Nstart;
AexcOut = 0.0;
+ 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;
+
}
// -----------------------------------------------------------------------------
@@ -271,7 +298,7 @@
// 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 +307,7 @@
while(fexc == fexcPast) {
// measurement finished
if(i > NfexcDes) {
+ gpaData.MeasPointFinished = false;
return 0.0f;
}
calcGPAmeasPara(fexcDes[i - 1]);
@@ -305,13 +333,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 = ((double)j - 1.0)/((double)Nsweep_i - 1.0);
+ dfexcj = div12pi*sin(pi4*dfexcj) - div812pi*sin(pi2*dfexcj) + dfexcj;
+ dfexc = fexcPast + (fexc - fexcPast)*dfexcj;
+ AexcOut = AexcPast + (Aexc - AexcPast)*dfexcj;
} else {
dfexc = fexc;
AexcOut = Aexc;
@@ -324,27 +353,34 @@
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];
// 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);
+ double Umag = sqrt(Ureal*Ureal + Uimag*Uimag);
+ double Ymag = sqrt(Yreal*Yreal + Yimag*Yimag);
+ gpaData.fexc = (float)fexc;
+ gpaData.absGyu = (float)(Ymag/Umag);
+ gpaData.angGyu = (float)(wrapAngle(atan2(Yimag, Yreal) - atan2(Uimag, Ureal))*rad2deg);
+ gpaData.absGyr = (float)(Ymag/Aexc);
+ gpaData.angGyr = (float)(wrapAngle(atan2(Yimag, Yreal) + fmod(piDiv2 - sinargR, pi2))*rad2deg);
+ gpaData.Umag = (float)(sqrt(Ureal*Ureal + Uimag*Uimag));
+ gpaData.Ymag = (float)(sqrt(Yreal*Yreal + Yimag*Yimag));
+ gpaData.Rmag = (float)Aexc;
+ gpaData.MeasPointFinished = true;
// 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 {
@@ -360,14 +396,14 @@
// 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, double 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)
@@ -381,9 +417,12 @@
{
fnyq = 1.0/2.0/Ts;
pi2 = 2.0*pi;
- pi2Ts = pi2*Ts;
+ pi4 = 4.0*pi;
+ pi2Ts = 2.0*pi*Ts;
piDiv2 = pi/2.0;
- rad2deg = 180.0f/(float)pi;
+ rad2deg = 180.0/pi;
+ div12pi = 1.0/(12.0*pi);
+ div812pi = 8.0/(12.0*pi);
}
void GPA::assignFilterStorage()
@@ -418,17 +457,6 @@
fexc = (double)Nper/(double)Nmeas/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)
{
// wrap angle from (-2pi,2pi) into (-pi,pi)
@@ -436,18 +464,13 @@
return angle;
}
-void GPA::printLine()
-{
- printf("--------------------------------------------------------------------------------\r\n");
-}
-
void GPA::printLongLine()
{
printf("-------------------------------------------------------------------------------------------------------\r\n");
}
// -----------------------------------------------------------------------------
-// public functions
+// public functions (mainly for debugging)
// -----------------------------------------------------------------------------
void GPA::printGPAfexcDes()
@@ -461,7 +484,7 @@
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]);
@@ -470,67 +493,16 @@
Aexc = 0.0;
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", (float)fexcDes[i], (float)fexc, (float)Aexc, Nmeas, Nper, Nsweep_i);
+ Nsweep_i = Nsweep;
}
printGPAmeasTime();
reset();
@@ -548,3 +520,13 @@
printLine();
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