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