WebSVN - FlightCtrl - Diff - Rev 1002 and 1003 - /branches/salvo_gps/Basis

 // +  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 // +  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 9.11.2008
+/*
+Driftkompensation fuer Gyros verbessert
+Linearsensor optional mit fixem Neutralwert
+Ersatzkompass abgeleitet aus Magnetkompass und Giergyro fuer nahezu neigungsunabhaengige Kompassfunktion
+*/
 #include "main.h"
+#include "eeprom.c"
 #include "eeprom.c"
 unsigned char h,m,s;
 volatile unsigned int I2CTimeout = 100;
 volatile int MesswertNick,MesswertRoll,MesswertGier,MesswertGierBias;
 unsigned char TrichterFlug = 0;
 long Umschlag180Nick = 250000L, Umschlag180Roll = 250000L;
 long  ErsatzKompass;
 int   ErsatzKompassInGrad; // Kompasswert in Grad
 int   GierGyroFehler = 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
+int w,v;
+//Salvo End
+ //Salvo 15.12.2007 Ersatzkompass und Giergyrokompensation
+long GyroKomp_Int;
+long int GyroGier_Comp;
+int GyroKomp_Value; //  Der ermittelte Kompasswert aus Gyro und Magnetkompass
+short int cnt_stickgier_zero =0;
+int gyrogier_kompass;
+//Salvo End
+ //Salvo 2.1.2008 Allgemeine Debugvariablen
+int debug_gp_0,debug_gp_1,debug_gp_2,debug_gp_3,debug_gp_4,debug_gp_5,debug_gp_6,debug_gp_7;
+//Salvo End
 float GyroFaktor;
 float IntegralFaktor;
 volatile int  DiffNick,DiffRoll;
 int  Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0;
 volatile unsigned char Motor_Vorne,Motor_Hinten,Motor_Rechts,Motor_Links, Count;
 unsigned char Parameter_J17Timing;              // for the J17 Output
 unsigned char Parameter_NaviGpsModeControl;     // Parameters for the Naviboard
 unsigned char Parameter_NaviGpsGain;
 unsigned char Parameter_NaviGpsP;
 unsigned char Parameter_NaviGpsI;
 unsigned char Parameter_NaviGpsD;
+unsigned char Parameter_NaviStickThreshold; //salvo 16.10.2008
 unsigned char Parameter_NaviGpsACC;
 unsigned char Parameter_ExternalControl;
 struct mk_param_struct EE_Parameter;
 signed int ExternStickNick = 0,ExternStickRoll = 0,ExternStickGier = 0, ExternHoehenValue = -20;
 int MaxStickNick = 0,MaxStickRoll = 0;
 unsigned int  modell_fliegt = 0;
 unsigned char MikroKopterFlags = 0;
+//Salvo 2.1.2008 Allgemeine Debugvariablen
+int debug_gp_0,debug_gp_1,debug_gp_2,debug_gp_3,debug_gp_4,debug_gp_5,debug_gp_6,debug_gp_7;
+//Salvo End
 void Piep(unsigned char Anzahl)
