James Kiwic
Fertig
Fork of
RT2_P3_students
by 
TeamSurface
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