![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
calculate
Dependencies: mbed X_NUCLEO_IKS01A3 Mahony_Algorithm
CalculateData.cpp@6:4641f2b2684b, 2020-05-01 (annotated)
- Committer:
- zollecy1
- Date:
- Fri May 01 08:44:21 2020 +0000
- Revision:
- 6:4641f2b2684b
- Parent:
- 5:62994bb9fff9
..
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 | 3:795998b31c32 | 30 | #include <Thread.h> |
zollecy1 | 3:795998b31c32 | 31 | #include "Liste.h" |
zollecy1 | 4:7d13076ecece | 32 | #include "MahonyAHRS.h" |
zollecy1 | 0:313fbc3a198a | 33 | |
zollecy1 | 0:313fbc3a198a | 34 | using namespace std; |
zollecy1 | 0:313fbc3a198a | 35 | |
zollecy1 | 1:48e219526d0f | 36 | static XNucleoIKS01A3 *mems_expansion_board; //Sensor-Board |
zollecy1 | 1:48e219526d0f | 37 | static LSM6DSOSensor *acc_gyro; //Gyro-Sensor |
zollecy1 | 1:48e219526d0f | 38 | static LIS2DW12Sensor *accelerometer; //Acceleroation-Sensor |
zollecy1 | 4:7d13076ecece | 39 | static LIS2MDLSensor *magnetometer; //Magnetometer |
zollecy1 | 5:62994bb9fff9 | 40 | const float accOffset[3] = {-0.007734011220907f, -0.007085474965694f, -9.910594323583762f}; |
zollecy1 | 2:4cccdc792719 | 41 | const int FILTERSIZE = 28; |
zollecy1 | 5:62994bb9fff9 | 42 | static float acc_filter[3][FILTERSIZE]; |
zollecy1 | 5:62994bb9fff9 | 43 | static float gyro_filter[3][FILTERSIZE]; |
zollecy1 | 5:62994bb9fff9 | 44 | static float speed_filter[3][FILTERSIZE]; |
zollecy1 | 5:62994bb9fff9 | 45 | float magtransformationmatrix[3][3]={{1.624f, 0.049f, 0.011f}, {-0.004f, 1.668f, 0.001f}, {0.074f, 0.001f, 1.635f}}; |
zollecy1 | 5:62994bb9fff9 | 46 | float bias[3]={-466.649f, -273.636f, 58.044f}; |
zollecy1 | 5:62994bb9fff9 | 47 | const float filter_koef[FILTERSIZE] = {1.54459639027449f, -5.19311640449656f, -2.26968649197199f, -1.43490906020467f, |
zollecy1 | 5:62994bb9fff9 | 48 | -0.996336387257677f, -0.507518334242666f, 0.129727243968288f, 0.905406882193463f, |
zollecy1 | 5:62994bb9fff9 | 49 | 1.76820384311742f, 2.66200095951371f, 3.49445629978250f, 4.20214691216170f, |
zollecy1 | 5:62994bb9fff9 | 50 | 4.71327076441993f, 4.98175738274206f, 4.98175738274206f, 4.71327076441993f, |
zollecy1 | 5:62994bb9fff9 | 51 | 4.20214691216170f, 3.49445629978250f, 2.66200095951371f, 1.76820384311742f, |
zollecy1 | 5:62994bb9fff9 | 52 | 0.905406882193463f, 0.129727243968288f, -0.507518334242666f, -0.996336387257677f, |
zollecy1 | 5:62994bb9fff9 | 53 | -1.43490906020467f, -2.26968649197199f, -5.19311640449656f, 1.54459639027449f}; |
zollecy1 | 0:313fbc3a198a | 54 | |
zollecy1 | 0:313fbc3a198a | 55 | |
zollecy1 | 0:313fbc3a198a | 56 | |
zollecy1 | 0:313fbc3a198a | 57 | CalculateData::CalculateData(PinName p0, PinName p1, PinName p2, PinName p3, |
zollecy1 | 1:48e219526d0f | 58 | PinName p4, PinName p5, PinName p6){ |
zollecy1 | 0:313fbc3a198a | 59 | /* Instantiate the expansion board */ |
zollecy1 | 0:313fbc3a198a | 60 | mems_expansion_board = XNucleoIKS01A3::instance(p0, p1, p2, p3, p4, p5, p6); |
zollecy1 | 0:313fbc3a198a | 61 | |
zollecy1 | 0:313fbc3a198a | 62 | /* Retrieve the composing elements of the expansion board */ |
zollecy1 | 0:313fbc3a198a | 63 | acc_gyro = mems_expansion_board->acc_gyro; |
zollecy1 | 0:313fbc3a198a | 64 | accelerometer = mems_expansion_board->accelerometer; |
zollecy1 | 4:7d13076ecece | 65 | magnetometer = mems_expansion_board->magnetometer; |
zollecy1 | 1:48e219526d0f | 66 | |
zollecy1 | 0:313fbc3a198a | 67 | /* Enable all sensors */ |
zollecy1 | 0:313fbc3a198a | 68 | accelerometer->enable_x(); |
zollecy1 | 4:7d13076ecece | 69 | acc_gyro->enable_x(); |
zollecy1 | 0:313fbc3a198a | 70 | acc_gyro->enable_g(); |
zollecy1 | 4:7d13076ecece | 71 | magnetometer->enable(); |
zollecy1 | 4:7d13076ecece | 72 | |
zollecy1 | 4:7d13076ecece | 73 | //initialisation Mahony |
zollecy1 | 5:62994bb9fff9 | 74 | mahony = new MahonyAHRS(200.0); |
zollecy1 | 0:313fbc3a198a | 75 | } |
zollecy1 | 0:313fbc3a198a | 76 | |
zollecy1 | 0:313fbc3a198a | 77 | |
zollecy1 | 0:313fbc3a198a | 78 | CalculateData::~CalculateData() { |
zollecy1 | 0:313fbc3a198a | 79 | |
zollecy1 | 0:313fbc3a198a | 80 | } |
zollecy1 | 0:313fbc3a198a | 81 | |
zollecy1 | 0:313fbc3a198a | 82 | |
zollecy1 | 1:48e219526d0f | 83 | |
zollecy1 | 0:313fbc3a198a | 84 | |
zollecy1 | 1:48e219526d0f | 85 | void CalculateData::enable(){ |
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 | speed[i]=0; |
zollecy1 | 1:48e219526d0f | 91 | speed_old[i]=0; |
zollecy1 | 3:795998b31c32 | 92 | pos[i]=0; |
zollecy1 | 1:48e219526d0f | 93 | } |
zollecy1 | 1:48e219526d0f | 94 | |
zollecy1 | 1:48e219526d0f | 95 | t_old = 0; |
zollecy1 | 3:795998b31c32 | 96 | counter=0; |
zollecy1 | 1:48e219526d0f | 97 | |
zollecy1 | 3:795998b31c32 | 98 | //Timer und Thread starten |
zollecy1 | 1:48e219526d0f | 99 | timer.start(); |
zollecy1 | 3:795998b31c32 | 100 | periodic_task=true; |
zollecy1 | 3:795998b31c32 | 101 | thread.start(callback(this, &CalculateData::run)); |
zollecy1 | 3:795998b31c32 | 102 | thread.set_priority(osPriorityHigh); |
zollecy1 | 0:313fbc3a198a | 103 | } |
zollecy1 | 0:313fbc3a198a | 104 | |
zollecy1 | 3:795998b31c32 | 105 | //Timer und Thread beenden |
zollecy1 | 1:48e219526d0f | 106 | void CalculateData::disable(){ |
zollecy1 | 3:795998b31c32 | 107 | periodic_task=false; |
zollecy1 | 3:795998b31c32 | 108 | timer.stop(); |
zollecy1 | 3:795998b31c32 | 109 | thread.join(); |
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 | 4:7d13076ecece | 118 | |
zollecy1 | 4:7d13076ecece | 119 | int buf_acc[3]; |
zollecy1 | 4:7d13076ecece | 120 | int buf_gyro[3]; |
zollecy1 | 4:7d13076ecece | 121 | int buf_mag[3]; |
zollecy1 | 4:7d13076ecece | 122 | |
zollecy1 | 3:795998b31c32 | 123 | while(periodic_task){ |
zollecy1 | 3:795998b31c32 | 124 | |
zollecy1 | 3:795998b31c32 | 125 | counter +=1; |
zollecy1 | 3:795998b31c32 | 126 | |
zollecy1 | 3:795998b31c32 | 127 | //Sensoren und Timer auslesen |
zollecy1 | 4:7d13076ecece | 128 | //accelerometer->get_x_axes(buf_acc); |
zollecy1 | 4:7d13076ecece | 129 | acc_gyro->get_x_axes(buf_acc); |
zollecy1 | 3:795998b31c32 | 130 | acc_gyro->get_g_axes(buf_gyro); |
zollecy1 | 4:7d13076ecece | 131 | magnetometer->get_m_axes(buf_mag); |
zollecy1 | 5:62994bb9fff9 | 132 | t[counter-1]=(float)timer.read_ms(); |
zollecy1 | 3:795998b31c32 | 133 | |
zollecy1 | 3:795998b31c32 | 134 | for(int i=0;i<3;i++){ |
zollecy1 | 3:795998b31c32 | 135 | array_acc[i][counter-1]=buf_acc[i]; |
zollecy1 | 3:795998b31c32 | 136 | array_gyro[i][counter-1]=buf_gyro[i]; |
zollecy1 | 4:7d13076ecece | 137 | array_mag[i][counter-1]=buf_mag[i]; |
zollecy1 | 3:795998b31c32 | 138 | } |
zollecy1 | 3:795998b31c32 | 139 | |
zollecy1 | 4:7d13076ecece | 140 | Thread::wait(2); |
zollecy1 | 1:48e219526d0f | 141 | } |
zollecy1 | 1:48e219526d0f | 142 | } |
zollecy1 | 1:48e219526d0f | 143 | |
zollecy1 | 1:48e219526d0f | 144 | |
zollecy1 | 3:795998b31c32 | 145 | |
zollecy1 | 3:795998b31c32 | 146 | void CalculateData::getValue(Liste *list){ |
zollecy1 | 4:7d13076ecece | 147 | //udates the Values |
zollecy1 | 4:7d13076ecece | 148 | update(); |
zollecy1 | 3:795998b31c32 | 149 | |
zollecy1 | 3:795998b31c32 | 150 | //save Data in List |
zollecy1 | 3:795998b31c32 | 151 | list->accX= acc[0]; |
zollecy1 | 3:795998b31c32 | 152 | list->accY= acc[1]; |
zollecy1 | 3:795998b31c32 | 153 | list->accZ= acc[2]; |
zollecy1 | 3:795998b31c32 | 154 | list->speedX= speed[0]; |
zollecy1 | 3:795998b31c32 | 155 | list->speedY= speed[1]; |
zollecy1 | 3:795998b31c32 | 156 | list->speedZ= speed[2]; |
zollecy1 | 3:795998b31c32 | 157 | list->posX= pos[0]; |
zollecy1 | 3:795998b31c32 | 158 | list->posY= pos[1]; |
zollecy1 | 3:795998b31c32 | 159 | list->posZ= pos[2]; |
zollecy1 | 3:795998b31c32 | 160 | list->time= t_old; |
zollecy1 | 3:795998b31c32 | 161 | } |
zollecy1 | 3:795998b31c32 | 162 | |
zollecy1 | 3:795998b31c32 | 163 | |
zollecy1 | 4:7d13076ecece | 164 | //calculate actual Values |
zollecy1 | 4:7d13076ecece | 165 | void CalculateData::update(){ |
zollecy1 | 4:7d13076ecece | 166 | |
zollecy1 | 4:7d13076ecece | 167 | for(int i=0;i<counter;i++){ |
zollecy1 | 5:62994bb9fff9 | 168 | |
zollecy1 | 5:62994bb9fff9 | 169 | float buff[3]; |
zollecy1 | 5:62994bb9fff9 | 170 | |
zollecy1 | 5:62994bb9fff9 | 171 | calibrationMag(array_mag[0][i],array_mag[1][i],array_mag[2][i], buff); |
zollecy1 | 5:62994bb9fff9 | 172 | |
zollecy1 | 5:62994bb9fff9 | 173 | mahony->update(-array_gyro[2][i]*PI/180000.0f, array_gyro[0][i]*PI/180000.0f, array_gyro[1][i]*PI/180000.0f, |
zollecy1 | 5:62994bb9fff9 | 174 | -array_acc[2][i]/1000.0f, array_acc[0][i]/1000.0f, array_acc[1][i]/1000.0f, |
zollecy1 | 5:62994bb9fff9 | 175 | buff[2], buff[0], -buff[1]); |
zollecy1 | 5:62994bb9fff9 | 176 | |
zollecy1 | 5:62994bb9fff9 | 177 | mahony->getEuler(); |
zollecy1 | 5:62994bb9fff9 | 178 | transform(array_acc, i, acc, mahony->getRoll(), mahony->getPitch(), mahony->getYaw()); |
zollecy1 | 5:62994bb9fff9 | 179 | |
zollecy1 | 6:4641f2b2684b | 180 | filterAcc(acc); |
zollecy1 | 5:62994bb9fff9 | 181 | |
zollecy1 | 5:62994bb9fff9 | 182 | integrate(acc, acc_old, speed, t[i], t_old); //in mm/s |
zollecy1 | 5:62994bb9fff9 | 183 | |
zollecy1 | 5:62994bb9fff9 | 184 | //filterSpeed(speed); |
zollecy1 | 5:62994bb9fff9 | 185 | |
zollecy1 | 5:62994bb9fff9 | 186 | integrate(speed, speed_old, pos, t[i], t_old); //in um |
zollecy1 | 4:7d13076ecece | 187 | |
zollecy1 | 4:7d13076ecece | 188 | for (int j=0; j<3;j++){ |
zollecy1 | 4:7d13076ecece | 189 | acc_old[j] = acc[j]; |
zollecy1 | 4:7d13076ecece | 190 | speed_old[j] = speed[j]; |
zollecy1 | 4:7d13076ecece | 191 | } |
zollecy1 | 5:62994bb9fff9 | 192 | t_old=t[i]; |
zollecy1 | 4:7d13076ecece | 193 | } |
zollecy1 | 4:7d13076ecece | 194 | counter = 0; |
zollecy1 | 6:4641f2b2684b | 195 | printf("Roll:\t%i\tPitch:\t%i\tYaw:\t%i\r\n",mahony->getRoll(),mahony->getPitch(),mahony->getYaw()); |
zollecy1 | 6:4641f2b2684b | 196 | //printf("%f, %f, %f, %f, %f, %f, %f\r\n",acc[0],acc[1],acc[2], t_old/1000.0f, speed[0]/1000.0f,speed[1]/1000.0f,speed[2]/1000.0f); |
zollecy1 | 5:62994bb9fff9 | 197 | //printf("%f, %f, %f\r\n",acc[0],acc[1],acc[2]); |
zollecy1 | 4:7d13076ecece | 198 | } |
zollecy1 | 3:795998b31c32 | 199 | |
zollecy1 | 3:795998b31c32 | 200 | |
zollecy1 | 3:795998b31c32 | 201 | |
zollecy1 | 3:795998b31c32 | 202 | |
zollecy1 | 3:795998b31c32 | 203 | |
zollecy1 | 3:795998b31c32 | 204 | |
zollecy1 | 1:48e219526d0f | 205 | //Daten Integrieren nach expl. Euler |
zollecy1 | 5:62994bb9fff9 | 206 | void CalculateData::integrate(float *x, float *x_old, float *y, float t, float t_old){ |
zollecy1 | 5:62994bb9fff9 | 207 | float dt = t-t_old; |
zollecy1 | 1:48e219526d0f | 208 | |
zollecy1 | 1:48e219526d0f | 209 | for(int i = 0;i < 3;i++){ |
zollecy1 | 3:795998b31c32 | 210 | y[i] = y[i] + ((x[i]+x_old[i])/2.0f)*dt; |
zollecy1 | 1:48e219526d0f | 211 | } |
zollecy1 | 1:48e219526d0f | 212 | |
zollecy1 | 1:48e219526d0f | 213 | } |
zollecy1 | 1:48e219526d0f | 214 | |
zollecy1 | 2:4cccdc792719 | 215 | |
zollecy1 | 1:48e219526d0f | 216 | |
zollecy1 | 1:48e219526d0f | 217 | |
zollecy1 | 3:795998b31c32 | 218 | //FIR-Filter (Accelerometer) |
zollecy1 | 5:62994bb9fff9 | 219 | void CalculateData::filterAcc(float *array){ |
zollecy1 | 2:4cccdc792719 | 220 | |
zollecy1 | 2:4cccdc792719 | 221 | double sum_array[] = {0, 0, 0}; |
zollecy1 | 1:48e219526d0f | 222 | |
zollecy1 | 3:795998b31c32 | 223 | for(int j=0;j<3;j++){ |
zollecy1 | 3:795998b31c32 | 224 | for(int i=0;i<FILTERSIZE-2;i++){ |
zollecy1 | 3:795998b31c32 | 225 | acc_filter[j][i] = acc_filter[j][i+1]; |
zollecy1 | 5:62994bb9fff9 | 226 | sum_array[j] += (filter_koef[i] * (float)acc_filter[j][i]); |
zollecy1 | 3:795998b31c32 | 227 | } |
zollecy1 | 5:62994bb9fff9 | 228 | acc_filter[j][FILTERSIZE-1] = array[j]; |
zollecy1 | 5:62994bb9fff9 | 229 | sum_array[j] += (filter_koef[FILTERSIZE-1] * (float)acc_filter[j][FILTERSIZE-1]); |
zollecy1 | 5:62994bb9fff9 | 230 | array[j] = sum_array[j]/FILTERSIZE; |
zollecy1 | 3:795998b31c32 | 231 | } |
zollecy1 | 3:795998b31c32 | 232 | |
zollecy1 | 3:795998b31c32 | 233 | } |
zollecy1 | 3:795998b31c32 | 234 | |
zollecy1 | 0:313fbc3a198a | 235 | |
zollecy1 | 3:795998b31c32 | 236 | //FIR-Filter (Speed) |
zollecy1 | 5:62994bb9fff9 | 237 | void CalculateData::filterSpeed(float *array){ |
zollecy1 | 3:795998b31c32 | 238 | double sum_array[] = {0, 0, 0}; |
zollecy1 | 3:795998b31c32 | 239 | |
zollecy1 | 3:795998b31c32 | 240 | for(int j=0;j<3;j++){ |
zollecy1 | 3:795998b31c32 | 241 | for(int i=0;i<FILTERSIZE-2;i++){ |
zollecy1 | 3:795998b31c32 | 242 | speed_filter[j][i] = speed_filter[j][i+1]; |
zollecy1 | 5:62994bb9fff9 | 243 | sum_array[j] += (filter_koef[i] * (float)speed_filter[j][i]); |
zollecy1 | 3:795998b31c32 | 244 | } |
zollecy1 | 3:795998b31c32 | 245 | speed_filter[j][FILTERSIZE-1] = array[j]; |
zollecy1 | 5:62994bb9fff9 | 246 | sum_array[j] += (filter_koef[FILTERSIZE-1] * (float)speed_filter[j][FILTERSIZE-1]); |
zollecy1 | 5:62994bb9fff9 | 247 | array[j] = sum_array[j]/FILTERSIZE; |
zollecy1 | 3:795998b31c32 | 248 | } |
zollecy1 | 1:48e219526d0f | 249 | } |
zollecy1 | 0:313fbc3a198a | 250 | |
zollecy1 | 0:313fbc3a198a | 251 | |
zollecy1 | 5:62994bb9fff9 | 252 | void CalculateData::calibrationMag(int x,int y,int z, float *buff){ |
zollecy1 | 5:62994bb9fff9 | 253 | buff[0] = magtransformationmatrix[0][0]*((float)x-bias[0])+ |
zollecy1 | 5:62994bb9fff9 | 254 | magtransformationmatrix[0][1]*((float)y-bias[1])+ |
zollecy1 | 5:62994bb9fff9 | 255 | magtransformationmatrix[0][2]*((float)z-bias[2]); |
zollecy1 | 5:62994bb9fff9 | 256 | |
zollecy1 | 5:62994bb9fff9 | 257 | buff[1] = magtransformationmatrix[1][0]*((float)x-bias[0])+ |
zollecy1 | 5:62994bb9fff9 | 258 | magtransformationmatrix[1][1]*((float)y-bias[1])+ |
zollecy1 | 5:62994bb9fff9 | 259 | magtransformationmatrix[1][2]*((float)z-bias[2]); |
zollecy1 | 5:62994bb9fff9 | 260 | |
zollecy1 | 5:62994bb9fff9 | 261 | buff[2] = magtransformationmatrix[2][0]*((float)x-bias[0])+ |
zollecy1 | 5:62994bb9fff9 | 262 | magtransformationmatrix[2][1]*((float)y-bias[1])+ |
zollecy1 | 5:62994bb9fff9 | 263 | magtransformationmatrix[2][2]*((float)z-bias[2]); |
zollecy1 | 5:62994bb9fff9 | 264 | } |
zollecy1 | 2:4cccdc792719 | 265 | |
zollecy1 | 5:62994bb9fff9 | 266 | |
zollecy1 | 2:4cccdc792719 | 267 | //Beschleunigung in globale Koordinaten Transformieren |
zollecy1 | 5:62994bb9fff9 | 268 | void CalculateData::transform(int old_val[3][100], int index, float *new_val, int roll, int pitch, int yaw){ |
zollecy1 | 5:62994bb9fff9 | 269 | |
zollecy1 | 5:62994bb9fff9 | 270 | //Koeffizienten zum umrechnen |
zollecy1 | 5:62994bb9fff9 | 271 | float sa = sin((float)yaw*PI/180.0f); |
zollecy1 | 5:62994bb9fff9 | 272 | float ca = cos((float)yaw*PI/180.0f); |
zollecy1 | 5:62994bb9fff9 | 273 | float sb = sin((float)(-pitch)*PI/180.0f); |
zollecy1 | 5:62994bb9fff9 | 274 | float cb = cos((float)(-pitch)*PI/180.0f); |
zollecy1 | 5:62994bb9fff9 | 275 | float sc = sin((float)roll*PI/180.0f); |
zollecy1 | 5:62994bb9fff9 | 276 | float cc = cos((float)roll*PI/180.0f); |
zollecy1 | 2:4cccdc792719 | 277 | |
zollecy1 | 2:4cccdc792719 | 278 | //Transformation |
zollecy1 | 5:62994bb9fff9 | 279 | /*new_val[0] = -(float)old_val[2][index]*(ca*cb) - (float)old_val[0][index]*(ca*sb*sc-cc*sa) + (float)old_val[1][index]*(ca*cc*sb+sa*sc); |
zollecy1 | 5:62994bb9fff9 | 280 | new_val[1] = -(float)old_val[2][index]*(cb*sa) - (float)old_val[0][index]*(ca*cc+sa*sb*sc) + (float)old_val[1][index]*(cc*sa*sb-ca*sc); |
zollecy1 | 5:62994bb9fff9 | 281 | new_val[2] = -(float)old_val[2][index]*(-sb) - (float)old_val[0][index]*(cb*sc) + (float)old_val[1][index]*(cb*cc); |
zollecy1 | 5:62994bb9fff9 | 282 | */ |
zollecy1 | 5:62994bb9fff9 | 283 | new_val[0] = (-(float)old_val[2][index]*(ca*cb) + (float)old_val[0][index]*(cb*sa) + (float)old_val[1][index]*(-sb))*0.0098f - accOffset[0]; |
zollecy1 | 5:62994bb9fff9 | 284 | new_val[1] = (-(float)old_val[2][index]*(ca*sb*sc-cc*sa) + (float)old_val[0][index]*(ca*cc+sa*sb*sc) + (float)old_val[1][index]*(cb*sc))*0.0098f - accOffset[1]; |
zollecy1 | 5:62994bb9fff9 | 285 | new_val[2] = (-(float)old_val[2][index]*(ca*cc*sb+sa*sc) + (float)old_val[0][index]*(cc*sa*sb-ca*sc) + (float)old_val[1][index]*(cb*cc))*0.0098f - accOffset[2]; |
zollecy1 | 2:4cccdc792719 | 286 | } |