WebSVN - FlightCtrl - Diff - Rev 515 and 525 - /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 6.10.2007
+/*
+Driftkompensation fuer Gyros verbessert
+Linearsensor mit fixem Neutralwert
+Ersatzkompass abgeleitet aus Magnetkompass und Giergyro fuer nahezu neigungsunabhaengige Kompassfunktion
+*/
 #include "main.h"
 #include "eeprom.c"
 unsigned char MAX_GAS,MIN_GAS;
 unsigned char Notlandung = 0;
 unsigned char HoehenReglerAktiv = 0;
 long Umschlag180Nick = 250000L, Umschlag180Roll = 250000L;
+//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 2.9.2007 Ersatzkompass
+volatile long GyroKomp_Int;
+volatile int GyroKomp_Value; //  Der ermittelte Kompasswert aus Gyro und Magnetkompass
+// Salvo End
 float GyroFaktor;
 float IntegralFaktor;
 volatile int  DiffNick,DiffRoll;
 int  Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0;
     GPS_Neutral();
     beeptime = 50;
         Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L;
         Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagRoll * 2500L;
     ExternHoehenValue = 0;
+//Salvo 13.10.2007 Ersatzkompass und Gas
+        GyroKomp_Int =  KompassValue * GYROKOMP_INC_GRAD_DEFAULT; //Neu ab 3.1.2007
+        gas_mittel      =       30;
+        gas_actual      =       gas_mittel;
+// Salvo End
+}
 //############################################################################
 // Bearbeitet die Messwerte
         Mittelwert_AccHoch = ((long)Mittelwert_AccHoch * 1 + ((long)AdWertAccHoch)) / 2L;
     IntegralAccNick += ACC_AMPLIFY * AdWertAccNick;
     IntegralAccRoll += ACC_AMPLIFY * AdWertAccRoll;
     IntegralAccZ    += Aktuell_az - NeutralAccZ;
 // Gier  ++++++++++++++++++++++++++++++++++++++++++++++++
+//Salvo 12.11.2007
+                        GyroKomp_Int  += MesswertGier;
+//Salvo End
             Mess_Integral_Gier +=  MesswertGier;
             Mess_Integral_Gier2 += MesswertGier;
 // Kopplungsanteil  +++++++++++++++++++++++++++++++++++++
       if(!Looping_Nick && !Looping_Roll && (EE_Parameter.GlobalConfig & CFG_ACHSENKOPPLUNG_AKTIV))
+         {
      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***************
-        Mittelwert();
+    GRN_ON;
     GRN_ON;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Gaswert ermitteln
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+        GasMischanteil = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120;
+    if(GasMischanteil > MAX_GAS - 20) GasMischanteil = MAX_GAS - 20;
+//Salvo 13.10.2007 langsame Gasreduktion bei Unterspannung. Als Ausgangswert wird der bei UBAT=k gemessen Mittelwert genommen
+// und dieser dann langsam zwangsweise reduziert
+        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;
         GasMischanteil = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120;
+// Salvo End
+    if(GasMischanteil < 0) GasMischanteil = 0;
     if(GasMischanteil > MAX_GAS - 20) GasMischanteil = MAX_GAS - 20;
-    if(GasMischanteil < 0) GasMischanteil = 0;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-// Emfang schlecht
+// Empfang schlecht
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    if(SenderOkay < 100)
+        {
                 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(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
                 if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75)  // ACC Neutralwerte speichern
+                    {
                     if(++delay_neutral > 200)  // nicht sofort
                         eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_Z],(int)NeutralAccZ / 256);
                         eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_Z+1],(int)NeutralAccZ % 256);
                         Piep(GetActiveParamSetNumber());
+                        }
+                    }
                  else delay_neutral = 0;
+                }
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Gas ist unten
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
     Mess_IntegralRoll2 -= IntegralFehlerRoll;
 //    IntegralFehlerNick = (IntegralFehlerNick * 1 + tmp_long) / 2;
+//    IntegralFehlerRoll = (IntegralFehlerRoll * 1 + tmp_long2) / 2;
+//Salvo Ersatzkompass Ueberlauf korrigieren
+                if (GyroKomp_Int >= ((long)360 * GYROKOMP_INC_GRAD_DEFAULT)) GyroKomp_Int = GyroKomp_Int - (GYROKOMP_INC_GRAD_DEFAULT *(long)360); //neu ab 3.11.2007
+                if (GyroKomp_Int < 0) GyroKomp_Int = GyroKomp_Int + (GYROKOMP_INC_GRAD_DEFAULT *(long)360); //neu ab 3.11.2007
-//    IntegralFehlerRoll = (IntegralFehlerRoll * 1 + tmp_long2) / 2;
+                ROT_OFF;
 // Salvo End
-DebugOut.Analog[17] = IntegralAccNick / 26;
+/*DebugOut.Analog[17] = IntegralAccNick / 26;
 DebugOut.Analog[18] = IntegralAccRoll / 26;
 DebugOut.Analog[19] = IntegralFehlerNick;// / 26;
 DebugOut.Analog[20] = IntegralFehlerRoll;// / 26;
 DebugOut.Analog[21] = MittelIntegralNick / 26;
 DebugOut.Analog[22] = MittelIntegralRoll / 26;
+DebugOut.Analog[28] = ausgleichNick;
-//DebugOut.Analog[28] = ausgleichNick;
+DebugOut.Analog[29] = ausgleichRoll;
-DebugOut.Analog[29] = ausgleichRoll;
+DebugOut.Analog[30] = LageKorrekturRoll * 10;
-DebugOut.Analog[30] = LageKorrekturRoll * 10;
+*/
          } else  last_r_n = 0;
         } else
