File content as of revision 10:f2ef74678956:

/**
 * 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"
#include "AHRS/MadgwickAHRS.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


class KalmanFilter {
  private:
      float P, K, xhat, Q, R;  
  public:
    KalmanFilter();
    KalmanFilter(float _Q, float _R);
    float update(float obs);
  
};
KalmanFilter::KalmanFilter(){
    P = 0.0;   // Convariance Matrix 
    K = 0.0;   // Kalman Gain
    xhat = 0.0;// Initial Predicted Value
    Q = 1e-3;  // Error     
    R = 0.01;
  }
  
  // override
KalmanFilter::KalmanFilter(float _Q, float _R){
    P = 0.0;   // Convariance Matrix 
    K = 0.0;   // Kalman Gain
    xhat = 0.0;// Initial Predicted Value
    Q = _Q;    // Error     
    R = _R;
  }
  
float KalmanFilter::update(float obs){
    // Predict
    float xhat_m = xhat ;   // xhat[k-1]
    float P_m    = P + Q;   // P[k-1]
  
    // Update
    float S = P_m + R;      // Remained Error
    K = P_m / S;            // Update Kalman Gain
    xhat = xhat_m + K * (obs - xhat_m); // predicted value
    P = (1 - K) * P_m;      // Convariance Matrix of Error BTW True Value and Predicted True Value 
  
    return xhat;
}

//define the mpu9250 object
mpu9250_spi *imu[2];
Serial pc(USBTX, USBRX);
SPI spi(p5, p6, p7);
KalmanFilter *kf[12];
Ticker ticker;
float theta[2][3],thetaOffset[2][3],x,y,z;

// Calibration wait
int count = 100;

// Sampling Term
float smplT = 10; //ms

void init(void){

    theta[0][0] = 0;
    theta[0][1] = 0;
    theta[0][2] = 0;
    theta[1][0] = 0;
    theta[1][1] = 0;
    theta[1][2] = 0;
    thetaOffset[0][0] = 0;
    thetaOffset[0][1] = 0;
    thetaOffset[0][2] = 0;
    thetaOffset[1][0] = 0;
    thetaOffset[1][1] = 0;
    thetaOffset[1][2] = 0;
    
    pc.baud(115200);
    
    imu[0] = new mpu9250_spi(spi, p8);
    imu[1] = new mpu9250_spi(spi, p9);
    
    for(int i=0; i<12; i++)     
        kf[i] = new KalmanFilter(1e-3, 0.001);
        
    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!");
            wait(90);
        }
        printf("\nWHOAMI=0x%2x\n",imu[i]->whoami()); //output the I2C address to know if SPI is working, it should be 104
        wait(0.1);
        printf("Gyro_scale=%u\n",imu[i]->set_gyro_scale(BITS_FS_2000DPS));    //Set full scale range for gyros
        wait(0.1);
        printf("Acc_scale=%u\n",imu[i]->set_acc_scale(BITS_FS_16G));          //Set full scale range for accs
        wait(0.1);
        printf("AK8963 WHIAM=0x%2x\n",imu[i]->AK8963_whoami());
        wait(0.1);
        imu[i]->AK8963_calib_Magnetometer();
        wait(0.1);
    }
    
}

void eventFunc(void)
{   
    count--;
    
    for(int i=0; i<2; i++) {
        
        imu[0]->deselect();
        imu[1]->deselect();
        
        imu[i]->select();
        imu[i]->read_acc();
        imu[i]->read_rot();
        
        x = kf[i*6  ]->update(imu[i]->gyroscope_data[0]) - thetaOffset[i][0];
        y = kf[i*6+1]->update(imu[i]->gyroscope_data[1]) - thetaOffset[i][1];
        z = kf[i*6+2]->update(imu[i]->gyroscope_data[2]) - thetaOffset[i][2];
        theta[i][0] += x * smplT / 1000 ; // x(n) = x(n-1) + dx*dt
        theta[i][1] += y * smplT / 1000 ;
        theta[i][2] += z * smplT / 1000 ;
            
        if (count == 0){
            thetaOffset[i][0] = x;
            thetaOffset[i][1] = y;
            thetaOffset[i][2] = z;
            
              theta[i][0] = 0; 
              theta[i][1] = 0;
              theta[i][2] = 0;         
        }
        if(count < 0)
        
            printf("%10.3f %10.3f %10.3f %10.3f %10.3f %10.3f %10.3f %10.3f %10.3f ",
                   x,y,z,
                   kf[i*6+3]->update(imu[i]->accelerometer_data[0]),
                   kf[i*6+4]->update(imu[i]->accelerometer_data[1]),
                   kf[i*6+5]->update(imu[i]->accelerometer_data[2]),
                   theta[i][0],
                   theta[i][1],
                   theta[i][2]
                  );
  
    }
    printf("\n");
}
int main()
{

    init();
    
    ticker.attach_us(eventFunc, smplT * 1000); // 10ms = 100Hz
    
    while(1) {
        
            /*
            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);
    }
}