File content as of revision 15:742683a8efda:

#include <Timer.h>
#include <math.h> 
#include "mbed.h"
#include "stdio.h"
#include "deklaration.h"
#include "messen.h"
                          
#define RAD 57.29577951

uint16_t zeit;
uint32_t zeit2;

uint8_t k;


                        
             
                                
int main()
{  
    gain_g = 0;
    z_off = 0;
    zeit = 0;
    k = 0;  
    drift_z = 0;
 
    Motor1.period_ms(2);
    Motor2.period_ms(2);
    Motor3.period_ms(2);
    Motor4.period_ms(2);
    initialisierung_gyro();
    initialisierung_acc();  
    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 (1)//(taster1) 
        {
            while(1)//(!taster4) 
            {                
                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++;                   
                    zeit = 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) * zeit * 0.000001 * 1/16.4);
                    pitch_g = pitch_g + ((y_g  - y_off) * zeit * 0.000001 * 1/16.4);
                    roll_g = roll_g + ((x_g - x_off) * zeit * 0.000001 * 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;
                    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;
                    roll = roll_g * 0.9 + roll_a * 0.1;
                    if (i == 2000)
                    {
                        pc.printf("gain: %2.5f\tpitch: %2.5f\troll: %2.5f\t\n\r",pitch, roll, gain);
                        i = 0;
                    }
                        
                }
            }
        }
    }
}