File content as of revision 4:f817736140b6:

#include "Sensor.h"
//#define _DEBUG_S
#ifdef _DEBUG_S
    #include "mbed.h"
    Serial p(USBTX, USBRX);
    Timer td;
    int printCounter = 0;
    int tIntStatus = 0;
    int tgetFIFOCount = 0;
    int tgetFIFOCheck = 0;
    int tgetFIFOreset = 0;
    int tgetFIFOBytes = 0;
    int teulars = 0;
    #define NL "\n\r"
    #define PRINT(x) p.printf(x)   //Serial.print(x)
    #define PRINTF p.printf   //Serial.print(x, y)
    #define PRINTLN(x) PRINT(x);PRINT(NL)
    #define START td.start(); 
    #define STOP td.stop()
    #define RESET td.reset()
    #define READ td.read_us()
    #define GET(x) x = READ
#else
    #define PRINT(x)
    #define PRINTF(x, y)
    #define PRINTLN(x)
    #define START 
    #define STOP  
    #define RESET 
    #define READ 
    #define GET(x)
#endif


Sensor::Sensor() : dmp()
{
    gyro_orientation[0] = 1;
    gyro_orientation[1] = 0;
    gyro_orientation[2] = 0;
    gyro_orientation[3] = 0;
    gyro_orientation[4] = 1;
    gyro_orientation[5] = 0;
    gyro_orientation[6] = 0;
    gyro_orientation[7] = 0;
    gyro_orientation[8] = 1;
//InvMpu mpu;
}

int Sensor::initialize()
{
    int result = 0;
    result = mpu_initialize(); 
    if (result != 0) return result;
    wait_ms(20);
    result = dmp_initialize();
    if (result != 0) return result;
    run_self_test();
    return 0;  
}

int Sensor::mpu_initialize()
{    
    unsigned char accel_fsr;
    unsigned short gyro_rate, gyro_fsr;
//    long accel_bias[3], gyro_bias[3];
    if( !dmp.testConnection() )
        return -1;
    /* Get/set hardware configuration. Start gyro. */
    /* Wake up all sensors. */
    if( dmp.mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL) )
        return -3;
    /* Push both gyro and accel data into the FIFO. */
    if( dmp.mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL) )
        return -4;
    if( dmp.mpu_set_sample_rate(SAMPLE_RATE) )
        return -5;
    /* Read back configuration in case it was set improperly. */
    dmp.mpu_get_sample_rate(&gyro_rate);
    dmp.mpu_get_gyro_fsr(&gyro_fsr);
    dmp.mpu_get_accel_fsr(&accel_fsr);
    
//    if( !(gyro_rate == SAMPLE_RATE && gyro_fsr == 2000 && accel_fsr == 2) )
    if( !(gyro_rate == SAMPLE_RATE) )
        return -6;

    return 0;
}

int Sensor::dmp_initialize()
{   
    // Instruction via MotionDriver_Tutorial
    if( dmp.dmp_load_motion_driver_firmware() )
        return -8;
    unsigned short temp = dmp.inv_orientation_matrix_to_scalar(gyro_orientation);
    if( dmp.dmp_set_orientation(temp) )
        return -9;
//    unsigned short mask = DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_SEND_CAL_GYRO |  DMP_FEATURE_GYRO_CAL | DMP_FEATURE_SEND_RAW_ACCEL ;
    unsigned short mask = DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_SEND_CAL_GYRO |  DMP_FEATURE_GYRO_CAL ;
//    unsigned short mask = DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_SEND_CAL_GYRO;
    if( dmp.dmp_enable_feature(mask) )
        return -10;
//    if( dmp.mpu_set_sample_rate(200) )
//        return -11;
    if( dmp.dmp_set_fifo_rate(100) )
        return -12; 
    if( dmp.mpu_set_dmp_state(1) )
        return -13;
//    if( dmp.mpu_set_lpf(188) )
//        return -14;
    return 0;
}

void Sensor::reset()
{
    dmp.reset();
}


int Sensor::run_self_test()
{
    int result;
    long gyro_cal[3], accel_cal[3];
    unsigned char i = 0;

    result = dmp.mpu_run_self_test(gyro_cal, accel_cal);
    if (result == 0x7) {
        /* Test passed. We can trust the gyro data here, so let's push it down
         * to the DMP.
         */
        for(i = 0; i<3; i++) {
            gyro_cal[i] = (long)(gyro_cal[i] * 32.8f); //convert to +-1000dps
            accel_cal[i] *= 4096.f; //convert to +-8G
            accel_cal[i] = accel[i] >> 16;
            gyro_cal[i] = (long)(gyro_cal[i] >> 16);
        }
        dmp.mpu_set_gyro_bias_reg(gyro_cal);
        dmp.mpu_set_accel_bias_6050_reg(accel_cal);
        return 0;
    }
    return -1;
}

int Sensor::updateAngles()
{
    int result = false;
    long quat[4];
    unsigned char more;
    short sensors;
    result = dmp.dmp_read_fifo(gyro, accel, quat, NULL, &sensors, &more);

    if (result == 0)
    {
        q.set(quat);
        dmp.dmpGetGravity(&gravity,&q);
        dmp.dmpGetYawPitchRoll(ypr,&q,&gravity);
    }
//    PRINTF("More: %d\n\r",more);
    if (more > 2) dmp.mpu_reset_fifo();
    return more;
}

float Sensor::getPitch()
{
    return ypr[1] * RAD2DEG;
}
float Sensor::getRoll()
{
    return ypr[2] * RAD2DEG;
}
float Sensor::getYaw()
{
    return ypr[0] * RAD2DEG;
}

float Sensor::getGyroPitch()
{
    return gyro[0] * GYRO_GAIN;
}
float Sensor::getGyroRoll()
{
    return gyro[1] * GYRO_GAIN;   
}
float Sensor::getGyroYaw()
{
    return gyro[2] * GYRO_GAIN;   
}