WebSVN - FlightCtrl - Diff - Rev 155 and 156

 // +  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 13.9.2007
+/*
+Driftkompensation fuer Gyros verbessert
+Linearsensor mit fixem neutralwert
+Ersatzkompass abgeleitet aus magnetkompass und Giergyro fuer nahezu neigungsubhaengige 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;
-static int SignalSchlecht = 0;
 uint8_t magkompass_ok=0;
  //Salvo 2.9.2007 Ersatzkompass
 volatile long GyroKomp_Int,GyroKomp_Int2;
         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?
     MesswertGier = 0;
     StartLuftdruck = Luftdruck;
     HoeheD = 0;
     Mess_Integral_Hoch = 0;
     KompassStartwert = KompassValue;
-    GPS_Neutral();
     beeptime = 50;
 //Salvo 2.9.2007 Ersatzkompass
         GyroKomp_Int = 0;
         GyroKomp_Int2 = 0;
 // Salvo End
     AccumulateGier = 0;   MessanzahlGier = 0;
     accumulate_AccRoll = 0;messanzahl_AccRoll = 0;
     accumulate_AccNick = 0;messanzahl_AccNick = 0;
     accumulate_AccHoch = 0;messanzahl_AccHoch = 0;
     Integral_Gier  = Mess_Integral_Gier;
-//    Integral_Gier2 = Mess_Integral_Gier2;
     IntegralNick = Mess_IntegralNick;
     IntegralRoll = Mess_IntegralRoll;
     IntegralNick2 = Mess_IntegralNick2;
     IntegralRoll2 = Mess_IntegralRoll2;
     // ADC einschalten
  Ki = (float) Parameter_I_Faktor * 0.0001;
  MAX_GAS = EE_Parameter.Gas_Max;
  MIN_GAS = EE_Parameter.Gas_Min;
+}
 //############################################################################
 //
 void MotorRegler(void)
 //############################################################################
      static int hoehenregler = 0;
      static char TimerWerteausgabe = 0;
      static char NeueKompassRichtungMerken = 0;
         Mittelwert();
 //******PROVISORISCH***************
-        Get_GPS_data();
+        short int n;
-        if (gps_act_position.status >  0)
+        n = Get_Rel_Position();
+        if (n == 0)
+        {
                 ROT_ON;
-                gps_act_position.status         = 0;
+//              gps_act_position.status         = 0;
+        }
 //******PROVISORISCH***************
                         if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung aktiviert?
+                          {
                              if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();
+                          }
+                        }
+                                                GPS_Save_Home();
+                    }
                  else delay_neutral = 0;
+                }
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Gas ist unten
                  if(IntegralFehlerNick > 500/DRIFT_FAKTOR)   AdNeutralNick++;
                  if(IntegralFehlerNick < -500/DRIFT_FAKTOR)  AdNeutralNick--;
                  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
+                        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--;
                  if(IntegralFehlerRoll > 500*4/DRIFT_FAKTOR)   AdNeutralRoll++;
                  if(IntegralFehlerRoll < -500*4/DRIFT_FAKTOR)  AdNeutralRoll--;
+                        }
 // Salvo End
-// Salvo 31.8.2007 Abgleich Giergyro nur wenn Kompass aktiv und ok ist ***********************
-// Ohne Kompass wird die Pseudo-Gyrodrift durch die Driftkompensation nur verschlimmert
-// Ohne Driftkompensation ist die Gierachse wesentlich stabiler
-//                      if ((EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV) && (!SignalSchlecht))
-// Salvo 13.9.2007
-//                      if ((EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV) && (magkompass_ok >0))
-//                      {
-//                       if(Mess_Integral_Gier2 > 500/DRIFT_FAKTOR)  AdNeutralGier--;
-//                       if(Mess_Integral_Gier2 <-500/DRIFT_FAKTOR)  AdNeutralGier++;
-//                      }
-//                      else  Mess_Integral_Gier2 = 0;
 // Salvo End ***********************
                         ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern
                 Mess_IntegralNick2 = IntegralNick;
                 Mess_IntegralRoll2 = IntegralRoll;
                 Mess_Integral_Gier2 = Integral_Gier;
                   ANALOG_ON;    // ADC einschalten
+                 }
+           }
            else magkompass_ok = 0;
+        }
 // Salvo End *************************
     if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV))
+     {
        w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln
        v = abs(IntegralRoll /512);
            if(v > w) w = v; // grösste Neigung ermitteln
-// Salvo 12.9.2007 Kompassdaempfung abschalten weil Ersatzkompass verwendet wird
-//         w=0, v=0;
-//Salvo End
-//       if(w < 20 && NeueKompassRichtungMerken && !SignalSchlecht)
+// Salvo 13.9.2007 Nur wenn Magnetkompass ordentliche Werte liefert
                 if ((magkompass_ok > 0)  &&  NeueKompassRichtungMerken)
+        {
-// Salvo 12.9.2007 Ersatzkompass nehmen statt Magnetkompass
-//      KompassStartwert = KompassValue;
-//        KompassStartwert = GyroKomp_Int/GYROKOMP_INC_GRAD_DEFAULT;
-// Salvo End *************************
-// Salvo 13.9.2007
-                KompassStartwert = KompassValue;
+                 KompassStartwert = KompassValue;
-// Salvo End
          NeueKompassRichtungMerken = 0;
+        }
 // Salvo 13.9.2007
        w=0;
 // Salvo End
           ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern
 // Salvo Kompasssteuerung **********************
                  if (magkompass_ok > 0) Mess_Integral_Gier += (KompassRichtung * w) / 32;  // nach Kompass ausrichten
-// Salvo End
-// Salvo 30.8.2007 Winkelbegrenzung **********************
-/*
-         if ((!SignalSchlecht) )
-                 {
-                        if (abs(KompassRichtung) < 135 )
-                        {
-                                Mess_Integral_Gier += (KompassRichtung * w) / 32;  // nach Kompass ausrichten
-                        }
-                 }
-*/
- // Salvo End *************************
 // Salvo End
