IMU.cpp

00001 /*
00002  * IMU.cpp
00003  * Copyright (c) 2018, ZHAW
00004  * All righSAMPLE_TIME reserved.
00005  */
00006 
00007 #include "IMU.h"
00008 #include "mbed.h"
00009 
00010 
00011 using namespace std;
00012 
00013 const float IMU::M_PI = 3.14159265358979323846f;    // the mathematical constant PI
00014 
00015 // Nick ====================================
00016 const float IMU::SAMPLE_TIME = 0.001f;
00017 const float IMU::STD_ALPHA = 0.02f; // Messrauschen sensor standardabweichung gx - R
00018 const float IMU::STD_OMEGA = 0.034f; // Messrauschen sensor standardabweichung gx - R
00019 //==========================================
00020 
00021 /**
00022  * Creates an IMU object.
00023  * @param spi a reference to an spi controller to use.
00024  * @param csAG the chip select output for the accelerometer and the gyro sensor.
00025  * @param csM the chip select output for the magnetometer.
00026  */
00027 IMU::IMU(SPI& spi, DigitalOut& csAG, DigitalOut& csM) : spi(spi), csAG(csAG), csM(csM), thread(osPriorityHigh, STACK_SIZE)
00028 {
00029 
00030     // initialize SPI interface
00031 
00032     spi.format(8, 3);
00033     spi.frequency(1000000);
00034 
00035     // reset chip select lines to logical high
00036 
00037     csAG = 1;
00038     csM = 1;
00039 
00040     // initialize accelerometer and gyro
00041 
00042     writeRegister(csAG, CTRL_REG1_G, 0xC3);     // ODR 952 Hz, full scale 245 deg/s
00043     writeRegister(csAG, CTRL_REG2_G, 0x00);     // disable interrupt generation
00044     writeRegister(csAG, CTRL_REG3_G, 0x00);     // disable low power mode, disable high pass filter, high pass cutoff frequency 57 Hz
00045     writeRegister(csAG, CTRL_REG4, 0x38);       // enable gyro in all 3 axis
00046     writeRegister(csAG, CTRL_REG5_XL, 0x38);    // no decimation, enable accelerometer in all 3 axis
00047     writeRegister(csAG, CTRL_REG6_XL, 0xC0);    // ODR 952 Hz, full scale 2g
00048     writeRegister(csAG, CTRL_REG7_XL, 0x00);    // high res mode disabled, filter bypassed
00049     writeRegister(csAG, CTRL_REG8, 0x00);       // 4-wire SPI interface, LSB at lower address
00050     writeRegister(csAG, CTRL_REG9, 0x04);       // disable gyro sleep mode, disable I2C interface, disable FIFO
00051     writeRegister(csAG, CTRL_REG10, 0x00);      // self test disabled
00052 
00053     // initialize magnetometer
00054 
00055     writeRegister(csM, CTRL_REG1_M, 0x10);      // temperature not compensated, low power mode for x & y axis, data rate 10 Hz
00056     writeRegister(csM, CTRL_REG2_M, 0x00);      // full scale 4 gauss
00057     writeRegister(csM, CTRL_REG3_M, 0x80);      // disable I2C interface, low power mode, SPI write only, continuous conversion mode
00058     writeRegister(csM, CTRL_REG4_M, 0x00);      // low power mode for z axis, LSB at lower address
00059     writeRegister(csM, CTRL_REG5_M, 0x00);      // fast read disabled
00060     
00061     gammaXFilter.setPeriod(SAMPLE_TIME);
00062     gammaXFilter.setFrequency(80.0f);
00063     gammaYFilter.setPeriod(SAMPLE_TIME);
00064     gammaYFilter.setFrequency(80.0f);
00065     d_gammaXFilter.setPeriod(SAMPLE_TIME);
00066     d_gammaXFilter.setFrequency(80.0f);
00067     d_gammaYFilter.setPeriod(SAMPLE_TIME);
00068     d_gammaYFilter.setFrequency(80.0f);
00069     
00070     
00071     thread.start(callback(this, &IMU::kalman));
00072 }
00073 
00074 /**
00075  * Deletes the IMU object.
00076  */
00077 IMU::~IMU() {}
00078 
00079 /**
00080  * This private method allows to write a register value.
00081  * @param cs the chip select output to use, either csAG or csM.
00082  * @param address the 7 bit address of the register.
00083  * @param value the value to write into the register.
00084  */
00085 void IMU::writeRegister(DigitalOut& cs, uint8_t address, uint8_t value)
00086 {
00087 
00088     cs = 0;
00089 
00090     spi.write(0x7F & address);
00091     spi.write(value & 0xFF);
00092 
00093     cs = 1;
00094 }
00095 
00096 /**
00097  * This private method allows to read a register value.
00098  * @param cs the chip select output to use, either csAG or csM.
00099  * @param address the 7 bit address of the register.
00100  * @return the value read from the register.
00101  */
00102 uint8_t IMU::readRegister(DigitalOut& cs, uint8_t address)
00103 {
00104 
00105     cs = 0;
00106 
00107     spi.write(0x80 | address);
00108     int32_t value = spi.write(0xFF);
00109 
00110     cs = 1;
00111 
00112     return static_cast<uint8_t>(value & 0xFF);
00113 }
00114 
00115 /**
00116  * Reads the gyroscope about the x-axis.
00117  * @return the rotational speed about the x-axis given in [rad/s].
00118  */
00119 float IMU::readGyroX()
00120 {
00121 
00122     uint8_t low = readRegister(csAG, OUT_X_L_G);
00123     uint8_t high = readRegister(csAG, OUT_X_H_G);
00124 
00125     int16_t value = static_cast<int16_t>((static_cast<uint16_t>(high) << 8) | static_cast<uint16_t>(low));
00126 
00127     return static_cast<float>(value)/32768.0f*245.0f*M_PI/180.0f;
00128 }
00129 
00130 /**
00131  * Reads the gyroscope about the y-axis.
00132  * @return the rotational speed about the y-axis given in [rad/s].
00133  */
00134 float IMU::readGyroY()
00135 {
00136 
00137     uint8_t low = readRegister(csAG, OUT_Y_L_G);
00138     uint8_t high = readRegister(csAG, OUT_Y_H_G);
00139 
00140     int16_t value = static_cast<int16_t>((static_cast<uint16_t>(high) << 8) | static_cast<uint16_t>(low));
00141 
00142     return static_cast<float>(value)/32768.0f*245.0f*M_PI/180.0f;
00143 }
00144 
00145 /**
00146  * Reads the gyroscope about the z-axis.
00147  * @return the rotational speed about the z-axis given in [rad/s].
00148  */
00149 float IMU::readGyroZ()
00150 {
00151 
00152     uint8_t low = readRegister(csAG, OUT_Z_L_G);
00153     uint8_t high = readRegister(csAG, OUT_Z_H_G);
00154 
00155     int16_t value = static_cast<int16_t>((static_cast<uint16_t>(high) << 8) | static_cast<uint16_t>(low));
00156 
00157     return static_cast<float>(value)/32768.0f*245.0f*M_PI/180.0f;
00158 }
00159 
00160 /**
00161  * Reads the acceleration in x-direction.
00162  * @return the acceleration in x-direction, given in [m/s2].
00163  */
00164 float IMU::readAccelerationX()
00165 {
00166 
00167     uint8_t low = readRegister(csAG, OUT_X_L_XL);
00168     uint8_t high = readRegister(csAG, OUT_X_H_XL);
00169 
00170     int16_t value = static_cast<int16_t>((static_cast<uint16_t>(high) << 8) | static_cast<uint16_t>(low));
00171 
00172     return static_cast<float>(value)/32768.0f*2.0f*9.81f;
00173 }
00174 
00175 /**
00176  * Reads the acceleration in y-direction.
00177  * @return the acceleration in y-direction, given in [m/s2].
00178  */
00179 float IMU::readAccelerationY()
00180 {
00181 
00182     uint8_t low = readRegister(csAG, OUT_Y_L_XL);
00183     uint8_t high = readRegister(csAG, OUT_Y_H_XL);
00184 
00185     int16_t value = static_cast<int16_t>((static_cast<uint16_t>(high) << 8) | static_cast<uint16_t>(low));
00186 
00187     return static_cast<float>(value)/32768.0f*2.0f*9.81f;
00188 }
00189 
00190 /**
00191  * Reads the acceleration in z-direction.
00192  * @return the acceleration in z-direction, given in [m/s2].
00193  */
00194 float IMU::readAccelerationZ()
00195 {
00196 
00197     uint8_t low = readRegister(csAG, OUT_Z_L_XL);
00198     uint8_t high = readRegister(csAG, OUT_Z_H_XL);
00199 
00200     int16_t value = static_cast<int16_t>((static_cast<uint16_t>(high) << 8) | static_cast<uint16_t>(low));
00201 
00202     return static_cast<float>(value)/32768.0f*2.0f*9.81f;
00203 }
00204 
00205 /**
00206  * Reads the magnetic field in x-direction.
00207  * @return the magnetic field in x-direction, given in [Gauss].
00208  */
00209 float IMU::readMagnetometerX()
00210 {
00211 
00212     uint8_t low = readRegister(csM, OUT_X_L_M);
00213     uint8_t high = readRegister(csM, OUT_X_H_M);
00214 
00215     int16_t value = static_cast<int16_t>((static_cast<uint16_t>(high) << 8) | static_cast<uint16_t>(low));
00216 
00217     return static_cast<float>(value)/32768.0f*4.0f;
00218 }
00219 
00220 /**
00221  * Reads the magnetic field in x-direction.
00222  * @return the magnetic field in x-direction, given in [Gauss].
00223  */
00224 float IMU::readMagnetometerY()
00225 {
00226 
00227     uint8_t low = readRegister(csM, OUT_Y_L_M);
00228     uint8_t high = readRegister(csM, OUT_Y_H_M);
00229 
00230     int16_t value = static_cast<int16_t>((static_cast<uint16_t>(high) << 8) | static_cast<uint16_t>(low));
00231 
00232     return static_cast<float>(value)/32768.0f*4.0f;
00233 }
00234 
00235 /**
00236  * Reads the magnetic field in x-direction.
00237  * @return the magnetic field in x-direction, given in [Gauss].
00238  */
00239 float IMU::readMagnetometerZ()
00240 {
00241 
00242     uint8_t low = readRegister(csM, OUT_Z_L_M);
00243     uint8_t high = readRegister(csM, OUT_Z_H_M);
00244 
00245     int16_t value = static_cast<int16_t>((static_cast<uint16_t>(high) << 8) | static_cast<uint16_t>(low));
00246 
00247     return static_cast<float>(value)/32768.0f*4.0f;
00248 }
00249 
00250 float IMU::getGammaX()
00251 {
00252     return gammaX;
00253 }
00254 
00255 float IMU::getGammaY()
00256 {
00257     return gammaY;
00258 }
00259 
00260 float IMU::getGammaZ()
00261 {
00262     return gammaZ;
00263 }
00264 
00265 float IMU::getDGammaX()
00266 {
00267     return d_gammaX;
00268 }
00269 
00270 float IMU::getDGammaY()
00271 {
00272     return d_gammaY;
00273 }
00274 
00275 float IMU::getDGammaZ()
00276 {
00277     return d_gammaZ;
00278 }
00279 
00280 void IMU::kalman()
00281 {
00282 
00283     Serial pc1(USBTX, USBRX); // tx, rx
00284     pc1.baud(100000);
00285 
00286     // Messrauschen sensor standardabweichung gx - R
00287     float R11 = STD_ALPHA*STD_ALPHA;
00288     float R22 = STD_OMEGA*STD_OMEGA;
00289 
00290     // Messrauschen prozessor - Q
00291     float Q11 = 0.001f;
00292     float Q22 = 0.001f;
00293 
00294     // Matrix A
00295     float A11 = 1.0f;
00296     float A12 = SAMPLE_TIME;
00297     float A21 = 0.0f;
00298     float A22 = 1.0f;
00299 
00300     // rot X
00301     float alpha_p_x = 0.0f;
00302     float omega_p_x = 0.0f;
00303     float Pk_x11 = 0.0f;
00304     float Pk_x12 = 0.0f;
00305     float Pk_x21 = 0.0f;
00306     float Pk_x22 = 0.0f;
00307     float alpha_korr_x = 0.0f;
00308     float omega_korr_x = 0.0f;
00309 
00310     // rot Y
00311     float alpha_p_y = 0.0f;
00312     float omega_p_y = 0.0f;
00313     float Pk_y11 = 0.0f;
00314     float Pk_y12 = 0.0f;
00315     float Pk_y21 = 0.0f;
00316     float Pk_y22 = 0.0f;
00317     float alpha_korr_y = 0.0f;
00318     float omega_korr_y = 0.0f;
00319 
00320     // rot Z
00321     float alpha_p_z = 0.0f;
00322     float omega_p_z = 0.0f;
00323     float Pk_z11 = 0.0f;
00324     float Pk_z12 = 0.0f;
00325     float Pk_z21 = 0.0f;
00326     float Pk_z22 = 0.0f;
00327     float alpha_korr_z = 0.0f;
00328     float omega_korr_z = 0.0f;
00329 
00330     double mx_f_vor=this->readMagnetometerX();
00331     double mx_vor=this->readMagnetometerX();
00332 
00333     double my_f_vor=this->readMagnetometerY();
00334     double my_vor=this->readMagnetometerY();
00335 
00336     int t = 0;
00337     float gamma_z_int = 0;
00338 
00339     // messung
00340     int * T_IMU = new int[2000];
00341     float * GX_IMU = new float[2000];
00342     float * GY_IMU = new float[2000];
00343 
00344     while (true) {
00345         /*Kalman Filter--------------------------------------------------*/
00346 
00347         /* Send IMU MEasure
00348         if(t==5100) {
00349             pc1.printf("invio dati IMU:\r\n\n");
00350             for(int j=0; j<2000; j++) {
00351                 pc1.printf("%d %.7f %.7f\r\n",*(T_IMU+j),*(GX_IMU+j),*(GY_IMU+j));
00352             }
00353             pc1.printf("fine dati IMU:\r\n\n");
00354             delete T_IMU;
00355             delete GX_IMU;
00356             delete GY_IMU;
00357         }
00358         */
00359 
00360         mutex.lock();
00361 
00362         //printf("IMU start\r\n");
00363 
00364         float ax = this->readAccelerationX();
00365         float ay = this->readAccelerationY();
00366         float az = this->readAccelerationZ();
00367 
00368         float gx = this->readGyroX();
00369         float gy = this->readGyroY();
00370         float gz = this->readGyroZ();
00371 
00372         float mx = this->readMagnetometerX();
00373         float my = this->readMagnetometerY();
00374         float mz = this->readMagnetometerZ();
00375 
00376         // LowPass Magnetometer
00377         float RC = 1.0/(10*2*3.14); // Cutoff 10Hz
00378         float dt = 1.0/SAMPLE_TIME;
00379         float alpha = dt/(RC+dt);
00380 
00381         float mx_f = mx_f_vor + (alpha*(mx-mx_vor));
00382         float my_f = my_f_vor + (alpha*(my-my_vor));
00383 
00384         mx_f_vor = mx_f;
00385         mx_vor = mx;
00386         my_f_vor = my_f;
00387         my_vor = my;
00388 
00389         // rot x
00390         float alpha_x = atan2(-ay,az);
00391         float omega_x = gx;
00392 
00393         // rot y
00394         float alpha_y = atan2(-ax,az);
00395         float omega_y = -gy;
00396 
00397         // rot z
00398         float mx_fil = (mx_f+0.3614f)*11.5937f;
00399         float my_fil = (my_f-0.4466f)*15.2002f;
00400         float alpha_z = atan2(my_fil,mx_fil);// Sostituire con calcolo gamma encoder
00401         float omega_z = gz;
00402 
00403         /*
00404         float alpha_z = 0.024f/(0.095f*3.0f*0.7071f)*(w1+w2+w3)
00405         */
00406 
00407         // Prediction
00408         // x
00409         alpha_p_x = alpha_p_x + SAMPLE_TIME*omega_x;
00410         omega_p_x = omega_p_x;
00411         Pk_x11 = Q11 + A11*(A11*Pk_x11 + A12*Pk_x21) + A12*(A11*Pk_x12 + A12*Pk_x22);
00412         Pk_x12 = A21*(A11*Pk_x11 + A12*Pk_x21) + A22*(A11*Pk_x12 + A12*Pk_x22);
00413         Pk_x21 = A11*(A21*Pk_x11 + A22*Pk_x21) + A12*(A21*Pk_x12 + A22*Pk_x22);
00414         Pk_x22 = Q22 + A21*(A21*Pk_x11 + A22*Pk_x21) + A22*(A21*Pk_x12 + A22*Pk_x22);
00415         // y
00416         alpha_p_y = alpha_p_y + SAMPLE_TIME*omega_y;
00417         omega_p_y = omega_p_y;
00418         Pk_y11 = Q11 + A11*(A11*Pk_y11 + A12*Pk_y21) + A12*(A11*Pk_y12 + A12*Pk_y22);
00419         Pk_y12 = A21*(A11*Pk_y11 + A12*Pk_y21) + A22*(A11*Pk_y12 + A12*Pk_y22);
00420         Pk_y21 = A11*(A21*Pk_y11 + A22*Pk_y21) + A12*(A21*Pk_y12 + A22*Pk_y22);
00421         Pk_y22 = Q22 + A21*(A21*Pk_y11 + A22*Pk_y21) + A22*(A21*Pk_y12 + A22*Pk_y22);
00422         // z
00423         alpha_p_z = alpha_p_z + SAMPLE_TIME*omega_z;
00424         omega_p_z = omega_p_z;
00425         Pk_z11 = Q11 + A11*(A11*Pk_z11 + A12*Pk_z21) + A12*(A11*Pk_z12 + A12*Pk_z22);
00426         Pk_z12 = A21*(A11*Pk_z11 + A12*Pk_z21) + A22*(A11*Pk_z12 + A12*Pk_z22);
00427         Pk_z21 = A11*(A21*Pk_z11 + A22*Pk_z21) + A12*(A21*Pk_z12 + A22*Pk_z22);
00428         Pk_z22 = Q22 + A21*(A21*Pk_z11 + A22*Pk_z21) + A22*(A21*Pk_z12 + A22*Pk_z22);
00429 
00430         // Correction
00431         // x
00432         float Kk_x11 = (Pk_x11*(Pk_x22 + R22))/(Pk_x11*R22 + Pk_x22*R11 + R11*R22 + Pk_x11*Pk_x22 - Pk_x12*Pk_x21) - (Pk_x12*Pk_x21)/(Pk_x11*R22 + Pk_x22*R11 + R11*R22 + Pk_x11*Pk_x22 - Pk_x12*Pk_x21);
00433         float Kk_x12 = (Pk_x12*(Pk_x11 + R11))/(Pk_x11*R22 + Pk_x22*R11 + R11*R22 + Pk_x11*Pk_x22 - Pk_x12*Pk_x21) - (Pk_x11*Pk_x12)/(Pk_x11*R22 + Pk_x22*R11 + R11*R22 + Pk_x11*Pk_x22 - Pk_x12*Pk_x21);
00434         float Kk_x21 = (Pk_x21*(Pk_x22 + R22))/(Pk_x11*R22 + Pk_x22*R11 + R11*R22 + Pk_x11*Pk_x22 - Pk_x12*Pk_x21) - (Pk_x21*Pk_x22)/(Pk_x11*R22 + Pk_x22*R11 + R11*R22 + Pk_x11*Pk_x22 - Pk_x12*Pk_x21);
00435         float Kk_x22 = (Pk_x22*(Pk_x11 + R11))/(Pk_x11*R22 + Pk_x22*R11 + R11*R22 + Pk_x11*Pk_x22 - Pk_x12*Pk_x21) - (Pk_x12*Pk_x21)/(Pk_x11*R22 + Pk_x22*R11 + R11*R22 + Pk_x11*Pk_x22 - Pk_x12*Pk_x21);
00436         alpha_korr_x = alpha_p_x + Kk_x11*(alpha_x-alpha_p_x) + Kk_x12*(omega_x - omega_p_x);
00437         omega_korr_x = omega_p_x + Kk_x21*(alpha_x-alpha_p_x) + Kk_x22*(omega_x-omega_p_x);
00438 
00439         // y
00440         float Kk_y11 = (Pk_y11*(Pk_y22 + R22))/(Pk_y11*R22 + Pk_y22*R11 + R11*R22 + Pk_y11*Pk_y22 - Pk_y12*Pk_y21) - (Pk_y12*Pk_y21)/(Pk_y11*R22 + Pk_y22*R11 + R11*R22 + Pk_y11*Pk_y22 - Pk_y12*Pk_y21);
00441         float Kk_y12 = (Pk_y12*(Pk_y11 + R11))/(Pk_y11*R22 + Pk_y22*R11 + R11*R22 + Pk_y11*Pk_y22 - Pk_y12*Pk_y21) - (Pk_y11*Pk_y12)/(Pk_y11*R22 + Pk_y22*R11 + R11*R22 + Pk_y11*Pk_y22 - Pk_y12*Pk_y21);
00442         float Kk_y21 = (Pk_y21*(Pk_y22 + R22))/(Pk_y11*R22 + Pk_y22*R11 + R11*R22 + Pk_y11*Pk_y22 - Pk_y12*Pk_y21) - (Pk_y21*Pk_y22)/(Pk_y11*R22 + Pk_y22*R11 + R11*R22 + Pk_y11*Pk_y22 - Pk_y12*Pk_y21);
00443         float Kk_y22 = (Pk_y22*(Pk_y11 + R11))/(Pk_y11*R22 + Pk_y22*R11 + R11*R22 + Pk_y11*Pk_y22 - Pk_y12*Pk_y21) - (Pk_y12*Pk_y21)/(Pk_y11*R22 + Pk_y22*R11 + R11*R22 + Pk_y11*Pk_y22 - Pk_y12*Pk_y21);
00444         alpha_korr_y = alpha_p_y + Kk_y11*(alpha_y-alpha_p_y) + Kk_y12*(omega_y - omega_p_y);
00445         omega_korr_y = omega_p_y + Kk_y21*(alpha_y-alpha_p_y) + Kk_y22*(omega_y-omega_p_y);
00446 
00447         // z
00448         float Kk_z11 = (Pk_z11*(Pk_z22 + R22))/(Pk_z11*R22 + Pk_z22*R11 + R11*R22 + Pk_z11*Pk_z22 - Pk_z12*Pk_z21) - (Pk_z12*Pk_z21)/(Pk_z11*R22 + Pk_z22*R11 + R11*R22 + Pk_z11*Pk_y22 - Pk_z12*Pk_z21);
00449         float Kk_z12 = (Pk_z12*(Pk_z11 + R11))/(Pk_z11*R22 + Pk_z22*R11 + R11*R22 + Pk_z11*Pk_z22 - Pk_z12*Pk_z21) - (Pk_z11*Pk_z12)/(Pk_z11*R22 + Pk_z22*R11 + R11*R22 + Pk_z11*Pk_y22 - Pk_z12*Pk_z21);
00450         float Kk_z21 = (Pk_z21*(Pk_z22 + R22))/(Pk_z11*R22 + Pk_z22*R11 + R11*R22 + Pk_z11*Pk_z22 - Pk_z12*Pk_z21) - (Pk_z21*Pk_z22)/(Pk_z11*R22 + Pk_z22*R11 + R11*R22 + Pk_z11*Pk_z22 - Pk_z12*Pk_z21);
00451         float Kk_z22 = (Pk_z22*(Pk_z11 + R11))/(Pk_z11*R22 + Pk_z22*R11 + R11*R22 + Pk_z11*Pk_z22 - Pk_z12*Pk_z21) - (Pk_z12*Pk_z21)/(Pk_z11*R22 + Pk_z22*R11 + R11*R22 + Pk_z11*Pk_z22 - Pk_z12*Pk_z21);
00452         alpha_korr_z = alpha_p_z + Kk_z11*(alpha_z-alpha_p_z) + Kk_z12*(omega_z - omega_p_z);
00453         omega_korr_z = omega_p_z + Kk_z21*(alpha_z-alpha_p_z) + Kk_z22*(omega_z-omega_p_z);
00454 
00455         // rot in z simple integration
00456         gamma_z_int = gz*0.05f + gamma_z_int;
00457         float f = t*t*(0.000000014370490f)+t*(-0.0012f) + 0.03f;
00458         t++;
00459         
00460         //printf("%.7f %.7f\r\n",alpha_korr_x,gammaXFilter.filter(alpha_korr_x));
00461         
00462         
00463         this->gammaX = gammaXFilter.filter(alpha_korr_x);
00464         this->gammaY = gammaYFilter.filter(alpha_korr_y);
00465         this->gammaZ = gamma_z_int-f;
00466         this->d_gammaX  = d_gammaXFilter.filter(omega_korr_x-0.01f);
00467         this->d_gammaY  = d_gammaYFilter.filter(omega_korr_y+0.006f);
00468         this->d_gammaZ  = gz;
00469 
00470 
00471         if(t<2001) {
00472             *(T_IMU+t) = t;
00473             *(GX_IMU+t) = gammaX;
00474             *(GY_IMU+t) = gammaY;
00475         }
00476 
00477         /*
00478         if(t<1001) {
00479             if(count==10) {
00480                 M[0][i]=gammaX;
00481                 i++;
00482                 count=0;
00483             } else {
00484                 count++;
00485             }
00486         }
00487 
00488         if(t==1100) {
00489             for(int j=0; j<100;j++) {
00490                 pc1.printf("IMU %.7f\r\n",M[0][j]);
00491             }
00492         }
00493         */
00494         //pc1.printf("%.2f %.7f %.7f\r\n", t/1000.0f,alpha_korr_x,omega_korr_x);
00495 
00496         //this->gammaX = alpha_korr_x;
00497         //this->gammaY = alpha_korr_y;
00498         //this->gammaZ = (gamma_z_int-f);
00499         //this->d_gammaX = omega_korr_x-0.01f;
00500         //this->d_gammaY = omega_korr_y+0.006f;
00501         //this->d_gammaZ = gz;
00502         //printf("IMU end\r\n");
00503 
00504         //printf("%.2f %.2f %.2f %.2f %.2f %.2f\r\n",gammaX,gammaY,gammaZ,d_gammaX,d_gammaY,d_gammaZ);
00505 
00506         mutex.unlock();
00507 
00508         thread.wait(1.0);
00509     }
00510 }