Diff: main.cpp

Revision:

4:7c9469bb52f4

Parent:

3:02877f5ca29e

--- a/main.cpp	Thu Mar 09 13:14:31 2017 +0000
+++ b/main.cpp	Mon May 22 18:45:18 2017 +0000
@@ -29,53 +29,55 @@
 
 kalman filter_roll;
 kalman filter_pitch;
-kalman filter_yaw; 
-    
-int main() {
-    pc.baud(115200);
-  
+kalman filter_yaw;
+
+int main()
+{
+    pc.baud(9600);
+
 //    GlobalTime.start();
 
-    // Parameters ( R_angle, Q_gyro, Q_angle ) 
-    kalman_init(&filter_pitch, R_matrix, Q_Gyro_matrix, Q_Accel_matrix); 
-    kalman_init(&filter_roll, R_matrix, Q_Gyro_matrix, Q_Accel_matrix); 
-    
+    // Parameters ( R_angle, Q_gyro, Q_angle )
+    kalman_init(&filter_pitch, R_matrix, Q_Gyro_matrix, Q_Accel_matrix);
+    kalman_init(&filter_roll, R_matrix, Q_Gyro_matrix, Q_Accel_matrix);
+
     kalman_init(&filter_yaw, R_matrix, Q_Gyro_matrix, Q_Accel_matrix);
-    
-    
+
+
     ProgramTimer.start();
     loopStartTime = ProgramTimer.read_us();
     timer = loopStartTime;
-    
+
     while(1) {
 //    wait_ms(10);
         //Aquire new values for the Gyro and Acc
-          accelerometer->Get_X_Axes(axesAcc);
-          gyroscope->Get_G_Axes(axesGyro);
-          magnetometer->Get_M_Axes(axesMag);
-        
+        accelerometer->Get_X_Axes(axesAcc);
+        gyroscope->Get_G_Axes(axesGyro);
+        magnetometer->Get_M_Axes(axesMag);
+
         //Calculate the resulting vector R from the 3 acc axes
         R = sqrt(std::pow((float)axesAcc[0] , 2) + std::pow((float)axesAcc[1] , 2) + std::pow((float)axesAcc[2] , 2));
-   
-        kalman_predict(&filter_roll, axesGyro[0],  (ProgramTimer.read_us() - timer)); 
+
+        kalman_predict(&filter_roll, axesGyro[0],  (ProgramTimer.read_us() - timer));
         kalman_update(&filter_roll, acos(axesAcc[0]/R));
-        
-        kalman_predict(&filter_pitch, axesGyro[1],  (ProgramTimer.read_us() - timer)); 
+
+        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_predict(&filter_yaw, axesGyro[2],  (ProgramTimer.read_us() - timer));
         kalman_update(&filter_yaw, acos(axesMag[2]/R));
-        
-        
+
+
         angle[0] = kalman_get_angle(&filter_roll);
         angle[1] = kalman_get_angle(&filter_pitch);
-        angle[2] = atan2((double)axesGyro[0],(double)axesGyro[1])*Rad2Dree;
-//        angle[2] = kalman_get_angle(&filter_yaw);
-        atan2((double)axesAcc[0],(double)axesAcc[1])*Rad2Dree; 
-                
+        //angle[2] = atan2((double)axesGyro[0],(double)axesGyro[1])*Rad2Dree;
+        angle[2] = kalman_get_angle(&filter_yaw);
+        atan2((double)axesAcc[0],(double)axesAcc[1])*Rad2Dree;
+
         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("%.6f ;\t  %.6f;\t %.6f; \n\r", Rad2Dree * angle[0], Rad2Dree * angle[1], Rad2Dree * angle[2]);
+
     }
 }