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