+{
  while(Anzahl--)
+ {
 //############################################################################
 //  Nullwerte ermitteln
 void SetNeutral(void)
 //############################################################################
+{
+        // Salvo 9.12.2007
+        RX_SWTCH_ON; //GPS Daten auf RX eingang schalten
+        // Salvo End
         NeutralAccX = 0;
         NeutralAccY = 0;
         NeutralAccZ = 0;
     AdNeutralNick = 0;
         AdNeutralRoll = 0;
     Mess_IntegralRoll2 = 0;
     Mess_Integral_Gier = 0;
     MesswertNick = 0;
     MesswertRoll = 0;
     MesswertGier = 0;
- Delay_ms_Mess(100);
+        Delay_ms_Mess(100);
     StartLuftdruck = Luftdruck;
     HoeheD = 0;
     Mess_Integral_Hoch = 0;
     KompassStartwert = KompassValue;
     GPS_Neutral();
     ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR;
     GierGyroFehler = 0;
     SendVersionToNavi = 1;
     LED_Init();
     MikroKopterFlags |= FLAG_CALIBRATE;
+//Salvo 13.10.2007 Ersatzkompass und Gas
+        GyroKomp_Int =  KompassValue * GIER_GRAD_FAKTOR; //Neu ab 15.10.2008
+//      gas_mittel      =       30;
+//      gas_actual      =       gas_mittel;
+// Salvo End
+}
 //############################################################################
 // Bearbeitet die Messwerte
     MesswertGierBias = (signed int) AdNeutralGierBias - AdWertGier;
     MesswertRoll = (signed int) AdWertRoll - AdNeutralRoll;
     MesswertNick = (signed int) AdWertNick - AdNeutralNick;
 //DebugOut.Analog[26] = MesswertNick;
-DebugOut.Analog[28] = MesswertRoll;
+//DebugOut.Analog[28] = MesswertRoll;
 // Beschleunigungssensor  ++++++++++++++++++++++++++++++++++++++++++++++++
         Mittelwert_AccNick = ((long)Mittelwert_AccNick * 1 + ((ACC_AMPLIFY * (long)AdWertAccNick))) / 2L;
     NaviAccRoll    += AdWertAccRoll;
     NaviCntAcc++;
     IntegralAccZ    += Aktuell_az - NeutralAccZ;
 // Gier  ++++++++++++++++++++++++++++++++++++++++++++++++
             ErsatzKompass +=  MesswertGier;
+//Salvo 12.11.2007
+                        GyroKomp_Int   += (long)MesswertGier;
+                        GyroGier_Comp  += (long)MesswertGier;
+//Salvo End
             Mess_Integral_Gier +=  MesswertGier;
 //            Mess_Integral_Gier2 += MesswertGier;
                     if(ErsatzKompass >= (360L * GIER_GRAD_FAKTOR)) ErsatzKompass -= 360L * GIER_GRAD_FAKTOR;  // 360° Umschlag
                     if(ErsatzKompass < 0)                          ErsatzKompass += 360L * GIER_GRAD_FAKTOR;
 // Kopplungsanteil  +++++++++++++++++++++++++++++++++++++
  CHK_POTI(Parameter_NaviGpsModeControl,EE_Parameter.NaviGpsModeControl,0,255);
  CHK_POTI(Parameter_NaviGpsGain,EE_Parameter.NaviGpsGain,0,255);
  CHK_POTI(Parameter_NaviGpsP,EE_Parameter.NaviGpsP,0,255);
  CHK_POTI(Parameter_NaviGpsI,EE_Parameter.NaviGpsI,0,255);
  CHK_POTI(Parameter_NaviGpsD,EE_Parameter.NaviGpsD,0,255);
+ CHK_POTI(Parameter_NaviStickThreshold,EE_Parameter.NaviStickThreshold,0,255); //Salvo 16.10.2008
  CHK_POTI(Parameter_NaviGpsACC,EE_Parameter.NaviGpsACC,0,255);
  CHK_POTI(Parameter_ExternalControl,EE_Parameter.ExternalControl,0,255);
  Ki = (float) Parameter_I_Faktor * 0.0001;
  MAX_GAS = EE_Parameter.Gas_Max;
  MIN_GAS = EE_Parameter.Gas_Min;
      static char TimerWerteausgabe = 0;
      static char NeueKompassRichtungMerken = 0;
      static long ausgleichNick, ausgleichRoll;
+        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();
-        Mittelwert();
+        if (n == 0)
+        {
+                ROT_ON; //led blitzen lassen
+        }
 //******PROVISORISCH***************
     GRN_ON;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Gaswert ermitteln
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
                 if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75)  // Neutralwerte
+                    {
                     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((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();
+                            }
                            SetNeutral();
                            Piep(GetActiveParamSetNumber());
+                                                        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 = 1000;
+                                                                Delay_ms(500);
+                                                        }
+                         }
+                        }
+                    }
                  else
                 if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75)  // ACC Neutralwerte speichern
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Einschalten
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
                     if(++delay_einschalten > 200)
