Diff: main.cpp

Revision:

10:f2ef74678956

Parent:

9:e700b2d586d6

--- a/main.cpp	Fri Jun 17 06:07:57 2016 +0000
+++ b/main.cpp	Wed Apr 26 07:52:10 2017 +0000
@@ -10,6 +10,7 @@
 
 #include "mbed.h"
 #include "MPU9250.h"
+#include "AHRS/MadgwickAHRS.h"
 
 /*
     MOSI (Master Out Slave In)  p5
@@ -20,65 +21,161 @@
 
 //  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);
+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];
+//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(1);
+        wait(0.1);
         printf("Gyro_scale=%u\n",imu[i]->set_gyro_scale(BITS_FS_2000DPS));    //Set full scale range for gyros
-        wait(1);
+        wait(0.1);
         printf("Acc_scale=%u\n",imu[i]->set_acc_scale(BITS_FS_16G));          //Set full scale range for accs
-        wait(1);
+        wait(0.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;
+void eventFunc(void)
+{   
+    count--;
+    
+    for(int i=0; i<2; i++) {
+        
+        imu[0]->deselect();
+        imu[1]->deselect();
         
-        //wait_us(1);
+        imu[i]->select();
+        imu[i]->read_acc();
+        imu[i]->read_rot();
         
-        for(int i=0; i<2; i++) {
+        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 ;
             
-            imu[0]->deselect();
-            imu[1]->deselect();
+        if (count == 0){
+            thetaOffset[i][0] = x;
+            thetaOffset[i][1] = y;
+            thetaOffset[i][2] = z;
             
-            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]
+              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 ",
@@ -95,7 +192,5 @@
                   );*/
             //myled = 0;
             //wait(0.5);
-        }
-        printf("\n");
     }
 }