WebSVN - FlightCtrl - Diff - Rev 189 and 330 - /branches/salvo_gps/Basis

 // +  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 // +  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 // +  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 // +  POSSIBILITY OF SUCH DAMAGE.
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+// Aenderungen von Peter Muehlenbrock ("Salvo") Stand 6.10.2007
+/*
+Driftkompensation fuer Gyros verbessert
+Linearsensor mit fixem Neutralwert
+Ersatzkompass abgeleitet aus Magnetkompass und Giergyro fuer nahezu neigungsunabhaengige Kompassfunktion
+*/
 #include "main.h"
 volatile int  KompassStartwert = 0;
 volatile int  KompassRichtung = 0;
 unsigned char MAX_GAS,MIN_GAS;
 unsigned char Notlandung = 0;
 unsigned char HoehenReglerAktiv = 0;
+//Salvo 12.10.2007
+uint8_t magkompass_ok=0;
+uint8_t gps_cmd = GPS_CMD_STOP;
+static int       ubat_cnt =0;
+static int gas_actual,gas_mittel; //Parameter fuer Gasreduzierung bei unterspannung
+//Salvo End
+ //Salvo 2.9.2007 Ersatzkompass
+volatile long GyroKomp_Int,GyroKomp_Int2;
+volatile int GyroKomp_Value; //  Der ermittelte Kompasswert aus Gyro und Magnetkompass
+// Salvo End
 float GyroFaktor;
 float IntegralFaktor;
 volatile int  DiffNick,DiffRoll;
 char MotorenEin = 0;
 int HoehenWert = 0;
 int SollHoehe = 0;
 unsigned char Looping_Nick = 0,Looping_Roll = 0;
+int w,v;
 float Kp =  FAKTOR_P;
 float Ki =  FAKTOR_I;
         NeutralAccY = 0;
         NeutralAccZ = 0;
     AdNeutralNick = 0;
         AdNeutralRoll = 0;
         AdNeutralGier = 0;
+        GPS_Neutral();
     CalibrierMittelwert();
     timer = SetDelay(5);
         while (!CheckDelay(timer));
         CalibrierMittelwert();
     if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung aktiviert?
+    {
      AdNeutralNick= abs(MesswertNick) / 2;
          AdNeutralRoll= abs(MesswertRoll) / 2;
          AdNeutralGier= abs(MesswertGier) / 2;
+    }
+// Salvo 1.9.2007 ACC mit festen neutralwerten *****
+        if (ACC_NEUTRAL_FIXED > 0)
+        {
+                NeutralAccX     = ACC_NICK_NEUTRAL;
+                NeutralAccY     = ACC_ROLL_NEUTRAL;
+        }
+        else
+        { // Automatisch bei Offsetabgleich ermitteln
-    NeutralAccY = abs(Mittelwert_AccRoll) / ACC_AMPLIFY;
+                NeutralAccX = abs(Mittelwert_AccNick) / ACC_AMPLIFY;
-        NeutralAccX = abs(Mittelwert_AccNick) / ACC_AMPLIFY;
+                NeutralAccY = abs(Mittelwert_AccRoll) / ACC_AMPLIFY;
+        }
+ // Salvo End
         NeutralAccZ = Aktuell_az;
         Mess_IntegralNick = 0;
     Mess_IntegralNick2 = 0;
     MesswertGier = 0;
     StartLuftdruck = Luftdruck;
     HoeheD = 0;
     Mess_Integral_Hoch = 0;
     KompassStartwert = KompassValue;
-    GPS_Neutral();
     beeptime = 50;
+//Salvo 13.10.2007 Ersatzkompass
+        GyroKomp_Int = 0;
+        gas_mittel      =       30;
+        gas_actual      =       gas_mittel;
+// Salvo End
+}
 //############################################################################
 // Bildet den Mittelwert aus den Messwerten
  EE_Parameter.MaxHoehe     = 251;      // Wert : 0-32   251 -> Poti1
  EE_Parameter.Hoehe_P      = 10;       // Wert : 0-32
  EE_Parameter.Luftdruck_D  = 50;       // Wert : 0-250
  EE_Parameter.Hoehe_ACC_Wirkung = 50;  // Wert : 0-250
  EE_Parameter.Hoehe_Verstaerkung = 4;  // Wert : 0-50
