WebSVN - FlightCtrl - Diff - Rev 1096 and 1099 - /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
+// Aenderungen von Peter Muehlenbrock ("Salvo") Stand 29.12.2008
 /*
 Driftkompensation fuer Gyros verbessert
 Linearsensor optional mit fixem Neutralwert
 Ersatzkompass abgeleitet aus Magnetkompass und Giergyro fuer nahezu neigungsunabhaengige Kompassfunktion
 */
     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 > 10) // nur Abgleichen wenn keine Stickbewegung da
+                        {
+          }
           if (GyroKomp_Int < 0)  GyroKomp_Int = GyroKomp_Int + 360L;
           GyroKomp_Int = (GyroKomp_Int%360L) * (long)GIER_GRAD_FAKTOR; // An Magnetkompasswert annaehern
+        }
+   }
-   else //Kompassfehler
+   else //Kompass wegen grosser Neigung ungueltig
+   {
     magkompass_ok = 0;
         GyroGier_Comp = 0;
+   }
-   Kompass_Value_Old    =       KompassValue;
+   Kompass_Value_Old =KompassValue;
+}
-else  magkompass_ok = 0;
 // Salvo End *************************
 // Salvo 6.10.2007
-        // GPS Home aktivieren wenn Knueppel in Ruhelage und Hoehenschalter aktiviert ist
+        //GPS Home aktivieren wenn Knueppel in Ruhelage und Hoehenschalter aktiviert ist
-        //GPS Hold Aktiveren wenn Knueppel in Ruhelage sind
+        //GPS Hold Aktivieren 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
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //  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;
-     }
+    }
     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 <-50000) Mess_Integral_Gier =-50000;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //  Kompass
-// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-//DebugOut.Analog[29] = (MaxStickNick + MaxStickRoll);
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Salvo 29.12.2008
-                if (NeueKompassRichtungMerken)
+        if (NeueKompassRichtungMerken)
-        {
+    {
-                 KompassStartwert = KompassValue;
+         KompassStartwert = KompassValue;
-         NeueKompassRichtungMerken = 0;
+     NeueKompassRichtungMerken = 0;
+    }
 // Salvo 13.9.2007
+    w=0;
-       w=0;
+//  w = (w * Parameter_KompassWirkung) / 64; // auf die Wirkung normieren
-// Salvo End
-       w = (w * Parameter_KompassWirkung) / 64;           // auf die Wirkung normieren
+// Salvo End
-       w = Parameter_KompassWirkung - w;                  // Wirkung ggf drosseln
+    w = Parameter_KompassWirkung - w; // Wirkung ggf drosseln
-       if(w >= 0)
+    if(w > 0)
+    {
 // Salvo Kompasssteuerung **********************
-                 if (magkompass_ok > 0) Mess_Integral_Gier += (KompassRichtung * w) / 32;  // nach Kompass ausrichten wenn dieser ok ist
+        if (magkompass_ok > 0) Mess_Integral_Gier += (KompassRichtung * w) / 32;  // nach Kompass ausrichten wenn dieser ok ist
 // Salvo End
-        }
+    }
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

Subversion Repositories FlightCtrl

(root)/branches/salvo_gps/Basis_V0071h/trunk/fc.c - Rev 1096 → 1099