RT2_Cuboid_demo / Mbed 2 deprecated nucf446-cuboid-balance1_strong

Cuboid

Dependencies:   mbed

GPA.cpp@28:fc53b2d62a1e, 2019-02-07 (annotated)

Committer:
pmic
Date:
Thu Feb 07 09:11:51 2019 +0000
Revision:
28:fc53b2d62a1e
Parent:
26:017198f20b5c 
Add new default instantiate option for GPA

Who changed what in which revision?

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