- EE_Parameter.Stick_P = 4; //2         // Wert : 1-6
+ EE_Parameter.Stick_P = 2; //2         // Wert : 1-6
- EE_Parameter.Stick_D = 8; //8         // Wert : 0-64
+ EE_Parameter.Stick_D = 4; //8         // Wert : 0-64
  EE_Parameter.Gier_P = 16;             // Wert : 1-20
  EE_Parameter.Gas_Min = 15;            // Wert : 0-32
  EE_Parameter.Gas_Max = 250;           // Wert : 33-250
  EE_Parameter.GyroAccFaktor = 26;      // Wert : 1-64
  EE_Parameter.KompassWirkung = 128;    // Wert : 0-250
  EE_Parameter.Gyro_P = 120; //80          // Wert : 0-250
  EE_Parameter.Gyro_I = 150;               // Wert : 0-250
- EE_Parameter.UnterspannungsWarnung = 94; // Wert : 0-250
+ EE_Parameter.UnterspannungsWarnung = 100; // Wert : 0-250
- EE_Parameter.NotGas = 35;                // Wert : 0-250     // Gaswert bei Empangsverlust
+ EE_Parameter.NotGas = 100;                // Wert : 0-250     // Gaswert bei Empangsverlust
- EE_Parameter.NotGasZeit = 20;            // Wert : 0-250     // Zeit bis auf NotGas geschaltet wird, wg. Rx-Problemen
+ EE_Parameter.NotGasZeit = 60;            // Wert : 0-250     // Zeit bis auf NotGas geschaltet wird, wg. Rx-Problemen
  EE_Parameter.UfoAusrichtung = 0;         // X oder + Formation
  EE_Parameter.I_Faktor = 0;
- EE_Parameter.UserParam1 = 0;             //zur freien Verwendung
+ EE_Parameter.UserParam1 = 8;             //zur freien Verwendung
- EE_Parameter.UserParam2 = 0;             //zur freien Verwendung
+ EE_Parameter.UserParam2 = 2;             //zur freien Verwendung
- EE_Parameter.UserParam3 = 0;             //zur freien Verwendung
+ EE_Parameter.UserParam3 = 12;             //zur freien Verwendung
  EE_Parameter.UserParam4 = 0;             //zur freien Verwendung
  EE_Parameter.ServoNickControl = 100;     // Wert : 0-250     // Stellung des Servos
  EE_Parameter.ServoNickComp = 40;         // Wert : 0-250     // Einfluss Gyro/Servo
  EE_Parameter.ServoNickCompInvert = 0;    // Wert : 0-250     // Richtung Einfluss Gyro/Servo
  EE_Parameter.ServoNickMin = 50;          // Wert : 0-250     // Anschlag
  EE_Parameter.MaxHoehe     = 251;     // Wert : 0-32   251 -> Poti1
  EE_Parameter.Hoehe_P      = 10;      // Wert : 0-32
  EE_Parameter.Luftdruck_D  = 50;      // Wert : 0-250
  EE_Parameter.Hoehe_ACC_Wirkung = 50; // Wert : 0-250
  EE_Parameter.Hoehe_Verstaerkung = 2; // Wert : 0-50
- EE_Parameter.Stick_P = 4; //2           // Wert : 1-6
+ EE_Parameter.Stick_P = 2; //2           // Wert : 1-6
  EE_Parameter.Stick_D = 0; //8           // Wert : 0-64
  EE_Parameter.Gier_P = 16;            // Wert : 1-20
  EE_Parameter.Gas_Min = 15;            // Wert : 0-32
  EE_Parameter.Gas_Max = 250;          // Wert : 33-250
  EE_Parameter.GyroAccFaktor = 26;     // Wert : 1-64
- EE_Parameter.KompassWirkung = 128;    // Wert : 0-250
+ EE_Parameter.KompassWirkung = 64;    // Wert : 0-250
  EE_Parameter.Gyro_P = 175; //80           // Wert : 0-250
  EE_Parameter.Gyro_I = 175;           // Wert : 0-250
- EE_Parameter.UnterspannungsWarnung = 94;  // Wert : 0-250
+ EE_Parameter.UnterspannungsWarnung = 100;  // Wert : 0-250
- EE_Parameter.NotGas = 35;                 // Wert : 0-250     // Gaswert bei Empangsverlust
+ EE_Parameter.NotGas = 100;                 // Wert : 0-250     // Gaswert bei Empangsverlust
- EE_Parameter.NotGasZeit = 20;             // Wert : 0-250     // Zeit bis auf NotGas geschaltet wird, wg. Rx-Problemen
+ EE_Parameter.NotGasZeit = 60;             // Wert : 0-250     // Zeit bis auf NotGas geschaltet wird, wg. Rx-Problemen
  EE_Parameter.UfoAusrichtung = 0;         // X oder + Formation
