Diff: main.cpp

Revision:

25:a8a3cbc57c61

Parent:

24:aaa5b4703555

Child:

27:67b388f6ac2e

--- a/main.cpp	Thu Sep 14 16:41:02 2017 +0000
+++ b/main.cpp	Mon Sep 18 10:42:00 2017 +0000
@@ -5,24 +5,12 @@
 #include "deklaration.h"
 #include "messen.h"
 #include "filter/Kalman.h"
-                          
+
 #define RAD 57.29577951
-
-double dt;
-uint32_t zeit2;
-
-uint8_t k;
-
-
-                        
-             
-                                
+                 
 int main()
 {  
-    gain_g = 0;
     z_off = 0;
-    dt = 0;
-    k = 0;  
     drift_z = 0;
  
     Motor1.period_ms(2);
@@ -31,48 +19,11 @@
     Motor4.period_ms(2);
     initialisierung_gyro();
     initialisierung_acc();
-    Kalman(&Q_angle, &Q_bias, &R_measure, &bias, P);
-    
-    if (taster1)
-    {
-        uint16_t flanke1,hilfe1=0,flanke2,hilfe2=0,flanke3,hilfe3=0,flanke4,hilfe4=0;
-        pc.printf("Taster-Modus aktiv\n\r");
-        n1=n2=n3=n4=625;
-        while(1)
-        {
-            flanke2 = taster2;
-            if ((flanke2 != 0) && (hilfe2 == 0)) 
-            {  
-                n1+=50;
-                n2+=50;
-                n3+=50;
-                n4+=50;
-            }
-            hilfe2=flanke2;
-            flanke3 = taster3;
-            if ((flanke3 != 0) && (hilfe3 == 0)) 
-            {  
-                n1-=50;
-                n2-=50;
-                n3-=50;
-                n4-=50;
-            }
-            hilfe3=flanke3;
-            flanke4 = taster4;
-            if ((flanke4 != 0) && (hilfe4 == 0)) 
-            {  
-                n1=n2=n3=n4=650;
-            }
-            hilfe4=flanke4;
-            Motor1.pulsewidth_us(n1);
-            Motor2.pulsewidth_us(n2);
-            Motor3.pulsewidth_us(n3);
-            Motor4.pulsewidth_us(n4); 
-             
-            pc.printf("Drehzahl=  %d\r= %d",n1);
-        }    
-    }
-    
+    Kalman_pitch();
+    Kalman_yaw();
+    Kalman_roll();
+    aktuell_roh(&z_g, &x_g, &y_g, &z_a, &x_a, &y_a);
+    wait(1);   
     if (taster2)
     {   
         viberationen(&rauschen, &Motor1, &Motor2, &Motor3, &Motor4, &taster4);
@@ -93,93 +44,49 @@
     {
         if (taster1) 
         {
-            while(1)//(!taster4) 
-            {   
-                uint16_t flanke1,hilfe1=0,flanke2,hilfe2=0,flanke3,hilfe3=0,flanke4,hilfe4=0;             
+            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);   
-                
-                /********//******/
-                /*Messen*//*Gyro*/
-                /********//******/   
-                timer.stop();
-                timer2.stop();                
+                             
                 timer.reset();
-                timer2.reset();
-                gain_g = 0;
-                pitch_g = 0;
-                roll_g = 0;
                 timer.start();
-                timer2.start();
                 int i = 0;
                 while(1)
                 {    
                     i++;                   
-                    dt = timer.read_us() * 0.000001;
+                    dt = timer.read_us() * 0.000001;                    //Zeit zwischen zwei Messpunkten
                     timer.reset();
-                    aktuell_roh(&z_g, &x_g, &y_g, &z_a, &x_a, &y_a);
-                    gain_g = ((z_g - z_off) * 1/16.4);
-                    pitch_g = ((y_g  - y_off) * 1/16.4);
-                    roll_g = ((x_g - x_off)  * 1/16.4);
-                    y = y_a / 16384.00;      
-                    x = x_a / 16384.00;      
-                    z = z_a / 16384.00;      
-                    roll_a = atan2(y, sqrt(x * x + z * z)) * RAD;
-                    pitch_a = atan2(-x, z) * RAD;
-                    newAngle = pitch_a;
-                    newRate = pitch_g;
-                    /*if (timer2.read_ms() >= 2000)
-                    {
-                        gain_g -= drift_z;
-                        pitch_g -= drift_y;
-                        roll_g -= drift_x;
-                        timer2.reset();
-                    }*/  
-                    //gain = gain_g;
-                    //pitch = pitch_g * 0.9 + pitch_a * 0.1;
+                    aktuell_roh(&z_g, &x_g, &y_g, &z_a, &x_a, &y_a);    //Rohdaten einlesen
+                
+                    y = y_a / 16384.00;                                 //Umwandlung in G-Kraft
+                    x = x_a / 16384.00;                                 //Umwandlung in G-Kraft
+                    z = z_a / 16384.00;                                 //Umwandlung in G-Kraft
+                    
+                    newAngle_pitch = atan2(-x, z) * RAD;                    //Umwandlung der G-Kraft in °
+                    newRate_pitch = ((y_g  - y_off) * 1/16.4);              //Offset subtrahiert +++ Umwandlung in °/s
                     
-                    pitch = getAngle(&newAngle, &newRate, &dt, &Q_angle, &Q_bias, &R_measure, &bias); 
+                    newAngle_roll = atan2(y, sqrt(x * x + z * z)) * RAD;    //Umwandlung der G-Kraft in °
+                    newRate_roll = ((x_g - x_off)  * 1/16.4);               //Offset subtrahiert +++ Umwandlung in °/s
+                    
+                    newAngle_yaw = ((z_g - z_off) * 1/16.4);                //Umwandlung der G-Kraft in °
+                    newRate_yaw = ((z_g - z_off) * 1/16.4);                 //Offset subtrahiert +++ Umwandlung in °/s
+
+                    pitch = getPitch(&newAngle_pitch, &newRate_pitch, &dt);
+                    yaw = getYaw(&newAngle_yaw, &newRate_yaw, &dt);
+                    roll = getRoll(&newAngle_roll, &newRate_roll, &dt);
+                    
                     if (i == 2000)
                     {
-                        pc.printf("roll: %2.5f°\tnewAngle: %2.5f°\tnewRate: %2.5f°/s\tdt: %2.5fus\tDrehzahl: %d\n\r",pitch, newAngle, newRate, dt*1000000,n1);
+                        pc.printf(" roll: %2.5f\t yaw: %2.5f\t pitch: %2.5f\t rohgyro: %2.5f\n\r",roll, yaw, pitch, newRate_pitch);
                         i = 0;
                     }
-                    flanke2 = taster2;
-                    if ((flanke2 != 0) && (hilfe2 == 0)) 
-                    {  
-                        n1+=50;
-                        n2+=50;
-                        n3+=50;
-                        n4+=50;
-                    }
-                    hilfe2=flanke2;
-                    flanke3 = taster3;
-                    if ((flanke3 != 0) && (hilfe3 == 0)) 
-                    {  
-                        n1-=50;
-                        n2-=50;
-                        n3-=50;
-                        n4-=50;
-                    }
-                    hilfe3=flanke3;
-                    flanke4 = taster4;
-                    if ((flanke4 != 0) && (hilfe4 == 0)) 
-                    {  
-                        n1=n2=n3=n4=650;
-                    }
-                    hilfe4=flanke4;
-                    Motor1.pulsewidth_us(n1);
-                    Motor2.pulsewidth_us(n2);
-                    Motor3.pulsewidth_us(n3);
-                    Motor4.pulsewidth_us(n4);
-                            
-                     
-                       
+                    Motorsteurung(&Motor1, &Motor2, &Motor3, &Motor4, &taster2, &taster3, &taster4, &n1, &n2, &n3, &n4);   
                 }
             }
         }
     }
-}
\ No newline at end of file
+}