Ruprecht Altenburger / Mbed 2 deprecated RT2_P3_students_G4

Template for group 4

Dependencies:   mbed

Fork of RT2_P3_students by RT2_P3_students

GPA.cpp@2:769ce5f06d3e, 2018-04-09 (annotated)

Committer:
altb
Date:
Mon Apr 09 08:01:29 2018 +0000
Revision:
2:769ce5f06d3e
Parent:
0:78ca29b4c49e
Child:
4:2cc56521aa16
Changes from pmic

Who changed what in which revision?

UserRevisionLine numberNew contents of line
altb 0:78ca29b4c49e 1 /*
altb 0:78ca29b4c49e 2 GPA: frequency point wise gain and phase analyser to measure the frequency
altb 2:769ce5f06d3e 3 respone of a dynamical system
altb 2:769ce5f06d3e 4
altb 0:78ca29b4c49e 5 hint: the measurements should only be perfomed in closed loop
altb 0:78ca29b4c49e 6 assumption: the system is at the desired steady state of interest when
altb 0:78ca29b4c49e 7 the measurment starts
altb 2:769ce5f06d3e 8
altb 0:78ca29b4c49e 9 exc(2) C: controller
altb 0:78ca29b4c49e 10 | P: plant
altb 2:769ce5f06d3e 11 v
altb 0:78ca29b4c49e 12 exc(1) --> o ->| C |--->o------->| P |----------> out
altb 0:78ca29b4c49e 13 ^ | |
altb 0:78ca29b4c49e 14 | --> inp | exc: excitation signal (E)
altb 0:78ca29b4c49e 15 | | inp: input plant (U)
altb 0:78ca29b4c49e 16 -------------------------------- out: output plant (Y)
altb 2:769ce5f06d3e 17
altb 0:78ca29b4c49e 18 instantiate option 1:
altb 0:78ca29b4c49e 19 GPA(float fMin, float fMax, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1)
altb 2:769ce5f06d3e 20
altb 0:78ca29b4c49e 21 fMin: minimal desired frequency that should be measured in Hz
altb 0:78ca29b4c49e 22 fMax: maximal desired frequency that should be measured in Hz
altb 0:78ca29b4c49e 23 NfexcDes: number of logarithmic equaly spaced frequency points
altb 0:78ca29b4c49e 24 NperMin: minimal number of periods that are used for each frequency point
altb 2:769ce5f06d3e 25 NmeasMin: minimal number of samples that are used for each frequency point
altb 0:78ca29b4c49e 26 Ts: sampling time
altb 2:769ce5f06d3e 27 Aexc0: excitation amplitude at fMin
altb 0:78ca29b4c49e 28 Aexc1: excitation amplitude at fMax
altb 2:769ce5f06d3e 29
altb 0:78ca29b4c49e 30 hints: the amplitude drops with 1/fexc, if you're using
altb 2:769ce5f06d3e 31 Axc1 = Aexc0/fMax then d/dt exc = const., this is recommended
altb 0:78ca29b4c49e 32 if your controller does not have a rolloff.
altb 0:78ca29b4c49e 33 if a desired frequency point is not measured try to increase Nmeas.
altb 2:769ce5f06d3e 34
altb 2:769ce5f06d3e 35 pseudo code for a closed loop measurement with a controller C:
altb 2:769ce5f06d3e 36
altb 0:78ca29b4c49e 37 excitation input at (1):
altb 2:769ce5f06d3e 38
altb 2:769ce5f06d3e 39 - measuring the plant P and the closed loop tf T = PC/(1 + PC):
altb 2:769ce5f06d3e 40 desTorque = pi_w(omega_desired - omega + excWobble);
altb 2:769ce5f06d3e 41 inpWobble = desTorque;
altb 2:769ce5f06d3e 42 outWobble = omega;
altb 2:769ce5f06d3e 43 excWobble = Wobble(excWobble, outWobble);
altb 2:769ce5f06d3e 44
altb 2:769ce5f06d3e 45 - measuring the controller C and the closed loop tf SC = C/(1 + PC)
altb 2:769ce5f06d3e 46 desTorque = pi_w(omega_desired - omega + excWobble);
altb 2:769ce5f06d3e 47 inpWobble = n_soll + excWobble - omega;
altb 2:769ce5f06d3e 48 outWobble = desTorque;
altb 2:769ce5f06d3e 49 excWobble = Wobble(inpWobble, outWobble);
altb 2:769ce5f06d3e 50
altb 0:78ca29b4c49e 51 excitation input at (2):
altb 0:78ca29b4c49e 52
altb 2:769ce5f06d3e 53 - measuring the plant P and the closed loop tf SP = P/(1 + PC):
altb 2:769ce5f06d3e 54 desTorque = pi_w(omega_desired - omega) + excWobble;
altb 2:769ce5f06d3e 55 inpWobble = desTorque;
altb 2:769ce5f06d3e 56 outWobble = omega;
altb 2:769ce5f06d3e 57 excWobble = Wobble(excWobble, outWobble);
altb 2:769ce5f06d3e 58
altb 0:78ca29b4c49e 59 usage:
altb 2:769ce5f06d3e 60 exc(k+1) = myGPA(inp(k), out(k)) does update the internal states of the
altb 2:769ce5f06d3e 61 gpa at the timestep k and returns the excitation signal for the timestep
altb 2:769ce5f06d3e 62 k+1. the results are plotted to a terminal (putty) over a serial
altb 2:769ce5f06d3e 63 connection and look as follows:
altb 0:78ca29b4c49e 64 -----------------------------------------------------------------------------------------
altb 0:78ca29b4c49e 65 fexc[Hz] |Gyu| ang(Gyu) |Gye| ang(Gye) |E| |U| |Y|
altb 0:78ca29b4c49e 66 -----------------------------------------------------------------------------------------
altb 2:769ce5f06d3e 67 1.000e+00 2.924e+01 -1.572e+00 1.017e+00 -4.983e-02 5.000e+00 1.739e-01 5.084e+00
altb 2:769ce5f06d3e 68
altb 0:78ca29b4c49e 69 in matlab you can use:
altb 0:78ca29b4c49e 70 dataNucleo = [... insert measurement data here ...];
altb 0:78ca29b4c49e 71 g = frd(dataNucleo(:,2).*exp(1i*dataNucleo(:,3)), dataNucleo(:,1), Ts, 'Units', 'Hz');
altb 0:78ca29b4c49e 72 gcl = frd(dataNucleo(:,4).*exp(1i*dataNucleo(:,5)), dataNucleo(:,1), Ts, 'Units', 'Hz');
altb 2:769ce5f06d3e 73
altb 0:78ca29b4c49e 74 if you're evaluating more than one measurement which contain equal frequency points try:
altb 0:78ca29b4c49e 75 dataNucleo = [dataNucleo1; dataNucleo2];
altb 0:78ca29b4c49e 76 [~, ind] = unique(dataNucleo(:,1),'stable');
altb 0:78ca29b4c49e 77 dataNucleo = dataNucleo(ind,:);
altb 2:769ce5f06d3e 78
altb 0:78ca29b4c49e 79 autor: M.E. Peter
altb 2:769ce5f06d3e 80 */
altb 0:78ca29b4c49e 81
altb 0:78ca29b4c49e 82 #include "GPA.h"
altb 0:78ca29b4c49e 83 #include "mbed.h"
altb 0:78ca29b4c49e 84 #include "math.h"
altb 0:78ca29b4c49e 85 #define pi 3.1415927f
altb 0:78ca29b4c49e 86
altb 0:78ca29b4c49e 87 using namespace std;
altb 0:78ca29b4c49e 88
altb 0:78ca29b4c49e 89 GPA::GPA(float fMin, float fMax, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1)
altb 0:78ca29b4c49e 90 {
altb 0:78ca29b4c49e 91 this->NfexcDes = NfexcDes;
altb 0:78ca29b4c49e 92 this->NperMin = NperMin;
altb 0:78ca29b4c49e 93 this->NmeasMin = NmeasMin;
altb 0:78ca29b4c49e 94 this->Ts = Ts;
altb 0:78ca29b4c49e 95
altb 0:78ca29b4c49e 96 // calculate logarithmic spaced frequency points
altb 0:78ca29b4c49e 97 fexcDes = (float*)malloc(NfexcDes*sizeof(float));
altb 0:78ca29b4c49e 98 fexcDesLogspace(fMin, fMax, NfexcDes);
altb 0:78ca29b4c49e 99
altb 0:78ca29b4c49e 100 // calculate coefficients for decreasing amplitude (1/fexc)
altb 0:78ca29b4c49e 101 this->aAexcDes = (Aexc1 - Aexc0)/(1.0f/fexcDes[NfexcDes-1] - 1.0f/fexcDes[0]);
altb 0:78ca29b4c49e 102 this->bAexcDes = Aexc0 - aAexcDes/fexcDes[0];
altb 0:78ca29b4c49e 103
altb 0:78ca29b4c49e 104 fnyq = 1/2.0f/Ts;
altb 0:78ca29b4c49e 105 pi2 = 2.0f*pi;
altb 0:78ca29b4c49e 106 pi2Ts = pi2*Ts;
altb 0:78ca29b4c49e 107 piDiv2 = pi/2.0f;
altb 0:78ca29b4c49e 108
altb 0:78ca29b4c49e 109 sU = (float*)malloc(3*sizeof(float));
altb 0:78ca29b4c49e 110 sY = (float*)malloc(3*sizeof(float));
altb 0:78ca29b4c49e 111 reset();
altb 0:78ca29b4c49e 112 }
altb 0:78ca29b4c49e 113
altb 0:78ca29b4c49e 114 GPA::~GPA() {}
altb 0:78ca29b4c49e 115
altb 0:78ca29b4c49e 116 void GPA::reset()
altb 0:78ca29b4c49e 117 {
altb 0:78ca29b4c49e 118 Nmeas = 0;
altb 0:78ca29b4c49e 119 Nper = 0;
altb 0:78ca29b4c49e 120 fexc = 0.0f;
altb 0:78ca29b4c49e 121 fexcPast = 0.0f;
altb 0:78ca29b4c49e 122 ii = 1; // iterating through desired frequency points
altb 0:78ca29b4c49e 123 jj = 1; // iterating through measurement points w.r.t. reachable frequency
altb 0:78ca29b4c49e 124 scaleG = 0.0f;
altb 0:78ca29b4c49e 125 cr = 0.0f;
altb 0:78ca29b4c49e 126 ci = 0.0f;
altb 0:78ca29b4c49e 127 for(int i = 0; i < 3; i++) {
altb 0:78ca29b4c49e 128 sU[i] = 0.0f;
altb 0:78ca29b4c49e 129 sY[i] = 0.0f;
altb 0:78ca29b4c49e 130 }
altb 0:78ca29b4c49e 131 sinarg = 0.0f;
altb 0:78ca29b4c49e 132 NmeasTotal = 0;
altb 0:78ca29b4c49e 133 Aexc = 0.0f;
altb 0:78ca29b4c49e 134 pi2Tsfexc = 0.0f;
altb 0:78ca29b4c49e 135 }
altb 0:78ca29b4c49e 136
altb 0:78ca29b4c49e 137 float GPA::update(float inp, float out)
altb 0:78ca29b4c49e 138 {
altb 0:78ca29b4c49e 139 // a new frequency point has been reached
altb 0:78ca29b4c49e 140 if(jj == 1) {
altb 0:78ca29b4c49e 141 // get a new unique frequency point
altb 0:78ca29b4c49e 142 while(fexc == fexcPast) {
altb 0:78ca29b4c49e 143 // measurement finished
altb 0:78ca29b4c49e 144 if(ii > NfexcDes) {
altb 0:78ca29b4c49e 145 return 0.0f;
altb 0:78ca29b4c49e 146 }
altb 0:78ca29b4c49e 147 calcGPAmeasPara(fexcDes[ii - 1]);
altb 0:78ca29b4c49e 148 // secure fexc is not higher or equal to nyquist frequency
altb 0:78ca29b4c49e 149 if(fexc >= fnyq) {
altb 0:78ca29b4c49e 150 fexc = fexcPast;
altb 0:78ca29b4c49e 151 }
altb 0:78ca29b4c49e 152 // no frequency found
altb 0:78ca29b4c49e 153 if(fexc == fexcPast) {
altb 0:78ca29b4c49e 154 ii += 1;
altb 0:78ca29b4c49e 155 } else {
altb 0:78ca29b4c49e 156 Aexc = aAexcDes/fexc + bAexcDes;
altb 0:78ca29b4c49e 157 pi2Tsfexc = pi2Ts*fexc;
altb 0:78ca29b4c49e 158 }
altb 0:78ca29b4c49e 159 }
altb 2:769ce5f06d3e 160 // secure sinarg starts at 0 (numerically maybe not given)
altb 2:769ce5f06d3e 161 sinarg = 0.0f;
altb 0:78ca29b4c49e 162 // filter scaling
altb 0:78ca29b4c49e 163 scaleG = 1.0f/sqrt((float)Nmeas);
altb 0:78ca29b4c49e 164 // filter coefficients
altb 0:78ca29b4c49e 165 cr = cos(pi2Tsfexc);
altb 0:78ca29b4c49e 166 ci = sin(pi2Tsfexc);
altb 0:78ca29b4c49e 167 // filter storage
altb 0:78ca29b4c49e 168 for(int i = 0; i < 3; i++) {
altb 0:78ca29b4c49e 169 sU[i] = 0.0f;
altb 0:78ca29b4c49e 170 sY[i] = 0.0f;
altb 0:78ca29b4c49e 171 }
altb 0:78ca29b4c49e 172 }
altb 0:78ca29b4c49e 173 // filter step for signal su
altb 0:78ca29b4c49e 174 sU[0] = scaleG*inp + 2.0f*cr*sU[1] - sU[2];
altb 0:78ca29b4c49e 175 sU[2] = sU[1];
altb 0:78ca29b4c49e 176 sU[1] = sU[0];
altb 0:78ca29b4c49e 177 // filter step for signal sy
altb 0:78ca29b4c49e 178 sY[0] = scaleG*out + 2.0f*cr*sY[1] - sY[2];
altb 0:78ca29b4c49e 179 sY[2] = sY[1];
altb 0:78ca29b4c49e 180 sY[1] = sY[0];
altb 0:78ca29b4c49e 181 // measurement of frequencypoint is finished
altb 0:78ca29b4c49e 182 if(jj == Nmeas) {
altb 0:78ca29b4c49e 183 jj = 1;
altb 0:78ca29b4c49e 184 ii += 1;
altb 2:769ce5f06d3e 185 fexcPast = fexc;
altb 0:78ca29b4c49e 186 // calculate the one point dft
altb 0:78ca29b4c49e 187 float Ureal = 2.0f*scaleG*(cr*sU[1] - sU[2]);
altb 0:78ca29b4c49e 188 float Uimag = 2.0f*scaleG*ci*sU[1];
altb 0:78ca29b4c49e 189 float Yreal = 2.0f*scaleG*(cr*sY[1] - sY[2]);
altb 0:78ca29b4c49e 190 float Yimag = 2.0f*scaleG*ci*sY[1];
altb 0:78ca29b4c49e 191 // calculate magnitude and angle
altb 0:78ca29b4c49e 192 float Umag = sqrt(Ureal*Ureal + Uimag*Uimag);
altb 0:78ca29b4c49e 193 float Ymag = sqrt(Yreal*Yreal + Yimag*Yimag);
altb 0:78ca29b4c49e 194 float absGyu = Ymag/Umag;
altb 0:78ca29b4c49e 195 float angGyu = atan2(Yimag, Yreal) - atan2(Uimag, Ureal);
altb 0:78ca29b4c49e 196 float absGye = Ymag/Aexc;
altb 0:78ca29b4c49e 197 float angGye = (atan2(Yimag, Yreal) + piDiv2);
altb 0:78ca29b4c49e 198 // user info
altb 2:769ce5f06d3e 199 if(ii == 2) {
altb 0:78ca29b4c49e 200 printLine();
altb 0:78ca29b4c49e 201 printf(" fexc[Hz] |Gyu| ang(Gyu) |Gye| ang(Gye) |E| |U| |Y|\r\n");
altb 0:78ca29b4c49e 202 printLine();
altb 0:78ca29b4c49e 203 }
altb 2:769ce5f06d3e 204 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);
altb 0:78ca29b4c49e 205 } else {
altb 0:78ca29b4c49e 206 jj += 1;
altb 0:78ca29b4c49e 207 }
altb 0:78ca29b4c49e 208 sinarg = fmod(sinarg + pi2Tsfexc, pi2);
altb 0:78ca29b4c49e 209 NmeasTotal += 1;
altb 0:78ca29b4c49e 210 return Aexc*sin(sinarg);
altb 0:78ca29b4c49e 211 }
altb 0:78ca29b4c49e 212
altb 0:78ca29b4c49e 213 void GPA::fexcDesLogspace(float fMin, float fMax, int NfexcDes)
altb 0:78ca29b4c49e 214 {
altb 0:78ca29b4c49e 215 // calculate logarithmic spaced frequency points
altb 0:78ca29b4c49e 216 float Gain = log10(fMax/fMin)/((float)NfexcDes - 1.0f);
altb 0:78ca29b4c49e 217 float expon = 0.0f;
altb 0:78ca29b4c49e 218 for(int i = 0; i < NfexcDes; i++) {
altb 0:78ca29b4c49e 219 fexcDes[i] = fMin*pow(10.0f, expon);
altb 0:78ca29b4c49e 220 expon += Gain;
altb 0:78ca29b4c49e 221 }
altb 0:78ca29b4c49e 222 }
altb 0:78ca29b4c49e 223
altb 0:78ca29b4c49e 224 void GPA::calcGPAmeasPara(float fexcDes_i)
altb 0:78ca29b4c49e 225 {
altb 0:78ca29b4c49e 226 // Nmeas has to be an integer
altb 0:78ca29b4c49e 227 Nper = NperMin;
altb 0:78ca29b4c49e 228 Nmeas = (int)floor((float)Nper/fexcDes_i/Ts + 0.5f);
altb 0:78ca29b4c49e 229 // secure that the minimal number of measurements is fullfilled
altb 0:78ca29b4c49e 230 int Ndelta = NmeasMin - Nmeas;
altb 0:78ca29b4c49e 231 if(Ndelta > 0) {
altb 0:78ca29b4c49e 232 Nper = (int)ceil((float)NmeasMin*fexcDes_i*Ts);
altb 0:78ca29b4c49e 233 Nmeas = (int)floor((float)Nper/fexcDes_i/Ts + 0.5f);
altb 0:78ca29b4c49e 234 }
altb 0:78ca29b4c49e 235 // evaluating reachable frequency
altb 0:78ca29b4c49e 236 fexc = (float)Nper/(float)Nmeas/Ts;
altb 0:78ca29b4c49e 237 }
altb 0:78ca29b4c49e 238
altb 0:78ca29b4c49e 239 void GPA::printLine()
altb 0:78ca29b4c49e 240 {
altb 0:78ca29b4c49e 241 printf("-----------------------------------------------------------------------------------------\r\n");
altb 0:78ca29b4c49e 242 }
altb 0:78ca29b4c49e 243
altb 0:78ca29b4c49e 244 void GPA::printGPAfexcDes()
altb 0:78ca29b4c49e 245 {
altb 0:78ca29b4c49e 246 printLine();
altb 0:78ca29b4c49e 247 for(int i = 0; i < NfexcDes; i++) {
altb 0:78ca29b4c49e 248 printf("%9.4f\r\n", fexcDes[i]);
altb 0:78ca29b4c49e 249 }
altb 0:78ca29b4c49e 250 }
altb 0:78ca29b4c49e 251
altb 0:78ca29b4c49e 252 void GPA::printGPAmeasPara()
altb 0:78ca29b4c49e 253 {
altb 0:78ca29b4c49e 254 printLine();
altb 0:78ca29b4c49e 255 printf(" fexcDes[Hz] fexc[Hz] Aexc Nmeas Nper\r\n");
altb 0:78ca29b4c49e 256 printLine();
altb 0:78ca29b4c49e 257 for(int i = 0; i < NfexcDes; i++) {
altb 0:78ca29b4c49e 258 calcGPAmeasPara(fexcDes[i]);
altb 0:78ca29b4c49e 259 if(fexc == fexcPast || fexc >= fnyq) {
altb 0:78ca29b4c49e 260 fexc = 0.0f;
altb 0:78ca29b4c49e 261 Nmeas = 0;
altb 0:78ca29b4c49e 262 Nper = 0;
altb 0:78ca29b4c49e 263 Aexc = 0;
altb 0:78ca29b4c49e 264 } else {
altb 0:78ca29b4c49e 265 Aexc = aAexcDes/fexc + bAexcDes;
altb 0:78ca29b4c49e 266 fexcPast = fexc;
altb 0:78ca29b4c49e 267 }
altb 0:78ca29b4c49e 268 NmeasTotal += Nmeas;
altb 0:78ca29b4c49e 269 printf("%12.2e %9.2e %10.2e %7i %6i \r\n", fexcDes[i], fexc, Aexc, Nmeas, Nper);
altb 0:78ca29b4c49e 270 }
altb 0:78ca29b4c49e 271 printGPAmeasTime();
altb 0:78ca29b4c49e 272 reset();
altb 0:78ca29b4c49e 273 }
altb 0:78ca29b4c49e 274
altb 0:78ca29b4c49e 275 void GPA::printGPAmeasTime()
altb 0:78ca29b4c49e 276 {
altb 0:78ca29b4c49e 277 printLine();
altb 0:78ca29b4c49e 278 printf(" number of data points: %9i\r\n", NmeasTotal);
altb 0:78ca29b4c49e 279 printf(" measurment time in sec: %9.2f\r\n", (float)NmeasTotal*Ts);
altb 0:78ca29b4c49e 280 }