Ruprecht Altenburger / Library_Cntrl2

2nd Library

GPA.cpp@1:b54eb3e24d2d, 2021-02-25 (annotated)

Committer:
altb2
Date:
Thu Feb 25 20:28:27 2021 +0000
Revision:
1:b54eb3e24d2d
Parent:
0:a201a6cd4c0c
Child:
4:db615f5fa407
.

Who changed what in which revision?

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