File content as of revision 2:66660dcf55fb:

#include "chair_MPU9250.h"
float total_yaw = 0;

chair_MPU9250::chair_MPU9250(Serial* out, Timer* time)
{
    imu = new MPU9250(SDA, SCL);
    usb = out;
    t = time;
    start = false;
}

chair_MPU9250::chair_MPU9250(PinName sda_pin, PinName scl_pin, Serial* out, Timer* time)
{
    imu = new MPU9250(sda_pin, scl_pin);
    usb = out;
    t = time;
    start = false;
}

void chair_MPU9250::setStart(){
    start = true;
    }
    
void chair_MPU9250::setup()
{
    t->start();
    // 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  
    usb->printf("setup complete\n");    
 
}

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 readTime;
    if(!start) {
        readTime = 0; 
        chair_MPU9250::setStart();
        }
    else {
        readTime = t->read();
        }
        
    
    if(imu->gz>.3 || imu->gz < -.3) {
        total_yaw = (total_yaw - readTime*imu->gz);
        t->reset();
        if(total_yaw > 360){
            total_yaw -= 360;
            }
        
        if(total_yaw < 0){
            total_yaw += 360;
            }
    }
    return total_yaw;
}