WebSVN - FlightCtrl - Diff - Rev 1085 and 1086 - /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"
 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_NaviGpsGain;
 unsigned char Parameter_NaviGpsP;
 unsigned char Parameter_NaviGpsI;
 unsigned char Parameter_NaviGpsD;
 unsigned char Parameter_NaviGpsACC;
+unsigned char Parameter_NaviStickThreshold; //salvo 16.10.2008
 unsigned char Parameter_NaviOperatingRadius;
 unsigned char Parameter_NaviWindCorrection;
 unsigned char Parameter_NaviSpeedCompensation;
 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;
         AdNeutralGier = 0;
     LED_Init();
     MikroKopterFlags |= FLAG_CALIBRATE;
     FromNaviCtrl_Value.Kalman_K = -1;
     FromNaviCtrl_Value.Kalman_MaxDrift = EE_Parameter.Driftkomp * 16;
     FromNaviCtrl_Value.Kalman_MaxFusion = 32;
+//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_AchsGegenKopplung1,EE_Parameter.AchsGegenKopplung1,0,255);
  CHK_POTI(Parameter_DynamicStability,EE_Parameter.DynamicStability,0,255);
  CHK_POTI_MM(Parameter_J16Timing,EE_Parameter.J16Timing,1,255);
  CHK_POTI_MM(Parameter_J17Timing,EE_Parameter.J17Timing,1,255);
-// CHK_POTI(Parameter_NaviGpsModeControl,EE_Parameter.NaviGpsModeControl,0,255);
+ CHK_POTI(Parameter_NaviGpsModeControl,EE_Parameter.NaviGpsModeControl,0,255);
- //CHK_POTI(Parameter_NaviGpsGain,EE_Parameter.NaviGpsGain,0,255);
+ CHK_POTI(Parameter_NaviGpsGain,EE_Parameter.NaviGpsGain,0,255);
-// CHK_POTI(Parameter_NaviGpsP,EE_Parameter.NaviGpsP,0,255);
+ CHK_POTI(Parameter_NaviGpsP,EE_Parameter.NaviGpsP,0,255);
-// CHK_POTI(Parameter_NaviGpsI,EE_Parameter.NaviGpsI,0,255);
+ CHK_POTI(Parameter_NaviGpsI,EE_Parameter.NaviGpsI,0,255);
-// CHK_POTI(Parameter_NaviGpsD,EE_Parameter.NaviGpsD,0,255);
+ CHK_POTI(Parameter_NaviGpsD,EE_Parameter.NaviGpsD,0,255);
-// CHK_POTI(Parameter_NaviGpsACC,EE_Parameter.NaviGpsACC,0,255);
+ CHK_POTI(Parameter_NaviGpsACC,EE_Parameter.NaviGpsACC,0,255);
 // CHK_POTI_MM(Parameter_NaviOperatingRadius,EE_Parameter.NaviOperatingRadius,10,255);
 // CHK_POTI(Parameter_NaviWindCorrection,EE_Parameter.NaviWindCorrection,0,255);
 // CHK_POTI(Parameter_NaviSpeedCompensation,EE_Parameter.NaviSpeedCompensation,0,255);
          static unsigned char delay_einschalten = 0,delay_ausschalten = 0;
      static int hoehenregler = 0;
      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();
+        if (n == 0)
+        {
+                ROT_ON; //led blitzen lassen
+        }
+//******PROVISORISCH***************
     GRN_ON;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Gaswert ermitteln
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
         GasMischanteil = StickGas;
                 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;
+                        }
+                    }
 //    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;
     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 4
+                                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
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //  Kompass
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //DebugOut.Analog[29] = (MaxStickNick + MaxStickRoll);
-    if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV))
-     {
-       int w,v,r,fehler,korrektur;
-       w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln
-       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;
+// Salvo 13.9.2007 Nur wenn Magnetkompass ordentliche Werte liefert
-           if(NeueKompassRichtungMerken)
-            {
-                 fehler = 0;
-                 ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR;
-                }
-       if(!KompassSignalSchlecht && w < 25)
+                if ((magkompass_ok > 0)  &&  NeueKompassRichtungMerken)
-        {
-        GierGyroFehler += fehler;
-        if(NeueKompassRichtungMerken)
-         {
-          beeptime = 200;
+        {
-//         KompassStartwert = KompassValue;
-          KompassStartwert = (ErsatzKompass/GIER_GRAD_FAKTOR);
+                 KompassStartwert = KompassValue;
-          NeueKompassRichtungMerken = 0;
          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)
-          {
+        {
-           v = 64 + ((MaxStickNick + MaxStickRoll)) / 8;
-           r = ((540 + (ErsatzKompass/GIER_GRAD_FAKTOR) - KompassStartwert) % 360) - 180;
-//           r = KompassRichtung;
+// Salvo Kompasssteuerung **********************
-           v = (r * w) / v;  // nach Kompass ausrichten
-           w = 3 * Parameter_KompassWirkung;
-           if(v > w) v = w; // Begrenzen
-           else
-           if(v < -w) v = -w;
-           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;
+        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[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[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[17] = FromNaviCtrl_Value.Distance;
-    DebugOut.Analog[18] = (int)FromNaviCtrl_Value.OsdBar;
-    DebugOut.Analog[19] = WinkelOut.CalcState;
-    DebugOut.Analog[20] = ServoValue;
-    DebugOut.Analog[27] = (int)FromNaviCtrl_Value.Kalman_MaxDrift;
-    DebugOut.Analog[29] = (int)FromNaviCtrl_Value.Kalman_K;
+//    DebugOut.Analog[18] = (int)FromNaviCtrl_Value.OsdBar;
-    DebugOut.Analog[30] = GPS_Nick;
+//    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[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;
-//    DebugOut.Analog[19] -= DebugOut.Analog[19]/128;
+    DebugOut.Analog[31] = GPS_Roll;

Subversion Repositories FlightCtrl

(root)/branches/salvo_gps/Basis_V0071h/trunk/fc.c - Rev 1085 → 1086