Subversion Repositories FlightCtrl

// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

// + Copyright (c) 10.2007 by Jochen Kromayer

// + es handelt sich um eine Beta-Software, die zu nichtkommerziellen Zwecken frei veröffentlich und weitergegeben werden darf. Für evtl.

// + Personen- und/der Sachschäden ist jegliche Haftung ausgeschlossen

// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

#include "main.h"

//Sinusfunktion von 0° bis 359°. Ergebnis wurde mit 1024 multipliziert, damit als Definition platzsparend int 

//verwendet werden kann. D.h., es muss also in der späteren Berechnung noch durch 1024 dividiert werden //(051207Kr)

const int c_sin[] = {0,18,36,54,71,89,107,125,143,160,178,195,213,230,248,265,282,299,316,333,350,367,384,400,

416,433,449,465,481,496,512,527,543,558,573,587,602,616,630,644,658,672,685,698,711,724,737,749,

761,773,784,796,807,818,828,839,849,859,868,878,887,896,904,912,920,928,935,943,949,956,962,968,

974,979,984,989,994,998,1002,1005,1008,1011,1014,1016,1018,1020,1022,1023,1023,1024,1024,1024,1023,1023,1022,1020,

1018,1016,1014,1011,1008,1005,1002,998,994,989,984,979,974,968,962,956,949,943,935,928,920,912,904,896,

887,878,868,859,849,839,828,818,807,796,784,773,761,749,737,724,711,698,685,672,658,644,630,616,

602,587,573,558,543,527,512,496,481,465,449,433,416,400,384,367,350,333,316,299,282,265,248,230,

213,195,178,160,143,125,107,89,71,54,36,18,0,-18,-36,-54,-71,-89,-107,-125,-143,-160,-178,-195,

-213,-230,-248,-265,-282,-299,-316,-333,-350,-367,-384,-400,-416,-433,-449,-465,-481,-496,-512,-527,-543,-558,-573,-587,

-602,-616,-630,-644,-658,-672,-685,-698,-711,-724,-737,-749,-761,-773,-784,-796,-807,-818,-828,-839,-849,-859,-868,-878,

-887,-896,-904,-912,-920,-928,-935,-943,-949,-956,-962,-968,-974,-979,-984,-989,-994,-998,-1002,-1005,-1008,-1011,-1014,-1016,

-1018,-1020,-1022,-1023,-1023,-1024,-1024,-1024,-1023,-1023,-1022,-1020,-1018,-1016,-1014,-1011,-1008,-1005,-1002,-998,-994,-989,-984,-979,

-974,-968,-962,-956,-949,-943,-935,-928,-920,-912,-904,-896,-887,-878,-868,-859,-849,-839,-828,-818,-807,-796,-784,-773,

-761,-749,-737,-724,-711,-698,-685,-672,-658,-644,-630,-616,-602,-587,-573,-558,-543,-527,-512,-496,-481,-465,-449,-433,

-416,-400,-384,-367,-350,-333,-316,-299,-282,-265,-248,-230,-213,-195,-178,-160,-143,-125,-107,-89,-71,-54,-36,-18};

//Cosinusfunktion von 0° bis 359°. Ergebnis wurde mit 1024 multipliziert, damit als Definition platzsparend int 

//verwendet werden kann. D.h., es muss also in der späteren Berechnung noch durch 1024 dividiert werden //(051207Kr)

