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