+                        {
+                                                int n;
+                                                // Salvo 9.12.2007
+                                                RX_SWTCH_ON; //GPS Daten auf RX eingang schalten
+                                                // Salvo End
                         delay_einschalten = 200;
                         modell_fliegt = 1;
                         MotorenEin = 1;
                         sollGier = 0;
                         Mess_Integral_Gier = 0;
                         Mess_IntegralRoll = 0;
                         Mess_IntegralNick2 = IntegralNick;
                         Mess_IntegralRoll2 = IntegralRoll;
                         SummeNick = 0;
                         SummeRoll = 0;
+                        n= GPS_CRTL(GPS_CMD_STOP); //GPS Lageregelung beenden
                         MikroKopterFlags |= FLAG_START;
+                        }
+                    }
                     else delay_einschalten = 0;
                 //Auf Neutralwerte setzen
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
                 if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75)
+                    {
                     if(++delay_ausschalten > 200)  // nicht sofort
+                        {
+                                                // Salvo 9.12.2007
+                                                RX_SWTCH_OFF; //Bluetooth Daten auf RX eingang schalten
+                                                // Salvo End
                         MotorenEin = 0;
                         delay_ausschalten = 200;
                         modell_fliegt = 0;
+                        }
+                    }
     int tmp_int;
         static int stick_nick,stick_roll;
     ParameterZuordnung();
     stick_nick = (stick_nick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P) / 4;
     stick_nick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D;
+//    StickNick = stick_nick - (GPS_Nick*(STICK_GAIN/2) + GPS_Nick2); //Salvo 23.10.2008
     StickNick = stick_nick - (GPS_Nick + GPS_Nick2);
 //    StickNick  = (StickNick  * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P) / 4;
     stick_roll = (stick_roll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P) / 4;
+    stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D;
-    stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D;
+//    StickRoll = stick_roll - (GPS_Roll*(STICK_GAIN/2) + GPS_Roll2); //Salvo 23.10.2008
+    StickRoll = stick_roll - (GPS_Roll + GPS_Roll2);
     StickRoll = stick_roll - (GPS_Roll + GPS_Roll2);
 //    StickRoll = (StickRoll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P) / 4;
 //    IntegralFehlerNick = (IntegralFehlerNick * 1 + tmp_long) / 2;
 //    IntegralFehlerRoll = (IntegralFehlerRoll * 1 + tmp_long2) / 2;
     if(GierGyroFehler > ABGLEICH_ANZAHL/2) { AdNeutralGier++; AdNeutralGierBias++; }
     if(GierGyroFehler <-ABGLEICH_ANZAHL/2) { AdNeutralGier--; AdNeutralGierBias--; }
-DebugOut.Analog[22] = MittelIntegralRoll / 26;
+//DebugOut.Analog[22] = MittelIntegralRoll / 26;
 //DebugOut.Analog[24] = GierGyroFehler;
