File content as of revision 0:17f575135219:

#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