Ruprecht Altenburger / Mbed OS mirror_actuator_V1

Mirror actuator for RT2 lab

Dependencies:   FastPWM

Library_Cntrl/GPA.cpp@16:28b6bb8a4b7f, 2021-05-02 (annotated)

Committer:
altb2
Date:
Sun May 02 08:55:44 2021 +0000
Revision:
16:28b6bb8a4b7f
Parent:
15:9f32f64eee5b 
Final commit 4 students

Who changed what in which revision?

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