File content as of revision 2:7b7060835269:

/* MPU9250 Basic Example Code
 by: Kris Winer
 date: April 1, 2014
 license: Beerware - Use this code however you'd like. If you
 find it useful you can buy me a beer some time.

 Demonstrate basic MPU-9250 functionality including parameterizing the register addresses, initializing the sensor,
 getting properly scaled accelerometer, gyroscope, and magnetometer data out. Added display functions to
 allow display to on breadboard monitor. Addition of 9 DoF sensor fusion using open source Madgwick and
 Mahony filter algorithms. Sketch runs on the 3.3 V 8 MHz Pro Mini and the Teensy 3.1.

 SDA and SCL should have external pull-up resistors (to 3.3V).
 10k resistors are on the EMSENSR-9250 breakout board.

 Hardware setup:
 MPU9250 Breakout --------- Arduino
 VDD ---------------------- 3.3V
 VDDI --------------------- 3.3V
 SDA ----------------------- A4
 SCL ----------------------- A5
 GND ---------------------- GND

 Note: The MPU9250 is an I2C sensor and uses the Arduino Wire library.
 Because the sensor is not 5V tolerant, we are using a 3.3 V 8 MHz Pro Mini or a 3.3 V Teensy 3.1.
 We have disabled the internal pull-ups used by the Wire library in the Wire.h/twi.c utility file.
 We are also using the 400 kHz fast I2C mode by setting the TWI_FREQ  to 400000L /twi.h utility file.
 */

//#include "ST_F401_84MHZ.h"
//F401_init84 myinit(0);
#include "mbed.h"
#include "MPU9250.h"
#include "math.h"

AnalogIn   pot(A0);

float sum = 0;
uint32_t sumCount = 0;

MPU9250 mpu9250;

int i;

DigitalOut myled(LED1);

float axf=0, ayf=0, azf=0, soma=0, naxf=0, Flag_botao=0, Flag_naxf=0, norma=0, norma2=0, g=0, Flag_Norma=0, Flag_axf=0, Flag_Volante=1;
float pr[10];

Timer t;

Serial pc(USBTX, USBRX); // tx, rx

volatile bool newData = false;

InterruptIn isrPin(D12);   //k64 D12  dragon PD_0

InterruptIn button1(USER_BUTTON);
volatile bool button1_pressed = false; // Used in the main loop
volatile bool button1_enabled = true; // Used for debouncing
Timeout button1_timeout; // Used for debouncing

// Enables button when bouncing is over
void button1_enabled_cb(void)
{
    button1_enabled = true;
}

// ISR handling button pressed event
void button1_onpressed_cb(void)
{
    if (button1_enabled) { // Disabled while the button is bouncing
        button1_enabled = false;
        button1_pressed = true; // To be read by the main loop
        button1_timeout.attach(callback(button1_enabled_cb), 0.3); // Debounce time 300 ms
    }
}

void mpuisr()
{
    newData=true;
}

