Subversion Repositories FlightCtrl

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

// + Copyright (c) 08.2007 by THE BOSCH TEAM

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

#include "main.h"

#include "math.h"

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

//verwendet werden kann. d.h., es muss also in der späteren Berechnung noch mit 0,001 multipliziert werden //(090907Kr)

const int c_sin[] = {0,17,34,52,69,87,104,121,139,156,173,190,

207,224,241,258,275,292,309,325,342,358,374,390,406,422,438,453,469,484,500,515,529,544,559,573,587,601,615,629,

642,656,669,682,694,707,719,731,743,754,766,777,788,798,809,819,829,838,848,857,866,874,882,891,898,906,913,920,

927,933,939,945,951,956,961,965,970,974,978,981,984,987,990,992,994,996,997,998,999,999,1000,999,999,998,997,996,

994,992,990,987,984,981,978,974,970,965,961,956,951,945,939,933,927,920,913,906,898,891,882,874,866,857,848,838,

829,819,809,798,788,777,766,754,743,731,719,707,694,682,669,656,642,629,615,601,587,573,559,544,529,515,500,484,

469,453,438,422,406,390,374,358,342,325,309,292,275,258,241,224,207,190,173,156,139,121,104,87,69,52,34,17,0,-17,

-34,-52,-69,-87,-104,-121,-139,-156,-173,-190,-207,-224,-241,-258,-275,-292,-309,-325,-342,-358,-374,-390,-406,

-422,-438,-453,-469,-484,-500,-515,-529,-544,-559,-573,-587,-601,-615,-629,-642,-656,-669,-682,-694,-707,-719,

-731,-743,-754,-766,-777,-788,-798,-809,-819,-829,-838,-848,-857,-866,-874,-882,-891,-898,-906,-913,-920,-927,

-933,-939,-945,-951,-956,-961,-965,-970,-974,-978,-981,-984,-987,-990,-992,-994,-996,-997,-998,-999,-999,-1000,

-999,-999,-998,-997,-996,-994,-992,-990,-987,-984,-981,-978,-974,-970,-965,-961,-956,-951,-945,-939,-933,-927,

-920,-913,-906,-898,-891,-882,-874,-866,-857,-848,-838,-829,-819,-809,-798,-788,-777,-766,-754,-743,-731,-719,

-707,-694,-682,-669,-656,-642,-629,-615,-601,-587,-573,-559,-544,-529,-515,-500,-484,-469,-453,-438,-422,-406,

-390,-374,-358,-342,-325,-309,-292,-275,-258,-241,-224,-207,-190,-173,-156,-139,-121,-104,-87,-69,-52,-34,-17};

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

//verwendet werden kann. D.h., es muss also in der späteren Berechnung noch mit 0,001 multipliziert werden //(090907Kr)

const int c_cos[] = {1000,999,999,998,997,996,994,992,990,987,984,981,

978,974,970,965,961,956,951,945,939,933,927,920,913,906,898,891,882,874,866,857,848,838,829,819,809,798,788,777,

766,754,743,731,719,707,694,682,669,656,642,629,615,601,587,573,559,544,529,515,500,484,469,453,438,422,406,390,

374,358,342,325,309,292,275,258,241,224,207,190,173,156,139,121,104,87,69,52,34,17,0,-17,-34,-52,-69,-87,-104,

-121,-139,-156,-173,-190,-207,-224,-241,-258,-275,-292,-309,-325,-342,-358,-374,-390,-406,-422,-438,-453,-469,

-484,-500,-515,-529,-544,-559,-573,-587,-601,-615,-629,-642,-656,-669,-682,-694,-707,-719,-731,-743,-754,-766,

-777,-788,-798,-809,-819,-829,-838,-848,-857,-866,-874,-882,-891,-898,-906,-913,-920,-927,-933,-939,-945,-951,

-956,-961,-965,-970,-974,-978,-981,-984,-987,-990,-992,-994,-996,-997,-998,-999,-999,-1000,-999,-999,-998,-997,

-996,-994,-992,-990,-987,-984,-981,-978,-974,-970,-965,-961,-956,-951,-945,-939,-933,-927,-920,-913,-906,-898,

-891,-882,-874,-866,-857,-848,-838,-829,-819,-809,-798,-788,-777,-766,-754,-743,-731,-719,-707,-694,-682,-669,

-656,-642,-629,-615,-601,-587,-573,-559,-544,-529,-515,-500,-484,-469,-453,-438,-422,-406,-390,-374,-358,-342,

-325,-309,-292,-275,-258,-241,-224,-207,-190,-173,-156,-139,-121,-104,-87,-69,-52,-34,-17,0,17,34,52,69,87,

104,121,139,156,173,190,207,224,241,258,275,292,309,325,342,358,374,390,406,422,438,453,469,484,500,515,529,544,

559,573,587,601,615,629,642,656,669,682,694,707,719,731,743,754,766,777,788,798,809,819,829,838,848,857,866,874,

882,891,898,906,913,920,927,933,939,945,951,956,961,965,970,974,978,981,984,987,990,992,994,996,997,998,999,999};

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)

