altb_pmic / Mbed 2 deprecated GRT_VC_PIDT1_musterloesung

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

GPA.cpp@4:37df0f6a1bc3, 2019-05-10 (annotated)

Committer:
pmic
Date:
Fri May 10 18:15:41 2019 +0000
Revision:
4:37df0f6a1bc3
Introduce GPA and implement two sets of measurements which are apropriate for a frequency reasponse measurements, rember, the GPA is written to be used in CL, especially for weakly damped systems. It's just an example for future usage

Who changed what in which revision?

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