BA / Mbed 2 deprecated RT2_P3_DAC_test

sfbsg

Dependencies:   mbed

GPA.cpp@0:8ab621116ccd, 2018-04-03 (annotated)

Committer:
borlanic
Date:
Tue Apr 03 15:17:11 2018 +0000
Revision:
0:8ab621116ccd
fg

Who changed what in which revision?

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