File content as of revision 25:a8a3cbc57c61:

#include <Timer.h>
#include <math.h> 
#include "mbed.h"
#include "stdio.h"
#include "deklaration.h"
#include "messen.h"
#include "filter/Kalman.h"

#define RAD 57.29577951
                 
int main()
{  
    z_off = 0;
    drift_z = 0;
 
    Motor1.period_ms(2);
    Motor2.period_ms(2);
    Motor3.period_ms(2);
    Motor4.period_ms(2);
    initialisierung_gyro();
    initialisierung_acc();
    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);
    }  
    if (taster3)            
    {         
        anlernen(&Motor1, &Motor2, &Motor3, &Motor4, &taster1, &taster2, &taster4);
    }
    
    pc.printf("Druecke Taster1 fuer den Start\n\r");   
    n1=n2=n3=n4=700;
    Motor1.pulsewidth_us(n1);
    Motor2.pulsewidth_us(n2);
    Motor3.pulsewidth_us(n3);
    Motor4.pulsewidth_us(n4); 
          
    while(1)
    {
        if (taster1) 
        {
            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();
                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 == 2000)
                    {
                        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;
                    }
                    Motorsteurung(&Motor1, &Motor2, &Motor3, &Motor4, &taster2, &taster3, &taster4, &n1, &n2, &n3, &n4);   
                }
            }
        }
    }
}