//***********************************************************************

// Waypoints

volatile char gotwaypoint = 0; //Flag für das Einlernen eines neuen Waypoints //(120907Kr)

unsigned char waypointindex = 0; //wird beim Lernen der Waypoints als Index zum Durchzählen verwendet

unsigned char waypointnummer = 0; //zählt im "Waypoint-Abflug-Modus" durch die gespeicherten Waypoints durch 

#define waypointanzahl 5 // Maximal mögliche Anzahl der lernbaren Waypoints wird festgelegt

        long                    waypoints_north[waypointanzahl]; // Definition für die Waypoints in Nordrichtung

  long                  waypoints_east[waypointanzahl];  // Definition für die Waypoints in Ostrichtung

//***********************************************************************

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

//******************************* WAYPOINTS EINLERNEN ANFANG

        //einlernen der Waypoints wenn 3D-Fix und Poti5 in MIN-Stellung

        if(actualPos.GPSFix == 3 && Poti5 < 70 && gotwaypoint == 0)

        {

                waypoints_north[waypointindex] = actualPos.northing;

                waypoints_east[waypointindex] = actualPos.easting;

    gotwaypoint = 1;

    beeptime = 50;  // kurzes Piepen um das Waypointlernen zu quittieren

    if(waypointindex < 100) // verhindert ein Überlaufen von waypoints_north bzw. _east 

    {

      waypointindex++;

                }

        }

        if(Poti5 > 70 && Poti5 < 150)

        {

                gotwaypoint = 0; //ermöglicht ein erneutes Lernen eines Waypoints für die nächste Minstellung von Poti5

        }

//******************************* WAYPOINTS EINLERNEN 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 && Poti6 > 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 Poti6 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;

                waypointindex = 0; //Zähler für die Waypoints wird beim Motorausschalten zurück gesetzt

                waypointnummer = 0; // Zähler fürs Abfliegen der Waypoints muss auch zurückgesetzt werden

            for (int i=0; i<100; i++) //zusätzlich werden alle gelernten Waypoints hier gelöscht

                  {

                        waypoints_north[i] = 0;

                        waypoints_east[i] = 0;

                  }

          }

    // wenn Schalter ausgeschaltet, dann wird die LED2 ausgeschaltet und das GPS Steuerkommando gelöscht

          if(Poti6 < 70)

          {

            // Sollpositionen müssen auch hier beschrieben werden, damit ausgeschlossen ist, dass falls gelandet wird und die GPS-Funktion

            // über Poti6 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 && Poti6 > 70 && Poti6 < 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 && Poti6 > 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, WAYPOINT oder HOMING)

    //DYNAMISCHES GPS-HOLD oder WAYPOINTS, je nach Kombination aus Poti6 (Mitte) und Poti5

                if(Poti6 < 150)

                {

      // wenn Poti5 in Mittel-Stellung ODER noch gar keine Waypoints gelernt wird GPS-HOLD aktiviert

                        if(Poti5 < 150 || (waypoints_north[0] == 0 && waypoints_east[0] == 0))

                        {

                                GPS_Positionsabweichung_North = (Soll_Position_North - NORTH_MITTEL);

                                GPS_Positionsabweichung_East = (Soll_Position_East - EAST_MITTEL);             

                                LED2_ON;

                        }

                        // wenn Poti5 in Max-Stellung und mindestens ein Waypoint eingelernt wurde wird WAYPOINTS aktiviert

                        if (Poti5 >= 150 && waypoints_north[0] != 0 && waypoints_east[0] != 0)

                        {

                                GPS_Positionsabweichung_North = (waypoints_north[waypointnummer] - NORTH_MITTEL);

                                GPS_Positionsabweichung_East = (waypoints_east[waypointnummer] - EAST_MITTEL);

                                // durch das ständige Neulernen der Soll_Positionen während WAYPOINTS wird erreicht, dass der MK nach dem Umschalten 

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

                                //wenn die Positionsabweichung kleiner als 150cm und noch ein weiterer eingelernter Waypoint vorhanden ist, wird dieser angeflogen

                                if (GPS_Positionsabweichung_North < 150 && GPS_Positionsabweichung_East < 150 && waypointnummer <= waypointindex)

                                {

                                        waypointnummer++;

                                        beeptime = 50;

                                }

                        }

                }

                //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 (Poti6 >= 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 (Poti6 >= 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-Einflusses für positive und negative 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. 

                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;

        }

}