Diff: main.cpp

Revision:

3:58ebc00f1a68

Parent:

2:4bc4e25328cc

Child:

4:1bc3ca07a412

--- a/main.cpp	Mon Aug 31 23:37:07 2015 +0000
+++ b/main.cpp	Tue Sep 01 18:03:27 2015 +0000
@@ -19,15 +19,18 @@
 #include "mbed.h"
 #include "FXOS8700Q.h"
 #include "FXAS21000.h"
-
+#include "kalman.c"
 
-//FXOS8700Q_acc combo_acc(D14, D15, FXOS8700CQ_SLAVE_ADDR0);
-//FXOS8700Q_mag combo_mag(D14, D15, FXOS8700CQ_SLAVE_ADDR0);
+#define PI             3.1415926535897932384626433832795
+#define Rad2Dree       57.295779513082320876798154814105
+
 FXOS8700Q_acc combo_acc(A5, A4, FXOS8700CQ_SLAVE_ADDR0);
 FXOS8700Q_mag combo_mag(A5, A4, FXOS8700CQ_SLAVE_ADDR0);
-//FXAS21000 gyro(D14, D15);
 FXAS21000 gyro(A5, A4);
 
+Timer GlobalTime;
+Timer ProgramTimer;
+
 //PwmOut M1(PTB18);
 PwmOut M1(D13);
 PwmOut M2(D12);
@@ -36,6 +39,14 @@
 
 Serial pc(USBTX, USBRX);
 
+kalman filter_pitch; 
+kalman filter_roll;
+
+float R;
+double angle[3];
+unsigned long timer;
+long loopStartTime;
+
 int main()
 {
     float gyro_data[3];
@@ -44,6 +55,8 @@
     
     printf("\r\nStarting\r\n\r\n");
     
+    GlobalTime.start();
+    
     M1.period(0.02f);               //Comparten el mismo timer
     M1.pulsewidth(0.0006f);
     M2.pulsewidth(0.0006f);
@@ -55,18 +68,36 @@
     printf("FXOS8700 Combo = %X\r\n", combo_acc.whoAmI());
     printf("FXAS21000 Gyro = %X\r\n", gyro.getWhoAmI());
     
+    kalman_init(&filter_pitch, R_matrix, Q_Gyro_matrix, Q_Accel_matrix); 
+    kalman_init(&filter_roll, R_matrix, Q_Gyro_matrix, Q_Accel_matrix);
+    
     wait(3);
     
+    ProgramTimer.start();
+    loopStartTime = ProgramTimer.read_us();
+    timer = loopStartTime;
+    
     while(1) {
         combo_acc.getAxis(adata);
-        printf("FXOS8700 Acc:   X:%6.3f Y:%6.3f Z:%6.3f\r\n", adata.x, adata.y, adata.z);
+        combo_mag.getAxis(mdata);
+        gyro.ReadXYZ(gyro_data);
+        
+        R = sqrt(std::pow(adata.x, 2) + std::pow(adata.y, 2) + std::pow(adata.z, 2));
         
-        combo_mag.getAxis(mdata);
+        kalman_predict(&filter_pitch, gyro_data[0],  (ProgramTimer.read_us() - timer)); 
+        kalman_update(&filter_pitch, acos(adata.x/R));
+        kalman_predict(&filter_roll, gyro_data[1],  (ProgramTimer.read_us() - timer)); 
+        kalman_update(&filter_roll, acos(adata.y/R));
+        
+        angle[0] = kalman_get_angle(&filter_pitch);
+        angle[1] = kalman_get_angle(&filter_roll);
+        
+        timer = ProgramTimer.read_us();
+    
+        printf("FXOS8700 Acc:   X:%6.3f Y:%6.3f Z:%6.3f\r\n", adata.x, adata.y, adata.z);
         printf("FXOS8700 Mag:   X:%6.2f Y:%6.2f Z:%6.2f\r\n", mdata.x, mdata.y, mdata.z);
-
-        gyro.ReadXYZ(gyro_data);
         printf("FXAS21000 Gyro: X:%6.2f Y:%6.2f Z:%6.2f\r\n", gyro_data[0], gyro_data[1], gyro_data[2]);
-    
+        printf("Roll Angle X: %.6f   Pitch Angle Y: %.6f \n\r", Rad2Dree * angle[1], Rad2Dree * angle[0]);
         printf("\r\n");
         
         wait(0.5);