Ruprecht Altenburger / Library_Cntrl2

2nd Library

GPA.cpp@2:35e1cf78ea6f, 2019-03-08 (annotated)

Committer:
altb2
Date:
Fri Mar 08 13:34:52 2019 +0000
Revision:
2:35e1cf78ea6f
Parent:
0:a201a6cd4c0c 
..

Who changed what in which revision?

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