Test of pmic GPA with filter
Dependencies: mbed
Fork of nucf446-cuboid-balance1_strong by
GPA.cpp@26:492c7ab05e67, 2018-04-10 (annotated)
- Committer:
- pmic
- Date:
- Tue Apr 10 12:24:57 2018 +0000
- Revision:
- 26:492c7ab05e67
- Parent:
- 25:e46687840dd6
correct new functinality
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
pmic | 8:d68e177e2571 | 1 | /* |
pmic | 8:d68e177e2571 | 2 | GPA: frequency point wise gain and phase analyser to measure the frequency |
pmic | 23:26a1ccd0a856 | 3 | respone of a dynamical system |
pmic | 16:e6fc0af484c2 | 4 | |
pmic | 8:d68e177e2571 | 5 | hint: the measurements should only be perfomed in closed loop |
pmic | 8:d68e177e2571 | 6 | assumption: the system is at the desired steady state of interest when |
pmic | 8:d68e177e2571 | 7 | the measurment starts |
pmic | 16:e6fc0af484c2 | 8 | |
pmic | 8:d68e177e2571 | 9 | exc(2) C: controller |
pmic | 8:d68e177e2571 | 10 | | P: plant |
pmic | 16:e6fc0af484c2 | 11 | v |
pmic | 8:d68e177e2571 | 12 | exc(1) --> o ->| C |--->o------->| P |----------> out |
pmic | 24:33ded7d7bcbd | 13 | ^ - | | |
pmic | 8:d68e177e2571 | 14 | | --> inp | exc: excitation signal (E) |
pmic | 8:d68e177e2571 | 15 | | | inp: input plant (U) |
pmic | 8:d68e177e2571 | 16 | -------------------------------- out: output plant (Y) |
pmic | 16:e6fc0af484c2 | 17 | |
pmic | 25:e46687840dd6 | 18 | instantiate option 1: (logarithmic equaly spaced frequency points) |
pmic | 24:33ded7d7bcbd | 19 | |
pmic | 8:d68e177e2571 | 20 | GPA(float fMin, float fMax, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1) |
pmic | 24:33ded7d7bcbd | 21 | |
pmic | 8:d68e177e2571 | 22 | fMin: minimal desired frequency that should be measured in Hz |
pmic | 8:d68e177e2571 | 23 | fMax: maximal desired frequency that should be measured in Hz |
pmic | 8:d68e177e2571 | 24 | NfexcDes: number of logarithmic equaly spaced frequency points |
pmic | 8:d68e177e2571 | 25 | NperMin: minimal number of periods that are used for each frequency point |
pmic | 23:26a1ccd0a856 | 26 | NmeasMin: minimal number of samples that are used for each frequency point |
pmic | 8:d68e177e2571 | 27 | Ts: sampling time |
pmic | 16:e6fc0af484c2 | 28 | Aexc0: excitation amplitude at fMin |
pmic | 8:d68e177e2571 | 29 | Aexc1: excitation amplitude at fMax |
pmic | 24:33ded7d7bcbd | 30 | |
pmic | 24:33ded7d7bcbd | 31 | instantiate option 2: (for a second, refined frequency grid measurement) |
pmic | 24:33ded7d7bcbd | 32 | |
pmic | 26:492c7ab05e67 | 33 | GPA(float f0, float f1, float *fexcDes, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1) |
pmic | 24:33ded7d7bcbd | 34 | |
pmic | 24:33ded7d7bcbd | 35 | f0: frequency point for the calculation of Aexc0 in Hz (should be chosen like in the first measurement) |
pmic | 24:33ded7d7bcbd | 36 | f1: frequency point for the calculation of Aexc1 in Hz (should be chosen like in the first measurement) |
pmic | 24:33ded7d7bcbd | 37 | *fexcDes: sorted frequency point array in Hz |
pmic | 26:492c7ab05e67 | 38 | NfexcDes: length of fexcDes |
pmic | 25:e46687840dd6 | 39 | |
pmic | 25:e46687840dd6 | 40 | instantiate option 3: (for an arbitary but sorted frequency grid measurement) |
pmic | 25:e46687840dd6 | 41 | |
pmic | 26:492c7ab05e67 | 42 | GPA(float *fexcDes, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1) |
pmic | 25:e46687840dd6 | 43 | |
pmic | 25:e46687840dd6 | 44 | *fexcDes: sorted frequency point array in Hz |
pmic | 25:e46687840dd6 | 45 | Aexc0: excitation amplitude at fexcDes[0] |
pmic | 25:e46687840dd6 | 46 | Aexc1: excitation amplitude at fexcDes[NfexcDes-1] |
pmic | 26:492c7ab05e67 | 47 | NfexcDes: length of fexcDes |
pmic | 16:e6fc0af484c2 | 48 | |
pmic | 24:33ded7d7bcbd | 49 | hints: the amplitude drops with 1/fexc, if you're using Axc1 = Aexc0/fMax then d/dt exc = const., |
pmic | 24:33ded7d7bcbd | 50 | this is recommended if your controller does not have a rolloff. if a desired frequency point |
pmic | 24:33ded7d7bcbd | 51 | is not measured try to increase Nmeas. |
pmic | 16:e6fc0af484c2 | 52 | |
pmic | 23:26a1ccd0a856 | 53 | pseudo code for a closed loop measurement with a controller C: |
pmic | 16:e6fc0af484c2 | 54 | |
pmic | 8:d68e177e2571 | 55 | excitation input at (1): |
pmic | 23:26a1ccd0a856 | 56 | |
pmic | 23:26a1ccd0a856 | 57 | - measuring the plant P and the closed loop tf T = PC/(1 + PC): |
pmic | 23:26a1ccd0a856 | 58 | desTorque = pi_w(omega_desired - omega + excWobble); |
pmic | 23:26a1ccd0a856 | 59 | inpWobble = desTorque; |
pmic | 23:26a1ccd0a856 | 60 | outWobble = omega; |
pmic | 23:26a1ccd0a856 | 61 | excWobble = Wobble(excWobble, outWobble); |
pmic | 23:26a1ccd0a856 | 62 | |
pmic | 23:26a1ccd0a856 | 63 | - measuring the controller C and the closed loop tf SC = C/(1 + PC) |
pmic | 23:26a1ccd0a856 | 64 | desTorque = pi_w(omega_desired - omega + excWobble); |
pmic | 23:26a1ccd0a856 | 65 | inpWobble = n_soll + excWobble - omega; |
pmic | 23:26a1ccd0a856 | 66 | outWobble = desTorque; |
pmic | 23:26a1ccd0a856 | 67 | excWobble = Wobble(inpWobble, outWobble); |
pmic | 16:e6fc0af484c2 | 68 | |
pmic | 8:d68e177e2571 | 69 | excitation input at (2): |
pmic | 23:26a1ccd0a856 | 70 | |
pmic | 23:26a1ccd0a856 | 71 | - measuring the plant P and the closed loop tf SP = P/(1 + PC): |
pmic | 23:26a1ccd0a856 | 72 | desTorque = pi_w(omega_desired - omega) + excWobble; |
pmic | 23:26a1ccd0a856 | 73 | inpWobble = desTorque; |
pmic | 23:26a1ccd0a856 | 74 | outWobble = omega; |
pmic | 23:26a1ccd0a856 | 75 | excWobble = Wobble(excWobble, outWobble); |
pmic | 16:e6fc0af484c2 | 76 | |
pmic | 8:d68e177e2571 | 77 | usage: |
pmic | 23:26a1ccd0a856 | 78 | exc(k+1) = myGPA(inp(k), out(k)) does update the internal states of the |
pmic | 23:26a1ccd0a856 | 79 | gpa at the timestep k and returns the excitation signal for the timestep |
pmic | 23:26a1ccd0a856 | 80 | k+1. the results are plotted to a terminal (putty) over a serial |
pmic | 23:26a1ccd0a856 | 81 | connection and look as follows: |
pmic | 8:d68e177e2571 | 82 | ----------------------------------------------------------------------------------------- |
pmic | 8:d68e177e2571 | 83 | fexc[Hz] |Gyu| ang(Gyu) |Gye| ang(Gye) |E| |U| |Y| |
pmic | 8:d68e177e2571 | 84 | ----------------------------------------------------------------------------------------- |
pmic | 23:26a1ccd0a856 | 85 | 1.000e+00 2.924e+01 -1.572e+00 1.017e+00 -4.983e-02 5.000e+00 1.739e-01 5.084e+00 |
pmic | 16:e6fc0af484c2 | 86 | |
pmic | 8:d68e177e2571 | 87 | in matlab you can use: |
pmic | 8:d68e177e2571 | 88 | dataNucleo = [... insert measurement data here ...]; |
pmic | 8:d68e177e2571 | 89 | g = frd(dataNucleo(:,2).*exp(1i*dataNucleo(:,3)), dataNucleo(:,1), Ts, 'Units', 'Hz'); |
pmic | 8:d68e177e2571 | 90 | gcl = frd(dataNucleo(:,4).*exp(1i*dataNucleo(:,5)), dataNucleo(:,1), Ts, 'Units', 'Hz'); |
pmic | 16:e6fc0af484c2 | 91 | |
pmic | 8:d68e177e2571 | 92 | if you're evaluating more than one measurement which contain equal frequency points try: |
pmic | 8:d68e177e2571 | 93 | dataNucleo = [dataNucleo1; dataNucleo2]; |
pmic | 8:d68e177e2571 | 94 | [~, ind] = unique(dataNucleo(:,1),'stable'); |
pmic | 8:d68e177e2571 | 95 | dataNucleo = dataNucleo(ind,:); |
pmic | 16:e6fc0af484c2 | 96 | |
pmic | 8:d68e177e2571 | 97 | autor: M.E. Peter |
pmic | 16:e6fc0af484c2 | 98 | */ |
pmic | 8:d68e177e2571 | 99 | |
pmic | 6:da0c9587ecae | 100 | #include "GPA.h" |
pmic | 6:da0c9587ecae | 101 | #include "mbed.h" |
pmic | 6:da0c9587ecae | 102 | #include "math.h" |
pmic | 22:715d351d0be7 | 103 | #define pi 3.141592653589793 |
pmic | 6:da0c9587ecae | 104 | |
pmic | 6:da0c9587ecae | 105 | using namespace std; |
pmic | 6:da0c9587ecae | 106 | |
pmic | 6:da0c9587ecae | 107 | GPA::GPA(float fMin, float fMax, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1) |
pmic | 6:da0c9587ecae | 108 | { |
pmic | 6:da0c9587ecae | 109 | this->NfexcDes = NfexcDes; |
pmic | 6:da0c9587ecae | 110 | this->NperMin = NperMin; |
pmic | 6:da0c9587ecae | 111 | this->NmeasMin = NmeasMin; |
pmic | 22:715d351d0be7 | 112 | this->Ts = (double)Ts; |
pmic | 6:da0c9587ecae | 113 | |
pmic | 6:da0c9587ecae | 114 | // calculate logarithmic spaced frequency points |
pmic | 22:715d351d0be7 | 115 | fexcDes = (double*)malloc(NfexcDes*sizeof(double)); |
pmic | 22:715d351d0be7 | 116 | fexcDesLogspace((double)fMin, (double)fMax, NfexcDes); |
pmic | 6:da0c9587ecae | 117 | |
pmic | 6:da0c9587ecae | 118 | // calculate coefficients for decreasing amplitude (1/fexc) |
pmic | 22:715d351d0be7 | 119 | this->aAexcDes = ((double)Aexc1 - (double)Aexc0)/(1.0/fexcDes[NfexcDes-1] - 1.0/fexcDes[0]); |
pmic | 22:715d351d0be7 | 120 | this->bAexcDes = (double)Aexc0 - aAexcDes/fexcDes[0]; |
pmic | 6:da0c9587ecae | 121 | |
pmic | 22:715d351d0be7 | 122 | fnyq = 1.0/2.0/(double)Ts; |
pmic | 22:715d351d0be7 | 123 | pi2 = 2.0*pi; |
pmic | 22:715d351d0be7 | 124 | pi2Ts = pi2*(double)Ts; |
pmic | 22:715d351d0be7 | 125 | piDiv2 = pi/2.0; |
pmic | 6:da0c9587ecae | 126 | |
pmic | 22:715d351d0be7 | 127 | sU = (double*)malloc(3*sizeof(double)); |
pmic | 22:715d351d0be7 | 128 | sY = (double*)malloc(3*sizeof(double)); |
pmic | 6:da0c9587ecae | 129 | reset(); |
pmic | 6:da0c9587ecae | 130 | } |
pmic | 6:da0c9587ecae | 131 | |
pmic | 26:492c7ab05e67 | 132 | GPA::GPA(float f0, float f1, float *fexcDes, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1) |
pmic | 24:33ded7d7bcbd | 133 | { |
pmic | 24:33ded7d7bcbd | 134 | // convert fexcDes from float to double, it is assumed that it is sorted |
pmic | 26:492c7ab05e67 | 135 | this->NfexcDes = NfexcDes; |
pmic | 24:33ded7d7bcbd | 136 | this->fexcDes = (double*)malloc(NfexcDes*sizeof(double)); |
pmic | 24:33ded7d7bcbd | 137 | for(int i = 0; i < NfexcDes; i++) { |
pmic | 24:33ded7d7bcbd | 138 | this->fexcDes[i] = (double)fexcDes[i]; |
pmic | 24:33ded7d7bcbd | 139 | } |
pmic | 24:33ded7d7bcbd | 140 | this->NperMin = NperMin; |
pmic | 24:33ded7d7bcbd | 141 | this->NmeasMin = NmeasMin; |
pmic | 24:33ded7d7bcbd | 142 | this->Ts = (double)Ts; |
pmic | 24:33ded7d7bcbd | 143 | |
pmic | 24:33ded7d7bcbd | 144 | // calculate coefficients for decreasing amplitude (1/fexc) |
pmic | 24:33ded7d7bcbd | 145 | this->aAexcDes = ((double)Aexc1 - (double)Aexc0)/(1.0/(double)f1 - 1.0/(double)f0); |
pmic | 24:33ded7d7bcbd | 146 | this->bAexcDes = (double)Aexc0 - aAexcDes/fexcDes[0]; |
pmic | 24:33ded7d7bcbd | 147 | |
pmic | 24:33ded7d7bcbd | 148 | fnyq = 1.0/2.0/(double)Ts; |
pmic | 24:33ded7d7bcbd | 149 | pi2 = 2.0*pi; |
pmic | 24:33ded7d7bcbd | 150 | pi2Ts = pi2*(double)Ts; |
pmic | 24:33ded7d7bcbd | 151 | piDiv2 = pi/2.0; |
pmic | 24:33ded7d7bcbd | 152 | |
pmic | 24:33ded7d7bcbd | 153 | sU = (double*)malloc(3*sizeof(double)); |
pmic | 24:33ded7d7bcbd | 154 | sY = (double*)malloc(3*sizeof(double)); |
pmic | 24:33ded7d7bcbd | 155 | reset(); |
pmic | 24:33ded7d7bcbd | 156 | } |
pmic | 24:33ded7d7bcbd | 157 | |
pmic | 26:492c7ab05e67 | 158 | GPA::GPA(float *fexcDes, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1) |
pmic | 25:e46687840dd6 | 159 | { |
pmic | 25:e46687840dd6 | 160 | // convert fexcDes from float to double, it is assumed that it is sorted |
pmic | 26:492c7ab05e67 | 161 | this->NfexcDes = NfexcDes; |
pmic | 25:e46687840dd6 | 162 | this->fexcDes = (double*)malloc(NfexcDes*sizeof(double)); |
pmic | 25:e46687840dd6 | 163 | for(int i = 0; i < NfexcDes; i++) { |
pmic | 25:e46687840dd6 | 164 | this->fexcDes[i] = (double)fexcDes[i]; |
pmic | 25:e46687840dd6 | 165 | } |
pmic | 25:e46687840dd6 | 166 | this->NperMin = NperMin; |
pmic | 25:e46687840dd6 | 167 | this->NmeasMin = NmeasMin; |
pmic | 25:e46687840dd6 | 168 | this->Ts = (double)Ts; |
pmic | 25:e46687840dd6 | 169 | |
pmic | 25:e46687840dd6 | 170 | // calculate coefficients for decreasing amplitude (1/fexc) |
pmic | 25:e46687840dd6 | 171 | this->aAexcDes = ((double)Aexc1 - (double)Aexc0)/(1.0/this->fexcDes[NfexcDes-1] - 1.0/this->fexcDes[0]); |
pmic | 25:e46687840dd6 | 172 | this->bAexcDes = (double)Aexc0 - aAexcDes/fexcDes[0]; |
pmic | 25:e46687840dd6 | 173 | |
pmic | 25:e46687840dd6 | 174 | fnyq = 1.0/2.0/(double)Ts; |
pmic | 25:e46687840dd6 | 175 | pi2 = 2.0*pi; |
pmic | 25:e46687840dd6 | 176 | pi2Ts = pi2*(double)Ts; |
pmic | 25:e46687840dd6 | 177 | piDiv2 = pi/2.0; |
pmic | 25:e46687840dd6 | 178 | |
pmic | 25:e46687840dd6 | 179 | sU = (double*)malloc(3*sizeof(double)); |
pmic | 25:e46687840dd6 | 180 | sY = (double*)malloc(3*sizeof(double)); |
pmic | 25:e46687840dd6 | 181 | reset(); |
pmic | 25:e46687840dd6 | 182 | } |
pmic | 25:e46687840dd6 | 183 | |
pmic | 6:da0c9587ecae | 184 | GPA::~GPA() {} |
pmic | 6:da0c9587ecae | 185 | |
pmic | 6:da0c9587ecae | 186 | void GPA::reset() |
pmic | 6:da0c9587ecae | 187 | { |
pmic | 6:da0c9587ecae | 188 | Nmeas = 0; |
pmic | 6:da0c9587ecae | 189 | Nper = 0; |
pmic | 22:715d351d0be7 | 190 | fexc = 0.0; |
pmic | 22:715d351d0be7 | 191 | fexcPast = 0.0; |
pmic | 6:da0c9587ecae | 192 | ii = 1; // iterating through desired frequency points |
pmic | 6:da0c9587ecae | 193 | jj = 1; // iterating through measurement points w.r.t. reachable frequency |
pmic | 22:715d351d0be7 | 194 | scaleG = 0.0; |
pmic | 22:715d351d0be7 | 195 | cr = 0.0; |
pmic | 22:715d351d0be7 | 196 | ci = 0.0; |
pmic | 6:da0c9587ecae | 197 | for(int i = 0; i < 3; i++) { |
pmic | 22:715d351d0be7 | 198 | sU[i] = 0.0; |
pmic | 22:715d351d0be7 | 199 | sY[i] = 0.0; |
pmic | 6:da0c9587ecae | 200 | } |
pmic | 22:715d351d0be7 | 201 | sinarg = 0.0; |
pmic | 6:da0c9587ecae | 202 | NmeasTotal = 0; |
pmic | 22:715d351d0be7 | 203 | Aexc = 0.0; |
pmic | 22:715d351d0be7 | 204 | pi2Tsfexc = 0.0; |
pmic | 6:da0c9587ecae | 205 | } |
pmic | 6:da0c9587ecae | 206 | |
pmic | 22:715d351d0be7 | 207 | float GPA::update(double inp, double out) |
pmic | 6:da0c9587ecae | 208 | { |
pmic | 6:da0c9587ecae | 209 | // a new frequency point has been reached |
pmic | 6:da0c9587ecae | 210 | if(jj == 1) { |
pmic | 6:da0c9587ecae | 211 | // get a new unique frequency point |
pmic | 6:da0c9587ecae | 212 | while(fexc == fexcPast) { |
pmic | 6:da0c9587ecae | 213 | // measurement finished |
pmic | 6:da0c9587ecae | 214 | if(ii > NfexcDes) { |
pmic | 6:da0c9587ecae | 215 | return 0.0f; |
pmic | 6:da0c9587ecae | 216 | } |
pmic | 6:da0c9587ecae | 217 | calcGPAmeasPara(fexcDes[ii - 1]); |
pmic | 6:da0c9587ecae | 218 | // secure fexc is not higher or equal to nyquist frequency |
pmic | 6:da0c9587ecae | 219 | if(fexc >= fnyq) { |
pmic | 6:da0c9587ecae | 220 | fexc = fexcPast; |
pmic | 6:da0c9587ecae | 221 | } |
pmic | 6:da0c9587ecae | 222 | // no frequency found |
pmic | 6:da0c9587ecae | 223 | if(fexc == fexcPast) { |
pmic | 6:da0c9587ecae | 224 | ii += 1; |
pmic | 6:da0c9587ecae | 225 | } else { |
pmic | 6:da0c9587ecae | 226 | Aexc = aAexcDes/fexc + bAexcDes; |
pmic | 6:da0c9587ecae | 227 | pi2Tsfexc = pi2Ts*fexc; |
pmic | 6:da0c9587ecae | 228 | } |
pmic | 6:da0c9587ecae | 229 | } |
pmic | 16:e6fc0af484c2 | 230 | // secure sinarg starts at 0 (numerically maybe not given) |
pmic | 22:715d351d0be7 | 231 | sinarg = 0.0; |
pmic | 6:da0c9587ecae | 232 | // filter scaling |
pmic | 22:715d351d0be7 | 233 | scaleG = 1.0/sqrt((double)Nmeas); |
pmic | 6:da0c9587ecae | 234 | // filter coefficients |
pmic | 6:da0c9587ecae | 235 | cr = cos(pi2Tsfexc); |
pmic | 6:da0c9587ecae | 236 | ci = sin(pi2Tsfexc); |
pmic | 6:da0c9587ecae | 237 | // filter storage |
pmic | 6:da0c9587ecae | 238 | for(int i = 0; i < 3; i++) { |
pmic | 22:715d351d0be7 | 239 | sU[i] = 0.0; |
pmic | 22:715d351d0be7 | 240 | sY[i] = 0.0; |
pmic | 6:da0c9587ecae | 241 | } |
pmic | 6:da0c9587ecae | 242 | } |
pmic | 6:da0c9587ecae | 243 | // filter step for signal su |
pmic | 22:715d351d0be7 | 244 | sU[0] = scaleG*inp + 2.0*cr*sU[1] - sU[2]; |
pmic | 6:da0c9587ecae | 245 | sU[2] = sU[1]; |
pmic | 6:da0c9587ecae | 246 | sU[1] = sU[0]; |
pmic | 6:da0c9587ecae | 247 | // filter step for signal sy |
pmic | 22:715d351d0be7 | 248 | sY[0] = scaleG*out + 2.0*cr*sY[1] - sY[2]; |
pmic | 6:da0c9587ecae | 249 | sY[2] = sY[1]; |
pmic | 6:da0c9587ecae | 250 | sY[1] = sY[0]; |
pmic | 6:da0c9587ecae | 251 | // measurement of frequencypoint is finished |
pmic | 6:da0c9587ecae | 252 | if(jj == Nmeas) { |
pmic | 6:da0c9587ecae | 253 | jj = 1; |
pmic | 7:87b823282bf0 | 254 | ii += 1; |
pmic | 16:e6fc0af484c2 | 255 | fexcPast = fexc; |
pmic | 6:da0c9587ecae | 256 | // calculate the one point dft |
pmic | 22:715d351d0be7 | 257 | double Ureal = 2.0*scaleG*(cr*sU[1] - sU[2]); |
pmic | 22:715d351d0be7 | 258 | double Uimag = 2.0*scaleG*ci*sU[1]; |
pmic | 22:715d351d0be7 | 259 | double Yreal = 2.0*scaleG*(cr*sY[1] - sY[2]); |
pmic | 22:715d351d0be7 | 260 | double Yimag = 2.0*scaleG*ci*sY[1]; |
pmic | 6:da0c9587ecae | 261 | // calculate magnitude and angle |
pmic | 22:715d351d0be7 | 262 | float Umag = (float)(sqrt(Ureal*Ureal + Uimag*Uimag)); |
pmic | 22:715d351d0be7 | 263 | float Ymag = (float)(sqrt(Yreal*Yreal + Yimag*Yimag)); |
pmic | 22:715d351d0be7 | 264 | float absGyu = (float)(Ymag/Umag); |
pmic | 22:715d351d0be7 | 265 | float angGyu = (float)(atan2(Yimag, Yreal) - atan2(Uimag, Ureal)); |
pmic | 22:715d351d0be7 | 266 | float absGye = (float)(Ymag/Aexc); |
pmic | 22:715d351d0be7 | 267 | float angGye = (float)(atan2(Yimag, Yreal) + piDiv2); |
pmic | 6:da0c9587ecae | 268 | // user info |
pmic | 16:e6fc0af484c2 | 269 | if(ii == 2) { |
pmic | 6:da0c9587ecae | 270 | printLine(); |
pmic | 6:da0c9587ecae | 271 | printf(" fexc[Hz] |Gyu| ang(Gyu) |Gye| ang(Gye) |E| |U| |Y|\r\n"); |
pmic | 6:da0c9587ecae | 272 | printLine(); |
pmic | 6:da0c9587ecae | 273 | } |
pmic | 23:26a1ccd0a856 | 274 | printf("%11.3e %10.3e %10.3e %10.3e %10.3e %10.3e %10.3e %10.3e\r\n", (float)fexc, absGyu, angGyu, absGye, angGye, (float)Aexc, Umag, Ymag); |
pmic | 6:da0c9587ecae | 275 | } else { |
pmic | 6:da0c9587ecae | 276 | jj += 1; |
pmic | 6:da0c9587ecae | 277 | } |
pmic | 6:da0c9587ecae | 278 | sinarg = fmod(sinarg + pi2Tsfexc, pi2); |
pmic | 6:da0c9587ecae | 279 | NmeasTotal += 1; |
pmic | 22:715d351d0be7 | 280 | return (float)(Aexc*sin(sinarg)); |
pmic | 6:da0c9587ecae | 281 | } |
pmic | 6:da0c9587ecae | 282 | |
pmic | 22:715d351d0be7 | 283 | void GPA::fexcDesLogspace(double fMin, double fMax, int NfexcDes) |
pmic | 6:da0c9587ecae | 284 | { |
pmic | 6:da0c9587ecae | 285 | // calculate logarithmic spaced frequency points |
pmic | 22:715d351d0be7 | 286 | double Gain = log10(fMax/fMin)/((double)NfexcDes - 1.0); |
pmic | 22:715d351d0be7 | 287 | double expon = 0.0; |
pmic | 6:da0c9587ecae | 288 | for(int i = 0; i < NfexcDes; i++) { |
pmic | 22:715d351d0be7 | 289 | fexcDes[i] = fMin*pow(10.0, expon); |
pmic | 6:da0c9587ecae | 290 | expon += Gain; |
pmic | 6:da0c9587ecae | 291 | } |
pmic | 6:da0c9587ecae | 292 | } |
pmic | 6:da0c9587ecae | 293 | |
pmic | 22:715d351d0be7 | 294 | void GPA::calcGPAmeasPara(double fexcDes_i) |
pmic | 6:da0c9587ecae | 295 | { |
pmic | 6:da0c9587ecae | 296 | // Nmeas has to be an integer |
pmic | 6:da0c9587ecae | 297 | Nper = NperMin; |
pmic | 22:715d351d0be7 | 298 | Nmeas = (int)floor((double)Nper/fexcDes_i/Ts + 0.5); |
pmic | 6:da0c9587ecae | 299 | // secure that the minimal number of measurements is fullfilled |
pmic | 6:da0c9587ecae | 300 | int Ndelta = NmeasMin - Nmeas; |
pmic | 6:da0c9587ecae | 301 | if(Ndelta > 0) { |
pmic | 22:715d351d0be7 | 302 | Nper = (int)ceil((double)NmeasMin*fexcDes_i*Ts); |
pmic | 22:715d351d0be7 | 303 | Nmeas = (int)floor((double)Nper/fexcDes_i/Ts + 0.5); |
pmic | 6:da0c9587ecae | 304 | } |
pmic | 6:da0c9587ecae | 305 | // evaluating reachable frequency |
pmic | 22:715d351d0be7 | 306 | fexc = (double)Nper/(double)Nmeas/Ts; |
pmic | 6:da0c9587ecae | 307 | } |
pmic | 6:da0c9587ecae | 308 | |
pmic | 6:da0c9587ecae | 309 | void GPA::printLine() |
pmic | 6:da0c9587ecae | 310 | { |
pmic | 6:da0c9587ecae | 311 | printf("-----------------------------------------------------------------------------------------\r\n"); |
pmic | 6:da0c9587ecae | 312 | } |
pmic | 6:da0c9587ecae | 313 | |
pmic | 6:da0c9587ecae | 314 | void GPA::printGPAfexcDes() |
pmic | 6:da0c9587ecae | 315 | { |
pmic | 6:da0c9587ecae | 316 | printLine(); |
pmic | 6:da0c9587ecae | 317 | for(int i = 0; i < NfexcDes; i++) { |
pmic | 22:715d351d0be7 | 318 | printf("%9.4f\r\n", (float)fexcDes[i]); |
pmic | 6:da0c9587ecae | 319 | } |
pmic | 6:da0c9587ecae | 320 | } |
pmic | 6:da0c9587ecae | 321 | |
pmic | 6:da0c9587ecae | 322 | void GPA::printGPAmeasPara() |
pmic | 6:da0c9587ecae | 323 | { |
pmic | 6:da0c9587ecae | 324 | printLine(); |
pmic | 6:da0c9587ecae | 325 | printf(" fexcDes[Hz] fexc[Hz] Aexc Nmeas Nper\r\n"); |
pmic | 6:da0c9587ecae | 326 | printLine(); |
pmic | 6:da0c9587ecae | 327 | for(int i = 0; i < NfexcDes; i++) { |
pmic | 6:da0c9587ecae | 328 | calcGPAmeasPara(fexcDes[i]); |
pmic | 6:da0c9587ecae | 329 | if(fexc == fexcPast || fexc >= fnyq) { |
pmic | 22:715d351d0be7 | 330 | fexc = 0.0; |
pmic | 6:da0c9587ecae | 331 | Nmeas = 0; |
pmic | 6:da0c9587ecae | 332 | Nper = 0; |
pmic | 22:715d351d0be7 | 333 | Aexc = 0.0; |
pmic | 6:da0c9587ecae | 334 | } else { |
pmic | 6:da0c9587ecae | 335 | Aexc = aAexcDes/fexc + bAexcDes; |
pmic | 6:da0c9587ecae | 336 | fexcPast = fexc; |
pmic | 6:da0c9587ecae | 337 | } |
pmic | 6:da0c9587ecae | 338 | NmeasTotal += Nmeas; |
pmic | 22:715d351d0be7 | 339 | printf("%12.2e %9.2e %10.2e %7i %6i \r\n", (float)fexcDes[i], (float)fexc, (float)Aexc, Nmeas, Nper); |
pmic | 6:da0c9587ecae | 340 | } |
pmic | 6:da0c9587ecae | 341 | printGPAmeasTime(); |
pmic | 6:da0c9587ecae | 342 | reset(); |
pmic | 6:da0c9587ecae | 343 | } |
pmic | 6:da0c9587ecae | 344 | |
pmic | 6:da0c9587ecae | 345 | void GPA::printGPAmeasTime() |
pmic | 6:da0c9587ecae | 346 | { |
pmic | 6:da0c9587ecae | 347 | printLine(); |
pmic | 6:da0c9587ecae | 348 | printf(" number of data points: %9i\r\n", NmeasTotal); |
pmic | 22:715d351d0be7 | 349 | printf(" measurment time in sec: %9.2f\r\n", (float)((double)NmeasTotal*Ts)); |
pmic | 26:492c7ab05e67 | 350 | } |
pmic | 26:492c7ab05e67 | 351 | |
pmic | 26:492c7ab05e67 | 352 | void GPA::printNfexcDes() |
pmic | 26:492c7ab05e67 | 353 | { |
pmic | 26:492c7ab05e67 | 354 | printLine(); |
pmic | 26:492c7ab05e67 | 355 | printf(" number of frequancy points: %2i\r\n", NfexcDes); |
pmic | 26:492c7ab05e67 | 356 | } |