File content as of revision 2:e8807de906c9:

#include "mbed.h"
#include "MPU9250.h" 
#include "math.h" 

Serial aa(D1, D0);
class ZMU9250
{
    public:
        ZMU9250()
        {
              
              //Set up I2C
              i2c.frequency(400000);  // use fast (400 kHz) I2C  
              this->t.start();        
              
              // Read the WHO_AM_I register, this is a good test of communication
              uint8_t whoami = this->mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250);  // Read WHO_AM_I register for MPU-9250
              if ((whoami == 0x71)||(whoami == 0x73)) // WHO_AM_I should always be 0x68
              {  
                wait(1);
                this->mpu9250.resetMPU9250(); // Reset registers to default in preparation for device calibration
                this->mpu9250.MPU9250SelfTest(SelfTest); // Start by performing self test and reporting values
                this->mpu9250.calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers  
                wait(2);
                this->mpu9250.initMPU9250(); 
                this->mpu9250.initAK8963(magCalibration);
                wait(1);
               }
               else
               {
                while(1) ; // Loop forever if communication doesn't happen
                }
                this->mpu9250.getAres(); // Get accelerometer sensitivity
                this->mpu9250.getGres(); // Get gyro sensitivity
                this->mpu9250.getMres(); // Get magnetometer sensitivity
                magbias[0] = +470;  // User environmental x-axis correction in milliGauss, should be automatically calculated
                magbias[1] = +120;  // User environmental x-axis correction in milliGauss
                magbias[2] = +125;  // User environmental x-axis correction in milliGauss
        }
        
        void Update()
        {
            if(this->mpu9250.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) {  // On interrupt, check if data ready interrupt
                this->mpu9250.readAccelData(accelCount);  // Read the x/y/z adc values   
                // Now we'll calculate the accleration value into actual g's
                ax = (float)accelCount[0]*aRes - accelBias[0];  // get actual g value, this depends on scale being set
                ay = (float)accelCount[1]*aRes - accelBias[1];   
                az = (float)accelCount[2]*aRes - accelBias[2];  
                this->mpu9250.readGyroData(gyroCount);  // Read the x/y/z adc values
                // Calculate the gyro value into actual degrees per second
                gx = (float)gyroCount[0]*gRes - gyroBias[0];  // get actual gyro value, this depends on scale being set
                gy = (float)gyroCount[1]*gRes - gyroBias[1];  
                gz = (float)gyroCount[2]*gRes - gyroBias[2];   
                this->mpu9250.readMagData(magCount);  // Read the x/y/z adc values   
                // Calculate the magnetometer values in milliGauss
                // Include factory calibration per data sheet and user environmental corrections
                mx = (float)magCount[0]*mRes*magCalibration[0] - magbias[0]+360.0f;  // get actual magnetometer value, this depends on scale being set
                my = (float)magCount[1]*mRes*magCalibration[1] - magbias[1]-210.0f;  
                mz = (float)magCount[2]*mRes*magCalibration[2] - magbias[2];
                //aa.printf("mx %f\tmy %f\tmz %fgx %f\tgy %f\tgz %fax %f\tay %f\taz %f\n\n\n",mx,my,mz,gx,gy,gz,ax,ay,az);
                //wait(0.5);
                
                
            } 
            Now = this->t.read_us();
            deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
            lastUpdate = Now;
            this->sum += deltat;
            sumCount++;
            this->mpu9250.MadgwickQuaternionUpdate(ax, ay, az, gx, gy, gz,  my,  mx, mz);
            delt_t = this->t.read_ms() - count;
            if (delt_t > 10) { // update LCD once per half-second independent of read rate
                tempCount = this->mpu9250.readTempData();  // Read the adc values
                temperature = ((float) tempCount) / 333.87f + 21.0f; // Temperature in degrees Centigrade
                this->axis_gx =gx;
                this->axis_gy =gy;
                this->axis_gz =gz;
                this->axis_ax =ax;
                this->axis_ay =ay;
                this->axis_az =az;
                count = this->t.read_ms(); 
                if(count > 1<<21) {
                    this->t.start(); // start the timer over again if ~30 minutes has passed
                    count = 0;
                    deltat= 0;
                    lastUpdate = this->t.read_us();
                } 
                this->sum = 0;
                sumCount = 0; 
            } 
        }
        
        
        float g_ax()
        {
          return axis_ax;   
        }
        float g_ay()
        {
          return axis_ay;   
        }
        float g_az()
        {
          return axis_az;   
        }
         float g_gx()
        {
          return axis_gx;   
        }
        float g_gy()
        {
          return axis_gy;   
        }
        float g_gz()
        {
          return axis_gz;   
        }
        
        
        
    private:
        float sum;
        uint32_t sumCount;
        char buffer[14];
        MPU9250 mpu9250;
        Timer t;
        float axis_az,axis_ay,axis_ax,axis_gz,axis_gy,axis_gx;
        
           
};