Diff: main.cpp

Revision:

29:3efe34986347

Parent:

28:f9349474a553

Child:

30:dc68b509f930

--- a/main.cpp	Thu Sep 21 16:15:45 2017 +0000
+++ b/main.cpp	Mon Sep 25 11:04:53 2017 +0000
@@ -6,12 +6,19 @@
 #include "messen.h"
 #include "filter/Kalman.h"
 
+double gyro_pitch;
+double gyro_yaw;
+double gyro_roll;
+
 #define RAD 57.29577951
                  
 int main()
 {  
     z_off = 0;
     drift_z = 0;
+    gyro_pitch = 0;
+    gyro_yaw = 0;
+    gyro_roll = 0;
  
     Motor1.period_ms(2);
     Motor2.period_ms(2);
@@ -92,13 +99,15 @@
         }
         if (taster2) 
         {
+            pc.printf("\n\rOffset und Driftberechnung wird durchgefuehrt, halte die Drohne still");
+                printf("\n\rpitch, yaw, roll, newAngle_pitch, newAngle_´roll, newRate_pitch, newRate_yaw, newRate_roll, n2\n\r"); 
             while(1) 
             {             
-                pc.printf("\n\rOffset und Driftberechnung wird durchgefuehrt, halte die Drohne still");
+                
                 offset_gyro(&z_off, &x_off, &y_off);             
                 //drift_gyro(&drift_z, &drift_x, &drift_y, &timer, &timer2, &gain_g, &roll_g, &pitch_g, &z_off, &x_off, &y_off);
-                pc.printf("\n\rOffgesamt:\n\rZ = %3.5f\tY = %3.5f\tZ = %3.5f\t\n\r", z_off, x_off, y_off);  
-                pc.printf("\n\rDrift:Z: %3.10f\tX: %3.10f\tY: %3.10f\n\r", drift_z, drift_x, drift_y);   
+                //pc.printf("\n\rOffgesamt:\n\rZ = %3.5f\tY = %3.5f\tZ = %3.5f\t\n\r", z_off, x_off, y_off);  
+                //pc.printf("\n\rDrift:Z: %3.10f\tX: %3.10f\tY: %3.10f\n\r", drift_z, drift_x, drift_y);   
                              
                 timer.reset();
                 timer.start();
@@ -129,12 +138,16 @@
                     yaw = getYaw(&newAngle_yaw, &newRate_yaw, &dt);
                     roll = getRoll(&newAngle_roll, &newRate_roll, &dt);
                     
-                    if (i == 1000)
+                    gyro_pitch += dt * newRate_pitch;
+                    gyro_yaw += dt * newRate_yaw;
+                    gyro_roll += dt * newRate_roll;
+                    
+                    if (i == 500)
                     {
-                        printf("%f2.3 \t%f2.3 \t%f2.3 \t%f2.3 \t%f2.3 \t%f2.3 \t%fd \t", pitch, yaw, roll, newAngle_pitch, newRate_pitch, newAngle_roll, newRate_roll, newAngle_yaw, n1);
+                        printf(" %f3.2 \t\t %f3.2 \t\t %f3.2 \t\t %f3.2 \t\t %f3.2 \t\t %f3.2 \t\t %f3.2 \t\t %f3.2 \t\t %d\n\r", pitch, yaw, roll, newAngle_pitch, newAngle_´roll, newRate_pitch, newRate_yaw, newRate_roll, n2); 
                         i = 0;
                     }
-                    if (timer2.read_ms() >= 2500)
+                    if (timer2.read_ms() >= 5000)
                     {
                             n1+=200;
                             n2+=200;