- EE_Parameter.I_Faktor = 0;
+ EE_Parameter.I_Faktor = 5;
- EE_Parameter.UserParam1 = 0;   //zur freien Verwendung
+ EE_Parameter.UserParam1 = 12;   //zur freien Verwendung
- EE_Parameter.UserParam2 = 0;   //zur freien Verwendung
+ EE_Parameter.UserParam2 = 2;   //zur freien Verwendung
- EE_Parameter.UserParam3 = 0;   //zur freien Verwendung
+ EE_Parameter.UserParam3 = 16;   //zur freien Verwendung
  EE_Parameter.UserParam4 = 0;   //zur freien Verwendung
  EE_Parameter.ServoNickControl = 100;     // Wert : 0-250     // Stellung des Servos
  EE_Parameter.ServoNickComp = 40;         // Wert : 0-250     // Einfluss Gyro/Servo
  EE_Parameter.ServoNickCompInvert = 0;    // Wert : 0-250     // Richtung Einfluss Gyro/Servo
  EE_Parameter.ServoNickMin = 50;           // Wert : 0-250     // Anschlag
          static unsigned int  modell_fliegt = 0;
      static int hoehenregler = 0;
      static char TimerWerteausgabe = 0;
      static char NeueKompassRichtungMerken = 0;
         Mittelwert();
+//****** GPS Daten holen ***************
+        short int n;
+        if (gps_alive_cnt > 0) gps_alive_cnt--; //Dekrementieren. Wenn 0 kommen keine ausreichenden GPS Meldungen (Timeout)
+        n = Get_Rel_Position();
+        if (n == 0)
+        {
+                ROT_ON; //led blitzen lassen
+        }
+//******PROVISORISCH***************
     GRN_ON;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Gaswert ermitteln
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+        GasMischanteil = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120;
+//Salvo 13.10.2007 langsame Gasreduktion bei Unterspannung. Als Ausgangswert wird der bei UBAT=k gemessen Mittelwert genommen
+// und dieser dann langsam zwangsweise reduziert
+        ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern
+        if (UBat <= EE_Parameter.UnterspannungsWarnung - 2)     //Unterhalb der Piepser Schwelle aktivieren
+        {
+                if (ubat_cnt > 700)
+                {
+                        ubat_cnt = 0;
+                        if (gas_actual > ((gas_mittel*12)/15)) gas_actual--;
+                }
+                else ubat_cnt++;
+                if (GasMischanteil > gas_actual) GasMischanteil = gas_actual;
+        }
+        else   //Falls UBAT wieder ok ist
+        {
+                if (ubat_cnt > 1000)
+                {
+                        gas_mittel = ((gas_mittel*9) + GasMischanteil)/10; //Filtern
+                        gas_actual = GasMischanteil;
+                }
+                else
+                {
+                        ubat_cnt++;
+                        if ((ubat_cnt % 10) == 0)
+                        {
+                                if (gas_actual < GasMischanteil) gas_actual++;
+                                else gas_actual = GasMischanteil;
+                }
+                }
+                GasMischanteil = gas_actual;
+        }
+        ANALOG_ON;      // ADC einschalten
-        GasMischanteil = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120;
+// Salvo End
     if(GasMischanteil < 0) GasMischanteil = 0;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-// Emfang schlecht
+// Empfang schlecht
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    if(SenderOkay < 100)
+        {
         if(!PcZugriff)
+                    {
                     unsigned char setting;
                     if(++delay_neutral > 200)  // nicht sofort
+                        {
                         GRN_OFF;
+                        SetNeutral();
                         MotorenEin = 0;
                         delay_neutral = 0;
                         modell_fliegt = 0;
                         if(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70 || abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) > 70)
+                        {
                          if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 3;
                          if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] <-70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 4;
                          if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] <-70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < 70) setting = 5;
                          eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET], setting);  // aktiven Datensatz merken
+                        }
-                        if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung aktiviert?
-                          {
-                             if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();
-                          }
                             ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE);
-                        SetNeutral();
                         Piep(GetActiveParamSetNumber());
+                        if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung aktiviert?
+                        {
+                          if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();
                         }
