RT2_Cuboid_demo / Cntrl_Lib

Library for control puposes

Dependents:  

Fork of Cntrl_Lib by Ruprecht Altenburger

GPA.cpp@7:7715f92c5182, 2019-01-07 (annotated)

Committer:
pmic
Date:
Mon Jan 07 13:31:38 2019 +0000
Revision:
7:7715f92c5182
Parent:
6:0c473884d24a
Child:
12:c8ec698c53ed
update gpa

Who changed what in which revision?

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