Subversion Repositories FlightCtrl

// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Copyright (c) 08.2007 by THE BOSCH TEAM
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#include "main.h"
#include "math.h"




long Soll_Position_North = 0;
long Soll_Position_East = 0;

long GPS_Positionsabweichung_North = 0;
long GPS_Positionsabweichung_East = 0;
long GPS_Geschwindigkeit_North = 0;
long GPS_Geschwindigkeit_East = 0;
long P_Einfluss_North = 0;
long D_Einfluss_North = 0;
long P_Einfluss_East = 0;
long D_Einfluss_East = 0;

signed int GPS_North = 0;
signed int GPS_East = 0;
signed int GPS_Nick = 0;
signed int GPS_Roll = 0;

volatile char gethome = 0; //Flag ob GPS_Home_Position gelernt ist //(280807Kr)
long GPS_Home_North = 0;      
long GPS_Home_East = 0;        

unsigned char blinkcount_LED2 = 0; //Hilfszähler für die blinkende LED (010907Kr)

//**************************** MIRCOS DEFINITIONSANFANG FÜR DIE FILTERUNG
// Filterung der GPS Messwerte vom Ublox-Empfänger. Es wird der gleitende Durchschnitt aus n Messwerten gebildet

#define MITTELWERTZAHL 8 // Anzahl der Messwerte fuer gleitenden Mittelwert
volatile char NeuerMittelwert = 0; //Flag ob ein neuer Mittelwert vorliegt, der mit 4 Hz berechnet wird, da nur dann neue Messwerte vom GPS vorliegen
volatile unsigned char  FilterPos = 0;
volatile long NORTH_MITTEL = 0;
volatile long EAST_MITTEL = 0;
volatile long long NORTH_LONG_LONG = 0;
volatile long long EAST_LONG_LONG = 0;
volatile int filterfilled = 0;

typedef struct {

        long                    NORTH[MITTELWERTZAHL];

} FILTER_NORTH;  // Filterdefinition für die Positions-Nordwerte aus dem GPS-Empfänger

typedef struct {
       
        long                    EAST[MITTELWERTZAHL];

} FILTER_EAST;  // Filterdefinition für die Positions-Ostwerte aus dem GPS-Empfänger

FILTER_NORTH filter_north;
FILTER_EAST filter_east;

//**************************** MIRCOS DEFINITIONSENDE FÜR DIE FILTERUNG


