main.cpp

00001 /**
00002  * Masahiro FURUKAWA - m.furukawa@ist.osaka-u.ac.jp
00003  *
00004  * June 17, 2016
00005  *
00006  * MPU9250 9DoF Sensor (Extended to Ch1 ~ Ch2)
00007  *
00008  **/
00009 
00010 
00011 #include "mbed.h"
00012 #include "MPU9250.h"
00013 #include "AHRS/MadgwickAHRS.h"
00014 
00015 /*
00016     MOSI (Master Out Slave In)  p5
00017     MISO (Master In Slave Out   p6
00018     SCK  (Serial Clock)         p7
00019     ~CS  (Chip Select)          p8
00020 */
00021 
00022 //  https://developer.mbed.org/users/kylongmu/code/MPU9250_SPI_Test/file/5839d1b118bc/main.cpp
00023 
00024 
00025 class KalmanFilter {
00026   private:
00027       float P, K, xhat, Q, R;  
00028   public:
00029     KalmanFilter();
00030     KalmanFilter(float _Q, float _R);
00031     float update(float obs);
00032   
00033 };
00034 KalmanFilter::KalmanFilter(){
00035     P = 0.0;   // Convariance Matrix 
00036     K = 0.0;   // Kalman Gain
00037     xhat = 0.0;// Initial Predicted Value
00038     Q = 1e-3;  // Error     
00039     R = 0.01;
00040   }
00041   
00042   // override
00043 KalmanFilter::KalmanFilter(float _Q, float _R){
00044     P = 0.0;   // Convariance Matrix 
00045     K = 0.0;   // Kalman Gain
00046     xhat = 0.0;// Initial Predicted Value
00047     Q = _Q;    // Error     
00048     R = _R;
00049   }
00050   
00051 float KalmanFilter::update(float obs){
00052     // Predict
00053     float xhat_m = xhat ;   // xhat[k-1]
00054     float P_m    = P + Q;   // P[k-1]
00055   
00056     // Update
00057     float S = P_m + R;      // Remained Error
00058     K = P_m / S;            // Update Kalman Gain
00059     xhat = xhat_m + K * (obs - xhat_m); // predicted value
00060     P = (1 - K) * P_m;      // Convariance Matrix of Error BTW True Value and Predicted True Value 
00061   
00062     return xhat;
00063 }
00064 
00065 //define the mpu9250 object
00066 mpu9250_spi *imu[2];
00067 Serial pc(USBTX, USBRX);
00068 SPI spi(p5, p6, p7);
00069 KalmanFilter *kf[12];
00070 Ticker ticker;
00071 float theta[2][3],thetaOffset[2][3],x,y,z;
00072 
00073 // Calibration wait
00074 int count = 100;
00075 
00076 // Sampling Term
00077 float smplT = 10; //ms
00078 
00079 void init(void){
00080 
00081     theta[0][0] = 0;
00082     theta[0][1] = 0;
00083     theta[0][2] = 0;
00084     theta[1][0] = 0;
00085     theta[1][1] = 0;
00086     theta[1][2] = 0;
00087     thetaOffset[0][0] = 0;
00088     thetaOffset[0][1] = 0;
00089     thetaOffset[0][2] = 0;
00090     thetaOffset[1][0] = 0;
00091     thetaOffset[1][1] = 0;
00092     thetaOffset[1][2] = 0;
00093     
00094     pc.baud(115200);
00095     
00096     imu[0] = new mpu9250_spi(spi, p8);
00097     imu[1] = new mpu9250_spi(spi, p9);
00098     
00099     for(int i=0; i<12; i++)     
00100         kf[i] = new KalmanFilter(1e-3, 0.001);
00101         
00102     for(int i=0; i<2; i++) {
00103     
00104         imu[0]->deselect();
00105         imu[1]->deselect();
00106         imu[i]->select();
00107 
00108         if(imu[i]->init(1,BITS_DLPF_CFG_188HZ)) { //INIT the mpu9250
00109             printf("\nCouldn't initialize MPU9250 via SPI!");
00110             wait(90);
00111         }
00112         printf("\nWHOAMI=0x%2x\n",imu[i]->whoami()); //output the I2C address to know if SPI is working, it should be 104
00113         wait(0.1);
00114         printf("Gyro_scale=%u\n",imu[i]->set_gyro_scale(BITS_FS_2000DPS));    //Set full scale range for gyros
00115         wait(0.1);
00116         printf("Acc_scale=%u\n",imu[i]->set_acc_scale(BITS_FS_16G));          //Set full scale range for accs
00117         wait(0.1);
00118         printf("AK8963 WHIAM=0x%2x\n",imu[i]->AK8963_whoami());
00119         wait(0.1);
00120         imu[i]->AK8963_calib_Magnetometer();
00121         wait(0.1);
00122     }
00123     
00124 }
00125 
00126 void eventFunc(void)
00127 {   
00128     count--;
00129     
00130     for(int i=0; i<2; i++) {
00131         
00132         imu[0]->deselect();
00133         imu[1]->deselect();
00134         
00135         imu[i]->select();
00136         imu[i]->read_acc();
00137         imu[i]->read_rot();
00138         
00139         x = kf[i*6  ]->update(imu[i]->gyroscope_data[0]) - thetaOffset[i][0];
00140         y = kf[i*6+1]->update(imu[i]->gyroscope_data[1]) - thetaOffset[i][1];
00141         z = kf[i*6+2]->update(imu[i]->gyroscope_data[2]) - thetaOffset[i][2];
00142         theta[i][0] += x * smplT / 1000 ; // x(n) = x(n-1) + dx*dt
00143         theta[i][1] += y * smplT / 1000 ;
00144         theta[i][2] += z * smplT / 1000 ;
00145             
00146         if (count == 0){
00147             thetaOffset[i][0] = x;
00148             thetaOffset[i][1] = y;
00149             thetaOffset[i][2] = z;
00150             
00151               theta[i][0] = 0; 
00152               theta[i][1] = 0;
00153               theta[i][2] = 0;         
00154         }
00155         if(count < 0)
00156         
00157             printf("%10.3f %10.3f %10.3f %10.3f %10.3f %10.3f %10.3f %10.3f %10.3f ",
00158                    x,y,z,
00159                    kf[i*6+3]->update(imu[i]->accelerometer_data[0]),
00160                    kf[i*6+4]->update(imu[i]->accelerometer_data[1]),
00161                    kf[i*6+5]->update(imu[i]->accelerometer_data[2]),
00162                    theta[i][0],
00163                    theta[i][1],
00164                    theta[i][2]
00165                   );
00166   
00167     }
00168     printf("\n");
00169 }
00170 int main()
00171 {
00172 
00173     init();
00174     
00175     ticker.attach_us(eventFunc, smplT * 1000); // 10ms = 100Hz
00176     
00177     while(1) {
00178         
00179             /*
00180             imu[i]->read_all();
00181             printf("%10.3f,%10.3f,%10.3f,%10.3f,%10.3f,%10.3f,%10.3f,%10.3f,%10.3f,%10.3f ",
00182                    imu[i]->Temperature,
00183                    imu[i]->gyroscope_data[0],
00184                    imu[i]->gyroscope_data[1],
00185                    imu[i]->gyroscope_data[2],
00186                    imu[i]->accelerometer_data[0],
00187                    imu[i]->accelerometer_data[1],
00188                    imu[i]->accelerometer_data[2],
00189                    imu[i]->Magnetometer[0],
00190                    imu[i]->Magnetometer[1],
00191                    imu[i]->Magnetometer[2]
00192                   );*/
00193             //myled = 0;
00194             //wait(0.5);
00195     }
00196 }