Diff: MPU9250/MPU9250.h

Revision:

0:17f575135219

diff -r 000000000000 -r 17f575135219 MPU9250/MPU9250.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MPU9250/MPU9250.h	Fri Sep 07 04:11:48 2018 +0000
@@ -0,0 +1,159 @@
+#ifndef MPU9250_H
+#define MPU9250_H
+ 
+#include "mbed.h"
+#include "math.h"
+#include "MPU9250_register.h"
+ 
+
+#define AK8963_ADDRESS   0x0C<<1
+
+// Using the MSENSR-9250 breakout board, ADO is set to 0 
+// Seven-bit device address is 110100 for ADO = 0 and 110101 for ADO = 1
+//mbed uses the eight-bit device address, so shift seven-bit addresses left by one!
+#define ADO 0
+#if ADO
+#define MPU9250_ADDRESS 0x69<<1  // Device address when ADO = 1
+#else
+#define MPU9250_ADDRESS 0x68<<1  // Device address when ADO = 0
+#endif
+
+#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
+#define Ki 0.0f
+
+#define IAM_MPU9250         0x71
+#define IAM_AK8963          0x48
+
+#define DECLINATION -7.45f //Declination at Tokyo is -7.45 degree 2017/06/14
+
+const float PI = 3.14159265358979323846f;
+const float LPGAIN_MAG = 0.0;
+
+
+enum Ascale {
+  AFS_2G = 0,
+  AFS_4G,
+  AFS_8G,
+  AFS_16G
+};
+
+enum Gscale {
+  GFS_250DPS = 0,
+  GFS_500DPS,
+  GFS_1000DPS,
+  GFS_2000DPS
+};
+
+enum Mscale {
+  MFS_14BITS = 0, // 0.6 mG per LSB
+  MFS_16BITS      // 0.15 mG per LSB
+};
+
+
+
+class MPU9250 {
+ 
+public:
+    //MPU9250();
+    MPU9250(PinName sda, PinName scl, RawSerial* serial_p);
+    ~MPU9250();
+
+    bool Initialize(void);
+    bool sensingAcGyMg(void);
+    void calculatePostureAngle(float degree[3]);
+    float calculateYawByMg(void);
+    
+    void MPU9250SelfTest(float * destination);
+
+    void pickupAccel(float accel[3]);
+    void pickupGyro(float gyro[3]);
+    void pickupMag(float mag[3]);
+    float pickupTemp(void);
+
+    void displayAccel(void);
+    void displayGyro(void);
+    void displayMag(void);
+    void displayAngle(void);
+    void displayQuaternion(void);
+    void displayTemperature(void);
+
+    void setMagBias(float bias_x, float bias_y, float bias_z);
+
+private:
+//add
+    I2C *i2c_p;
+    I2C &i2c;
+
+    RawSerial* pc_p;
+
+    Timer timer;
+
+    uint8_t Ascale;     // AFS_2G, AFS_4G, AFS_8G, AFS_16G
+    uint8_t Gscale; // GFS_250DPS, GFS_500DPS, GFS_1000DPS, GFS_2000DPS
+    uint8_t Mscale; // MFS_14BITS or MFS_16BITS, 14-bit or 16-bit magnetometer resolution
+    uint8_t Mmode;        // Either 8 Hz 0x02) or 100 Hz (0x06) magnetometer data ODR  
+    float aRes, gRes, mRes;      // scale resolutions per LSB for the sensors
+
+    int16_t accelCount[3];  // Stores the 16-bit signed accelerometer sensor output
+    int16_t gyroCount[3];   // Stores the 16-bit signed gyro sensor output
+    int16_t magCount[3];    // Stores the 16-bit signed magnetometer sensor output
+    float magCalibration[3], magbias[3];  // Factory mag calibration and mag bias
+    float gyroBias[3], accelBias[3]; // Bias corrections for gyro and accelerometer
+    float ax, ay, az, gx, gy, gz, mx, my, mz; // variables to hold latest sensor data values 
+    int16_t tempCount;   // Stores the real internal chip temperature in degrees Celsius
+    float temperature;
+    float SelfTest[6];
+
+// parameters for 6 DoF sensor fusion calculations
+    float GyroMeasError;    // gyroscope measurement error in rads/s (start at 60 deg/s), then reduce after ~10 s to 3
+    float beta;  // compute beta
+    float GyroMeasDrift;      // gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s)
+    float zeta ;  // compute zeta, the other free parameter in the Madgwick scheme usually set to a small or zero value
+
+    float pitch, yaw, roll;
+    float deltat;                             // integration interval for both filter schemes
+    int lastUpdate, firstUpdate, Now;    // used to calculate integration interval                               // used to calculate integration interval
+    float q[4];           // vector to hold quaternion
+    float eInt[3];              // vector to hold integral error for Mahony method
+
+    float lpmag[3];
+
+  void writeByte(uint8_t address, uint8_t subAddress, uint8_t data);
+  char readByte(uint8_t address, uint8_t subAddress);
+  void readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest);
+    
+  void initializeValue(void);
+
+    void initMPU9250(void);
+    void initAK8963(float * destination);
+    void resetMPU9250(void);
+    void calibrateMPU9250(float * dest1, float * dest2);
+
+    void getMres(void);
+    void getGres(void);
+    void getAres(void);
+    
+    void readAccelData(int16_t * destination);
+    void readGyroData(int16_t * destination);
+    void readMagData(int16_t * destination);
+    int16_t readTempData(void);
+
+    uint8_t Whoami_MPU9250(void);
+    uint8_t Whoami_AK8963(void);
+
+    void sensingAccel(void);
+    void sensingGyro(void);
+    void sensingMag(void);
+    void sensingTemp(void);
+
+    void MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz);
+    void MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz);  
+    void translateQuaternionToDeg(float quaternion[4]);
+    void calibrateDegree(void);
+
+    void transformCoordinateFromCompassToMPU();
+protected:
+
+};
+
+#endif
\ No newline at end of file