const int c_cos[] = {1024,1024,1023,1023,1022,1020,1018,1016,1014,1011,1008,1005,1002,998,994,989,984,979,974,968,962,956,949,943,

935,928,920,912,904,896,887,878,868,859,849,839,828,818,807,796,784,773,761,749,737,724,711,698,

685,672,658,644,630,616,602,587,573,558,543,527,512,496,481,465,449,433,416,400,384,367,350,333,

316,299,282,265,248,230,213,195,178,160,143,125,107,89,71,54,36,18,0,-18,-36,-54,-71,-89,

-107,-125,-143,-160,-178,-195,-213,-230,-248,-265,-282,-299,-316,-333,-350,-367,-384,-400,-416,-433,-449,-465,-481,-496,

-512,-527,-543,-558,-573,-587,-602,-616,-630,-644,-658,-672,-685,-698,-711,-724,-737,-749,-761,-773,-784,-796,-807,-818,

-828,-839,-849,-859,-868,-878,-887,-896,-904,-912,-920,-928,-935,-943,-949,-956,-962,-968,-974,-979,-984,-989,-994,-998,

-1002,-1005,-1008,-1011,-1014,-1016,-1018,-1020,-1022,-1023,-1023,-1024,-1024,-1024,-1023,-1023,-1022,-1020,-1018,-1016,-1014,-1011,-1008,-1005,

-1002,-998,-994,-989,-984,-979,-974,-968,-962,-956,-949,-943,-935,-928,-920,-912,-904,-896,-887,-878,-868,-859,-849,-839,

-828,-818,-807,-796,-784,-773,-761,-749,-737,-724,-711,-698,-685,-672,-658,-644,-630,-616,-602,-587,-573,-558,-543,-527,

-512,-496,-481,-465,-449,-433,-416,-400,-384,-367,-350,-333,-316,-299,-282,-265,-248,-230,-213,-195,-178,-160,-143,-125,

-107,-89,-71,-54,-36,-18,0,18,36,54,71,89,107,125,143,160,178,195,213,230,248,265,282,299,

316,333,350,367,384,400,416,433,449,465,481,496,512,527,543,558,573,587,602,616,630,644,658,672,

685,698,711,724,737,749,761,773,784,796,807,818,828,839,849,859,868,878,887,896,904,912,920,928,

935,943,949,956,962,968,974,979,984,989,994,998,1002,1005,1008,1011,1014,1016,1018,1020,1022,1023,1023,1024};

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;

signed int GPS_StickSense = 15;

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)

#define GPS_Limit 35            //(031207Kr)

#define Limit_D_Anteil 30       //(031207Kr)

//**************************** 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 GPS Frequenz 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

        //bilde neuen Positionsmittelwert, wenn 3D-Fix und neue GPS-Daten vorliegen

        //  if(actualPos.GPSFix == 3 && newData_navPosUtm == 1)

        if(actualPos.GPSFix == 3)   // hier umkommentieren, wenn dauernd (also nicht nur mit der GPS-Updaterate) 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;

                newData_navPosUtm = 0;         

        }

//******************************* 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;

                 //hierdurch soll verhindert werden, dass wenn nach schonmal aktiver GPS Funktion (und damit 3D-fix) der GPS-Empfänger

                 //abgezogen werden sollte weiterhin ein 3D-fix angezeigt wird. Grund ist, dass in diesem Fall in der UART.c nach einem

                 //Abziehen der Status nicht mehr auf Null zurückgesetzt wird. Dies geschied nun beim Ausschalten der GPS-Funktion.

                 //Sie ist dann ohne erneuten 3D-Fix nicht mehr aktivierbar. (111107Kr)  

                  actualPos.GPSFix = 0;

          }

          // 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 > GPS_StickSense || StickNick < -GPS_StickSense || StickRoll > GPS_StickSense || StickRoll < -GPS_StickSense))  

          {

                  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 <= GPS_StickSense && StickNick >= -GPS_StickSense && StickRoll <= GPS_StickSense && StickRoll >= -GPS_StickSense)

        {      

                //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) / 2048;

                D_Einfluss_North = (D_GPS_Verstaerkung * GPS_Geschwindigkeit_North) / 256;

                P_Einfluss_East = (P_GPS_Verstaerkung * GPS_Positionsabweichung_East) / 2048;

                D_Einfluss_East = (D_GPS_Verstaerkung * GPS_Geschwindigkeit_East) / 256;

                // 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 (>5m) von der Home-Position weg befindet.

                if (Poti3 >= 150 && GPS_Home_North != 0 && GPS_Home_East != 0 && (abs(GPS_Positionsabweichung_North) > 500 || abs(GPS_Positionsabweichung_East) > 500))

                {

                        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_cos[KompassValue]*GPS_North)/1024 - (c_sin[KompassValue]*GPS_East)/1024);

                GPS_Roll = ((c_cos[KompassValue]*GPS_East)/1024 + (c_sin[KompassValue]*GPS_North)/1024);

                // Begrenzung des maximalen GPS Einflusses für positive und negative Werte

                if (GPS_Nick > GPS_Limit) GPS_Nick = GPS_Limit;

                if (GPS_Roll > GPS_Limit) GPS_Roll = GPS_Limit;

                if (GPS_Nick < -1 * GPS_Limit) GPS_Nick = -1 * GPS_Limit;

                if (GPS_Roll < -1 * GPS_Limit) GPS_Roll = -1 * GPS_Limit;

                //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;

        }

}