+                                                GPS_Save_Home(); //Daten sind jetzt hoffentlich verfuegbar
+                                                if (gps_home_position.status > 0 )
+                                                {
+                                                        Delay_ms(1000);  //akustisch verkuenden dass GPS Home Daten da sind
+                                                        beeptime = 2000;
+                                                        Delay_ms(500);
+                                                }
+                       }
+                    }
                  else delay_neutral = 0;
+                }
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Gas ist unten
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Gyro-Drift kompensieren
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 #define DRIFT_FAKTOR 3
     if(ZaehlMessungen >= 1000 / DRIFT_FAKTOR)
+                {
+// Salvo 12.9.2007 Ersatzkompass *******
+                    ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern
+                        if (GyroKomp_Int/GYROKOMP_INC_GRAD_DEFAULT >=360 ) GyroKomp_Int = 0;
+                        ANALOG_ON;      // ADC einschalten
+                        ROT_OFF;
-        {
+// Salvo End
-        IntegralFehlerNick = IntegralNick2 - IntegralNick;
+               IntegralFehlerNick = IntegralNick2 - IntegralNick;
-        IntegralFehlerRoll = IntegralRoll2 - IntegralRoll;
+           IntegralFehlerRoll = IntegralRoll2 - IntegralRoll;
-        ZaehlMessungen = 0;
+           ZaehlMessungen = 0;
+// Salvo 1.9.2007 *************************
+// Abgleich Roll und Nick Gyro vollsteandig nur, wenn nahezu waagrechte Lage, ansonsten abschwaechen
+                   ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern
+                        w = (abs(Mittelwert_AccNick));
+                        v = (abs(Mittelwert_AccRoll));
+                ANALOG_ON;      // ADC einschalten
+                        // Abgleich Roll und Nick Gyro nur wenn auch keine wilden Manoever stattfinden 26.9.2007
+                        if ((abs(StickNick - GPS_Nick) < 30) && (abs(StickRoll - GPS_Roll) < 30))
+                        {
+                                if ((w  < ACC_WAAGRECHT_LIMIT) && (v < ACC_WAAGRECHT_LIMIT))
+                                {
-        if(IntegralFehlerNick > 500/DRIFT_FAKTOR)   AdNeutralNick++;
+                         if(IntegralFehlerNick > 500/DRIFT_FAKTOR)   AdNeutralNick++;
-        if(IntegralFehlerNick < -500/DRIFT_FAKTOR)  AdNeutralNick--;
+                         if(IntegralFehlerNick < -500/DRIFT_FAKTOR)  AdNeutralNick--;
-        if(IntegralFehlerRoll > 500/DRIFT_FAKTOR)   AdNeutralRoll++;
+                         if(IntegralFehlerRoll > 500/DRIFT_FAKTOR)   AdNeutralRoll++;
-        if(IntegralFehlerRoll < -500/DRIFT_FAKTOR)  AdNeutralRoll--;
+                         if(IntegralFehlerRoll < -500/DRIFT_FAKTOR)  AdNeutralRoll--;
+                                }
+                                else if ((w < 2*ACC_WAAGRECHT_LIMIT) && (v < 2*ACC_WAAGRECHT_LIMIT)) // langsamer kompensieren, weil ACC Werte unsicherer sind, was die Neigung angeht
+                                {
+                         if(IntegralFehlerNick > 500*2/DRIFT_FAKTOR)   AdNeutralNick++;
+                         if(IntegralFehlerNick < -500*2/DRIFT_FAKTOR)  AdNeutralNick--;
+                         if(IntegralFehlerRoll > 500*2/DRIFT_FAKTOR)   AdNeutralRoll++;
+                         if(IntegralFehlerRoll < -500*2/DRIFT_FAKTOR)  AdNeutralRoll--;
+                                }
+/*                              else  // noch langsamer kompensieren, weil ACC Werte unsicherer sind, was die Neigung angeht
+                                {
+                         if(IntegralFehlerNick > 500*4/DRIFT_FAKTOR)   AdNeutralNick++;
-//        if(Mess_Integral_Gier2 > 500/DRIFT_FAKTOR)  AdNeutralGier--;   // macht nur mit Referenz (Kompass Sinn)
+                         if(IntegralFehlerNick < -500*4/DRIFT_FAKTOR)  AdNeutralNick--;
+                         if(IntegralFehlerRoll > 500*4/DRIFT_FAKTOR)   AdNeutralRoll++;
-//        if(Mess_Integral_Gier2 <-500/DRIFT_FAKTOR)  AdNeutralGier++;   // macht nur mit Referenz (Kompass Sinn)
+                         if(IntegralFehlerRoll < -500*4/DRIFT_FAKTOR)  AdNeutralRoll--;
+                                }
+*/                      }
+// Salvo End
-    ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern
+                        ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern
-        Mess_IntegralNick2 = IntegralNick;
+                Mess_IntegralNick2 = IntegralNick;
-        Mess_IntegralRoll2 = IntegralRoll;
+                Mess_IntegralRoll2 = IntegralRoll;
-        Mess_Integral_Gier2 = Integral_Gier;
+                Mess_Integral_Gier2 = Integral_Gier;
-    ANALOG_ON;  // ADC einschalten
+                ANALOG_ON;      // ADC einschalten
+        }
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Integrale auf ACC-Signal abgleichen
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
   if(IntegralFaktor && !Looping_Nick && !Looping_Roll)
+  {
    tmp_long = 0;
    tmp_long2 = 0;
+  }
  ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern
+ // Salvo 1.9.2007 Volle Korrektur nur wenn waagrechte Lage, sonst abgeschawecht *****************
+        w = (abs(Mittelwert_AccNick));
+        v = (abs(Mittelwert_AccRoll));
+        if ((w  < ACC_WAAGRECHT_LIMIT) && (v < ACC_WAAGRECHT_LIMIT))
+        {
-    Mess_IntegralNick -= tmp_long;
+        Mess_IntegralNick -= tmp_long;
-    Mess_IntegralRoll -= tmp_long2;
+        Mess_IntegralRoll -= tmp_long2;
+        }
+        else if ((w  < 2 * ACC_WAAGRECHT_LIMIT) && (v < 2 * ACC_WAAGRECHT_LIMIT))
+        {
+        Mess_IntegralNick -= tmp_long/2; //Vorher 8
+        Mess_IntegralRoll -= tmp_long2/2;
+        }
+        else if ((w  < 4 * ACC_WAAGRECHT_LIMIT) && (v < 4 * ACC_WAAGRECHT_LIMIT))
+        {
+        Mess_IntegralNick -= tmp_long/4;
+        Mess_IntegralRoll -= tmp_long2/4;
+        }
+        else
+        {
+        Mess_IntegralNick -= tmp_long/8;
+        Mess_IntegralRoll -= tmp_long2/8;
+        }
+// Salvo End ***********************
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //  Gieren
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
     if(abs(StickGier) > 20) // war 35
+     {
     sollGier = tmp_int;
     Mess_Integral_Gier -= tmp_int;
     if(Mess_Integral_Gier > 25000) Mess_Integral_Gier = 25000;  // begrenzen
     if(Mess_Integral_Gier <-25000) Mess_Integral_Gier =-25000;
+ // Salvo Gewolltes Gieren ignorieren 30.8.2007 **********************
+    Mess_Integral_Gier2  -= tmp_int;
 // Salvo End *************************
+ ANALOG_ON;     // ADC einschalten
+// Salvo Ersatzkompass  26.9.2007 **********************
+        if ((Kompass_Neuer_Wert > 0))
+        {
+           Kompass_Neuer_Wert = 0;
+           w = (abs(Mittelwert_AccNick));
+           v = (abs(Mittelwert_AccRoll));
+           if  ((w  < ACC_WAAGRECHT_LIMIT) && (v < ACC_WAAGRECHT_LIMIT)) //Ersatzkompass nur mit Magnetkompass aktualisieren wenn alle sok
+           {
+                if  ((abs(KompassValue - Kompass_Value_Old)) <= 5) // Aufeinanderfolgende Werte duerfen nur minimal abweichen
+                 {
+                  ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern
+                  magkompass_ok = 1; // Flag dass Magnetkompass stabil arbeitet
+                  GyroKomp_Int = (GyroKomp_Int )/GYROKOMP_INC_GRAD_DEFAULT;
+                  w = KompassValue - GyroKomp_Int;
+                  if ((w > 0) && (w < 180))
+                  {
+                   ++GyroKomp_Int;
+                  }
+                  else if ((w > 0) && (w >= 180))
+                  {
+                   --GyroKomp_Int;
+                  }
+                  else if ((w < 0) && (w >= -180))
+                  {
+                   --GyroKomp_Int;
+                  }
+                  else if ((w < 0) && (w < -180))
+                  {
+                   ++GyroKomp_Int;
+                  }
+                  if (GyroKomp_Int < 0)  GyroKomp_Int = GyroKomp_Int + 360;
+                  GyroKomp_Int = (GyroKomp_Int%360) * GYROKOMP_INC_GRAD_DEFAULT; // An Magnetkompasswert annaehern
+                  ANALOG_ON;    // ADC einschalten
+                 }
+           }
+           else magkompass_ok = 0;
+        }
+// Salvo End *************************
+// Salvo 6.10.2007
+        // GPS Home aktivieren wenn Knueppel in Ruhelage und Hoehenschalter aktiviert ist
+        //GPS Hold Aktiveren wenn Knueppel in Ruhelage sind
+        if ((EE_Parameter.GlobalConfig & CFG_GPS_AKTIV) && (abs(StickRoll) < GPS_STICK_HOLDOFF) && (abs(StickNick) < GPS_STICK_HOLDOFF) && (gps_alive_cnt > 0))
+        {
+                if (Parameter_MaxHoehe > 200)
+                {
+                        if ( gps_cmd == GPS_CMD_REQ_HOLD) gps_cmd = GPS_CMD_STOP; // erst mal stoppen, denn altes Kommando wurde noch nicht beendet
+                        else gps_cmd = GPS_CMD_REQ_HOME;
+                        n = GPS_CRTL(gps_cmd);
+                }
+                else
+                {
+                        if ( gps_cmd == GPS_CMD_REQ_HOME) gps_cmd = GPS_CMD_STOP; // erst mal stoppen, denn altes Kommando wurde noch nicht beendet
+                        else gps_cmd = GPS_CMD_REQ_HOLD;
+                        n= GPS_CRTL(gps_cmd);
+                }
+        }
- ANALOG_ON;     // ADC einschalten
+        else (n= GPS_CRTL(GPS_CMD_STOP)); //GPS Lageregelung beenden
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //  Kompass
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-    if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV))
-     {
-       int w,v;
-       static int SignalSchlecht = 0;
-       w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln
+    if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV) && (Kompass_present > 0))
+     {
+           if(v > w) w = v; // grösste Neigung ermitteln
-       v = abs(IntegralRoll /512);
-       if(v > w) w = v; // grösste Neigung ermitteln
+// Salvo 13.9.2007 Nur wenn Magnetkompass ordentliche Werte liefert
-       if(w < 25 && NeueKompassRichtungMerken && !SignalSchlecht)
+                if ((magkompass_ok > 0)  &&  NeueKompassRichtungMerken)
+        {
-         KompassStartwert = KompassValue;
+                 KompassStartwert = KompassValue;
+         NeueKompassRichtungMerken = 0;
+        }
+// Salvo 13.9.2007
-         NeueKompassRichtungMerken = 0;
+       w=0;
 // Salvo End
        w = (w * Parameter_KompassWirkung) / 64;           // auf die Wirkung normieren
        w = Parameter_KompassWirkung - w;                  // Wirkung ggf drosseln
        if(w > 0)
+        {
+          ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern
-        {
+// Salvo Kompasssteuerung **********************
-          ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern
+                 if (magkompass_ok > 0) Mess_Integral_Gier += (KompassRichtung * w) / 32;  // nach Kompass ausrichten
-          if(!SignalSchlecht) Mess_Integral_Gier += (KompassRichtung * w) / 32;  // nach Kompass ausrichten
-          ANALOG_ON;  // ADC einschalten
+// Salvo End
-          if(SignalSchlecht) SignalSchlecht--;
+          ANALOG_ON;  // ADC einschalten
+        }
         else SignalSchlecht = 500; // so lange das Signal taub stellen --> ca. 1 sek
+     }
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
     DebugOut.Analog[3] = Mittelwert_AccRoll;
     DebugOut.Analog[4] = MesswertGier;
     DebugOut.Analog[5] = HoehenWert;
     DebugOut.Analog[6] = (Mess_Integral_Hoch / 512);
     DebugOut.Analog[8] = KompassValue;
+        DebugOut.Analog[9] = GyroKomp_Int/GYROKOMP_INC_GRAD_DEFAULT;
 /*    DebugOut.Analog[16] = motor_rx[0];
     DebugOut.Analog[17] = motor_rx[1];
     DebugOut.Analog[18] = motor_rx[2];

Subversion Repositories FlightCtrl

(root)/branches/salvo_gps/Basis_V0064/trunk/fc.c - Rev 189 → 330