IMU.h

00001 /*
00002  * IMU.h
00003  * Copyright (c) 2018, ZHAW
00004  * All rights reserved.
00005  */
00006 
00007 #ifndef IMU_H_
00008 #define IMU_H_
00009 
00010 #include <cstdlib>
00011 #include <stdint.h>
00012 #include "mbed.h"
00013 #include "LowpassFilter.h"
00014 
00015 
00016 /**
00017  * This is a device driver class for the ST LSM9DS1 inertial measurement unit.
00018  */
00019 class IMU {
00020 
00021     public:
00022         
00023                     IMU(SPI& spi, DigitalOut& csAG, DigitalOut& csM);
00024         virtual     ~IMU();
00025         float       readGyroX();
00026         float       readGyroY();
00027         float       readGyroZ();
00028         float       readAccelerationX();
00029         float       readAccelerationY();
00030         float       readAccelerationZ();
00031         float       readMagnetometerX();
00032         float       readMagnetometerY();
00033         float       readMagnetometerZ();
00034         // Nick =================
00035         float       getGammaX();
00036         float       getGammaY();
00037         float       getGammaZ();
00038         float       getDGammaX();
00039         float       getDGammaY();
00040         float       getDGammaZ();
00041         void        kalman();
00042         //=======================
00043         
00044     private:
00045         
00046         static const uint8_t    WHO_AM_I = 0x0F;
00047         static const uint8_t    CTRL_REG1_G = 0x10;
00048         static const uint8_t    CTRL_REG2_G = 0x11;
00049         static const uint8_t    CTRL_REG3_G = 0x12;
00050         static const uint8_t    OUT_X_L_G = 0x18;
00051         static const uint8_t    OUT_X_H_G = 0x19;
00052         static const uint8_t    OUT_Y_L_G = 0x1A;
00053         static const uint8_t    OUT_Y_H_G = 0x1B;
00054         static const uint8_t    OUT_Z_L_G = 0x1C;
00055         static const uint8_t    OUT_Z_H_G = 0x1D;
00056         static const uint8_t    CTRL_REG4 = 0x1E;
00057         static const uint8_t    CTRL_REG5_XL = 0x1F;
00058         static const uint8_t    CTRL_REG6_XL = 0x20;
00059         static const uint8_t    CTRL_REG7_XL = 0x21;
00060         static const uint8_t    CTRL_REG8 = 0x22;
00061         static const uint8_t    CTRL_REG9 = 0x23;
00062         static const uint8_t    CTRL_REG10 = 0x24;
00063         static const uint8_t    OUT_X_L_XL = 0x28;
00064         static const uint8_t    OUT_X_H_XL = 0x29;
00065         static const uint8_t    OUT_Y_L_XL = 0x2A;
00066         static const uint8_t    OUT_Y_H_XL = 0x2B;
00067         static const uint8_t    OUT_Z_L_XL = 0x2C;
00068         static const uint8_t    OUT_Z_H_XL = 0x2D;
00069         
00070         static const uint8_t    WHO_AM_I_M = 0x0F;
00071         static const uint8_t    CTRL_REG1_M = 0x20;
00072         static const uint8_t    CTRL_REG2_M = 0x21;
00073         static const uint8_t    CTRL_REG3_M = 0x22;
00074         static const uint8_t    CTRL_REG4_M = 0x23;
00075         static const uint8_t    CTRL_REG5_M = 0x24;
00076         static const uint8_t    OUT_X_L_M = 0x28;
00077         static const uint8_t    OUT_X_H_M = 0x29;
00078         static const uint8_t    OUT_Y_L_M = 0x2A;
00079         static const uint8_t    OUT_Y_H_M = 0x2B;
00080         static const uint8_t    OUT_Z_L_M = 0x2C;
00081         static const uint8_t    OUT_Z_H_M = 0x2D;
00082         
00083         static const float      M_PI;
00084         static const float      SAMPLE_TIME;
00085         static const float      STD_ALPHA;
00086         static const float      STD_OMEGA;
00087         static const uint32_t   STACK_SIZE = 4096;  // stack size of thread, given in [bytes]
00088         
00089         float           gammaX; // angle in X from kalman-Filter
00090         float           gammaY; // angle in Y from kalman-Filter
00091         float           gammaZ; // angle in Z from kalman-Filter
00092         float           d_gammaX; // angular velocity in X from kalman-Filter
00093         float           d_gammaY; // angular velocity in Y from kalman-Filter
00094         float           d_gammaZ; // angular velocity in Z from kalman-Filter
00095         
00096         LowpassFilter       gammaXFilter;
00097         LowpassFilter       gammaYFilter;
00098         LowpassFilter       d_gammaXFilter;
00099         LowpassFilter       d_gammaYFilter;
00100         
00101         Thread          thread;
00102         Ticker          ticker;
00103         Mutex           mutex;      // mutex to lock critical sections
00104         
00105         
00106         SPI&            spi;
00107         DigitalOut&     csAG;
00108         DigitalOut&     csM;
00109         
00110         void            writeRegister(DigitalOut& cs, uint8_t address, uint8_t value);
00111         uint8_t         readRegister(DigitalOut& cs, uint8_t address);
00112 };
00113 
00114 #endif /* IMU_H_ */
00115