File content as of revision 24:aaa5b4703555:

#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

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);
    Motor2.period_ms(2);
    Motor3.period_ms(2);
    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);
        }    
    }
    
    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)//(!taster4) 
            {   
                uint16_t flanke1,hilfe1=0,flanke2,hilfe2=0,flanke3,hilfe3=0,flanke4,hilfe4=0;             
                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;
                    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;
                    
                    pitch = getAngle(&newAngle, &newRate, &dt, &Q_angle, &Q_bias, &R_measure, &bias); 
                    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);
                        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);
                            
                     
                       
                }
            }
        }
    }
}