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