altb_pmic / Lib_Cntrl

Control Library by altb

Dependents:   My_Libraries IndNav_QK3_T265

GPA.cpp@15:c70cad2f4e64, 2020-01-16 (annotated)

Committer:
pmic
Date:
Thu Jan 16 09:12:50 2020 +0000
Revision:
15:c70cad2f4e64
Parent:
6:694fe8894215 
Revisit IIR_filter.h and IIR_filter.cpp. Change internal double to float arithmetic.

Who changed what in which revision?

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