+    GierGyroFehler = 0;
+//Salvo Ersatzkompass Ueberlauf korrigieren
+                if (GyroKomp_Int >= ((long)360 * GIER_GRAD_FAKTOR)) GyroKomp_Int = GyroKomp_Int - (GIER_GRAD_FAKTOR *(long)360); //neu ab 3.11.2007
+                if (GyroKomp_Int < 0) GyroKomp_Int = GyroKomp_Int + (GIER_GRAD_FAKTOR *(long)360); //neu ab 3.11.2007
-    GierGyroFehler = 0;
+                ROT_OFF;
 // Salvo End
 /*DebugOut.Analog[17] = IntegralAccNick / 26;
         else
+        {
          cnt = 0;
          KompassSignalSchlecht = 1000;
+        }
+                w = (abs(Mittelwert_AccNick));
+                v = (abs(Mittelwert_AccRoll));
+//Salvo 16.10.2008
+                if  ((w  < ACC_WAAGRECHT_LIMIT*2) && (v < ACC_WAAGRECHT_LIMIT*2)) // Gyro nur in annaehernd waagrechter Lage nachtrimmen
+                {
-        if(cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp;
+         if(cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp;
-        if(IntegralFehlerNick >  FEHLER_LIMIT)   AdNeutralNick += cnt;
+         if(IntegralFehlerNick >  FEHLER_LIMIT)   AdNeutralNick += cnt;
-        if(IntegralFehlerNick < -FEHLER_LIMIT)   AdNeutralNick -= cnt;
+         if(IntegralFehlerNick < -FEHLER_LIMIT)   AdNeutralNick -= cnt;
+                }
+//Salvo End
 // Roll +++++++++++++++++++++++++++++++++++++++++++++++++
         cnt = 1;// + labs(IntegralFehlerNick) / 4096;
         ausgleichRoll = 0;
         } else
+        {
          cnt = 0;
          KompassSignalSchlecht = 1000;
+        }
+//Salvo 26.12.2007
+                if  ((w  < ACC_WAAGRECHT_LIMIT*2) && (v < ACC_WAAGRECHT_LIMIT*2)) // Gyro nur in annaehernd waagrechter Lage nachtrimmen
+                {
+                if(cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp;
+                if(IntegralFehlerRoll >  FEHLER_LIMIT)   AdNeutralRoll += cnt;
+                if(IntegralFehlerRoll < -FEHLER_LIMIT)   AdNeutralRoll -= cnt;
+            }
+//Salvo End
-        if(cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp;
-        if(IntegralFehlerRoll >  FEHLER_LIMIT)   AdNeutralRoll += cnt;
         if(IntegralFehlerRoll < -FEHLER_LIMIT)   AdNeutralRoll -= cnt;
+  }
   else
+  {
    LageKorrekturRoll = 0;
     MittelIntegralNick2 = 0;
     MittelIntegralRoll2 = 0;
     ZaehlMessungen = 0;
+ }
 //DebugOut.Analog[31] = StickRoll / (26*IntegralFaktor);
+// Salvo Ersatzkompass  und Giergyrokompensation 15.12.2007 **********************
+if ((Kompass_Neuer_Wert > 0)) //nur wenn Kompass einen neuen Wert geliefert hat
+{
+           Kompass_Neuer_Wert = 0;
+   w = (abs(Mittelwert_AccNick));
+   v = (abs(Mittelwert_AccRoll));
+   if  ((w  < ACC_WAAGRECHT_LIMIT) && (v < ACC_WAAGRECHT_LIMIT)) //Ersatzkompass und Giergyro nur mit Magnetkompass aktualisieren wenn alles ok
+   {
+        if  ((abs(KompassValue - Kompass_Value_Old)) <= 5) // Aufeinanderfolgende Werte duerfen nur minimal abweichen
+        {
+          if ((abs(StickGier) < 20)) //Giergyroabgleich mit Kompass
+          {
+                        if (cnt_stickgier_zero < 255) cnt_stickgier_zero += 1;
+                        if (cnt_stickgier_zero > 2) // nur Abgleichen wenn keine Stickbewegung da
+                        {
+                                w = (int) (GyroGier_Comp/(long)GIER_GRAD_FAKTOR);
+                                v = KompassValue - gyrogier_kompass; // realen Drehwinkel seit letztem Ruecksetzen von GyroGier_Comp bestimmen
+                                if (v <-180) v +=360; // Uberlaufkorrektur
+                                if (v > 180) v -=360; // Uberlaufkorrektur
+                                v = w-v;  //Differenz Gyro zu Kompass ist der Driftfehler
+                                #define GIER_COMP_MAX 2
+                                if (v > GIER_COMP_MAX)  v= GIER_COMP_MAX;
+                                if (v < -GIER_COMP_MAX) v= - GIER_COMP_MAX;
+                                if (abs(w) > 1)
+                                {
+                                        GyroGier_Comp           = 0;
+                                        gyrogier_kompass        = KompassValue; // Kompasswert merken
+                                        AdNeutralGier           -= v;
+                                }
+                        }
+          }
+          else
+          {
+                gyrogier_kompass        = KompassValue; // Kompasswert merken
+                cnt_stickgier_zero      = 0;
+                GyroGier_Comp = 0;
+          }
+          magkompass_ok = 1; // Flag dass Magnetkompass stabil arbeitet
+          GyroKomp_Int = (GyroKomp_Int )/(long)GIER_GRAD_FAKTOR;
+          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 + 360L;
+          GyroKomp_Int = (GyroKomp_Int%360L) * (long)GIER_GRAD_FAKTOR; // An Magnetkompasswert annaehern
+        }
+   }
+   else //Kompassfehler
+   {
+    magkompass_ok = 0;
+        GyroGier_Comp = 0;
+   }
+   Kompass_Value_Old    =       KompassValue;
+}
+// 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 ((abs(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]]) < Parameter_NaviStickThreshold)
+        && (abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) < Parameter_NaviStickThreshold) && (gps_alive_cnt > 0))
+        {
+                if ((Parameter_NaviGpsModeControl > 170) && (EE_Parameter.GlobalConfig & CFG_GPS_AKTIV)) //Hoehenschalter und GPS Flag aktiv
+                {
+                        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 ((Parameter_NaviGpsModeControl < 170) && (EE_Parameter.GlobalConfig & CFG_GPS_AKTIV)) //Hoehenschalter Mittelstellung und GPS Flag aktiv
+                {
+                        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);
+                }
+                else  // GPS komplett aus
+                {
+                        if (gps_cmd != GPS_CMD_STOP)
+                        {
+                                gps_cmd = GPS_CMD_STOP;
+                                n= GPS_CRTL(gps_cmd); //GPS Lageregelung beenden
+                        }
+                }
+        }
+        else
+        {
+                if (gps_cmd != GPS_CMD_STOP)
+                {
+                        gps_cmd = GPS_CMD_STOP;
+                        n= GPS_CRTL(gps_cmd); //GPS Lageregelung beenden
+                }
+        }
+        if (gps_state != GPS_CRTL_IDLE) if (TimerWerteausgabe == 12)  LED_J16_OFF; //led im GPS Mode schnell blinken lassen
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //  Gieren
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //    if(GasMischanteil < 35) { if(StickGier > 10) StickGier = 10; else if(StickGier < -10) StickGier = -10;};
     if(abs(StickGier) > 15) // war 35
-     {
-      KompassSignalSchlecht = 1000;
+     {
-      if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX))
-       {
-         NeueKompassRichtungMerken = 1;
-        };
+      if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX)) NeueKompassRichtungMerken = 1;
+     }
     tmp_int  = (long) EE_Parameter.Gier_P * ((long)StickGier * abs(StickGier)) / 512L; // expo  y = ax + bx²
     tmp_int += (EE_Parameter.Gier_P * StickGier) / 4;
     sollGier = tmp_int;
     Mess_Integral_Gier -= tmp_int;
-    if(Mess_Integral_Gier > 50000) Mess_Integral_Gier = 50000;  // begrenzen
+    if(Mess_Integral_Gier > GIER_INTEGRAL_MAX) Mess_Integral_Gier = GIER_INTEGRAL_MAX;  // begrenzen Salvo 18.10.2008
-    if(Mess_Integral_Gier <-50000) Mess_Integral_Gier =-50000;
+    if(Mess_Integral_Gier <-GIER_INTEGRAL_MAX) Mess_Integral_Gier =-GIER_INTEGRAL_MAX;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-//  Kompass
-// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-//DebugOut.Analog[29] = (MaxStickNick + MaxStickRoll);
+//  Kompass
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-    if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV))
-     {
-       int w,v,r,fehler,korrektur;
-       w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln
+    if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV) && (Kompass_present > 0))
-       v = abs(IntegralRoll /512);
-       if(v > w) w = v; // grösste Neigung ermitteln
-       korrektur = w / 8 + 1;
-       fehler = ((540 + KompassValue - (ErsatzKompass/GIER_GRAD_FAKTOR)) % 360) - 180;
-//DebugOut.Analog[25] = KompassSignalSchlecht;
+     {
-       if(!KompassSignalSchlecht && w < 25)
+           if(v > w) w = v; // grösste Neigung ermitteln
-        {
-        GierGyroFehler += fehler;
+// Salvo 13.9.2007 Nur wenn Magnetkompass ordentliche Werte liefert
-        if(NeueKompassRichtungMerken)
-         {
+                if ((magkompass_ok > 0)  &&  NeueKompassRichtungMerken)
-          beeptime = 200;
-//         KompassStartwert = KompassValue;
+        {
-          KompassStartwert = (ErsatzKompass/GIER_GRAD_FAKTOR);
-          NeueKompassRichtungMerken = 0;
+                 KompassStartwert = KompassValue;
+         NeueKompassRichtungMerken = 0;
-         }
+        }
+// Salvo 13.9.2007
-        }
+       w=0;
-       ErsatzKompass += (fehler * 8) / korrektur;
+// Salvo End
-       w = (w * Parameter_KompassWirkung) / 32;           // auf die Wirkung normieren
+       w = (w * Parameter_KompassWirkung) / 64;           // auf die Wirkung normieren
        w = Parameter_KompassWirkung - w;                  // Wirkung ggf drosseln
-       if(w >= 0)
-        {
-          if(!KompassSignalSchlecht)
+       if(w > 0)
-          {
-           v = 64 + ((MaxStickNick + MaxStickRoll)) / 8;
-           r = ((540 + (ErsatzKompass/GIER_GRAD_FAKTOR) - KompassStartwert) % 360) - 180;
+        {
-//           r = KompassRichtung;
-           v = (r * w) / v;  // nach Kompass ausrichten
-           w = 3 * Parameter_KompassWirkung;
-           if(v > w) v = w; // Begrenzen
-           else
-           if(v < -w) v = -w;
+// Salvo Kompasssteuerung **********************
-           Mess_Integral_Gier += v;
-          }
+                 if (magkompass_ok > 0) Mess_Integral_Gier += (KompassRichtung * w) / 32;  // nach Kompass ausrichten
-          if(KompassSignalSchlecht) KompassSignalSchlecht--;
 // Salvo End
+        }
         else KompassSignalSchlecht = 500; // so lange das Signal taub stellen --> ca. 1 sek
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //  Debugwerte zuordnen
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+  if(!TimerWerteausgabe--)
+   {
+    TimerWerteausgabe = 24;
+        // Salvo 13.12.2007 Beleuchtung steuern
+        if (!(beeptime & BeepMuster)) LED_J16_FLASH;
-  if(!TimerWerteausgabe--)
+        else if (MotorenEin) LED_J16_ON;
         else LED_J16_OFF;
-    TimerWerteausgabe = 24;
+        // Salvo End
     DebugOut.Analog[0] = IntegralNick / EE_Parameter.GyroAccFaktor;
     DebugOut.Analog[1] = IntegralRoll / EE_Parameter.GyroAccFaktor;
     DebugOut.Analog[2] = Mittelwert_AccNick;
+    DebugOut.Analog[3] = Mittelwert_AccRoll;
-    DebugOut.Analog[3] = Mittelwert_AccRoll;
+    DebugOut.Analog[4] = MesswertGier;
-    DebugOut.Analog[4] = MesswertGier;
+    DebugOut.Analog[5] = HoehenWert;
-    DebugOut.Analog[5] = HoehenWert;
+//    DebugOut.Analog[6] = (Mess_Integral_Hoch / 512);
-    DebugOut.Analog[6] = (Mess_Integral_Hoch / 512);
+    DebugOut.Analog[8] = KompassValue;
-    DebugOut.Analog[8] = KompassValue;
+    DebugOut.Analog[9] = UBat;
-    DebugOut.Analog[9] = UBat;
+    DebugOut.Analog[10] = Mess_Integral_Gier / 128;
-    DebugOut.Analog[11] = ErsatzKompass / GIER_GRAD_FAKTOR;
+//    DebugOut.Analog[10] = SenderOkay;
-    DebugOut.Analog[10] = SenderOkay;
+    DebugOut.Analog[11] = GyroKomp_Int / GIER_GRAD_FAKTOR;
     //DebugOut.Analog[16] = Mittelwert_AccHoch;
-//    DebugOut.Analog[17] = FromNaviCtrl_Value.Distance;
-//    DebugOut.Analog[18] = (int)FromNaviCtrl_Value.OsdBar;
-    DebugOut.Analog[19] = WinkelOut.CalcState;
-    DebugOut.Analog[20] = ServoValue;
+//    DebugOut.Analog[17] = FromNaviCtrl_Value.Distance;
 //    DebugOut.Analog[18] = (int)FromNaviCtrl_Value.OsdBar;
+//    DebugOut.Analog[19] = WinkelOut.CalcState;
+//    DebugOut.Analog[20] = ServoValue;
+//    DebugOut.Analog[19] -= DebugOut.Analog[19]/128;
+//    if(DebugOut.Analog[19] > 0) DebugOut.Analog[19]--; else DebugOut.Analog[19]++;
+        DebugOut.Analog[23] = debug_gp_0;
+    DebugOut.Analog[24] = debug_gp_1;
+    DebugOut.Analog[25] = debug_gp_2;
+        DebugOut.Analog[26] = gps_rel_act_position.utm_east; //in 10cm ausgeben
+        DebugOut.Analog[27] = gps_rel_act_position.utm_north;
-    DebugOut.Analog[30] = GPS_Nick;
+        DebugOut.Analog[28] = gps_rel_act_position.utm_alt;
-    DebugOut.Analog[31] = GPS_Roll;
+        DebugOut.Analog[29] = gps_state + (gps_sub_state*10)+(50*gps_cmd);
     DebugOut.Analog[30] = GPS_Nick;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Gier-Anteil
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 #define MUL_G  1.0
     GierMischanteil = MesswertGier - sollGier * STICK_GAIN;     // Regler für Gier
 // GierMischanteil = 0;
 #define MIN_GIERGAS  (40*STICK_GAIN)  // unter diesem Gaswert trotzdem Gieren
    if(GasMischanteil > MIN_GIERGAS)
+    {
      if(GierMischanteil > (GasMischanteil / 2)) GierMischanteil = GasMischanteil / 2;
      if(GierMischanteil < -(MIN_GIERGAS / 2)) GierMischanteil = -(MIN_GIERGAS / 2);
+    }
     tmp_int = MAX_GAS*STICK_GAIN;
     if(GierMischanteil > ((tmp_int - GasMischanteil))) GierMischanteil = ((tmp_int - GasMischanteil));
     if(GierMischanteil < -((tmp_int - GasMischanteil))) GierMischanteil = -((tmp_int - GasMischanteil));
+        DebugOut.Analog[6] = GierMischanteil; //Salvo 19.10.2008
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Nick-Achse
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

Subversion Repositories FlightCtrl

(root)/branches/salvo_gps/Basis_v0070d/trunk/fc.c - Rev 1002 → 1003