Diff: main.cpp

Revision:

20:76e25a3c8684

Parent:

19:ac98ad146067

Child:

21:f09823a13ac6

--- a/main.cpp	Mon Sep 11 11:07:54 2017 +0000
+++ b/main.cpp	Mon Sep 11 12:39:27 2017 +0000
@@ -8,7 +8,7 @@
                           
 #define RAD 57.29577951
 
-uint16_t zeit;
+uint16_t dt;
 uint32_t zeit2;
 
 uint8_t k;
@@ -21,7 +21,7 @@
 {  
     gain_g = 0;
     z_off = 0;
-    zeit = 0;
+    dt = 0;
     k = 0;  
     drift_z = 0;
  
@@ -111,7 +111,7 @@
                 while(1)
                 {    
                     i++;                   
-                    zeit = timer.read_us();
+                    dt = timer.read_us();
                     timer.reset();
                     aktuell_roh(&z_g, &x_g, &y_g, &z_a, &x_a, &y_a);
                     gain_g = gain_g + ((z_g - z_off) * 1/16.4);
@@ -122,7 +122,8 @@
                     z = z_a / 16384.00;      
                     roll_a = atan2(y, sqrt(x * x + z * z)) * RAD;
                     pitch_a = atan2(-x, z) * RAD;
-                    
+                    newAngle = roll_a;
+                    newRate = roll_g;
                     if (timer2.read_ms() >= 2000)
                     {
                         gain_g -= drift_z;
@@ -130,12 +131,12 @@
                         roll_g -= drift_x;
                         timer2.reset();
                     }  
-                    gain = gain_g;
-                    pitch = pitch_g * 0.9 + pitch_a * 0.1;
-                    roll = roll_g * 0.9 + roll_a * 0.1;
+                    //gain = gain_g;
+                    //pitch = pitch_g * 0.9 + pitch_a * 0.1;
+                    roll = getAngle(&newAngle, &newRate, &dt, &Q_angle, &Q_bias, &R_measure, &bias); 
                     if (i == 2000)
                     {
-                        pc.printf("gain: %2.5f\tpitch: %2.5f\troll: %2.5f\t\n\r",pitch, roll, gain);
+                        pc.printf("gain: %2.5f\tpitch: %2.5f\troll: %2.5f\t\n\r",angle);
                         i = 0;
                     }