-          ANALOG_ON;  // ADC einschalten
-          if(SignalSchlecht) SignalSchlecht--;
+          ANALOG_ON;  // ADC einschalten
-        }
         else SignalSchlecht = 500; // so lange das Signal taub stellen --> ca. 1 sek
+     }
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
     DebugOut.Analog[4] = MesswertGier;
     DebugOut.Analog[5] = HoehenWert;
     DebugOut.Analog[6] = (Mess_Integral_Hoch / 512);
     DebugOut.Analog[7] = GasMischanteil;
     DebugOut.Analog[8] = KompassValue;
+        DebugOut.Analog[9] =  gps_rel_act_position.utm_east;
+        DebugOut.Analog[10] = gps_rel_act_position.utm_north;
+        DebugOut.Analog[11] = gps_rel_act_position.status;
 // ******provisorisch
 //    DebugOut.Analog[9]  = cnt1;
 //    DebugOut.Analog[10] = cnt1;
 //      DebugOut.Analog[11] = cnt2;
 //    DebugOut.Analog[10]  = (gps_act_position.utm_east/10) % 10000;
 //    DebugOut.Analog[11] = (gps_act_position.utm_north/10) % 10000;
 // ******provisorisch
 /*
         DebugOut.Analog[9] = GyroKomp_Int/GYROKOMP_INC_GRAD_DEFAULT;
     DebugOut.Analog[10] = magkompass_ok;
+        DebugOut.Analog[11] = Mess_Integral_Gier2;
-        DebugOut.Analog[11] = Mess_Integral_Gier2;
+*/
 /*
         DebugOut.Analog[11] = GyroKomp_Inc_Grad;
         DebugOut.Analog[12] = GyroKomp_Value;
 */

Subversion Repositories FlightCtrl

(root)/branches/salvo_gps/fc.c @ 156 - Rev 155 → 156