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.01f;
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) {
00028     
00029     // initialize SPI interface
00030     
00031     spi.format(8, 3);
00032     spi.frequency(1000000);
00033     
00034     // reset chip select lines to logical high
00035     
00036     csAG = 1;
00037     csM = 1;
00038     
00039     // initialize accelerometer and gyro
00040     
00041     writeRegister(csAG, CTRL_REG1_G, 0xC3);     // ODR 952 Hz, full scale 245 deg/s
00042     writeRegister(csAG, CTRL_REG2_G, 0x00);     // disable interrupt generation
00043     writeRegister(csAG, CTRL_REG3_G, 0x00);     // disable low power mode, disable high pass filter, high pass cutoff frequency 57 Hz
00044     writeRegister(csAG, CTRL_REG4, 0x38);       // enable gyro in all 3 axis
00045     writeRegister(csAG, CTRL_REG5_XL, 0x38);    // no decimation, enable accelerometer in all 3 axis
00046     writeRegister(csAG, CTRL_REG6_XL, 0xC0);    // ODR 952 Hz, full scale 2g
00047     writeRegister(csAG, CTRL_REG7_XL, 0x00);    // high res mode disabled, filter bypassed
00048     writeRegister(csAG, CTRL_REG8, 0x00);       // 4-wire SPI interface, LSB at lower address
00049     writeRegister(csAG, CTRL_REG9, 0x04);       // disable gyro sleep mode, disable I2C interface, disable FIFO
00050     writeRegister(csAG, CTRL_REG10, 0x00);      // self test disabled
00051     
00052     // initialize magnetometer
00053     
00054     writeRegister(csM, CTRL_REG1_M, 0x10);      // temperature not compensated, low power mode for x & y axis, data rate 10 Hz
00055     writeRegister(csM, CTRL_REG2_M, 0x00);      // full scale 4 gauss
00056     writeRegister(csM, CTRL_REG3_M, 0x80);      // disable I2C interface, low power mode, SPI write only, continuous conversion mode
00057     writeRegister(csM, CTRL_REG4_M, 0x00);      // low power mode for z axis, LSB at lower address
00058     writeRegister(csM, CTRL_REG5_M, 0x00);      // fast read disabled
00059     
00060     thread.start(callback(this, &IMU::kalman));
00061 }
00062 
00063 /**
00064  * Deletes the IMU object.
00065  */
00066 IMU::~IMU() {}
00067 
00068 /**
00069  * This private method allows to write a register value.
00070  * @param cs the chip select output to use, either csAG or csM.
00071  * @param address the 7 bit address of the register.
00072  * @param value the value to write into the register.
00073  */
00074 void IMU::writeRegister(DigitalOut& cs, uint8_t address, uint8_t value) {
00075     
00076     cs = 0;
00077     
00078     spi.write(0x7F & address);
00079     spi.write(value & 0xFF);
00080     
00081     cs = 1;
00082 }
00083 
00084 /**
00085  * This private method allows to read a register value.
00086  * @param cs the chip select output to use, either csAG or csM.
00087  * @param address the 7 bit address of the register.
00088  * @return the value read from the register.
00089  */
00090 uint8_t IMU::readRegister(DigitalOut& cs, uint8_t address) {
00091     
00092     cs = 0;
00093     
00094     spi.write(0x80 | address);
00095     int32_t value = spi.write(0xFF);
00096     
00097     cs = 1;
00098     
00099     return static_cast<uint8_t>(value & 0xFF);
00100 }
00101 
00102 /**
00103  * Reads the gyroscope about the x-axis.
00104  * @return the rotational speed about the x-axis given in [rad/s].
00105  */
00106 float IMU::readGyroX() {
00107     
00108     uint8_t low = readRegister(csAG, OUT_X_L_G);
00109     uint8_t high = readRegister(csAG, OUT_X_H_G);
00110     
00111     int16_t value = static_cast<int16_t>((static_cast<uint16_t>(high) << 8) | static_cast<uint16_t>(low));
00112     
00113     return static_cast<float>(value)/32768.0f*245.0f*M_PI/180.0f;
00114 }
00115 
00116 /**
00117  * Reads the gyroscope about the y-axis.
00118  * @return the rotational speed about the y-axis given in [rad/s].
00119  */
00120 float IMU::readGyroY() {
00121 
00122     uint8_t low = readRegister(csAG, OUT_Y_L_G);
00123     uint8_t high = readRegister(csAG, OUT_Y_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 z-axis.
00132  * @return the rotational speed about the z-axis given in [rad/s].
00133  */
00134 float IMU::readGyroZ() {
00135     
00136     uint8_t low = readRegister(csAG, OUT_Z_L_G);
00137     uint8_t high = readRegister(csAG, OUT_Z_H_G);
00138     
00139     int16_t value = static_cast<int16_t>((static_cast<uint16_t>(high) << 8) | static_cast<uint16_t>(low));
00140     
00141     return static_cast<float>(value)/32768.0f*245.0f*M_PI/180.0f;
00142 }
00143 
00144 /**
00145  * Reads the acceleration in x-direction.
00146  * @return the acceleration in x-direction, given in [m/s2].
00147  */
00148 float IMU::readAccelerationX() {
00149     
00150     uint8_t low = readRegister(csAG, OUT_X_L_XL);
00151     uint8_t high = readRegister(csAG, OUT_X_H_XL);
00152     
00153     int16_t value = static_cast<int16_t>((static_cast<uint16_t>(high) << 8) | static_cast<uint16_t>(low));
00154     
00155     return static_cast<float>(value)/32768.0f*2.0f*9.81f;
00156 }
00157 
00158 /**
00159  * Reads the acceleration in y-direction.
00160  * @return the acceleration in y-direction, given in [m/s2].
00161  */
00162 float IMU::readAccelerationY() {
00163     
00164     uint8_t low = readRegister(csAG, OUT_Y_L_XL);
00165     uint8_t high = readRegister(csAG, OUT_Y_H_XL);
00166     
00167     int16_t value = static_cast<int16_t>((static_cast<uint16_t>(high) << 8) | static_cast<uint16_t>(low));
00168     
00169     return static_cast<float>(value)/32768.0f*2.0f*9.81f;
00170 }
00171 
00172 /**
00173  * Reads the acceleration in z-direction.
00174  * @return the acceleration in z-direction, given in [m/s2].
00175  */
00176 float IMU::readAccelerationZ() {
00177     
00178     uint8_t low = readRegister(csAG, OUT_Z_L_XL);
00179     uint8_t high = readRegister(csAG, OUT_Z_H_XL);
00180     
00181     int16_t value = static_cast<int16_t>((static_cast<uint16_t>(high) << 8) | static_cast<uint16_t>(low));
00182     
00183     return static_cast<float>(value)/32768.0f*2.0f*9.81f;
00184 }
00185 
00186 /**
00187  * Reads the magnetic field in x-direction.
00188  * @return the magnetic field in x-direction, given in [Gauss].
00189  */
00190 float IMU::readMagnetometerX() {
00191     
00192     uint8_t low = readRegister(csM, OUT_X_L_M);
00193     uint8_t high = readRegister(csM, OUT_X_H_M);
00194     
00195     int16_t value = static_cast<int16_t>((static_cast<uint16_t>(high) << 8) | static_cast<uint16_t>(low));
00196     
00197     return static_cast<float>(value)/32768.0f*4.0f;
00198 }
00199 
00200 /**
00201  * Reads the magnetic field in x-direction.
00202  * @return the magnetic field in x-direction, given in [Gauss].
00203  */
00204 float IMU::readMagnetometerY() {
00205     
00206     uint8_t low = readRegister(csM, OUT_Y_L_M);
00207     uint8_t high = readRegister(csM, OUT_Y_H_M);
00208     
00209     int16_t value = static_cast<int16_t>((static_cast<uint16_t>(high) << 8) | static_cast<uint16_t>(low));
00210     
00211     return static_cast<float>(value)/32768.0f*4.0f;
00212 }
00213 
00214 /**
00215  * Reads the magnetic field in x-direction.
00216  * @return the magnetic field in x-direction, given in [Gauss].
00217  */
00218 float IMU::readMagnetometerZ() {
00219     
00220     uint8_t low = readRegister(csM, OUT_Z_L_M);
00221     uint8_t high = readRegister(csM, OUT_Z_H_M);
00222     
00223     int16_t value = static_cast<int16_t>((static_cast<uint16_t>(high) << 8) | static_cast<uint16_t>(low));
00224     
00225     return static_cast<float>(value)/32768.0f*4.0f;
00226 }
00227 
00228 float IMU::getGammaX(){
00229     return gammaX;
00230 }
00231 
00232 float IMU::getGammaY(){
00233     return gammaY;
00234 }
00235 
00236 float IMU::getGammaZ(){
00237     return gammaZ;
00238 }
00239 
00240 float IMU::getDGammaX(){
00241     return d_gammaX;
00242 }
00243 
00244 float IMU::getDGammaY(){
00245     return d_gammaY;
00246 }
00247 
00248 float IMU::getDGammaZ(){
00249     return d_gammaZ;
00250 }
00251 
00252 void IMU::kalman(){
00253         
00254     // Messrauschen sensor standardabweichung gx - R
00255     float R11 = STD_ALPHA*STD_ALPHA;
00256     float R22 = STD_OMEGA*STD_OMEGA;
00257     
00258     // Messrauschen prozessor - Q
00259     float Q11 = 0.001f;
00260     float Q22 = 0.001f;
00261     
00262     // Matrix A
00263     float A11 = 1.0f;
00264     float A12 = SAMPLE_TIME;
00265     float A21 = 0.0f;
00266     float A22 = 1.0f;
00267 
00268     // rot X
00269     float alpha_p_x = 0.0f;
00270     float omega_p_x = 0.0f;
00271     float Pk_x11 = 0.0f;
00272     float Pk_x12 = 0.0f;
00273     float Pk_x21 = 0.0f;
00274     float Pk_x22 = 0.0f;
00275     float alpha_korr_x = 0.0f;
00276     float omega_korr_x = 0.0f;
00277 
00278     // rot Y
00279     float alpha_p_y = 0.0f;
00280     float omega_p_y = 0.0f;
00281     float Pk_y11 = 0.0f;
00282     float Pk_y12 = 0.0f;
00283     float Pk_y21 = 0.0f;
00284     float Pk_y22 = 0.0f;
00285     float alpha_korr_y = 0.0f;
00286     float omega_korr_y = 0.0f;
00287     
00288     // rot Z
00289     float alpha_p_z = 0.0f;
00290     float omega_p_z = 0.0f;
00291     float Pk_z11 = 0.0f;
00292     float Pk_z12 = 0.0f;
00293     float Pk_z21 = 0.0f;
00294     float Pk_z22 = 0.0f;
00295     float alpha_korr_z = 0.0f;
00296     float omega_korr_z = 0.0f;
00297     
00298     double mx_f_vor=this->readMagnetometerX();
00299     double mx_vor=this->readMagnetometerX();
00300     
00301     double my_f_vor=this->readMagnetometerY();
00302     double my_vor=this->readMagnetometerY();
00303     
00304     while (true) {
00305         /*Kalman Filter--------------------------------------------------*/
00306 
00307         float ax = this->readAccelerationX();
00308         float ay = this->readAccelerationY();
00309         float az = this->readAccelerationZ();
00310 
00311         float gx = this->readGyroX();
00312         float gy = this->readGyroY();
00313         float gz = this->readGyroZ();
00314         
00315         float mx = this->readMagnetometerX();
00316         float my = this->readMagnetometerY();
00317         float mz = this->readMagnetometerZ();
00318         
00319         // LowPass Magnetometer
00320         float RC = 1.0/(10*2*3.14); // Cutoff 10Hz
00321         float dt = 1.0/SAMPLE_TIME; 
00322         float alpha = dt/(RC+dt);
00323         
00324         float mx_f = mx_f_vor + (alpha*(mx-mx_vor));
00325         float my_f = my_f_vor + (alpha*(my-my_vor));
00326         
00327         mx_f_vor = mx_f;
00328         mx_vor = mx;
00329         my_f_vor = my_f;
00330         my_vor = my;
00331         
00332         // rot x
00333         float alpha_x = atan2(-ay,az);
00334         float omega_x = gx;
00335         
00336         // rot y
00337         float alpha_y = atan2(-ax,az);
00338         float omega_y = gy;
00339         
00340         // rot z
00341         float mx_fil = (mx_f+0.3614f)*11.5937f;
00342         float my_fil = (my_f-0.4466f)*15.2002f;
00343         float alpha_z = atan2(my_fil,mx_fil);// Sostituire con calcolo gamma encoder
00344         float omega_z = gz;
00345         
00346         /*
00347         float alpha_z = 0.024f/(0.095f*3.0f*0.7071f)*(w1+w2+w3)
00348         */
00349 
00350         // Prediction
00351         // x
00352         alpha_p_x = alpha_p_x + SAMPLE_TIME*omega_x;
00353         omega_p_x = omega_p_x;
00354         Pk_x11 = Q11 + A11*(A11*Pk_x11 + A12*Pk_x21) + A12*(A11*Pk_x12 + A12*Pk_x22);
00355         Pk_x12 = A21*(A11*Pk_x11 + A12*Pk_x21) + A22*(A11*Pk_x12 + A12*Pk_x22);
00356         Pk_x21 = A11*(A21*Pk_x11 + A22*Pk_x21) + A12*(A21*Pk_x12 + A22*Pk_x22);
00357         Pk_x22 = Q22 + A21*(A21*Pk_x11 + A22*Pk_x21) + A22*(A21*Pk_x12 + A22*Pk_x22);
00358         // y
00359         alpha_p_y = alpha_p_y + SAMPLE_TIME*omega_y;
00360         omega_p_y = omega_p_y;
00361         Pk_y11 = Q11 + A11*(A11*Pk_y11 + A12*Pk_y21) + A12*(A11*Pk_y12 + A12*Pk_y22);
00362         Pk_y12 = A21*(A11*Pk_y11 + A12*Pk_y21) + A22*(A11*Pk_y12 + A12*Pk_y22);
00363         Pk_y21 = A11*(A21*Pk_y11 + A22*Pk_y21) + A12*(A21*Pk_y12 + A22*Pk_y22);
00364         Pk_y22 = Q22 + A21*(A21*Pk_y11 + A22*Pk_y21) + A22*(A21*Pk_y12 + A22*Pk_y22);
00365         // z
00366         alpha_p_z = alpha_p_z + SAMPLE_TIME*omega_z;
00367         omega_p_z = omega_p_z;
00368         Pk_z11 = Q11 + A11*(A11*Pk_z11 + A12*Pk_z21) + A12*(A11*Pk_z12 + A12*Pk_z22);
00369         Pk_z12 = A21*(A11*Pk_z11 + A12*Pk_z21) + A22*(A11*Pk_z12 + A12*Pk_z22);
00370         Pk_z21 = A11*(A21*Pk_z11 + A22*Pk_z21) + A12*(A21*Pk_z12 + A22*Pk_z22);
00371         Pk_z22 = Q22 + A21*(A21*Pk_z11 + A22*Pk_z21) + A22*(A21*Pk_z12 + A22*Pk_z22);
00372 
00373         // Correction
00374         // x
00375         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);
00376         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);
00377         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);
00378         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);
00379         alpha_korr_x = alpha_p_x + Kk_x11*(alpha_x-alpha_p_x) + Kk_x12*(omega_x - omega_p_x);
00380         omega_korr_x = omega_p_x + Kk_x21*(alpha_x-alpha_p_x) + Kk_x22*(omega_x-omega_p_x);
00381                 
00382         // y
00383         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);
00384         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);
00385         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);
00386         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);
00387         alpha_korr_y = alpha_p_y + Kk_y11*(alpha_y-alpha_p_y) + Kk_y12*(omega_y - omega_p_y);
00388         omega_korr_y = omega_p_y + Kk_y21*(alpha_y-alpha_p_y) + Kk_y22*(omega_y-omega_p_y);
00389         
00390         // z
00391         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);
00392         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);
00393         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);
00394         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);
00395         alpha_korr_z = alpha_p_z + Kk_z11*(alpha_z-alpha_p_z) + Kk_z12*(omega_z - omega_p_z);
00396         omega_korr_z = omega_p_z + Kk_z21*(alpha_z-alpha_p_z) + Kk_z22*(omega_z-omega_p_z);
00397         
00398         this->gammaX = alpha_korr_x;
00399         this->gammaY = alpha_korr_y;
00400         this->gammaZ = alpha_korr_z;
00401         this->d_gammaX = omega_korr_x;
00402         this->d_gammaY = omega_korr_y;
00403         this->d_gammaZ = omega_korr_z;
00404         
00405         thread.wait(50);
00406         }
00407 }