Test program with the RT black boxes

Dependencies:   mbed

Committer:
pmic
Date:
Wed May 15 13:58:10 2019 +0000
Revision:
43:a577f752dd7a
Parent:
39:590aa4d162da
Add new instantiation option to surpress the printout for rt mbed

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pmic 8:d68e177e2571 1 /*
pmic 35:009a1efb5e82 2 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
pmic 35:009a1efb5e82 3
pmic 36:7b78426ea107 4 Hint: If the plant has a pole at zero, is unstable or weakly damped the measurement has to be perfomed
pmic 35:009a1efb5e82 5 in closed loop (this is NOT tfestimate, the algorithm is based on the one point DFT).
pmic 35:009a1efb5e82 6 Assumption: The system is and remains at the desired steady state of interest when the measurment starts
pmic 30:48b233b596b8 7
pmic 35:009a1efb5e82 8 Instantiate option 0: ("Not a Jedi yet" users, for logarithmic equaly spaced frequency points)
pmic 35:009a1efb5e82 9
pmic 35:009a1efb5e82 10 GPA(float fMin, float fMax, int NfexcDes, float Aexc0, float Aexc1, float Ts)
pmic 35:009a1efb5e82 11
pmic 35:009a1efb5e82 12 fMin: Minimal desired frequency that should be measured in Hz
pmic 35:009a1efb5e82 13 fMax: Maximal desired frequency that should be measured in Hz
pmic 35:009a1efb5e82 14 NfexcDes: Number of logarithmic equaly spaced frequency points between fMin and fMax
pmic 35:009a1efb5e82 15 Aexc0: Excitation amplitude at fMin
pmic 35:009a1efb5e82 16 Aexc1: Excitation amplitude at fMax
pmic 35:009a1efb5e82 17 Ts: Sampling time in sec
pmic 35:009a1efb5e82 18
pmic 35:009a1efb5e82 19 Default values are as follows:
pmic 35:009a1efb5e82 20 int NperMin = 3;
pmic 35:009a1efb5e82 21 int NmeasMin = (int)ceil(1.0f/Ts);
pmic 35:009a1efb5e82 22 int NstartMin = (int)ceil(3.0f/Ts);
pmic 35:009a1efb5e82 23 int NsweepMin = 0;
pmic 10:a28f393c6716 24
pmic 35:009a1efb5e82 25 Instantiate option 1: ("Jedi or Sith Lord", for logarithmic equaly spaced frequency points)
pmic 18:a715b1674b67 26
pmic 34:faa89845b641 27 GPA(float fMin, float fMax, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1, int NstartMin, int NsweepMin)
pmic 34:faa89845b641 28
pmic 34:faa89845b641 29 fMin: Minimal desired frequency that should be measured in Hz
pmic 34:faa89845b641 30 fMax: Maximal desired frequency that should be measured in Hz
pmic 34:faa89845b641 31 NfexcDes: Number of logarithmic equaly spaced frequency points
pmic 34:faa89845b641 32 NperMin: Minimal number of periods that are used for each frequency point
pmic 34:faa89845b641 33 NmeasMin: Minimal number of samples that are used for each frequency point
pmic 35:009a1efb5e82 34 Ts: Sampling time in sec
pmic 34:faa89845b641 35 Aexc0: Excitation amplitude at fMin
pmic 34:faa89845b641 36 Aexc1: Excitation amplitude at fMax
pmic 34:faa89845b641 37 NstartMin: Minimal number of samples to sweep to the first frequency point (can be equal 0)
pmic 34:faa89845b641 38 NsweepMin: Minimal number of samples to sweep from freq. point to freq. point (can be equal 0)
pmic 30:48b233b596b8 39
pmic 30:48b233b596b8 40
pmic 30:48b233b596b8 41 Instantiate option 2: (for a second, refined frequency grid measurement)
pmic 18:a715b1674b67 42
pmic 34:faa89845b641 43 GPA(float f0, float f1, float *fexcDes, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1, int NstartMin, int NsweepMin)
pmic 34:faa89845b641 44
pmic 34:faa89845b641 45 f0: Frequency point for the calculation of Aexc0 in Hz (should be chosen like in the first measurement)
pmic 34:faa89845b641 46 f1: Frequency point for the calculation of Aexc1 in Hz (should be chosen like in the first measurement)
pmic 34:faa89845b641 47 *fexcDes: Sorted frequency point array in Hz
pmic 34:faa89845b641 48 NfexcDes: Length of fexcDes
pmic 35:009a1efb5e82 49
pmic 35:009a1efb5e82 50 For the other parameters see above.
pmic 30:48b233b596b8 51
pmic 30:48b233b596b8 52 Instantiate option 3: (for an arbitary but sorted frequency grid measurement)
pmic 19:27ae2ce2e302 53
pmic 34:faa89845b641 54 GPA(float *fexcDes, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1, int NstartMin, int NsweepMin)
pmic 34:faa89845b641 55
pmic 34:faa89845b641 56 *fexcDes: Sorted frequency point array in Hz
pmic 34:faa89845b641 57 Aexc0: Excitation amplitude at fexcDes[0]
pmic 34:faa89845b641 58 Aexc1: Excitation amplitude at fexcDes[NfexcDes-1]
pmic 34:faa89845b641 59 NfexcDes: Length of fexcDes
pmic 35:009a1efb5e82 60
pmic 35:009a1efb5e82 61 For the other parameters see above.
pmic 10:a28f393c6716 62
pmic 35:009a1efb5e82 63 Note: The amplitude drops with 1/fexc, if you're using Axc1 = Aexc0/fMax then d/dt exc = const.,
pmic 35:009a1efb5e82 64 this is recommended if your controller does not have a rolloff. If a desired frequency point
pmic 35:009a1efb5e82 65 is not measured (could not be reached) try to increase Nmeas.
pmic 30:48b233b596b8 66
pmic 34:faa89845b641 67
pmic 34:faa89845b641 68 Block diagram:
pmic 10:a28f393c6716 69
pmic 34:faa89845b641 70 w (const.) exc(2) C: controller
pmic 34:faa89845b641 71 | | P: plant
pmic 34:faa89845b641 72 v e v
pmic 34:faa89845b641 73 exc(1) --> o ->| C |--->o------->| P |----------> out (y)
pmic 34:faa89845b641 74 ^ - | |
pmic 34:faa89845b641 75 | --> inp (u) | exc (R): excitation signal
pmic 34:faa89845b641 76 | | inp (U): input plant
pmic 34:faa89845b641 77 -------------------------------- out (Y): output plant
pmic 34:faa89845b641 78
pmic 34:faa89845b641 79
pmic 34:faa89845b641 80 Pseudo code for an open loop measurement:
pmic 34:faa89845b641 81
pmic 34:faa89845b641 82 - Measuring the plant P = Gyu = Gyr:
pmic 34:faa89845b641 83
pmic 30:48b233b596b8 84 u = w + exc;
pmic 34:faa89845b641 85 ... write output u here! it follows exc(k+1) ...
pmic 30:48b233b596b8 86 exc = Wobble(exc, y);
pmic 34:faa89845b641 87
pmic 34:faa89845b641 88 Closed loop FRF calculus with a stabilizing controller C:
pmic 43:a577f752dd7a 89 S = 1/(1 + C*P); % ( exc1 -> e , 1/(1 + C*P) ) sensitivity, contr. error rejection, robustness (1/max|S|)
pmic 43:a577f752dd7a 90 T = 1 - S; % ( w -> y , C*P/(1 + C*P) ) compl. sensitivity, tracking performance
pmic 43:a577f752dd7a 91 CS = C*S; % ( exc1 -> u , C/(1 + C*P) ) control effort, disturbance plant output on plant input
pmic 43:a577f752dd7a 92 PS = P*S; % ( exc2 -> y , P/(1 + C*P) ) compliance, disturbance plant input on plant output
pmic 34:faa89845b641 93
pmic 10:a28f393c6716 94
pmic 31:b78b299c248b 95 Pseudo code for a closed loop measurement with stabilizing controller C:
pmic 30:48b233b596b8 96
pmic 34:faa89845b641 97 Excitation at excitation input (1):
pmic 34:faa89845b641 98
pmic 34:faa89845b641 99 - Measuring the plant P = Gyu and the closed loop tf T = PC/(1 + PC) = Gyr:
pmic 34:faa89845b641 100
pmic 30:48b233b596b8 101 u = C(w - y + exc);
pmic 34:faa89845b641 102 ... write output u here! it follows exc(k+1) ...
pmic 30:48b233b596b8 103 exc = Wobble(u, y);
pmic 11:ed2638662dfa 104
pmic 34:faa89845b641 105 Closed loop FRF calculus:
pmic 34:faa89845b641 106 S = 1 - T;
pmic 34:faa89845b641 107 PS = P*S;
pmic 34:faa89845b641 108 CS = T/P;
pmic 34:faa89845b641 109 C = C/S;
pmic 10:a28f393c6716 110
pmic 34:faa89845b641 111 Excitation at excitation input (2):
pmic 11:ed2638662dfa 112
pmic 34:faa89845b641 113 - Measuring the plant P = Gyu and the closed loop tf PS = P/(1 + PC) = Gyr:
pmic 34:faa89845b641 114
pmic 30:48b233b596b8 115 u = C(w - y) + exc;
pmic 34:faa89845b641 116 ... write output u here! it follows exc(k+1) ...
pmic 30:48b233b596b8 117 exc = Wobble(u, y);
pmic 30:48b233b596b8 118
pmic 34:faa89845b641 119 Closed loop FRF calculus:
pmic 34:faa89845b641 120 S = PS/P;
pmic 34:faa89845b641 121 T = 1 - S;
pmic 34:faa89845b641 122 CS = T/P;
pmic 34:faa89845b641 123 C = C/S;
pmic 10:a28f393c6716 124
pmic 30:48b233b596b8 125
pmic 30:48b233b596b8 126 Usage:
pmic 11:ed2638662dfa 127 exc(k+1) = myGPA(inp(k), out(k)) does update the internal states of the
pmic 11:ed2638662dfa 128 gpa at the timestep k and returns the excitation signal for the timestep
pmic 30:48b233b596b8 129 k+1. The FRF data are plotted to a terminal (Putty) over a serial
pmic 11:ed2638662dfa 130 connection and look as follows:
pmic 30:48b233b596b8 131
pmic 30:48b233b596b8 132 --------------------------------------------------------------------------------
pmic 33:4de33aa9bc86 133 fexc[Hz] |Gyu| deg(Gyu) |Gyr| deg(Gyr) |U| |Y| |R|
pmic 30:48b233b596b8 134 --------------------------------------------------------------------------------
pmic 30:48b233b596b8 135 5.0000e-02 1.001e+00 -0.309 1.001e+00 -0.309 4.000e-01 4.000e-01 4.005e-01
pmic 30:48b233b596b8 136 . . . . . . . .
pmic 30:48b233b596b8 137 . . . . . . . .
pmic 30:48b233b596b8 138 . . . . . . . .
pmic 30:48b233b596b8 139
pmic 30:48b233b596b8 140 In Matlab you can use the editor as follows:
pmic 30:48b233b596b8 141 data = [... insert measurement data here ...];
pmic 30:48b233b596b8 142 gyu = frd(data(:,2).*exp(1i*data(:,3)*pi/180), data(:,1), Ts, 'Units', 'Hz');
pmic 30:48b233b596b8 143 gyr = frd(data(:,4).*exp(1i*data(:,5)*pi/180), data(:,1), Ts, 'Units', 'Hz');
pmic 10:a28f393c6716 144
pmic 30:48b233b596b8 145 If you're evaluating more than one measurement which contain equal frequency points use:
pmic 30:48b233b596b8 146 data = [data1; data2];
pmic 30:48b233b596b8 147 [~, ind] = unique(data(:,1), 'stable');
pmic 30:48b233b596b8 148 data = data(ind,:);
pmic 10:a28f393c6716 149
pmic 34:faa89845b641 150
pmic 34:faa89845b641 151 Autor and Copyrigth: 2018 / 2019 / M.E. Peter
pmic 30:48b233b596b8 152
pmic 10:a28f393c6716 153 */
pmic 8:d68e177e2571 154
pmic 6:da0c9587ecae 155 #include "GPA.h"
pmic 6:da0c9587ecae 156 #include "mbed.h"
pmic 6:da0c9587ecae 157 #include "math.h"
pmic 18:a715b1674b67 158 #define pi 3.141592653589793
pmic 6:da0c9587ecae 159
pmic 6:da0c9587ecae 160 using namespace std;
pmic 6:da0c9587ecae 161
pmic 30:48b233b596b8 162 // -----------------------------------------------------------------------------
pmic 30:48b233b596b8 163 // instantiate
pmic 30:48b233b596b8 164 // -----------------------------------------------------------------------------
pmic 30:48b233b596b8 165
pmic 35:009a1efb5e82 166 GPA::GPA(float fMin, float fMax, int NfexcDes, float Aexc0, float Aexc1, float Ts)
pmic 35:009a1efb5e82 167 {
pmic 35:009a1efb5e82 168 int NperMin = 3;
pmic 35:009a1efb5e82 169 int NmeasMin = (int)ceil(1.0f/Ts);
pmic 35:009a1efb5e82 170 int NstartMin = (int)ceil(3.0f/Ts);
pmic 35:009a1efb5e82 171 int NsweepMin = 0;
pmic 35:009a1efb5e82 172
pmic 43:a577f752dd7a 173 doPrint = true;
pmic 35:009a1efb5e82 174 assignParameters(NfexcDes, NperMin, NmeasMin, (double)Ts, NstartMin, NsweepMin);
pmic 35:009a1efb5e82 175
pmic 35:009a1efb5e82 176 // calculate logarithmic spaced frequency points
pmic 35:009a1efb5e82 177 fexcDes = (double*)malloc(NfexcDes*sizeof(double));
pmic 35:009a1efb5e82 178 fexcDesLogspace((double)fMin, (double)fMax, NfexcDes);
pmic 35:009a1efb5e82 179
pmic 35:009a1efb5e82 180 calculateDecreasingAmplitudeCoefficients((double)Aexc0, (double)Aexc1);
pmic 35:009a1efb5e82 181 initializeConstants((double)Ts);
pmic 35:009a1efb5e82 182 assignFilterStorage();
pmic 35:009a1efb5e82 183 reset();
pmic 35:009a1efb5e82 184 }
pmic 35:009a1efb5e82 185
pmic 30:48b233b596b8 186 GPA::GPA(float fMin, float fMax, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1, int NstartMin, int NsweepMin)
pmic 6:da0c9587ecae 187 {
pmic 43:a577f752dd7a 188 doPrint = true;
pmic 30:48b233b596b8 189 assignParameters(NfexcDes, NperMin, NmeasMin, (double)Ts, NstartMin, NsweepMin);
pmic 6:da0c9587ecae 190
pmic 6:da0c9587ecae 191 // calculate logarithmic spaced frequency points
pmic 18:a715b1674b67 192 fexcDes = (double*)malloc(NfexcDes*sizeof(double));
pmic 18:a715b1674b67 193 fexcDesLogspace((double)fMin, (double)fMax, NfexcDes);
pmic 6:da0c9587ecae 194
pmic 30:48b233b596b8 195 calculateDecreasingAmplitudeCoefficients((double)Aexc0, (double)Aexc1);
pmic 30:48b233b596b8 196 initializeConstants((double)Ts);
pmic 30:48b233b596b8 197 assignFilterStorage();
pmic 18:a715b1674b67 198 reset();
pmic 18:a715b1674b67 199 }
pmic 6:da0c9587ecae 200
pmic 30:48b233b596b8 201 GPA::GPA(float f0, float f1, float *fexcDes, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1, int NstartMin, int NsweepMin)
pmic 18:a715b1674b67 202 {
pmic 43:a577f752dd7a 203 doPrint = true;
pmic 30:48b233b596b8 204 assignParameters(NfexcDes, NperMin, NmeasMin, (double)Ts, NstartMin, NsweepMin);
pmic 30:48b233b596b8 205
pmic 18:a715b1674b67 206 // convert fexcDes from float to double, it is assumed that it is sorted
pmic 18:a715b1674b67 207 this->fexcDes = (double*)malloc(NfexcDes*sizeof(double));
pmic 18:a715b1674b67 208 for(int i = 0; i < NfexcDes; i++) {
pmic 18:a715b1674b67 209 this->fexcDes[i] = (double)fexcDes[i];
pmic 18:a715b1674b67 210 }
pmic 30:48b233b596b8 211
pmic 30:48b233b596b8 212 calculateDecreasingAmplitudeCoefficients((double)Aexc0, (double)Aexc1);
pmic 30:48b233b596b8 213 initializeConstants((double)Ts);
pmic 30:48b233b596b8 214 assignFilterStorage();
pmic 6:da0c9587ecae 215 reset();
pmic 6:da0c9587ecae 216 }
pmic 6:da0c9587ecae 217
pmic 30:48b233b596b8 218 GPA::GPA(float *fexcDes, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1, int NstartMin, int NsweepMin)
pmic 19:27ae2ce2e302 219 {
pmic 43:a577f752dd7a 220 doPrint = true;
pmic 30:48b233b596b8 221 assignParameters(NfexcDes, NperMin, NmeasMin, (double)Ts, NstartMin, NsweepMin);
pmic 30:48b233b596b8 222
pmic 19:27ae2ce2e302 223 // convert fexcDes from float to double, it is assumed that it is sorted
pmic 19:27ae2ce2e302 224 this->fexcDes = (double*)malloc(NfexcDes*sizeof(double));
pmic 19:27ae2ce2e302 225 for(int i = 0; i < NfexcDes; i++) {
pmic 19:27ae2ce2e302 226 this->fexcDes[i] = (double)fexcDes[i];
pmic 19:27ae2ce2e302 227 }
pmic 19:27ae2ce2e302 228
pmic 30:48b233b596b8 229 calculateDecreasingAmplitudeCoefficients((double)Aexc0, (double)Aexc1);
pmic 30:48b233b596b8 230 initializeConstants((double)Ts);
pmic 30:48b233b596b8 231 assignFilterStorage();
pmic 19:27ae2ce2e302 232 reset();
pmic 19:27ae2ce2e302 233 }
pmic 19:27ae2ce2e302 234
pmic 43:a577f752dd7a 235 GPA::GPA(float fMin, float fMax, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1, int NstartMin, int NsweepMin, bool doPrint)
pmic 43:a577f752dd7a 236 {
pmic 43:a577f752dd7a 237 this->doPrint = doPrint;
pmic 43:a577f752dd7a 238 assignParameters(NfexcDes, NperMin, NmeasMin, (double)Ts, NstartMin, NsweepMin);
pmic 43:a577f752dd7a 239
pmic 43:a577f752dd7a 240 // calculate logarithmic spaced frequency points
pmic 43:a577f752dd7a 241 fexcDes = (double*)malloc(NfexcDes*sizeof(double));
pmic 43:a577f752dd7a 242 fexcDesLogspace((double)fMin, (double)fMax, NfexcDes);
pmic 43:a577f752dd7a 243
pmic 43:a577f752dd7a 244 calculateDecreasingAmplitudeCoefficients((double)Aexc0, (double)Aexc1);
pmic 43:a577f752dd7a 245 initializeConstants((double)Ts);
pmic 43:a577f752dd7a 246 assignFilterStorage();
pmic 43:a577f752dd7a 247 reset();
pmic 43:a577f752dd7a 248 }
pmic 43:a577f752dd7a 249
pmic 30:48b233b596b8 250 // -----------------------------------------------------------------------------
pmic 30:48b233b596b8 251 // virtual and reset
pmic 30:48b233b596b8 252 // -----------------------------------------------------------------------------
pmic 30:48b233b596b8 253
pmic 6:da0c9587ecae 254 GPA::~GPA() {}
pmic 6:da0c9587ecae 255
pmic 6:da0c9587ecae 256 void GPA::reset()
pmic 6:da0c9587ecae 257 {
pmic 6:da0c9587ecae 258 Nmeas = 0;
pmic 6:da0c9587ecae 259 Nper = 0;
pmic 30:48b233b596b8 260 dfexc = 0.0;
pmic 18:a715b1674b67 261 fexc = 0.0;
pmic 18:a715b1674b67 262 fexcPast = 0.0;
pmic 30:48b233b596b8 263 i = 1; // iterating through desired frequency points
pmic 30:48b233b596b8 264 j = 1; // iterating through measurement points w.r.t. reachable frequency
pmic 18:a715b1674b67 265 scaleG = 0.0;
pmic 18:a715b1674b67 266 cr = 0.0;
pmic 18:a715b1674b67 267 ci = 0.0;
pmic 6:da0c9587ecae 268 for(int i = 0; i < 3; i++) {
pmic 18:a715b1674b67 269 sU[i] = 0.0;
pmic 18:a715b1674b67 270 sY[i] = 0.0;
pmic 6:da0c9587ecae 271 }
pmic 18:a715b1674b67 272 sinarg = 0.0;
pmic 6:da0c9587ecae 273 NmeasTotal = 0;
pmic 18:a715b1674b67 274 Aexc = 0.0;
pmic 18:a715b1674b67 275 pi2Tsfexc = 0.0;
pmic 30:48b233b596b8 276 Nsweep = NstartMin;
pmic 30:48b233b596b8 277 bfexc = 0.0;
pmic 30:48b233b596b8 278 afexc = 0.0;
pmic 30:48b233b596b8 279 aAexc = 0.0;
pmic 30:48b233b596b8 280 bAexc = 0.0;
pmic 30:48b233b596b8 281 AexcOut = 0.0;
pmic 37:352bacb64f3f 282 gpaData.fexc = 0.0f;
pmic 37:352bacb64f3f 283 gpaData.absGyu = 0.0f;
pmic 37:352bacb64f3f 284 gpaData.angGyu = 0.0f;
pmic 37:352bacb64f3f 285 gpaData.absGyr = 0.0f;
pmic 37:352bacb64f3f 286 gpaData.angGyr = 0.0f;
pmic 37:352bacb64f3f 287 gpaData.Umag = 0.0f;
pmic 37:352bacb64f3f 288 gpaData.Ymag = 0.0f;
pmic 37:352bacb64f3f 289 gpaData.Rmag = 0.0f;
pmic 37:352bacb64f3f 290 gpaData.MeasPointFinished = false;
pmic 37:352bacb64f3f 291
pmic 6:da0c9587ecae 292 }
pmic 6:da0c9587ecae 293
pmic 30:48b233b596b8 294 // -----------------------------------------------------------------------------
pmic 30:48b233b596b8 295 // update (operator)
pmic 30:48b233b596b8 296 // -----------------------------------------------------------------------------
pmic 30:48b233b596b8 297
pmic 18:a715b1674b67 298 float GPA::update(double inp, double out)
pmic 6:da0c9587ecae 299 {
pmic 6:da0c9587ecae 300 // a new frequency point has been reached
pmic 30:48b233b596b8 301 if(j == 1) {
pmic 30:48b233b596b8 302 // user info
pmic 43:a577f752dd7a 303 if(i == 1 & doPrint) {
pmic 30:48b233b596b8 304 printLine();
pmic 33:4de33aa9bc86 305 printf(" fexc[Hz] |Gyu| deg(Gyu) |Gyr| deg(Gyr) |U| |Y| |R|\r\n");
pmic 30:48b233b596b8 306 printLine();
pmic 30:48b233b596b8 307 }
pmic 6:da0c9587ecae 308 // get a new unique frequency point
pmic 6:da0c9587ecae 309 while(fexc == fexcPast) {
pmic 6:da0c9587ecae 310 // measurement finished
pmic 30:48b233b596b8 311 if(i > NfexcDes) {
pmic 39:590aa4d162da 312 gpaData.MeasPointFinished = false;
pmic 6:da0c9587ecae 313 return 0.0f;
pmic 6:da0c9587ecae 314 }
pmic 30:48b233b596b8 315 calcGPAmeasPara(fexcDes[i - 1]);
pmic 6:da0c9587ecae 316 // secure fexc is not higher or equal to nyquist frequency
pmic 6:da0c9587ecae 317 if(fexc >= fnyq) {
pmic 6:da0c9587ecae 318 fexc = fexcPast;
pmic 6:da0c9587ecae 319 }
pmic 6:da0c9587ecae 320 // no frequency found
pmic 6:da0c9587ecae 321 if(fexc == fexcPast) {
pmic 30:48b233b596b8 322 i += 1;
pmic 6:da0c9587ecae 323 } else {
pmic 6:da0c9587ecae 324 Aexc = aAexcDes/fexc + bAexcDes;
pmic 6:da0c9587ecae 325 pi2Tsfexc = pi2Ts*fexc;
pmic 6:da0c9587ecae 326 }
pmic 6:da0c9587ecae 327 }
pmic 6:da0c9587ecae 328 // filter scaling
pmic 18:a715b1674b67 329 scaleG = 1.0/sqrt((double)Nmeas);
pmic 6:da0c9587ecae 330 // filter coefficients
pmic 6:da0c9587ecae 331 cr = cos(pi2Tsfexc);
pmic 6:da0c9587ecae 332 ci = sin(pi2Tsfexc);
pmic 30:48b233b596b8 333 // set filter storage zero
pmic 6:da0c9587ecae 334 for(int i = 0; i < 3; i++) {
pmic 18:a715b1674b67 335 sU[i] = 0.0;
pmic 18:a715b1674b67 336 sY[i] = 0.0;
pmic 6:da0c9587ecae 337 }
pmic 30:48b233b596b8 338 // calculate the parameters for the frequency sweep from fexcPast to fexc
pmic 30:48b233b596b8 339 if(Nsweep > 0) calcGPAsweepPara();
pmic 37:352bacb64f3f 340 gpaData.MeasPointFinished = false;
pmic 6:da0c9587ecae 341 }
pmic 30:48b233b596b8 342 // perfomre the sweep or measure
pmic 30:48b233b596b8 343 if(j <= Nsweep) {
pmic 30:48b233b596b8 344 dfexc = afexc*(double)j + bfexc;
pmic 30:48b233b596b8 345 AexcOut = aAexc*(double)j + bAexc;
pmic 30:48b233b596b8 346 } else {
pmic 30:48b233b596b8 347 dfexc = fexc;
pmic 30:48b233b596b8 348 AexcOut = Aexc;
pmic 30:48b233b596b8 349 // one point DFT filter step for signal su
pmic 30:48b233b596b8 350 sU[0] = scaleG*inp + 2.0*cr*sU[1] - sU[2];
pmic 30:48b233b596b8 351 sU[2] = sU[1];
pmic 30:48b233b596b8 352 sU[1] = sU[0];
pmic 30:48b233b596b8 353 // one point DFT filter step for signal sy
pmic 30:48b233b596b8 354 sY[0] = scaleG*out + 2.0*cr*sY[1] - sY[2];
pmic 30:48b233b596b8 355 sY[2] = sY[1];
pmic 30:48b233b596b8 356 sY[1] = sY[0];
pmic 30:48b233b596b8 357 }
pmic 30:48b233b596b8 358 // secure sinarg starts at 0 (numerically maybe not given)
pmic 30:48b233b596b8 359 if(j == 1 || j == Nsweep + 1) sinarg = 0.0;
pmic 6:da0c9587ecae 360 // measurement of frequencypoint is finished
pmic 30:48b233b596b8 361 if(j == Nmeas + Nsweep) {
pmic 10:a28f393c6716 362 fexcPast = fexc;
pmic 30:48b233b596b8 363 AexcPast = Aexc;
pmic 30:48b233b596b8 364 Nsweep = NsweepMin;
pmic 6:da0c9587ecae 365 // calculate the one point dft
pmic 28:f1e7a69b51ae 366 double Ureal = 2.0*scaleG*(cr*sU[1] - sU[2]);
pmic 28:f1e7a69b51ae 367 double Uimag = 2.0*scaleG*ci*sU[1];
pmic 28:f1e7a69b51ae 368 double Yreal = 2.0*scaleG*(cr*sY[1] - sY[2]);
pmic 28:f1e7a69b51ae 369 double Yimag = 2.0*scaleG*ci*sY[1];
pmic 6:da0c9587ecae 370 // calculate magnitude and angle
pmic 37:352bacb64f3f 371 double Umag = sqrt(Ureal*Ureal + Uimag*Uimag);
pmic 37:352bacb64f3f 372 double Ymag = sqrt(Yreal*Yreal + Yimag*Yimag);
pmic 37:352bacb64f3f 373 gpaData.fexc = (float)fexc;
pmic 37:352bacb64f3f 374 gpaData.absGyu = (float)(Ymag/Umag);
pmic 37:352bacb64f3f 375 gpaData.angGyu = (float)(wrapAngle(atan2(Yimag, Yreal) - atan2(Uimag, Ureal))*rad2deg);
pmic 37:352bacb64f3f 376 gpaData.absGyr = (float)(Ymag/Aexc);
pmic 37:352bacb64f3f 377 gpaData.angGyr = (float)(wrapAngle(atan2(Yimag, Yreal) + piDiv2)*rad2deg);
pmic 37:352bacb64f3f 378 gpaData.Umag = (float)(sqrt(Ureal*Ureal + Uimag*Uimag));
pmic 37:352bacb64f3f 379 gpaData.Ymag = (float)(sqrt(Yreal*Yreal + Yimag*Yimag));
pmic 37:352bacb64f3f 380 gpaData.Rmag = (float)Aexc;
pmic 37:352bacb64f3f 381 gpaData.MeasPointFinished = true;
pmic 6:da0c9587ecae 382 // user info
pmic 43:a577f752dd7a 383 if(doPrint) {
pmic 37:352bacb64f3f 384 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);
pmic 43:a577f752dd7a 385 }
pmic 30:48b233b596b8 386 i += 1;
pmic 30:48b233b596b8 387 j = 1;
pmic 6:da0c9587ecae 388 } else {
pmic 30:48b233b596b8 389 j += 1;
pmic 6:da0c9587ecae 390 }
pmic 30:48b233b596b8 391 // calculate the excitation
pmic 30:48b233b596b8 392 sinarg = fmod(sinarg + pi2Ts*dfexc, pi2);
pmic 6:da0c9587ecae 393 NmeasTotal += 1;
pmic 30:48b233b596b8 394 return (float)(AexcOut*sin(sinarg));
pmic 30:48b233b596b8 395 }
pmic 30:48b233b596b8 396
pmic 30:48b233b596b8 397 // -----------------------------------------------------------------------------
pmic 30:48b233b596b8 398 // private functions
pmic 30:48b233b596b8 399 // -----------------------------------------------------------------------------
pmic 30:48b233b596b8 400
pmic 30:48b233b596b8 401 void GPA::assignParameters(int NfexcDes, int NperMin, int NmeasMin, double Ts, int NstartMin, int NsweepMin)
pmic 30:48b233b596b8 402 {
pmic 30:48b233b596b8 403 this->NfexcDes = NfexcDes;
pmic 30:48b233b596b8 404 this->NperMin = NperMin;
pmic 30:48b233b596b8 405 this->NmeasMin = NmeasMin;
pmic 30:48b233b596b8 406 this->Ts = Ts;
pmic 30:48b233b596b8 407 this->NstartMin = NstartMin;
pmic 30:48b233b596b8 408 this->NsweepMin = NsweepMin;
pmic 30:48b233b596b8 409 }
pmic 30:48b233b596b8 410
pmic 30:48b233b596b8 411 void GPA::calculateDecreasingAmplitudeCoefficients(double Aexc0, double Aexc1)
pmic 30:48b233b596b8 412 {
pmic 30:48b233b596b8 413 // calculate coefficients for decreasing amplitude (1/fexc)
pmic 30:48b233b596b8 414 this->aAexcDes = (Aexc1 - Aexc0)/(1.0/fexcDes[NfexcDes-1] - 1.0/fexcDes[0]);
pmic 30:48b233b596b8 415 this->bAexcDes = Aexc0 - aAexcDes/fexcDes[0];
pmic 30:48b233b596b8 416 }
pmic 30:48b233b596b8 417
pmic 30:48b233b596b8 418 void GPA::initializeConstants(double Ts)
pmic 30:48b233b596b8 419 {
pmic 30:48b233b596b8 420 fnyq = 1.0/2.0/Ts;
pmic 30:48b233b596b8 421 pi2 = 2.0*pi;
pmic 30:48b233b596b8 422 pi2Ts = pi2*Ts;
pmic 30:48b233b596b8 423 piDiv2 = pi/2.0;
pmic 39:590aa4d162da 424 rad2deg = 180.0/pi;
pmic 30:48b233b596b8 425 }
pmic 30:48b233b596b8 426
pmic 30:48b233b596b8 427 void GPA::assignFilterStorage()
pmic 30:48b233b596b8 428 {
pmic 30:48b233b596b8 429 sU = (double*)malloc(3*sizeof(double));
pmic 30:48b233b596b8 430 sY = (double*)malloc(3*sizeof(double));
pmic 6:da0c9587ecae 431 }
pmic 6:da0c9587ecae 432
pmic 18:a715b1674b67 433 void GPA::fexcDesLogspace(double fMin, double fMax, int NfexcDes)
pmic 6:da0c9587ecae 434 {
pmic 6:da0c9587ecae 435 // calculate logarithmic spaced frequency points
pmic 18:a715b1674b67 436 double Gain = log10(fMax/fMin)/((double)NfexcDes - 1.0);
pmic 18:a715b1674b67 437 double expon = 0.0;
pmic 6:da0c9587ecae 438 for(int i = 0; i < NfexcDes; i++) {
pmic 18:a715b1674b67 439 fexcDes[i] = fMin*pow(10.0, expon);
pmic 6:da0c9587ecae 440 expon += Gain;
pmic 6:da0c9587ecae 441 }
pmic 6:da0c9587ecae 442 }
pmic 6:da0c9587ecae 443
pmic 18:a715b1674b67 444 void GPA::calcGPAmeasPara(double fexcDes_i)
pmic 6:da0c9587ecae 445 {
pmic 6:da0c9587ecae 446 // Nmeas has to be an integer
pmic 6:da0c9587ecae 447 Nper = NperMin;
pmic 18:a715b1674b67 448 Nmeas = (int)floor((double)Nper/fexcDes_i/Ts + 0.5);
pmic 6:da0c9587ecae 449 // secure that the minimal number of measurements is fullfilled
pmic 6:da0c9587ecae 450 int Ndelta = NmeasMin - Nmeas;
pmic 6:da0c9587ecae 451 if(Ndelta > 0) {
pmic 18:a715b1674b67 452 Nper = (int)ceil((double)NmeasMin*fexcDes_i*Ts);
pmic 18:a715b1674b67 453 Nmeas = (int)floor((double)Nper/fexcDes_i/Ts + 0.5);
pmic 6:da0c9587ecae 454 }
pmic 6:da0c9587ecae 455 // evaluating reachable frequency
pmic 18:a715b1674b67 456 fexc = (double)Nper/(double)Nmeas/Ts;
pmic 6:da0c9587ecae 457 }
pmic 6:da0c9587ecae 458
pmic 30:48b233b596b8 459 void GPA::calcGPAsweepPara()
pmic 30:48b233b596b8 460 {
pmic 30:48b233b596b8 461 // calculate linear frequency sweep parameters
pmic 30:48b233b596b8 462 double ksta = ceil(Ts*(double)Nsweep/2.0*(fexc + fexcPast));
pmic 30:48b233b596b8 463 Nsweep = (int)floor(2.0*ksta/Ts/(fexc + fexcPast) + 0.5);
pmic 30:48b233b596b8 464 bfexc = 2.0*ksta/Ts/(double)Nsweep - fexc;
pmic 30:48b233b596b8 465 afexc = (fexc - bfexc)/((double)Nsweep + 1.0);
pmic 30:48b233b596b8 466 aAexc = (Aexc - AexcPast)/((double)Nsweep + 1.0);
pmic 30:48b233b596b8 467 bAexc = AexcPast;
pmic 30:48b233b596b8 468 }
pmic 30:48b233b596b8 469
pmic 30:48b233b596b8 470 double GPA::wrapAngle(double angle)
pmic 30:48b233b596b8 471 {
pmic 30:48b233b596b8 472 // wrap angle from (-2pi,2pi) into (-pi,pi)
pmic 30:48b233b596b8 473 if(abs(angle) > pi) angle -= copysign(-pi2, angle); // -1*sign(angle)*2*pi + angle;
pmic 30:48b233b596b8 474 return angle;
pmic 30:48b233b596b8 475 }
pmic 30:48b233b596b8 476
pmic 30:48b233b596b8 477 void GPA::printLongLine()
pmic 30:48b233b596b8 478 {
pmic 30:48b233b596b8 479 printf("-------------------------------------------------------------------------------------------------------\r\n");
pmic 30:48b233b596b8 480 }
pmic 30:48b233b596b8 481
pmic 30:48b233b596b8 482 // -----------------------------------------------------------------------------
pmic 30:48b233b596b8 483 // public functions
pmic 30:48b233b596b8 484 // -----------------------------------------------------------------------------
pmic 30:48b233b596b8 485
pmic 6:da0c9587ecae 486 void GPA::printGPAfexcDes()
pmic 6:da0c9587ecae 487 {
pmic 6:da0c9587ecae 488 printLine();
pmic 6:da0c9587ecae 489 for(int i = 0; i < NfexcDes; i++) {
pmic 18:a715b1674b67 490 printf("%9.4f\r\n", (float)fexcDes[i]);
pmic 6:da0c9587ecae 491 }
pmic 6:da0c9587ecae 492 }
pmic 6:da0c9587ecae 493
pmic 6:da0c9587ecae 494 void GPA::printGPAmeasPara()
pmic 6:da0c9587ecae 495 {
pmic 6:da0c9587ecae 496 printLine();
pmic 30:48b233b596b8 497 printf(" fexcDes[Hz] fexc[Hz] Aexc Nmeas Nper Nsweep\r\n");
pmic 6:da0c9587ecae 498 printLine();
pmic 6:da0c9587ecae 499 for(int i = 0; i < NfexcDes; i++) {
pmic 6:da0c9587ecae 500 calcGPAmeasPara(fexcDes[i]);
pmic 6:da0c9587ecae 501 if(fexc == fexcPast || fexc >= fnyq) {
pmic 18:a715b1674b67 502 fexc = 0.0;
pmic 30:48b233b596b8 503 Aexc = 0.0;
pmic 6:da0c9587ecae 504 Nmeas = 0;
pmic 6:da0c9587ecae 505 Nper = 0;
pmic 30:48b233b596b8 506 Nsweep = 0;
pmic 30:48b233b596b8 507 afexc = 0.0;
pmic 30:48b233b596b8 508 bfexc = 0.0;
pmic 30:48b233b596b8 509 aAexc = 0.0;
pmic 30:48b233b596b8 510 bAexc = 0.0;
pmic 30:48b233b596b8 511
pmic 6:da0c9587ecae 512 } else {
pmic 6:da0c9587ecae 513 Aexc = aAexcDes/fexc + bAexcDes;
pmic 30:48b233b596b8 514 if(Nsweep > 0) calcGPAsweepPara();
pmic 33:4de33aa9bc86 515 else{
pmic 33:4de33aa9bc86 516 afexc = 0.0;
pmic 33:4de33aa9bc86 517 bfexc = 0.0;
pmic 33:4de33aa9bc86 518 aAexc = 0.0;
pmic 33:4de33aa9bc86 519 bAexc = 0.0;
pmic 33:4de33aa9bc86 520 }
pmic 6:da0c9587ecae 521 fexcPast = fexc;
pmic 30:48b233b596b8 522 AexcPast = Aexc;
pmic 6:da0c9587ecae 523 }
pmic 6:da0c9587ecae 524 NmeasTotal += Nmeas;
pmic 30:48b233b596b8 525 NmeasTotal += Nsweep;
pmic 30:48b233b596b8 526 printf("%11.4e %12.4e %10.3e %7i %6i %7i\r\n", (float)fexcDes[i], (float)fexc, (float)Aexc, Nmeas, Nper, Nsweep);
pmic 30:48b233b596b8 527 Nsweep = NsweepMin;
pmic 30:48b233b596b8 528 }
pmic 30:48b233b596b8 529 printGPAmeasTime();
pmic 30:48b233b596b8 530 reset();
pmic 30:48b233b596b8 531 }
pmic 30:48b233b596b8 532
pmic 30:48b233b596b8 533 void GPA::printFullGPAmeasPara()
pmic 30:48b233b596b8 534 {
pmic 30:48b233b596b8 535 printLongLine();
pmic 30:48b233b596b8 536 printf(" fexcDes[Hz] fexc[Hz] Aexc Nmeas Nper Nsweep afexc bfexc aAexc bAexc\r\n");
pmic 30:48b233b596b8 537 printLongLine();
pmic 30:48b233b596b8 538 for(int i = 0; i < NfexcDes; i++) {
pmic 30:48b233b596b8 539 calcGPAmeasPara(fexcDes[i]);
pmic 30:48b233b596b8 540 if(fexc == fexcPast || fexc >= fnyq) {
pmic 30:48b233b596b8 541 fexc = 0.0;
pmic 30:48b233b596b8 542 Aexc = 0.0;
pmic 30:48b233b596b8 543 Nmeas = 0;
pmic 30:48b233b596b8 544 Nper = 0;
pmic 30:48b233b596b8 545 Nsweep = 0;
pmic 30:48b233b596b8 546 afexc = 0.0;
pmic 30:48b233b596b8 547 bfexc = 0.0;
pmic 30:48b233b596b8 548 aAexc = 0.0;
pmic 30:48b233b596b8 549 bAexc = 0.0;
pmic 30:48b233b596b8 550
pmic 30:48b233b596b8 551 } else {
pmic 30:48b233b596b8 552 Aexc = aAexcDes/fexc + bAexcDes;
pmic 30:48b233b596b8 553 if(Nsweep > 0) calcGPAsweepPara();
pmic 33:4de33aa9bc86 554 else{
pmic 33:4de33aa9bc86 555 afexc = 0.0;
pmic 33:4de33aa9bc86 556 bfexc = 0.0;
pmic 33:4de33aa9bc86 557 aAexc = 0.0;
pmic 33:4de33aa9bc86 558 bAexc = 0.0;
pmic 33:4de33aa9bc86 559 }
pmic 30:48b233b596b8 560 fexcPast = fexc;
pmic 30:48b233b596b8 561 AexcPast = Aexc;
pmic 30:48b233b596b8 562 }
pmic 30:48b233b596b8 563 NmeasTotal += Nmeas;
pmic 30:48b233b596b8 564 NmeasTotal += Nsweep;
pmic 30:48b233b596b8 565 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);
pmic 30:48b233b596b8 566 Nsweep = NsweepMin;
pmic 6:da0c9587ecae 567 }
pmic 6:da0c9587ecae 568 printGPAmeasTime();
pmic 6:da0c9587ecae 569 reset();
pmic 6:da0c9587ecae 570 }
pmic 6:da0c9587ecae 571
pmic 6:da0c9587ecae 572 void GPA::printGPAmeasTime()
pmic 6:da0c9587ecae 573 {
pmic 6:da0c9587ecae 574 printLine();
pmic 30:48b233b596b8 575 printf(" Number of data points : %11i\r\n", NmeasTotal);
pmic 30:48b233b596b8 576 printf(" Measurment time in sec: %12.2f\r\n", (float)((double)NmeasTotal*Ts));
pmic 20:1d5e89b2f22e 577 }
pmic 20:1d5e89b2f22e 578
pmic 20:1d5e89b2f22e 579 void GPA::printNfexcDes()
pmic 20:1d5e89b2f22e 580 {
pmic 20:1d5e89b2f22e 581 printLine();
pmic 30:48b233b596b8 582 printf(" Number of frequancy points: %3i\r\n", NfexcDes);
pmic 20:1d5e89b2f22e 583 }
pmic 37:352bacb64f3f 584
pmic 37:352bacb64f3f 585 void GPA::printLine()
pmic 37:352bacb64f3f 586 {
pmic 37:352bacb64f3f 587 printf("--------------------------------------------------------------------------------\r\n");
pmic 37:352bacb64f3f 588 }
pmic 37:352bacb64f3f 589
pmic 37:352bacb64f3f 590 GPA::gpadata_t GPA::getGPAdata()
pmic 37:352bacb64f3f 591 {
pmic 37:352bacb64f3f 592 return gpaData;
pmic 37:352bacb64f3f 593 }