calculate
Dependencies: mbed X_NUCLEO_IKS01A3 Mahony_Algorithm
CalculateData.cpp@2:4cccdc792719, 2020-04-15 (annotated)
- Committer:
- zollecy1
- Date:
- Wed Apr 15 12:13:15 2020 +0000
- Revision:
- 2:4cccdc792719
- Parent:
- 1:48e219526d0f
- Child:
- 3:795998b31c32
.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
zollecy1 | 0:313fbc3a198a | 1 | /** |
zollecy1 | 0:313fbc3a198a | 2 | |**********************************************************************; |
zollecy1 | 0:313fbc3a198a | 3 | * Project : Projektarbeit Systemtechnik PES4 |
zollecy1 | 0:313fbc3a198a | 4 | * |
zollecy1 | 0:313fbc3a198a | 5 | * Program name : Beispiel |
zollecy1 | 0:313fbc3a198a | 6 | * |
zollecy1 | 0:313fbc3a198a | 7 | * Author : PES4 Team1 |
zollecy1 | 0:313fbc3a198a | 8 | * |
zollecy1 | 0:313fbc3a198a | 9 | * Team : **Team 1** |
zollecy1 | 0:313fbc3a198a | 10 | * Fabio Bernard |
zollecy1 | 0:313fbc3a198a | 11 | * Lukas Egli |
zollecy1 | 0:313fbc3a198a | 12 | * Matthias Ott |
zollecy1 | 0:313fbc3a198a | 13 | * Pascal Novacki |
zollecy1 | 0:313fbc3a198a | 14 | * Robin Wanner |
zollecy1 | 0:313fbc3a198a | 15 | * Vincent Vescoli |
zollecy1 | 0:313fbc3a198a | 16 | * Cyrill Zoller |
zollecy1 | 0:313fbc3a198a | 17 | * |
zollecy1 | 0:313fbc3a198a | 18 | * Date created : 20.02.2020 |
zollecy1 | 0:313fbc3a198a | 19 | * |
zollecy1 | 0:313fbc3a198a | 20 | * Purpose : Beispiel |
zollecy1 | 0:313fbc3a198a | 21 | * |
zollecy1 | 0:313fbc3a198a | 22 | |**********************************************************************; |
zollecy1 | 0:313fbc3a198a | 23 | **/ |
zollecy1 | 0:313fbc3a198a | 24 | |
zollecy1 | 1:48e219526d0f | 25 | #define PI (3.14159265) |
zollecy1 | 0:313fbc3a198a | 26 | #include "CalculateData.h" |
zollecy1 | 0:313fbc3a198a | 27 | #include "mbed.h" |
zollecy1 | 0:313fbc3a198a | 28 | #include "XNucleoIKS01A3.h" |
zollecy1 | 1:48e219526d0f | 29 | #include <math.h> |
zollecy1 | 0:313fbc3a198a | 30 | |
zollecy1 | 0:313fbc3a198a | 31 | using namespace std; |
zollecy1 | 0:313fbc3a198a | 32 | |
zollecy1 | 1:48e219526d0f | 33 | const float CalculateData::PERIOD = 1.0f; //Periode von 10 ms |
zollecy1 | 1:48e219526d0f | 34 | static XNucleoIKS01A3 *mems_expansion_board; //Sensor-Board |
zollecy1 | 1:48e219526d0f | 35 | static LSM6DSOSensor *acc_gyro; //Gyro-Sensor |
zollecy1 | 1:48e219526d0f | 36 | static LIS2DW12Sensor *accelerometer; //Acceleroation-Sensor |
zollecy1 | 2:4cccdc792719 | 37 | const int FILTERSIZE = 28; |
zollecy1 | 1:48e219526d0f | 38 | static int acc_filter[3][FILTERSIZE]; |
zollecy1 | 1:48e219526d0f | 39 | static int gyro_filter[3][FILTERSIZE]; |
zollecy1 | 2:4cccdc792719 | 40 | static int speed_filter[3][FILTERSIZE]; |
zollecy1 | 2:4cccdc792719 | 41 | const double filter_koef[FILTERSIZE] = {0.04002232346037, -0.1345598020407, -0.05881026752833, -0.03718019471321, |
zollecy1 | 2:4cccdc792719 | 42 | -0.02581625686635, -0.01315040166028, 0.003361386671892, 0.0234601656009, |
zollecy1 | 2:4cccdc792719 | 43 | 0.04581625763125, 0.06897560043795, 0.0905455051097, 0.1088826077838, |
zollecy1 | 2:4cccdc792719 | 44 | 0.1221264326899, 0.1290832392388, 0.1290832392388, 0.1221264326899, |
zollecy1 | 2:4cccdc792719 | 45 | 0.1088826077838, 0.0905455051097, 0.06897560043795, 0.04581625763125, |
zollecy1 | 2:4cccdc792719 | 46 | 0.0234601656009, 0.003361386671892, -0.01315040166028, -0.02581625686635, |
zollecy1 | 2:4cccdc792719 | 47 | -0.03718019471321, -0.05881026752833, -0.1345598020407, 0.04002232346037 |
zollecy1 | 2:4cccdc792719 | 48 | }; |
zollecy1 | 2:4cccdc792719 | 49 | |
zollecy1 | 0:313fbc3a198a | 50 | |
zollecy1 | 0:313fbc3a198a | 51 | |
zollecy1 | 0:313fbc3a198a | 52 | |
zollecy1 | 0:313fbc3a198a | 53 | |
zollecy1 | 0:313fbc3a198a | 54 | CalculateData::CalculateData(PinName p0, PinName p1, PinName p2, PinName p3, |
zollecy1 | 1:48e219526d0f | 55 | PinName p4, PinName p5, PinName p6){ |
zollecy1 | 0:313fbc3a198a | 56 | /* Instantiate the expansion board */ |
zollecy1 | 0:313fbc3a198a | 57 | mems_expansion_board = XNucleoIKS01A3::instance(p0, p1, p2, p3, p4, p5, p6); |
zollecy1 | 0:313fbc3a198a | 58 | |
zollecy1 | 0:313fbc3a198a | 59 | /* Retrieve the composing elements of the expansion board */ |
zollecy1 | 0:313fbc3a198a | 60 | acc_gyro = mems_expansion_board->acc_gyro; |
zollecy1 | 0:313fbc3a198a | 61 | accelerometer = mems_expansion_board->accelerometer; |
zollecy1 | 1:48e219526d0f | 62 | |
zollecy1 | 0:313fbc3a198a | 63 | /* Enable all sensors */ |
zollecy1 | 0:313fbc3a198a | 64 | accelerometer->enable_x(); |
zollecy1 | 1:48e219526d0f | 65 | //acc_gyro->enable_x(); |
zollecy1 | 0:313fbc3a198a | 66 | acc_gyro->enable_g(); |
zollecy1 | 0:313fbc3a198a | 67 | } |
zollecy1 | 0:313fbc3a198a | 68 | |
zollecy1 | 0:313fbc3a198a | 69 | |
zollecy1 | 0:313fbc3a198a | 70 | CalculateData::~CalculateData() { |
zollecy1 | 0:313fbc3a198a | 71 | |
zollecy1 | 0:313fbc3a198a | 72 | } |
zollecy1 | 0:313fbc3a198a | 73 | |
zollecy1 | 0:313fbc3a198a | 74 | |
zollecy1 | 1:48e219526d0f | 75 | |
zollecy1 | 0:313fbc3a198a | 76 | |
zollecy1 | 1:48e219526d0f | 77 | void CalculateData::enable(){ |
zollecy1 | 1:48e219526d0f | 78 | //Anfangsbedingungen auf NULL |
zollecy1 | 1:48e219526d0f | 79 | for (int i = 0; i<3; i++){ |
zollecy1 | 1:48e219526d0f | 80 | acc[i]=0; |
zollecy1 | 1:48e219526d0f | 81 | acc_old[i]=0; |
zollecy1 | 1:48e219526d0f | 82 | gyro[i]=0; |
zollecy1 | 1:48e219526d0f | 83 | gyro_old[i]=0; |
zollecy1 | 1:48e219526d0f | 84 | speed[i]=0; |
zollecy1 | 1:48e219526d0f | 85 | speed_old[i]=0; |
zollecy1 | 1:48e219526d0f | 86 | angle[i]=0; |
zollecy1 | 1:48e219526d0f | 87 | pos[i]=0; |
zollecy1 | 1:48e219526d0f | 88 | } |
zollecy1 | 1:48e219526d0f | 89 | |
zollecy1 | 1:48e219526d0f | 90 | t = 0; |
zollecy1 | 1:48e219526d0f | 91 | t_old = 0; |
zollecy1 | 1:48e219526d0f | 92 | |
zollecy1 | 1:48e219526d0f | 93 | //Timer und Ticker starten |
zollecy1 | 1:48e219526d0f | 94 | timer.start(); |
zollecy1 | 2:4cccdc792719 | 95 | ticker.attach(callback(this, &CalculateData::run), PERIOD); |
zollecy1 | 0:313fbc3a198a | 96 | } |
zollecy1 | 0:313fbc3a198a | 97 | |
zollecy1 | 1:48e219526d0f | 98 | //Timer und Ticker beenden |
zollecy1 | 1:48e219526d0f | 99 | void CalculateData::disable(){ |
zollecy1 | 2:4cccdc792719 | 100 | //ticker.detach(); |
zollecy1 | 1:48e219526d0f | 101 | timer.stop(); |
zollecy1 | 0:313fbc3a198a | 102 | } |
zollecy1 | 0:313fbc3a198a | 103 | |
zollecy1 | 0:313fbc3a198a | 104 | |
zollecy1 | 0:313fbc3a198a | 105 | |
zollecy1 | 0:313fbc3a198a | 106 | |
zollecy1 | 0:313fbc3a198a | 107 | |
zollecy1 | 1:48e219526d0f | 108 | //Periodischer Task |
zollecy1 | 1:48e219526d0f | 109 | void CalculateData::run() { |
zollecy1 | 1:48e219526d0f | 110 | //Letzte Werte speichern |
zollecy1 | 1:48e219526d0f | 111 | for (int i=0; i<3;i++){ |
zollecy1 | 1:48e219526d0f | 112 | acc_old[i] = acc[i]; |
zollecy1 | 1:48e219526d0f | 113 | gyro_old[i] = gyro[i]; |
zollecy1 | 1:48e219526d0f | 114 | speed_old[i] = speed[i]; |
zollecy1 | 1:48e219526d0f | 115 | } |
zollecy1 | 1:48e219526d0f | 116 | t_old = t; |
zollecy1 | 1:48e219526d0f | 117 | |
zollecy1 | 1:48e219526d0f | 118 | //Sensoren und Timer auslesen |
zollecy1 | 1:48e219526d0f | 119 | accelerometer->get_x_axes(acc); |
zollecy1 | 1:48e219526d0f | 120 | acc_gyro->get_g_axes(gyro); |
zollecy1 | 1:48e219526d0f | 121 | t=timer.read_ms(); |
zollecy1 | 1:48e219526d0f | 122 | |
zollecy1 | 2:4cccdc792719 | 123 | filter(acc, 1); //Daten Filtern |
zollecy1 | 2:4cccdc792719 | 124 | filter(gyro, 2); //Daten Filtern |
zollecy1 | 1:48e219526d0f | 125 | integrate(gyro, gyro_old, angle, t, t_old); //Integral -> Winkel |
zollecy1 | 2:4cccdc792719 | 126 | //transform(acc, angle); //Beschleunigung in Globale Koordinaten transformieren |
zollecy1 | 1:48e219526d0f | 127 | integrate(acc, acc_old, speed, t, t_old); //Integral -> Geschwindigkeit |
zollecy1 | 2:4cccdc792719 | 128 | filter(speed, 3); //Daten Filtern |
zollecy1 | 1:48e219526d0f | 129 | integrate(speed, speed_old, pos, t, t_old); //Integral -> Position |
zollecy1 | 1:48e219526d0f | 130 | } |
zollecy1 | 1:48e219526d0f | 131 | |
zollecy1 | 1:48e219526d0f | 132 | |
zollecy1 | 1:48e219526d0f | 133 | //Daten Integrieren nach expl. Euler |
zollecy1 | 1:48e219526d0f | 134 | void CalculateData::integrate(int *x, int *x_old, int *y, int t, int t_old){ |
zollecy1 | 1:48e219526d0f | 135 | int dt = t-t_old; |
zollecy1 | 1:48e219526d0f | 136 | |
zollecy1 | 1:48e219526d0f | 137 | for(int i = 0;i < 3;i++){ |
zollecy1 | 2:4cccdc792719 | 138 | y[i] = (uint32_t)((double)y[i] + ((double)(x[i] + (double)x_old[i])/2.0)*dt/1000.0); |
zollecy1 | 1:48e219526d0f | 139 | } |
zollecy1 | 1:48e219526d0f | 140 | |
zollecy1 | 1:48e219526d0f | 141 | } |
zollecy1 | 1:48e219526d0f | 142 | |
zollecy1 | 2:4cccdc792719 | 143 | |
zollecy1 | 1:48e219526d0f | 144 | |
zollecy1 | 1:48e219526d0f | 145 | |
zollecy1 | 1:48e219526d0f | 146 | //Filtern der Daten mittels FIR-Filter |
zollecy1 | 2:4cccdc792719 | 147 | void CalculateData::filter(int *array, int type){ |
zollecy1 | 2:4cccdc792719 | 148 | |
zollecy1 | 2:4cccdc792719 | 149 | double sum_array[] = {0, 0, 0}; |
zollecy1 | 1:48e219526d0f | 150 | |
zollecy1 | 2:4cccdc792719 | 151 | switch(type){ |
zollecy1 | 2:4cccdc792719 | 152 | |
zollecy1 | 2:4cccdc792719 | 153 | case 1: |
zollecy1 | 2:4cccdc792719 | 154 | for(int j=0;j<3;j++){ |
zollecy1 | 2:4cccdc792719 | 155 | for(int i=0;i<FILTERSIZE-2;i++){ |
zollecy1 | 2:4cccdc792719 | 156 | acc_filter[j][i] = acc_filter[j][i+1]; |
zollecy1 | 2:4cccdc792719 | 157 | sum_array[j] += (filter_koef[i] * (double)acc_filter[j][i]); |
zollecy1 | 2:4cccdc792719 | 158 | } |
zollecy1 | 2:4cccdc792719 | 159 | acc_filter[j][FILTERSIZE-1] = array[j]; |
zollecy1 | 2:4cccdc792719 | 160 | sum_array[j] += (filter_koef[FILTERSIZE-1] * (double)acc_filter[j][FILTERSIZE-1]); |
zollecy1 | 2:4cccdc792719 | 161 | array[j] = (int)sum_array[j]; |
zollecy1 | 2:4cccdc792719 | 162 | } |
zollecy1 | 2:4cccdc792719 | 163 | break; |
zollecy1 | 1:48e219526d0f | 164 | |
zollecy1 | 2:4cccdc792719 | 165 | case 2: |
zollecy1 | 2:4cccdc792719 | 166 | for(int j=0;j<3;j++){ |
zollecy1 | 2:4cccdc792719 | 167 | for(int i=0;i<FILTERSIZE-2;i++){ |
zollecy1 | 2:4cccdc792719 | 168 | gyro_filter[j][i] = gyro_filter[j][i+1]; |
zollecy1 | 2:4cccdc792719 | 169 | sum_array[j] += (filter_koef[i] * (double)gyro_filter[j][i]); |
zollecy1 | 2:4cccdc792719 | 170 | } |
zollecy1 | 2:4cccdc792719 | 171 | gyro_filter[j][FILTERSIZE-1] = array[j]; |
zollecy1 | 2:4cccdc792719 | 172 | sum_array[j] += (filter_koef[FILTERSIZE-1] * (double)gyro_filter[j][FILTERSIZE-1]); |
zollecy1 | 2:4cccdc792719 | 173 | array[j] = (int)sum_array[j]; |
zollecy1 | 2:4cccdc792719 | 174 | } |
zollecy1 | 2:4cccdc792719 | 175 | break; |
zollecy1 | 1:48e219526d0f | 176 | |
zollecy1 | 2:4cccdc792719 | 177 | case 3: |
zollecy1 | 2:4cccdc792719 | 178 | for(int j=0;j<3;j++){ |
zollecy1 | 2:4cccdc792719 | 179 | for(int i=0;i<FILTERSIZE-2;i++){ |
zollecy1 | 2:4cccdc792719 | 180 | speed_filter[j][i] = speed_filter[j][i+1]; |
zollecy1 | 2:4cccdc792719 | 181 | sum_array[j] += (filter_koef[i] * (double)speed_filter[j][i]); |
zollecy1 | 2:4cccdc792719 | 182 | } |
zollecy1 | 2:4cccdc792719 | 183 | speed_filter[j][FILTERSIZE-1] = array[j]; |
zollecy1 | 2:4cccdc792719 | 184 | sum_array[j] += (filter_koef[FILTERSIZE-1] * (double)speed_filter[j][FILTERSIZE-1]); |
zollecy1 | 2:4cccdc792719 | 185 | array[j] = (int)sum_array[j]; |
zollecy1 | 2:4cccdc792719 | 186 | } |
zollecy1 | 2:4cccdc792719 | 187 | default: |
zollecy1 | 2:4cccdc792719 | 188 | break; |
zollecy1 | 2:4cccdc792719 | 189 | } |
zollecy1 | 1:48e219526d0f | 190 | } |
zollecy1 | 0:313fbc3a198a | 191 | |
zollecy1 | 0:313fbc3a198a | 192 | |
zollecy1 | 0:313fbc3a198a | 193 | |
zollecy1 | 0:313fbc3a198a | 194 | |
zollecy1 | 1:48e219526d0f | 195 | void CalculateData::getAccelerometer(int *array){ |
zollecy1 | 1:48e219526d0f | 196 | for(int i=0;i<3;i++){ |
zollecy1 | 1:48e219526d0f | 197 | array[i] = acc[i]; |
zollecy1 | 1:48e219526d0f | 198 | } |
zollecy1 | 1:48e219526d0f | 199 | } |
zollecy1 | 1:48e219526d0f | 200 | |
zollecy1 | 1:48e219526d0f | 201 | |
zollecy1 | 1:48e219526d0f | 202 | void CalculateData::getSpeed(int *array){ |
zollecy1 | 1:48e219526d0f | 203 | for(int i=0;i<3;i++){ |
zollecy1 | 1:48e219526d0f | 204 | array[i] = speed[i]; |
zollecy1 | 1:48e219526d0f | 205 | } |
zollecy1 | 1:48e219526d0f | 206 | } |
zollecy1 | 0:313fbc3a198a | 207 | |
zollecy1 | 0:313fbc3a198a | 208 | |
zollecy1 | 1:48e219526d0f | 209 | void CalculateData::getPosition(int *array){ |
zollecy1 | 1:48e219526d0f | 210 | for(int i=0;i<3;i++){ |
zollecy1 | 1:48e219526d0f | 211 | array[i] = pos[i]; |
zollecy1 | 1:48e219526d0f | 212 | } |
zollecy1 | 1:48e219526d0f | 213 | } |
zollecy1 | 1:48e219526d0f | 214 | |
zollecy1 | 1:48e219526d0f | 215 | |
zollecy1 | 1:48e219526d0f | 216 | void CalculateData::getGyro(int *array){ |
zollecy1 | 1:48e219526d0f | 217 | for(int i=0;i<3;i++){ |
zollecy1 | 1:48e219526d0f | 218 | array[i] = gyro[i]; |
zollecy1 | 1:48e219526d0f | 219 | } |
zollecy1 | 1:48e219526d0f | 220 | } |
zollecy1 | 1:48e219526d0f | 221 | |
zollecy1 | 1:48e219526d0f | 222 | |
zollecy1 | 1:48e219526d0f | 223 | void CalculateData::getAngle(int *array){ |
zollecy1 | 1:48e219526d0f | 224 | for(int i=0;i<3;i++){ |
zollecy1 | 1:48e219526d0f | 225 | array[i] = angle[i]; |
zollecy1 | 1:48e219526d0f | 226 | } |
zollecy1 | 2:4cccdc792719 | 227 | } |
zollecy1 | 2:4cccdc792719 | 228 | |
zollecy1 | 2:4cccdc792719 | 229 | |
zollecy1 | 2:4cccdc792719 | 230 | /* |
zollecy1 | 2:4cccdc792719 | 231 | //Beschleunigung in globale Koordinaten Transformieren |
zollecy1 | 2:4cccdc792719 | 232 | void CalculateData::transform(int *acc, int *angle){ |
zollecy1 | 2:4cccdc792719 | 233 | double acc_new[3]; //Neue Werte |
zollecy1 | 2:4cccdc792719 | 234 | double a = angle[0]*PI/(1000.0*180.0); //Winkel alpha in rad |
zollecy1 | 2:4cccdc792719 | 235 | double b = angle[1]*PI/(1000.0*180.0); //Winkel beta in rad |
zollecy1 | 2:4cccdc792719 | 236 | double c = angle[2]*PI/(1000.0*180.0); //Winkel gamma in rad |
zollecy1 | 2:4cccdc792719 | 237 | |
zollecy1 | 2:4cccdc792719 | 238 | //Transformation |
zollecy1 | 2:4cccdc792719 | 239 | acc_new[0] = (double)acc[0]*cos(a)*cos(c) - (double)acc[1]*cos(b)*sin(c) + (double)acc[2]*sin(b); |
zollecy1 | 2:4cccdc792719 | 240 | acc_new[1] = (double)acc[0]*(cos(a)*sin(c)+sin(a)*sin(b)*cos(c)) + (double)acc[1]*(cos(a)*cos(c)-sin(a)*sin(b)*cos(c)) - (double)acc[2]*sin(a)*cos(b); |
zollecy1 | 2:4cccdc792719 | 241 | acc_new[2] = (double)acc[0]*(sin(a)*sin(c)-cos(a)*sin(b)*cos(c)) - (double)acc[1]*(sin(a)*cos(c)+cos(a)*sin(b)*sin(c)) + (double)acc[2]*cos(a)*cos(b); |
zollecy1 | 2:4cccdc792719 | 242 | |
zollecy1 | 2:4cccdc792719 | 243 | for(int i=0;i<3;i++){ |
zollecy1 | 2:4cccdc792719 | 244 | acc[i] = (int)acc_new[i]; |
zollecy1 | 2:4cccdc792719 | 245 | } |
zollecy1 | 2:4cccdc792719 | 246 | } |
zollecy1 | 2:4cccdc792719 | 247 | */ |