Diff: main.cpp

Revision:

27:67b388f6ac2e

Parent:

25:a8a3cbc57c61

Child:

28:f9349474a553

--- a/main.cpp	Mon Sep 18 10:53:51 2017 +0000
+++ b/main.cpp	Thu Sep 21 16:05:54 2017 +0000
@@ -33,7 +33,7 @@
         anlernen(&Motor1, &Motor2, &Motor3, &Motor4, &taster1, &taster2, &taster4);
     }
     
-    pc.printf("Druecke Taster1 fuer den Start\n\r");   
+    pc.printf("Druecke Taster1 fuer den Start und Taster2 fuers rauschen\n\r");   
     n1=n2=n3=n4=700;
     Motor1.pulsewidth_us(n1);
     Motor2.pulsewidth_us(n2);
@@ -54,6 +54,8 @@
                              
                 timer.reset();
                 timer.start();
+                timer2.reset();
+                timer2.start();
                 int i = 0;
                 while(1)
                 {    
@@ -79,14 +81,74 @@
                     yaw = getYaw(&newAngle_yaw, &newRate_yaw, &dt);
                     roll = getRoll(&newAngle_roll, &newRate_roll, &dt);
                     
-                    if (i == 2000)
+                    if (i == 1000)
                     {
-                        pc.printf(" roll: %2.5f\t yaw: %2.5f\t pitch: %2.5f\t rohgyro: %2.5f\n\r",roll, yaw, pitch, newRate_pitch);
+                        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);
                         i = 0;
                     }
                     Motorsteurung(&Motor1, &Motor2, &Motor3, &Motor4, &taster2, &taster3, &taster4, &n1, &n2, &n3, &n4);   
                 }
             }
         }
+        if (taster2) 
+        {
+            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);   
+                             
+                timer.reset();
+                timer.start();
+                timer2.reset();
+                timer2.start();
+                int i = 0;
+                while(1)
+                {    
+                    i++;                   
+                    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);    //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
+                    
+                    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 == 1000)
+                    {
+                        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);
+                        i = 0;
+                    }
+                    if (timer2.read_ms() >= 2500)
+                    {
+                            n1+=200;
+                            n2+=200;
+                            n3+=200;
+                            n4+=200;
+                            Motor1.pulsewidth_us(n1);
+                            Motor2.pulsewidth_us(n2);
+                            Motor3.pulsewidth_us(n3);
+                            Motor4.pulsewidth_us(n4);
+                            timer2.reset();
+                    }
+                    Motorsteurung(&Motor1, &Motor2, &Motor3, &Motor4, &taster2, &taster3, &taster4, &n1, &n2, &n3, &n4);   
+                }
+            }
+        }
     }
 }