Diff: main.cpp

Revision:

2:cde1397d3fc4

Parent:

1:39129aaf5c80

--- a/main.cpp	Wed Mar 08 09:03:52 2017 +0000
+++ b/main.cpp	Wed Mar 08 10:11:54 2017 +0000
@@ -9,6 +9,7 @@
 /* Instantiate the expansion board */
 static X_NUCLEO_IKS01A1 *mems_expansion_board = X_NUCLEO_IKS01A1::Instance(D14, D15);
 
+
 /* Retrieve the composing elements of the expansion board */
 static GyroSensor *gyroscope = mems_expansion_board->GetGyroscope();
 static MotionSensor *accelerometer = mems_expansion_board->GetAccelerometer();
@@ -31,7 +32,6 @@
 kalman filter_yaw; 
     
 int main() {
-//    int count = 0;
     pc.baud(115200);
   
 //    GlobalTime.start();
@@ -59,17 +59,14 @@
         kalman_update(&filter_roll, acos(axesAcc[0]/R));
         kalman_predict(&filter_pitch, axesGyro[1],  (ProgramTimer.read_us() - timer)); 
         kalman_update(&filter_pitch, acos(axesAcc[1]/R));
-        kalman_predict(&filter_yaw, axesGyro[2],  (ProgramTimer.read_us() - timer)); 
-        kalman_update(&filter_yaw, acos(axesAcc[2]/R));
-        
+                
         
-        angle[0] = kalman_get_angle(&filter_roll);
-        angle[1] = kalman_get_angle(&filter_pitch);
-        angle[2] = kalman_get_angle(&filter_yaw);
-                
+        angle[0] = (kalman_get_angle(&filter_roll)*Rad2Dree)-90;
+        angle[1] = -(kalman_get_angle(&filter_pitch)*Rad2Dree)+90;
+                        
         timer = ProgramTimer.read_us();
         
-        pc.printf("Roll Angle X: %.6f   Pitch Angle Y: %.6f Yaw Angle Z: %.6f \n\r", Rad2Dree * angle[0], Rad2Dree * angle[1], Rad2Dree * angle[2]);
+        pc.printf("Roll Angle X: %.6f   Pitch Angle Y: %.6f \n\r", angle[0], angle[1]);
         
     }
 }