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