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"

long Soll_Position_North = 0;

long Soll_Position_East = 0;

long GPS_Positionsabweichung_North = 0;

long GPS_Positionsabweichung_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;

signed int GPS_Nick_tmp = 0;  //(250308Kr)

signed int GPS_Roll_tmp = 0;  //(250308Kr)

signed int GPS_Nick_ACC = 0;  //(250308Kr)

signed int GPS_Roll_ACC = 0;  //(250308Kr)

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 50       //(031207Kr)

#define GPS_StickSense 12       //(241207Kr)

#define Limit_ACC_Anteil 25 //(250308Kr)

void gps_main(void)

{

    //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 = actualPos.northing;

          GPS_Home_East = actualPos.easting;

    // 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 = actualPos.northing;

          Soll_Position_East = actualPos.easting;

          beeptime = 50;

          gethome  = 1;

          }

        //schaltet bei Verwendung des GPS/MM3-Boards von Walter, GPS an RX von UART0 //(050208Kr)

          if (MotorenEin == 1)

          {

      SWITCH_GPS_2_RX;         

          }

    //löscht die GPS_Home_Position beim MotorAUSschalten wieder und resetet das Flag gethome //(280807Kr)

        //zusätzlich wird bei Verwendung des GPS/MM3-Boards von Walter BT wieder an RX von UART0 geschaltet //(050208Kr)

          if (MotorenEin == 0)

          {            

      SWITCH_BT_2_RX;          

          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 = actualPos.northing;

                  Soll_Position_East = actualPos.easting;

                  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 > 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 = actualPos.northing;  // Soll-Position wird geschrieben

                  Soll_Position_East = actualPos.easting;  // 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 && 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 in Mittelstellung ist UND KEINE Notlandung, wird die zuletzt gelernte Soll_Position angeflogen

                if(Poti3 < 150 && Notlandung == 0)  //(050208Kr)

                {

                        GPS_Positionsabweichung_North = (Soll_Position_North - actualPos.northing);

                        GPS_Positionsabweichung_East = (Soll_Position_East - actualPos.easting);

                LED2_ON;

                }

                //andernfalls, wenn Schalter in max. Stellung ODER Notlandung und zusätzlich die GPS_Home_Positionen beim Motorenstarten gelernt

                //werden konnten, wird zur GPS_Home_Position geflogen

                if ((Poti3 >= 150 || Notlandung == 1) && GPS_Home_North != 0 && GPS_Home_East != 0)  //(050208Kr)

                {

                        GPS_Positionsabweichung_North = (GPS_Home_North - actualPos.northing);

                        GPS_Positionsabweichung_East = (GPS_Home_East - actualPos.easting);

                        // 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 = actualPos.northing;

                        Soll_Position_East = actualPos.easting;

                        LED2_ON;

                }

                //verhindert, dass beim Umschalten von "GPS-HOLD" auf "HOMING" oder bei einer Notlandung 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 || Notlandung == 1) && GPS_Home_North == 0 && GPS_Home_East == 0) //(050208Kr)

                {

                        GPS_Positionsabweichung_North = (Soll_Position_North - actualPos.northing);

                        GPS_Positionsabweichung_East = (Soll_Position_East - actualPos.easting);

                        LED2_ON;

                        beeptime = 50;

                }

                //Berechnung der Einzelkomponenten des PD-Reglers

                P_Einfluss_North = (P_GPS_Verstaerkung * GPS_Positionsabweichung_North) / 2048;

                D_Einfluss_North = (D_GPS_Verstaerkung * actualPos.velNorth) / 256;

                P_Einfluss_East = (P_GPS_Verstaerkung * GPS_Positionsabweichung_East) / 2048;

                D_Einfluss_East = (D_GPS_Verstaerkung * actualPos.velEast) / 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 < -Limit_D_Anteil) D_Einfluss_North = -Limit_D_Anteil;

                        if (D_Einfluss_East < -Limit_D_Anteil) D_Einfluss_East = -Limit_D_Anteil;

                }

                //PD-Regler

                GPS_North = (-P_Einfluss_North + D_Einfluss_North);

                GPS_East = (P_Einfluss_East - D_Einfluss_East);

                //Diese Variante funktioniert genausogut und ist noch optimierter von der Ausührungszeit und Genauigkeit

                GPS_Nick_tmp = (((long)-sin_i(KompassValue)*(long)GPS_East) + ((long)cos_i(KompassValue)*(long)GPS_North))/4096;

                GPS_Roll_tmp = (((long)cos_i(KompassValue)*(long)GPS_East) + ((long)sin_i(KompassValue)*(long)GPS_North))/4096;

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

    //Einflüsse der ACC Sensoren (körperfest)

    GPS_Nick_ACC = ((long)ACC_GPS_Verstaerkung * (long)Mittelwert_AccNick) / 1024;  //mindestens durch 23,34 dividiert, um Überläufe zu verhindern

    GPS_Roll_ACC = ((long)ACC_GPS_Verstaerkung * (long)Mittelwert_AccRoll) / 1024;  //mindestens durch 23,34 dividiert, um Überläufe zu verhindern

    if (GPS_Nick_ACC > Limit_ACC_Anteil) GPS_Nick_ACC = Limit_ACC_Anteil;

                if (GPS_Roll_ACC > Limit_ACC_Anteil) GPS_Roll_ACC = Limit_ACC_Anteil;

                if (GPS_Nick_ACC < -Limit_ACC_Anteil) GPS_Nick_ACC = -Limit_ACC_Anteil;

                if (GPS_Roll_ACC < -Limit_ACC_Anteil) GPS_Roll_ACC = -Limit_ACC_Anteil;

    GPS_Nick = GPS_Nick_tmp + GPS_Nick_ACC;

                GPS_Roll = GPS_Roll_tmp + GPS_Roll_ACC;

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

                // 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 < -GPS_Limit) GPS_Nick = -GPS_Limit;

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

                //newData_navPosUtm = 0;

        }

}

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

//  TO-DO:

//  1.) Vorzeichen von GPS_NICK_ACC und GPS_ROLL_ACC experimentell ermitteln

//  2.) Teilerfaktor (momentan 512) für sinnvollen Wertebereich ermitteln

//  3.) Testen ob Mittelwert_ACCXXX die richtige Signalgröße ist oder evntl. doch z.B. AdWertAccXXXX, ...

//  4.) Zeitverzögerung für P_Einfluss_XXXX nach neutralen Knüppeln integrieren

//  5.) Modularisieren / Unterfunktionen 

//  6.) I-Anteil in die GPS-Regelung integrieren

//  7.) Updaterate für die zeitaufwändige Kompassberechnung auf 5Hz reduzieren

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