void gps_main(void)
{

//**************************** MIRCOS FILTERUNG ANFANG
       
//      if(actualPos.GPSFix == 3 && actualPos.newData == 1) //bilde neuen Positionsmittelwert, wenn 3D-Fix und neue GPS-Daten vorliegen
if(actualPos.GPSFix == 3)   // hier auskommentieren, wenn dauernd (also nicht nur alle 250ms) die GPS-Berechnung durchgeführt werden soll

        {

                filter_north.NORTH[FilterPos] = actualPos.northing;
                filter_east.EAST[FilterPos] = actualPos.easting;
                FilterPos++;
                if (FilterPos==MITTELWERTZAHL) FilterPos=0;

                if (filterfilled<MITTELWERTZAHL) filterfilled++;

                NORTH_LONG_LONG = 0;
                EAST_LONG_LONG = 0;
                for (int i=0; i<filterfilled; i++)
                {
                        NORTH_LONG_LONG += filter_north.NORTH[i];
                        EAST_LONG_LONG += filter_east.EAST[i];
                }
                NORTH_LONG_LONG /= filterfilled;
                EAST_LONG_LONG /= filterfilled;
               
                NORTH_MITTEL = NORTH_LONG_LONG;
                EAST_MITTEL = EAST_LONG_LONG;
               
                NeuerMittelwert = 1;
                //actualPos.newData = 0;  // hier auskommentieren, wenn dauernd (also nicht nur alle 250ms) die GPS-Berechnung durchgeführt werden soll
        }
//******************************* MIRCOS FILTERUNG ENDE




    //wenn kein 3D-Fix vorhanden, über das Poti jedoch irgendeine GPS Funktion aktiv ist, werden die GPS-Steuerbefehle
    //auf Null gesetzt, um unkontrolliertes Wegfliegen durch das GPS zu verhindern. Dadurch wird abgesichert
    //(auch im laufenden Flug), dass bei einem Verlust der GPS-Verbindung GPS deaktivert wird
          if (actualPos.GPSFix < 3 && Poti3 > 70)
          {
          GPS_Nick = 0;
          GPS_Roll = 0;
          LED2_OFF;
          beeptime = 50;
          }



    // speichert die GPS_Home_Position beim 3D-Fix, MotorEINschalten und wenn noch nicht zu weit weggeflogen wurde
    // (modell_fliegt_gps < 2000) einmalig ab. Die Flugzeitabfrage sichert ab, dass bei einem Losfliegen ohne 3D-Fix die Home_Positinen
    // irgendwann später dort gelernt werden, wo dann zu ersten mal ein 3D-Fix vorhanden ist
          if (actualPos.GPSFix == 3 && MotorenEin == 1 && gethome == 0 && modell_fliegt_gps < 2000)
          {            
                GPS_Home_North = NORTH_MITTEL;
                GPS_Home_East = EAST_MITTEL;
    // Muss hier auch eingelernt werden, damit falls das Poti3 beim Motorenstarten in
    // Mittelstellung sein sollte, trotzdem schon eine Sollposition vorliegt. Andernfalls würde
    // GPS_Nick und _Roll maximal werden bis zum ersten Mal Stick_Nick und _Roll gesteuert würde
          Soll_Position_North = NORTH_MITTEL;
          Soll_Position_East = EAST_MITTEL;
          beeptime = 50;
          gethome  = 1;
          }




    //löscht die GPS_Home_Position beim MotorAUSschalten wieder und resetet das Flag gethome //(280807Kr)
          if (MotorenEin == 0)
          {            
          GPS_Home_North = 0;
          GPS_Home_East = 0;
          gethome  = 0;
          modell_fliegt_gps = 0;
          }




    // wenn Schalter ausgeschaltet, dann wird die LED2 ausgeschaltet und das GPS Steuerkommando gelöscht
          if(Poti3 < 70)
          {
            // Sollpositionen müssen auch hier beschrieben werden, damit ausgeschlossen ist, dass falls gelandet wird und die GPS-Funktion
            // über Poti3 deaktivert und anschließend wieder aktiviert wird keine Soll-Positionen vorliegen.
                  Soll_Position_North = NORTH_MITTEL;
                  Soll_Position_East = EAST_MITTEL;
                       
                  GPS_Nick = 0;
                  GPS_Roll = 0;
                  LED2_OFF;
          }
         
         
          // wenn 3D-Fix, Schalter im mittleren Bereich und vom Pilot gesteuert wird, wird ständig neue GPS-Sollposition eingelernt
          if(actualPos.GPSFix == 3 && Poti3 > 70 && Poti3 < 150 && (StickNick > 12 || StickNick < -12 || StickRoll > 12 || StickRoll < -12))  
          {
                  GPS_Nick = 0;   //wird genullt, damit das letzte GPS Kommando nicht ständig gegen den Piloten ansteuert
                  GPS_Roll = 0;   //wird genullt, damit das letzte GPS Kommando nicht ständig gegen den Piloten ansteuert
                       
                  Soll_Position_North = NORTH_MITTEL;  // Soll-Position wird geschrieben
                  Soll_Position_East = EAST_MITTEL;  // Soll-Position wird geschrieben
                       
                  //++++++++++++++++++++++++++++++++++++
                  //Sorgt lediglich dafür, dass die LED2 langsam blinkt. Dauer ist durch die "> x" Abfrage festgelegt
                  //++++++++++++++++++++++++++++++++++++
                  if(blinkcount_LED2 >= 200) // maximal erlaubt 255 wegen unsigned char
                  {
                  LED2_FLASH;
                  blinkcount_LED2 = 0;
                  }
                  blinkcount_LED2++;
                  //++++++++++++++++++++++++++++++++++++
          }    

       
        // wenn 3D-Fix, Schalter im mittleren Bereich (oder höher), neuer Mittelwert vorhanden, wenigstens einmal Sollposition gelernt und der Pilot nicht selbst steuert wird eine GPS-Funktion aktiviert
        if (actualPos.GPSFix == 3 && Poti3 > 70 && NeuerMittelwert == 1 && Soll_Position_North != 0 && Soll_Position_East != 0 && StickNick <= 12 && StickNick >= -12 && StickRoll <= 12 && StickRoll >= -12)
        {      
                //Berechnung der GPS-Positionsabweichung je nach aktivierter GPS-Funktion (GPS_HOLD oder HOMING)
               
                //DYNAMISCHES GPS-HOLD, wenn Schalter noch in Mittelstellung ist wird die zuletzt gelernte Soll_Position angeflogen
                if(Poti3 < 150)
                {
                GPS_Positionsabweichung_North = (Soll_Position_North - NORTH_MITTEL);
                GPS_Positionsabweichung_East = (Soll_Position_East - EAST_MITTEL);             
                LED2_ON;
                }
               
                //andernfalls, wenn Schalter in max. Stellung und zusätzlich die GPS_Home_Positionen beim Motorenstarten gelernt
                //werden konnten, wird zur GPS_Home_Position geflogen
                if (Poti3 >= 150 && GPS_Home_North != 0 && GPS_Home_East != 0)
                {
                        GPS_Positionsabweichung_North = (GPS_Home_North - NORTH_MITTEL);
                        GPS_Positionsabweichung_East = (GPS_Home_East - EAST_MITTEL);
                        // durch das ständige Neulernen der Soll_Positionen während HOMING wird erreicht, dass der MK nach dem Umschalten
                        // von HOMING zurück zu GPS-HOLD nicht wieder an die alte Soll_Position zurück fliegt
                        Soll_Position_North = NORTH_MITTEL;
                        Soll_Position_East = EAST_MITTEL;
                        LED2_ON;
                }
               
                //verhindert, dass beim Umschalten von Mittelstellung (GPS_HOLD) auf Maximalstellung (HOMING) OHNE gelernte Homeposition
                //die GPS Funktion unkontrolliert steuert. Daher wird GPS_HOLD aktiv gehalten, die Sollpositon wird bei Stickbefehlen
                //jedoch nicht wie beim dynamischen GPS_HOLD überschrieben. Zusätzlich wird der Summer eingeschaltet.
                if (Poti3 >= 150 && GPS_Home_North == 0 && GPS_Home_East == 0)
                {
                        GPS_Positionsabweichung_North = (Soll_Position_North - NORTH_MITTEL);
                        GPS_Positionsabweichung_East = (Soll_Position_East - EAST_MITTEL);
                        LED2_ON;
                        beeptime = 50;
                }
               
                //GPS-Geschwindigkeiten
                GPS_Geschwindigkeit_North = actualPos.velNorth; //actualPos.velNorth ist die Geschwindigkeiten aus dem GPS Empfänger entlang der Nord-Süd Richtung
                GPS_Geschwindigkeit_East = actualPos.velEast; //actualPos.velEast ist die Geschwindigkeiten aus dem GPS Empfänger entlang der Ost-West Richtung
               
               
                //Berechnung der Einzelkomponenten des PD-Reglers
                P_Einfluss_North = (P_GPS_Verstaerkung * GPS_Positionsabweichung_North)*0.0005; //entspricht einer Skalierung von 1/2000
                D_Einfluss_North = (D_GPS_Verstaerkung * GPS_Geschwindigkeit_North)*0.00333; //entspricht einer Skalierung von 1/300
               
                P_Einfluss_East = (P_GPS_Verstaerkung * GPS_Positionsabweichung_East)*0.0005; //entspricht einer Skalierung von 1/2000
                D_Einfluss_East = (D_GPS_Verstaerkung * GPS_Geschwindigkeit_East)*0.00333; //entspricht einer Skalierung von 1/300
               
               
                // Begrenzung des maximalen D-Anteils für pos. und neg. Werte. Grenze muss so geählt werden,
                // dass das Limit im normalen Position-Hold nicht erreicht wird und somit keinen Einfluss hat.
                // Grund: Beim Zufliegen auf weiter entfernte Ziele bremst der D-Anteil kurz vor dem Ziel den Mikrokopter sehr stark ab.
                // Dieses Limit wirkt nur, wenn sich der MK im HOMING Modus befindet und sich noch weit (>2,5m) von der Home-Position weg befindet.
                if (Poti3 >= 150 && GPS_Home_North != 0 && GPS_Home_East != 0 && (abs(GPS_Positionsabweichung_North) > 250 || abs(GPS_Positionsabweichung_East) > 250))
                {
                        if (D_Einfluss_North > Limit_D_Anteil) D_Einfluss_North = Limit_D_Anteil;
                        if (D_Einfluss_East > Limit_D_Anteil) D_Einfluss_East = Limit_D_Anteil;
                        if (D_Einfluss_North < -1 * Limit_D_Anteil) D_Einfluss_North = -1 * Limit_D_Anteil;
                        if (D_Einfluss_East < -1 * Limit_D_Anteil) D_Einfluss_East = -1 * Limit_D_Anteil;
                }
               
                // PD-Regler
                GPS_North = (-P_Einfluss_North + D_Einfluss_North);
                GPS_East = (P_Einfluss_East - D_Einfluss_East);
               
                //Umrechnen vom globalen North- bzw. East- in das körperfeste X- bzw. Y- Koordinatensystem
                GPS_Nick = (-c_sin[KompassValue]*0.001*GPS_East + c_cos[KompassValue]*0.001*GPS_North); // die "0" kann später durch den Messwert des Kompasssensors (KompassValue/0.01745329251) ersetzt werden
                GPS_Roll = (c_cos[KompassValue]*0.001*GPS_East + c_sin[KompassValue]*0.001*GPS_North); // die "0" kann später durch den Messwert des Kompasssensors (KompassValue/180*3.1415926535) ersetzt werden
               
                // Begrenzung des maximalen GPS Einflusses für positive und negative Werte
                if (GPS_Nick > 35) GPS_Nick = 35;
                if (GPS_Roll > 35) GPS_Roll = 35;
                if (GPS_Nick < -35) GPS_Nick = -35;
                if (GPS_Roll < -35) GPS_Roll = -35;
               
                //Funktion wird dadurch nur alle 250 ms aufgerufen, wenn neue Mittelwerte aus den GPS-Daten vorliegen.
                //FUNKTIONIERT MOMENTAN NOCH NICHT, DA STÄNDIG OHNE UNTERBRECHUNG NEUE MITTELWERTE BERECHNET WERDEN.
                //GRUND: DIE ABFRAGE NACH NEUE GPS-DATEN ERFOLGT NOCH NICHT
                NeuerMittelwert = 0;
        }
}