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:
6:ea0804dc7cae
Parent:
5:abfc7660fde9
Child:
7:758a94e02aa7

File content as of revision 6:ea0804dc7cae:

/**
 * 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);
  
    int i=0;
  
     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();
        
        while(1) {
            //myled = 1;
            wait(0.1);
            /*
            imu[i]->read_temp();
            imu[i]->read_acc();
            imu[i]->read_rot();
            imu[i]->AK8963_read_Magnetometer();
            */
            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\n", 
                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);
        }
        
    /*
    uint64_t xyzt[6];
    short xi[6],yi[6],zi[6];
    
    while(1) {
        
        
        for (int i=0; i<6; i++) 
            xyzt[i] = adxl362[i]->scan();
            
        for (int i=0; i<6; i++) {
            uint16_t x = static_cast<uint16_t>(0xFFFF&(xyzt[i]>>48));
            uint16_t y = static_cast<uint16_t>(0xFFFF&(xyzt[i]>>32));
            uint16_t z = static_cast<uint16_t>(0xFFFF&(xyzt[i]>>16));
            
            if(x & 1<<16) xi[i] = -1 * static_cast<short>(~x) + 1; else xi[i] = x;
            if(y & 1<<16) yi[i] = -1 * static_cast<short>(~y) + 1; else yi[i] = y;
            if(z & 1<<16) zi[i] = -1 * static_cast<short>(~z) + 1; else zi[i] = z;
        }
        for (int i=0; i<6; i++) {            
            printf("%d %d %d ", xi[i], yi[i], zi[i]);
        }
        printf("\r\n");
        wait_us(1);
    }
    */
}