int main()
{
    pc.baud(9600);

    //Set up I2C
    i2c.frequency(400000);  // use fast (400 kHz) I2C

    //pc.printf("CPU SystemCoreClock is %d Hz\r\n", SystemCoreClock);

    t.start();
    isrPin.rise(&mpuisr);

    // Read the WHO_AM_I register, this is a good test of communication
    uint8_t whoami = mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250);  // Read WHO_AM_I register for MPU-9250
    //pc.printf("I AM 0x%x\n\r", whoami);
    //pc.printf("I SHOULD BE 0x71\n\r");

    if (whoami == 0x73) { // WHO_AM_I should always be 0x68
        //pc.printf("MPU9250 is online...\n\r");
        wait(1);


        mpu9250.resetMPU9250(); // Reset registers to default in preparation for device calibration
        mpu9250.calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
        //pc.printf("x gyro bias = %f\n\r", gyroBias[0]);
        //pc.printf("y gyro bias = %f\n\r", gyroBias[1]);
        //pc.printf("z gyro bias = %f\n\r", gyroBias[2]);
        //pc.printf("x accel bias = %f\n\r", accelBias[0]);
        //pc.printf("y accel bias = %f\n\r", accelBias[1]);
        //pc.printf("z accel bias = %f\n\r", accelBias[2]);
        wait(2);
        mpu9250.initMPU9250();
        //pc.printf("MPU9250 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
        mpu9250.initAK8963(magCalibration);
        //pc.printf("AK8963 initialized for active data mode....\n\r"); // Initialize device for active mode read of magnetometer
        //pc.printf("Accelerometer full-scale range = %f  g\n\r", 2.0f*(float)(1<<Ascale));
        //pc.printf("Gyroscope full-scale range = %f  deg/s\n\r", 250.0f*(float)(1<<Gscale));
        //if(Mscale == 0) pc.printf("Magnetometer resolution = 14  bits\n\r");
        //if(Mscale == 1) pc.printf("Magnetometer resolution = 16  bits\n\r");
        //if(Mmode == 2) pc.printf("Magnetometer ODR = 8 Hz\n\r");
        //if(Mmode == 6) pc.printf("Magnetometer ODR = 100 Hz\n\r");
        wait(2);
    }
     else {
        //pc.printf("Could not connect to MPU9250: \n\r");
        //pc.printf("%#x \n",  whoami);


        while(1) ; // Loop forever if communication doesn't happen
    }

    mpu9250.getAres(); // Get accelerometer sensitivity
    mpu9250.getGres(); // Get gyro sensitivity
    mpu9250.getMres(); // Get magnetometer sensitivity
    //pc.printf("Accelerometer sensitivity is %f LSB/g \n\r", 1.0f/aRes);
    //pc.printf("Gyroscope sensitivity is %f LSB/deg/s \n\r", 1.0f/gRes);
    //pc.printf("Magnetometer sensitivity is %f LSB/G \n\r", 1.0f/mRes);
    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

    while(1) {  //abertur do primeiro while
                static int readycnt=0;
                // If intPin goes high, all data registers have new data

#if USE_ISR
                if(newData) {//if num 1
                        newData=false;
                        mpu9250.readByte(MPU9250_ADDRESS, INT_STATUS);  //? need this with ISR
#else
                                if(mpu9250.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) {  //if num2 // On interrupt, check if data ready interrupt
                #endif
                                        readycnt++;
                                        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];
                
                                        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];
                            
                                        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];  // get actual magnetometer value, this depends on scale being set
                                        my = (float)magCount[1]*mRes*magCalibration[1] - magbias[1];
                                        mz = (float)magCount[2]*mRes*magCalibration[2] - magbias[2];
                                        }//if num 2

                            Now = t.read_us();
                            deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
                            lastUpdate = Now;
                    
                            sum += deltat;
                            sumCount++;

        // Pass gyro rate as rad/s
                            uint32_t us = t.read_us();
                            mpu9250.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f,  my,  mx, mz);
                            us = t.read_us()-us;
                    // mpu9250.MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
                    
                            // Serial print and/or display at 0.5 s rate independent of data rates
                            delt_t = t.read_ms() - count;
        if (delt_t > 500) { // update LCD once per half-second independent of read rate //if num 3
            //pc.printf("readycnt %d us %d\n",readycnt,us);
            readycnt=0;
            //pc.printf("ax = %f", 1000*ax);
            //pc.printf(" ay = %f", 1000*ay);
            //pc.printf(" az = %f  mg\n\r", 1000*az);

            //pc.printf("gx = %f", gx);
            //pc.printf(" gy = %f", gy);
            //pc.printf(" gz = %f  deg/s\n\r", gz);

            //pc.printf("gx = %f", mx);
            //pc.printf(" gy = %f", my);
            //pc.printf(" gz = %f  mG\n\r", mz);

            tempCount = mpu9250.readTempData();  // Read the adc values
            temperature = ((float) tempCount) / 333.87f + 21.0f; // Temperature in degrees Centigrade
            //pc.printf("temperature = %f  C\n\r", temperature);

            //pc.printf("q0 = %f\n\r", q[0]);
            //pc.printf("q1 = %f\n\r", q[1]);
            //pc.printf("q2 = %f\n\r", q[2]);
            //pc.printf("q3 = %f\n\r", q[3]);

            yaw   = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);
            pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
            roll  = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
            pitch *= 180.0f / PI;
            yaw   *= 180.0f / PI;
            yaw   -= 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
            roll  *= 180.0f / PI;

            //pc.printf("Yaw, Pitch, Roll: %f %f %f\n\r", yaw, pitch, roll);
            //pc.printf("average rate = %f\n\r", (float) sumCount/sum);

            //myled= !myled;
            count = t.read_ms();
            sum = 0;
            sumCount = 0;
         if(i==0){   
            for(i=0;i<10;i++)
            {
            pr[i] = sqrt((1000000*ax*ax)+(1000000*ay*ay)+(1000000*az*az));
            soma = soma + pr[i];
            }
        
        norma = soma/(10000);
        }
        axf = (ax*1000)/norma;
        ayf = (ay*1000)/norma;
        azf = (az*1000)/norma;
            
        //pc.printf("norma = %f\n", norma);  
        pc.printf("axf = %f\n", axf);
        pc.printf("ayf = %f\n", ayf);
        pc.printf("azf = %f\n", azf);
        
        norma2=sqrt((axf*axf)+(ayf*ayf)+(azf*azf));
        g=1000;
        
        button1.fall(callback(button1_onpressed_cb));
         
        if (button1_pressed) { 
            button1_pressed = false;
            Flag_botao=1;
            };
            
        if(norma2>1000){
            Flag_Norma=1;         
            }else{
             Flag_Norma=0; 
                }
            
        if(axf>-10){
            Flag_axf=1;         
            }else{
             Flag_axf=0; 
                }
        
        if(naxf<-100){
            Flag_naxf=1;         
            }else{
             Flag_naxf=0; 
                }      
          
            
    float angle = pot.read()*3600;
        
        if ((angle < 1050) && (angle > 950) )
        {
             Flag_Volante= 1;
        }else{
             Flag_Volante=0; 
                }
       
    if(Flag_Volante==1&&Flag_axf==1&&Flag_botao==1&&Flag_DRS=0)  {
        myled = 0;
        Flag_botao=0;
        pc.printf("drs disponivel\n");
        Flag_DRS=1;
        } else{
        pc.printf("drs nao disponivel\n");
        }
    
    if (Flag_Volante==0 || Flag_naxf==1) {
        myled = 1;
        pc.printf("desativando drs\n");
        Flag_DRS=0;
        };
        
        
        pc.printf("angulo: %f\n", angle);
        pc.printf("percentage: %3.3f%%\n", pot.read()*100.0f);
        pc.printf("Flag Volante %f\n", Flag_Volante);
        pc.printf("Flag axf %f\n", Flag_axf);
        pc.printf("Flag Norma %f\n", Flag_Norma);
        pc.printf("Norma %f\n\n", norma2);
        wait(0.2);   
            
        
          
        }
       
    }

}