calculate
Dependencies: mbed X_NUCLEO_IKS01A3 Mahony_Algorithm
CalculateData.cpp@1:48e219526d0f, 2020-04-13 (annotated)
- Committer:
- zollecy1
- Date:
- Mon Apr 13 09:37:17 2020 +0000
- Revision:
- 1:48e219526d0f
- Parent:
- 0:313fbc3a198a
- Child:
- 2:4cccdc792719
.;
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 | const int FILTERSIZE = 10; |
zollecy1 | 1:48e219526d0f | 35 | static XNucleoIKS01A3 *mems_expansion_board; //Sensor-Board |
zollecy1 | 1:48e219526d0f | 36 | static LSM6DSOSensor *acc_gyro; //Gyro-Sensor |
zollecy1 | 1:48e219526d0f | 37 | static LIS2DW12Sensor *accelerometer; //Acceleroation-Sensor |
zollecy1 | 1:48e219526d0f | 38 | static int acc_filter[3][FILTERSIZE]; |
zollecy1 | 1:48e219526d0f | 39 | static int gyro_filter[3][FILTERSIZE]; |
zollecy1 | 1:48e219526d0f | 40 | const double filter_koef[] = {0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1}; |
zollecy1 | 0:313fbc3a198a | 41 | |
zollecy1 | 0:313fbc3a198a | 42 | |
zollecy1 | 0:313fbc3a198a | 43 | |
zollecy1 | 0:313fbc3a198a | 44 | |
zollecy1 | 0:313fbc3a198a | 45 | CalculateData::CalculateData(PinName p0, PinName p1, PinName p2, PinName p3, |
zollecy1 | 1:48e219526d0f | 46 | PinName p4, PinName p5, PinName p6){ |
zollecy1 | 0:313fbc3a198a | 47 | /* Instantiate the expansion board */ |
zollecy1 | 0:313fbc3a198a | 48 | mems_expansion_board = XNucleoIKS01A3::instance(p0, p1, p2, p3, p4, p5, p6); |
zollecy1 | 0:313fbc3a198a | 49 | |
zollecy1 | 0:313fbc3a198a | 50 | /* Retrieve the composing elements of the expansion board */ |
zollecy1 | 0:313fbc3a198a | 51 | acc_gyro = mems_expansion_board->acc_gyro; |
zollecy1 | 0:313fbc3a198a | 52 | accelerometer = mems_expansion_board->accelerometer; |
zollecy1 | 1:48e219526d0f | 53 | |
zollecy1 | 0:313fbc3a198a | 54 | /* Enable all sensors */ |
zollecy1 | 0:313fbc3a198a | 55 | accelerometer->enable_x(); |
zollecy1 | 1:48e219526d0f | 56 | //acc_gyro->enable_x(); |
zollecy1 | 0:313fbc3a198a | 57 | acc_gyro->enable_g(); |
zollecy1 | 0:313fbc3a198a | 58 | } |
zollecy1 | 0:313fbc3a198a | 59 | |
zollecy1 | 0:313fbc3a198a | 60 | |
zollecy1 | 0:313fbc3a198a | 61 | CalculateData::~CalculateData() { |
zollecy1 | 0:313fbc3a198a | 62 | |
zollecy1 | 0:313fbc3a198a | 63 | } |
zollecy1 | 0:313fbc3a198a | 64 | |
zollecy1 | 0:313fbc3a198a | 65 | |
zollecy1 | 1:48e219526d0f | 66 | |
zollecy1 | 0:313fbc3a198a | 67 | |
zollecy1 | 1:48e219526d0f | 68 | void CalculateData::enable(){ |
zollecy1 | 1:48e219526d0f | 69 | //Mittelwert der Fehler der Sensoren mit g berechnen |
zollecy1 | 1:48e219526d0f | 70 | acc_err = {0, 0, 0}; |
zollecy1 | 1:48e219526d0f | 71 | gyro_err = {0, 0, 0}; |
zollecy1 | 1:48e219526d0f | 72 | for(int i=0;i<10;i++){ |
zollecy1 | 1:48e219526d0f | 73 | accelerometer->get_x_axes(acc); |
zollecy1 | 1:48e219526d0f | 74 | acc_gyro->get_g_axes(gyro); |
zollecy1 | 1:48e219526d0f | 75 | for(int k=0;k<3;k++){ |
zollecy1 | 1:48e219526d0f | 76 | acc_err[i] += acc[i]; |
zollecy1 | 1:48e219526d0f | 77 | gyro_err[i] += gyro[i]; |
zollecy1 | 1:48e219526d0f | 78 | } |
zollecy1 | 1:48e219526d0f | 79 | } |
zollecy1 | 1:48e219526d0f | 80 | |
zollecy1 | 1:48e219526d0f | 81 | for(int i=0; i<3; i++){ |
zollecy1 | 1:48e219526d0f | 82 | acc_err[i] /= 10; |
zollecy1 | 1:48e219526d0f | 83 | gyro_err[i] /= 10; |
zollecy1 | 1:48e219526d0f | 84 | } |
zollecy1 | 1:48e219526d0f | 85 | |
zollecy1 | 1:48e219526d0f | 86 | //Anfangsbedingungen auf NULL |
zollecy1 | 1:48e219526d0f | 87 | for (int i = 0; i<3; i++){ |
zollecy1 | 1:48e219526d0f | 88 | acc[i]=0; |
zollecy1 | 1:48e219526d0f | 89 | acc_old[i]=0; |
zollecy1 | 1:48e219526d0f | 90 | gyro[i]=0; |
zollecy1 | 1:48e219526d0f | 91 | gyro_old[i]=0; |
zollecy1 | 1:48e219526d0f | 92 | speed[i]=0; |
zollecy1 | 1:48e219526d0f | 93 | speed_old[i]=0; |
zollecy1 | 1:48e219526d0f | 94 | angle[i]=0; |
zollecy1 | 1:48e219526d0f | 95 | pos[i]=0; |
zollecy1 | 1:48e219526d0f | 96 | } |
zollecy1 | 1:48e219526d0f | 97 | |
zollecy1 | 1:48e219526d0f | 98 | t = 0; |
zollecy1 | 1:48e219526d0f | 99 | t_old = 0; |
zollecy1 | 1:48e219526d0f | 100 | |
zollecy1 | 1:48e219526d0f | 101 | //Timer und Ticker starten |
zollecy1 | 1:48e219526d0f | 102 | timer.start(); |
zollecy1 | 1:48e219526d0f | 103 | //ticker.attach(callback(this, &CalculateData::run), PERIOD); |
zollecy1 | 0:313fbc3a198a | 104 | } |
zollecy1 | 0:313fbc3a198a | 105 | |
zollecy1 | 1:48e219526d0f | 106 | //Timer und Ticker beenden |
zollecy1 | 1:48e219526d0f | 107 | void CalculateData::disable(){ |
zollecy1 | 1:48e219526d0f | 108 | ticker.detach(); |
zollecy1 | 1:48e219526d0f | 109 | timer.stop(); |
zollecy1 | 0:313fbc3a198a | 110 | } |
zollecy1 | 0:313fbc3a198a | 111 | |
zollecy1 | 0:313fbc3a198a | 112 | |
zollecy1 | 0:313fbc3a198a | 113 | |
zollecy1 | 0:313fbc3a198a | 114 | |
zollecy1 | 0:313fbc3a198a | 115 | |
zollecy1 | 1:48e219526d0f | 116 | //Periodischer Task |
zollecy1 | 1:48e219526d0f | 117 | void CalculateData::run() { |
zollecy1 | 1:48e219526d0f | 118 | //Letzte Werte speichern |
zollecy1 | 1:48e219526d0f | 119 | for (int i=0; i<3;i++){ |
zollecy1 | 1:48e219526d0f | 120 | acc_old[i] = acc[i]; |
zollecy1 | 1:48e219526d0f | 121 | gyro_old[i] = gyro[i]; |
zollecy1 | 1:48e219526d0f | 122 | speed_old[i] = speed[i]; |
zollecy1 | 1:48e219526d0f | 123 | } |
zollecy1 | 1:48e219526d0f | 124 | t_old = t; |
zollecy1 | 1:48e219526d0f | 125 | |
zollecy1 | 1:48e219526d0f | 126 | //Sensoren und Timer auslesen |
zollecy1 | 1:48e219526d0f | 127 | accelerometer->get_x_axes(acc); |
zollecy1 | 1:48e219526d0f | 128 | acc_gyro->get_g_axes(gyro); |
zollecy1 | 1:48e219526d0f | 129 | t=timer.read_ms(); |
zollecy1 | 1:48e219526d0f | 130 | |
zollecy1 | 1:48e219526d0f | 131 | filter(acc, gyro); //Daten Filtern |
zollecy1 | 1:48e219526d0f | 132 | integrate(gyro, gyro_old, angle, t, t_old); //Integral -> Winkel |
zollecy1 | 1:48e219526d0f | 133 | transform(acc, angle); //Beschleunigung in Globale Koordinaten transformieren |
zollecy1 | 1:48e219526d0f | 134 | integrate(acc, acc_old, speed, t, t_old); //Integral -> Geschwindigkeit |
zollecy1 | 1:48e219526d0f | 135 | integrate(speed, speed_old, pos, t, t_old); //Integral -> Position |
zollecy1 | 1:48e219526d0f | 136 | } |
zollecy1 | 1:48e219526d0f | 137 | |
zollecy1 | 1:48e219526d0f | 138 | |
zollecy1 | 1:48e219526d0f | 139 | //Daten Integrieren nach expl. Euler |
zollecy1 | 1:48e219526d0f | 140 | void CalculateData::integrate(int *x, int *x_old, int *y, int t, int t_old){ |
zollecy1 | 1:48e219526d0f | 141 | int dt = t-t_old; |
zollecy1 | 1:48e219526d0f | 142 | |
zollecy1 | 1:48e219526d0f | 143 | for(int i = 0;i < 3;i++){ |
zollecy1 | 1:48e219526d0f | 144 | y[i] = y[i] + ((x[i] + x_old[i])/2)*dt; |
zollecy1 | 1:48e219526d0f | 145 | } |
zollecy1 | 1:48e219526d0f | 146 | |
zollecy1 | 1:48e219526d0f | 147 | } |
zollecy1 | 1:48e219526d0f | 148 | |
zollecy1 | 1:48e219526d0f | 149 | //Beschleunigung in globale Koordinaten Transformieren |
zollecy1 | 1:48e219526d0f | 150 | void CalculateData::transform(int *acc, int *angle){ |
zollecy1 | 1:48e219526d0f | 151 | double acc_new[3]; //Neue Werte |
zollecy1 | 1:48e219526d0f | 152 | double a = angle[0]*PI/(1000.0*180.0); //Winkel alpha in rad |
zollecy1 | 1:48e219526d0f | 153 | double b = angle[1]*PI/(1000.0*180.0); //Winkel beta in rad |
zollecy1 | 1:48e219526d0f | 154 | double c = angle[2]*PI/(1000.0*180.0); //Winkel gamma in rad |
zollecy1 | 1:48e219526d0f | 155 | |
zollecy1 | 1:48e219526d0f | 156 | //Transformation |
zollecy1 | 1:48e219526d0f | 157 | acc_new[0] = (double)acc[0]*cos(a)*cos(c) - (double)acc[1]*cos(b)*sin(c) + (double)acc[2]*sin(b); |
zollecy1 | 1:48e219526d0f | 158 | 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 | 1:48e219526d0f | 159 | 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 | 1:48e219526d0f | 160 | |
zollecy1 | 1:48e219526d0f | 161 | for(int i=0;i<3;i++){ |
zollecy1 | 1:48e219526d0f | 162 | acc[i] = (int)acc_new[i]; |
zollecy1 | 1:48e219526d0f | 163 | } |
zollecy1 | 1:48e219526d0f | 164 | } |
zollecy1 | 1:48e219526d0f | 165 | |
zollecy1 | 1:48e219526d0f | 166 | |
zollecy1 | 1:48e219526d0f | 167 | //Filtern der Daten mittels FIR-Filter |
zollecy1 | 1:48e219526d0f | 168 | void CalculateData::filter(int *acc, int *gyro){ |
zollecy1 | 1:48e219526d0f | 169 | double sum_acc[] = {0, 0, 0}; |
zollecy1 | 1:48e219526d0f | 170 | double sum_gyro[] = {0, 0, 0}; |
zollecy1 | 1:48e219526d0f | 171 | |
zollecy1 | 1:48e219526d0f | 172 | for(int j=0;j<3;j++){ |
zollecy1 | 1:48e219526d0f | 173 | for(int i=0;i<FILTERSIZE-2;i++){ |
zollecy1 | 1:48e219526d0f | 174 | acc_filter[j][i] = acc_filter[j][i+1]; |
zollecy1 | 1:48e219526d0f | 175 | gyro_filter[j][i] = gyro_filter[j][i+1]; |
zollecy1 | 1:48e219526d0f | 176 | sum_acc[j] += (filter_koef[i] * (double)acc_filter[j][i]); |
zollecy1 | 1:48e219526d0f | 177 | sum_gyro[j] += (filter_koef[i] * (double)gyro_filter[j][i]); |
zollecy1 | 1:48e219526d0f | 178 | } |
zollecy1 | 1:48e219526d0f | 179 | acc_filter[j][FILTERSIZE-1] = acc[j]; |
zollecy1 | 1:48e219526d0f | 180 | gyro_filter[j][FILTERSIZE-1] = gyro[j]; |
zollecy1 | 1:48e219526d0f | 181 | |
zollecy1 | 1:48e219526d0f | 182 | sum_acc[j] += (filter_koef[FILTERSIZE-1] * (double)acc_filter[j][FILTERSIZE-1]); |
zollecy1 | 1:48e219526d0f | 183 | sum_gyro[j] += (filter_koef[FILTERSIZE-1] * (double)gyro_filter[j][FILTERSIZE-1]); |
zollecy1 | 1:48e219526d0f | 184 | |
zollecy1 | 1:48e219526d0f | 185 | acc[j] = (int)sum_acc[j]; |
zollecy1 | 1:48e219526d0f | 186 | gyro[j] = (int)sum_gyro[j]; |
zollecy1 | 1:48e219526d0f | 187 | } |
zollecy1 | 1:48e219526d0f | 188 | } |
zollecy1 | 0:313fbc3a198a | 189 | |
zollecy1 | 0:313fbc3a198a | 190 | |
zollecy1 | 0:313fbc3a198a | 191 | |
zollecy1 | 0:313fbc3a198a | 192 | |
zollecy1 | 1:48e219526d0f | 193 | void CalculateData::getAccelerometer(int *array){ |
zollecy1 | 1:48e219526d0f | 194 | for(int i=0;i<3;i++){ |
zollecy1 | 1:48e219526d0f | 195 | array[i] = acc[i]; |
zollecy1 | 1:48e219526d0f | 196 | } |
zollecy1 | 1:48e219526d0f | 197 | } |
zollecy1 | 1:48e219526d0f | 198 | |
zollecy1 | 1:48e219526d0f | 199 | |
zollecy1 | 1:48e219526d0f | 200 | void CalculateData::getSpeed(int *array){ |
zollecy1 | 1:48e219526d0f | 201 | for(int i=0;i<3;i++){ |
zollecy1 | 1:48e219526d0f | 202 | array[i] = speed[i]; |
zollecy1 | 1:48e219526d0f | 203 | } |
zollecy1 | 1:48e219526d0f | 204 | } |
zollecy1 | 0:313fbc3a198a | 205 | |
zollecy1 | 0:313fbc3a198a | 206 | |
zollecy1 | 1:48e219526d0f | 207 | void CalculateData::getPosition(int *array){ |
zollecy1 | 1:48e219526d0f | 208 | for(int i=0;i<3;i++){ |
zollecy1 | 1:48e219526d0f | 209 | array[i] = pos[i]; |
zollecy1 | 1:48e219526d0f | 210 | } |
zollecy1 | 1:48e219526d0f | 211 | } |
zollecy1 | 1:48e219526d0f | 212 | |
zollecy1 | 1:48e219526d0f | 213 | |
zollecy1 | 1:48e219526d0f | 214 | void CalculateData::getGyro(int *array){ |
zollecy1 | 1:48e219526d0f | 215 | for(int i=0;i<3;i++){ |
zollecy1 | 1:48e219526d0f | 216 | array[i] = gyro[i]; |
zollecy1 | 1:48e219526d0f | 217 | } |
zollecy1 | 1:48e219526d0f | 218 | } |
zollecy1 | 1:48e219526d0f | 219 | |
zollecy1 | 1:48e219526d0f | 220 | |
zollecy1 | 1:48e219526d0f | 221 | void CalculateData::getAngle(int *array){ |
zollecy1 | 1:48e219526d0f | 222 | for(int i=0;i<3;i++){ |
zollecy1 | 1:48e219526d0f | 223 | array[i] = angle[i]; |
zollecy1 | 1:48e219526d0f | 224 | } |
zollecy1 | 1:48e219526d0f | 225 | } |