File content as of revision 0:fd1a49d15f7f:

#include "chair_MPU9250.h"

Timer t;

chair_MPU9250::chair_MPU9250(Serial* out)
{
    imu = new MPU9250(SDA, SCL);
    usb = out;
}

chair_MPU9250::chair_MPU9250(PinName sda_pin, PinName scl_pin, Serial* out)
{
    imu = new MPU9250(sda_pin, scl_pin);
    usb = out;
}

void chair_MPU9250::setup()
{
    // Read the WHO_AM_I register, this is a good test of communication
    uint8_t whoami = imu->readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250);  // Read WHO_AM_I register for MPU-9250
    usb->printf("I AM 0x%x\n\r", whoami);
    usb->printf("I SHOULD BE 0x71\n\r");

    if (whoami == 0x71) { // WHO_AM_I should always be 0x68
        usb->printf("MPU9250 is online...\n\r");

        wait(1);

        imu->resetMPU9250(); // Reset registers to default in preparation for device calibration
        imu->calibrateMPU9250(imu->gyroBias, imu->accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
        imu->initMPU9250();
        imu->initAK8963(imu->magCalibration);
        wait(2);
    } else {
        usb->printf("Could not connect to MPU9250: \n\r");
        usb->printf("%#x \n",  whoami);

        while(1) ; // Loop forever if communication doesn't happen
    }
    
        
    imu->getAres(); // Get accelerometer sensitivity
    imu->getGres(); // Get gyro sensitivity
    imu->getMres(); // Get magnetometer sensitivity
    usb->printf("Accelerometer sensitivity is %f LSB/g \n\r", 1.0f/imu->aRes);
    usb->printf("Gyroscope sensitivity is %f LSB/deg/s \n\r", 1.0f/imu->gRes);
    usb->printf("Magnetometer sensitivity is %f LSB/G \n\r", 1.0f/imu->mRes);
    imu->magbias[0] = +470.;  // User environmental x-axis correction in milliGauss, should be automatically calculated
    imu->magbias[1] = +120.;  // User environmental x-axis correction in milliGauss
    imu->magbias[2] = +125.;  // User environmental x-axis correction in milliGauss
    t.start();        
 
}

double chair_MPU9250::yaw()
{

// If intPin goes high, all data registers have new data
    if(imu->readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) {  // On interrupt, check if data ready interrupt

        imu->readAccelData(imu->accelCount);  // Read the x/y/z adc values
        // Now we'll calculate the accleration value into actual g's
        imu->ax = (float)imu->accelCount[0]*imu->aRes - imu->accelBias[0];  // get actual g value, this depends on scale being set
        imu->ay = (float)imu->accelCount[1]*imu->aRes - imu->accelBias[1];
        imu->az = (float)imu->accelCount[2]*imu->aRes - imu->accelBias[2];

        imu->readGyroData(imu->gyroCount);  // Read the x/y/z adc values
        // Calculate the gyro value into actual degrees per second
        imu->gx = (float)imu->gyroCount[0]*imu->gRes - imu->gyroBias[0];  // get actual gyro value, this depends on scale being set
        imu->gy = (float)imu->gyroCount[1]*imu->gRes - imu->gyroBias[1];
        imu->gz = (float)imu->gyroCount[2]*imu->gRes - imu->gyroBias[2];

        imu->readMagData(imu->magCount);  // Read the x/y/z adc values
        // Calculate the magnetometer values in milliGauss
        // Include factory calibration per data sheet and user environmental corrections
        imu->mx = (float)imu->magCount[0]*imu->mRes*imu->magCalibration[0] - imu->magbias[0];  // get actual magnetometer value, this depends on scale being set
        imu->my = (float)imu->magCount[1]*imu->mRes*imu->magCalibration[1] - imu->magbias[1];
        imu->mz = (float)imu->magCount[2]*imu->mRes*imu->magCalibration[2] - imu->magbias[2];
    }

    float yaw = 0;
    if(imu->gz>.3 || imu->gz < -.3) {
        yaw = (yaw - t.read()*imu->gz);
        t.reset();
        if(yaw > 360)
            yaw -= 360;
        if(yaw < 0)
            yaw += 360;
    }

    return (double)yaw;
}