MPU-9250 with Kalman Filter

Dependencies:   ADXL362-helloworld MPU9250_SPI mbed

Fork of ADXL362-helloworld by Analog Devices

main.cpp

Committer:
mfurukawa
Date:
2016-06-17
Revision:
9:e700b2d586d6
Parent:
8:03f9b5289083
Child:
10:f2ef74678956

File content as of revision 9:e700b2d586d6:

/**
 * Masahiro FURUKAWA - m.furukawa@ist.osaka-u.ac.jp
 *
 * June 17, 2016
 *
 * MPU9250 9DoF Sensor (Extended to Ch1 ~ Ch2)
 *
 **/


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

/*
    MOSI (Master Out Slave In)  p5
    MISO (Master In Slave Out   p6
    SCK  (Serial Clock)         p7
    ~CS  (Chip Select)          p8
*/

//  https://developer.mbed.org/users/kylongmu/code/MPU9250_SPI_Test/file/5839d1b118bc/main.cpp

int main()
{

    Serial pc(USBTX, USBRX);
    pc.baud(115200);

    SPI spi(p5, p6, p7);

    //define the mpu9250 object
    mpu9250_spi *imu[2];

    imu[0] = new mpu9250_spi(spi, p8);
    imu[1] = new mpu9250_spi(spi, p9);

    for(int i=0; i<2; i++) {

        imu[0]->deselect();
        imu[1]->deselect();
        imu[i]->select();

        if(imu[i]->init(1,BITS_DLPF_CFG_188HZ)) { //INIT the mpu9250
            printf("\nCouldn't initialize MPU9250 via SPI!");
        }
        printf("\nWHOAMI=0x%2x\n",imu[i]->whoami()); //output the I2C address to know if SPI is working, it should be 104
        wait(1);
        printf("Gyro_scale=%u\n",imu[i]->set_gyro_scale(BITS_FS_2000DPS));    //Set full scale range for gyros
        wait(1);
        printf("Acc_scale=%u\n",imu[i]->set_acc_scale(BITS_FS_16G));          //Set full scale range for accs
        wait(1);
        printf("AK8963 WHIAM=0x%2x\n",imu[i]->AK8963_whoami());
        wait(0.1);
        imu[i]->AK8963_calib_Magnetometer();
        wait(0.1);
    }
    imu[0]->select();
    imu[1]->deselect();
    while(1) {

        //myled = 1;
        
        //wait_us(1);
        
        for(int i=0; i<2; i++) {
            
            imu[0]->deselect();
            imu[1]->deselect();
            
            imu[i]->select();
            imu[i]->read_acc();
            imu[i]->read_rot();
            
            printf("%10.3f,%10.3f,%10.3f %10.3f,%10.3f,%10.3f ",
                   imu[i]->gyroscope_data[0],
                   imu[i]->gyroscope_data[1],
                   imu[i]->gyroscope_data[2],
                   imu[i]->accelerometer_data[0],
                   imu[i]->accelerometer_data[1],
                   imu[i]->accelerometer_data[2]
                  );
            /*
            imu[i]->read_all();
            printf("%10.3f,%10.3f,%10.3f,%10.3f,%10.3f,%10.3f,%10.3f,%10.3f,%10.3f,%10.3f ",
                   imu[i]->Temperature,
                   imu[i]->gyroscope_data[0],
                   imu[i]->gyroscope_data[1],
                   imu[i]->gyroscope_data[2],
                   imu[i]->accelerometer_data[0],
                   imu[i]->accelerometer_data[1],
                   imu[i]->accelerometer_data[2],
                   imu[i]->Magnetometer[0],
                   imu[i]->Magnetometer[1],
                   imu[i]->Magnetometer[2]
                  );*/
            //myled = 0;
            //wait(0.5);
        }
        printf("\n");
    }
}