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