+        {
          cnt = 0;
+        }
-DebugOut.Analog[27] = ausgleichRoll;
+//DebugOut.Analog[27] = ausgleichRoll;
         if(cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp;
 //if(cnt > 1) beeptime = 50;
         if(IntegralFehlerRoll >  FEHLER_LIMIT)   AdNeutralRoll += cnt;
         if(IntegralFehlerRoll < -FEHLER_LIMIT)   AdNeutralRoll -= cnt;
-DebugOut.Analog[23] = AdNeutralNick;//10*(AdNeutralNick - StartNeutralNick);
+//DebugOut.Analog[23] = AdNeutralNick;//10*(AdNeutralNick - StartNeutralNick);
-DebugOut.Analog[24] = 10*(AdNeutralRoll - StartNeutralRoll);
+//DebugOut.Analog[24] = 10*(AdNeutralRoll - StartNeutralRoll);
+  }
   else
+  {
    LageKorrekturRoll = 0;
    LageKorrekturNick = 0;
     MittelIntegralRoll = 0;
     MittelIntegralNick2 = 0;
     MittelIntegralRoll2 = 0;
     ZaehlMessungen = 0;
+ }
-//DebugOut.Analog[31] = StickRoll / (26*IntegralFaktor);
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //  Gieren
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
     sollGier = tmp_int;
     Mess_Integral_Gier -= tmp_int;
     if(Mess_Integral_Gier > 50000) Mess_Integral_Gier = 50000;  // begrenzen
     if(Mess_Integral_Gier <-50000) Mess_Integral_Gier =-50000;
+// 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 alles ok
+           {
+                if  ((abs(KompassValue - Kompass_Value_Old)) <= 5) // Aufeinanderfolgende Werte duerfen nur minimal abweichen
+                 {
+                  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
+                 }
+           }
+           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);
+                }
+        }
+        else
+        {
+                if (gps_cmd != GPS_CMD_STOP)
+                {
+                        gps_cmd = GPS_CMD_STOP;
+                        n= GPS_CRTL(gps_cmd); //GPS Lageregelung beenden
+                }
+        }
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //  Kompass
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-    if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV))
+    if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV) && (Kompass_present > 0))
-     {
-       int w,v;
-       static int SignalSchlecht = 0;
-       w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln
-       v = abs(IntegralRoll /512);
+     {
+           if(v > w) w = v; // grösste Neigung ermitteln
-       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
+       w=0;
 // Salvo End
        w = (w * Parameter_KompassWirkung) / 64;           // auf die Wirkung normieren
        w = Parameter_KompassWirkung - w;                  // Wirkung ggf drosseln
        if(w > 0)
+        {
-        {
+// Salvo Kompasssteuerung **********************
-          if(!SignalSchlecht) Mess_Integral_Gier += (KompassRichtung * w) / 32;  // nach Kompass ausrichten
+                 if (magkompass_ok > 0) Mess_Integral_Gier += (KompassRichtung * w) / 32;  // nach Kompass ausrichten
-          if(SignalSchlecht) SignalSchlecht--;
+// Salvo End
-        }
+        }
         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;
+// Diverse parameter Debugging
+        DebugOut.Analog[16] =  dataset_cnt;
-    DebugOut.Analog[9] = UBat;
+        DebugOut.Analog[17] =  UBat;
-    DebugOut.Analog[10] = SenderOkay;
+        DebugOut.Analog[18] =  MesswertNick;
-    DebugOut.Analog[16] = Mittelwert_AccHoch;
+        DebugOut.Analog[19] =  MesswertRoll;
+        DebugOut.Analog[20] =  MesswertGier;
+        DebugOut.Analog[21] =  StickNick;
+        DebugOut.Analog[22] =  StickRoll;
+        DebugOut.Analog[23] =  StickGier;
+// GPS Debugging
+        DebugOut.Analog[26] =  gps_rel_act_position.utm_east;
+        DebugOut.Analog[27] =  gps_rel_act_position.utm_north;
+        DebugOut.Analog[28] =  gps_rel_act_position.utm_alt;
+        DebugOut.Analog[29] =  gps_sub_state+(20*gps_cmd);
 /*    DebugOut.Analog[16] = motor_rx[0];
     DebugOut.Analog[17] = motor_rx[1];
     DebugOut.Analog[18] = motor_rx[2];
     if(Looping_Roll) MesswertRoll = MesswertRoll * GyroFaktor;
     else             MesswertRoll = IntegralRoll * IntegralFaktor + MesswertRoll * GyroFaktor;
 //    MesswertGier = MesswertGier * (GyroFaktor/2) + Integral_Gier * IntegralFaktor;
     MesswertGier = MesswertGier * (GyroFaktor) + Integral_Gier * IntegralFaktor/2;
-DebugOut.Analog[28] = MesswertRoll;
+//DebugOut.Analog[28] = MesswertRoll;
 DebugOut.Analog[25] = IntegralRoll * IntegralFaktor;
 DebugOut.Analog[31] = StickRoll;// / (26*IntegralFaktor);

Subversion Repositories FlightCtrl

(root)/branches/salvo_gps/Basis_v0067g/trunk/fc.c - Rev 515 → 525