MPU9250.h

00001 #ifndef MPU9250_H
00002 #define MPU9250_H
00003  
00004 #include "mbed.h"
00005 #include "math.h"
00006 #include "MPU9250_register.h"
00007  
00008 
00009 #define AK8963_ADDRESS   0x0C<<1
00010 
00011 // Using the MSENSR-9250 breakout board, ADO is set to 0 
00012 // Seven-bit device address is 110100 for ADO = 0 and 110101 for ADO = 1
00013 //mbed uses the eight-bit device address, so shift seven-bit addresses left by one!
00014 #define ADO 0
00015 #if ADO
00016 #define MPU9250_ADDRESS 0x69<<1  // Device address when ADO = 1
00017 #else
00018 #define MPU9250_ADDRESS 0x68<<1  // Device address when ADO = 0
00019 #endif
00020 
00021 #define Kp 2.0f * 5.0f // these are the free parameters in the Mahony filter and fusion scheme, Kp for proportional feedback, Ki for integral
00022 #define Ki 0.0f
00023 
00024 #define IAM_MPU9250         0x71
00025 #define IAM_AK8963          0x48
00026 
00027 #define DECLINATION -7.45f //Declination at Tokyo is -7.45 degree 2017/06/14
00028 
00029 const float PI = 3.14159265358979323846f;
00030 const float LPGAIN_MAG = 0.0;
00031 
00032 
00033 enum Ascale {
00034   AFS_2G = 0,
00035   AFS_4G,
00036   AFS_8G,
00037   AFS_16G
00038 };
00039 
00040 enum Gscale {
00041   GFS_250DPS = 0,
00042   GFS_500DPS,
00043   GFS_1000DPS,
00044   GFS_2000DPS
00045 };
00046 
00047 enum Mscale {
00048   MFS_14BITS = 0, // 0.6 mG per LSB
00049   MFS_16BITS      // 0.15 mG per LSB
00050 };
00051 
00052 
00053 
00054 class MPU9250 {
00055  
00056 public:
00057     //MPU9250();
00058     MPU9250(PinName sda, PinName scl, RawSerial* serial_p);
00059     ~MPU9250();
00060 
00061     bool Initialize(void);
00062     bool sensingAcGyMg(void);
00063     void calculatePostureAngle(float degree[3]);
00064     float calculateYawByMg(void);
00065     
00066     void MPU9250SelfTest(float * destination);
00067 
00068     void pickupAccel(float accel[3]);
00069     void pickupGyro(float gyro[3]);
00070     void pickupMag(float mag[3]);
00071     float pickupTemp(void);
00072 
00073     void displayAccel(void);
00074     void displayGyro(void);
00075     void displayMag(void);
00076     void displayAngle(void);
00077     void displayQuaternion(void);
00078     void displayTemperature(void);
00079 
00080     void setMagBias(float bias_x, float bias_y, float bias_z);
00081 
00082 private:
00083 //add
00084     I2C *i2c_p;
00085     I2C &i2c;
00086 
00087     RawSerial* pc_p;
00088 
00089     Timer timer;
00090 
00091     uint8_t Ascale;     // AFS_2G, AFS_4G, AFS_8G, AFS_16G
00092     uint8_t Gscale; // GFS_250DPS, GFS_500DPS, GFS_1000DPS, GFS_2000DPS
00093     uint8_t Mscale; // MFS_14BITS or MFS_16BITS, 14-bit or 16-bit magnetometer resolution
00094     uint8_t Mmode;        // Either 8 Hz 0x02) or 100 Hz (0x06) magnetometer data ODR  
00095     float aRes, gRes, mRes;      // scale resolutions per LSB for the sensors
00096 
00097     int16_t accelCount[3];  // Stores the 16-bit signed accelerometer sensor output
00098     int16_t gyroCount[3];   // Stores the 16-bit signed gyro sensor output
00099     int16_t magCount[3];    // Stores the 16-bit signed magnetometer sensor output
00100     float magCalibration[3], magbias[3];  // Factory mag calibration and mag bias
00101     float gyroBias[3], accelBias[3]; // Bias corrections for gyro and accelerometer
00102     float ax, ay, az, gx, gy, gz, mx, my, mz; // variables to hold latest sensor data values 
00103     int16_t tempCount;   // Stores the real internal chip temperature in degrees Celsius
00104     float temperature;
00105     float SelfTest[6];
00106 
00107 // parameters for 6 DoF sensor fusion calculations
00108     float GyroMeasError;    // gyroscope measurement error in rads/s (start at 60 deg/s), then reduce after ~10 s to 3
00109     float beta;  // compute beta
00110     float GyroMeasDrift;      // gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s)
00111     float zeta ;  // compute zeta, the other free parameter in the Madgwick scheme usually set to a small or zero value
00112 
00113     float pitch, yaw, roll;
00114     float deltat;                             // integration interval for both filter schemes
00115     int lastUpdate, firstUpdate, Now;    // used to calculate integration interval                               // used to calculate integration interval
00116     float q[4];           // vector to hold quaternion
00117     float eInt[3];              // vector to hold integral error for Mahony method
00118 
00119     float lpmag[3];
00120 
00121   void writeByte(uint8_t address, uint8_t subAddress, uint8_t data);
00122   char readByte(uint8_t address, uint8_t subAddress);
00123   void readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest);
00124     
00125   void initializeValue(void);
00126 
00127     void initMPU9250(void);
00128     void initAK8963(float * destination);
00129     void resetMPU9250(void);
00130     void calibrateMPU9250(float * dest1, float * dest2);
00131 
00132     void getMres(void);
00133     void getGres(void);
00134     void getAres(void);
00135     
00136     void readAccelData(int16_t * destination);
00137     void readGyroData(int16_t * destination);
00138     void readMagData(int16_t * destination);
00139     int16_t readTempData(void);
00140 
00141     uint8_t Whoami_MPU9250(void);
00142     uint8_t Whoami_AK8963(void);
00143 
00144     void sensingAccel(void);
00145     void sensingGyro(void);
00146     void sensingMag(void);
00147     void sensingTemp(void);
00148 
00149     void MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz);
00150     void MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz);  
00151     void translateQuaternionToDeg(float quaternion[4]);
00152     void calibrateDegree(void);
00153 
00154     void transformCoordinateFromCompassToMPU();
00155 protected:
00156 
00157 };
00158 
00159 #endif