WebSVN - FlightCtrl - Diff - Rev 420 and 446 - /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"
 #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;
 unsigned char Parameter_Luftdruck_D  = 48;      // Wert : 0-250
 unsigned char Parameter_MaxHoehe     = 251;      // Wert : 0-250
 unsigned char Parameter_Hoehe_P      = 16;      // Wert : 0-32
 unsigned char Parameter_Hoehe_ACC_Wirkung = 58; // Wert : 0-250
 unsigned char Parameter_KompassWirkung = 64;    // Wert : 0-250
-unsigned char Parameter_Gyro_P = 150;           // Wert : 10-250
+unsigned char Parameter_Gyro_P = 50;           // Wert : 10-250
 unsigned char Parameter_Gyro_I = 150;           // Wert : 0-250
 unsigned char Parameter_Gier_P = 2;             // Wert : 1-20
 unsigned char Parameter_I_Faktor = 10;          // Wert : 1-20
 unsigned char Parameter_UserParam1 = 0;
 unsigned char Parameter_UserParam2 = 0;
     AdNeutralNick = 0;
         AdNeutralRoll = 0;
         AdNeutralGier = 0;
     Parameter_AchsKopplung1 = 0;
     Parameter_AchsGegenKopplung1 = 0;
     CalibrierMittelwert();
     Delay_ms_Mess(100);
         CalibrierMittelwert();
     if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung aktiviert?
+     {
      AdNeutralNick= AdWertNick;
          AdNeutralRoll= AdWertRoll;
          AdNeutralGier= AdWertGier;
      StartNeutralRoll = AdNeutralRoll;
      StartNeutralNick = AdNeutralNick;
+// 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;
     KompassStartwert = KompassValue;
     GPS_Neutral();
     beeptime = 50;
         Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L;
         Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagRoll * 2500L;
+//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 - 704;//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();
-        Mittelwert();
+        if (n == 0)
+        {
+                ROT_ON; //led blitzen lassen
+        }
 //******PROVISORISCH***************
+    GRN_ON;
     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
+        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;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-// Emfang schlecht
+// Empfang schlecht
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    if(SenderOkay < 100)
+        {
         if(!PcZugriff)
+                    {
                     unsigned char setting = 2;
                     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
      Looping_Roll = 0;
      Looping_Nick = 0;
+    }
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Integrale auf ACC-Signal abgleichen
     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
+                ROT_OFF;
-//    IntegralFehlerRoll = (IntegralFehlerRoll * 1 + tmp_long2) / 2;
+// Salvo End
 /*
 DebugOut.Analog[30] = I_LageRoll * 10;
 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[22] = MittelIntegralRoll / 26;
+DebugOut.Analog[28] = ausgleichNick;
-DebugOut.Analog[28] = ausgleichNick;
+DebugOut.Analog[29] = ausgleichRoll;
-DebugOut.Analog[29] = ausgleichRoll;
+*/
         if(IntegralFehlerNick >  FEHLER_LIMIT)   AdNeutralNick += cnt;
         if(IntegralFehlerNick < -FEHLER_LIMIT)   AdNeutralNick -= cnt;
 // Roll +++++++++++++++++++++++++++++++++++++++++++++++++
         cnt = 1;// + labs(IntegralFehlerRoll) / 4096;
-ausgleichRoll = 0;
+                ausgleichRoll = 0;
         if(IntegralFehlerRoll >  FEHLER_LIMIT2)
+         {
            if(last_r_p)
+           {
     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 (n= GPS_CRTL(GPS_CMD_STOP)); //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];

Subversion Repositories FlightCtrl

(root)/branches/salvo_gps/Basis_V0066c/trunk/fc.c @ 446 - Rev 420 → 446