Diff: main.cpp

Revision:

31:adfa5a816488

Parent:

30:dc68b509f930

--- a/main.cpp	Thu Oct 05 18:02:40 2017 +0000
+++ b/main.cpp	Mon Oct 09 08:57:22 2017 +0000
@@ -10,6 +10,20 @@
 double gyro_yaw;
 double gyro_roll;
 
+double e_pitch = 0;
+double w_pitch = 0;
+
+double yalt_pitch = 0;
+double ealt_pitch = 0;
+double ealt2_pitch = 0;
+double q0_pitch = 0.1;        //Kp + Ki * Ta + Kd/Ta
+double q1_pitch  = 0;       //-Ki - 2 * Kd/Ta
+double q2_pitch = 0;        //Kd/Ta
+
+
+
+
+
 #define RAD 57.29577951
                  
 int main()
@@ -172,5 +186,87 @@
                 }
             }
         }
+        if (taster3)    //Start Regelung
+        {
+            pc.printf("\n\rOffset und Driftberechnung wird durchgefuehrt, halte die Drohne still");
+                printf("\n\rpitch, yaw, roll, newAngle_pitch, newAngle_roll, gyro_pitch, gyro_yaw, gyro_roll, n2\n\r"); 
+            while(1) 
+            {             
+                
+                offset_gyro(&z_off, &x_off, &y_off);
+                
+                int i = 0;
+                             
+                timer.reset();
+                timer.start();
+                timer2.reset();
+                timer2.start();
+                
+                n1 = n2 = n3 = n4 = 1200;
+                Motor1.pulsewidth_us(n1);
+                Motor2.pulsewidth_us(n2);
+                Motor3.pulsewidth_us(n3);
+                Motor4.pulsewidth_us(n4);
+                
+                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);
+                    
+                    e_pitch = w_pitch - pitch;
+                    y_pitch = yalt_pitch + q0_pitch * e_pitch + q1_pitch * ealt_pitch + q2_pitch * ealt2_pitch;
+                    ealt2_pitch = ealt_pitch;
+                    ealt_pitch = e_pitch;
+                    yalt_pitch = y_pitch;
+                    
+                    n1 = n4 = y_pitch * 15.5 + 1200;
+                    n2 = n3 = y_pitch * -15.5 + 1200;
+                    
+                    if (i == 500)
+                    {
+                        printf("%3.2f\t%d\t%d\t%3.2f\t\n\r", pitch, n1, n2, q0_pitch); 
+                        i = 0;
+                    }
+                    
+                    if (taster1)
+                    {
+                           q0_pitch += 0.1;
+                    }
+                    if (taster2)
+                    {
+                           q0_pitch -= 0.1;
+                    }
+                    if (taster3)
+                    {
+                        n1=n2=n3=n4=700;
+                        Motor1.pulsewidth_us(n1);
+                        Motor2.pulsewidth_us(n2);
+                        Motor3.pulsewidth_us(n3);
+                        Motor4.pulsewidth_us(n4);
+                        while(1);  
+                    } 
+                }
+            }
+        }
     }
 }
\ No newline at end of file