WebSVN - FlightCtrl - Diff - Rev 744 and 805 - /branches/V0.82b-Arthur-P/fc.c

 unsigned char h,m,s;
 volatile unsigned int I2CTimeout = 100;
 volatile int MesswertNick,MesswertRoll,MesswertGier;
 volatile int AdNeutralNick = 0,AdNeutralRoll = 0,AdNeutralGier = 0,StartNeutralRoll = 0,StartNeutralNick = 0;
 volatile int Mittelwert_AccNick, Mittelwert_AccRoll,Mittelwert_AccHoch, NeutralAccX=0, NeutralAccY=0;
+int NaviAccNick, NaviAccRoll,NaviCntAcc = 0;
 volatile float NeutralAccZ = 0;
 unsigned char CosinusNickWinkel = 0, CosinusRollWinkel = 0;
 long IntegralNick = 0,IntegralNick2 = 0;
 long IntegralRoll = 0,IntegralRoll2 = 0;
 long IntegralAccNick = 0,IntegralAccRoll = 0,IntegralAccZ = 0;
         Mittelwert_AccNick = ((long)Mittelwert_AccNick * 1 + ((ACC_AMPLIFY * (long)AdWertAccNick))) / 2L;
         Mittelwert_AccRoll = ((long)Mittelwert_AccRoll * 1 + ((ACC_AMPLIFY * (long)AdWertAccRoll))) / 2L;
         Mittelwert_AccHoch = ((long)Mittelwert_AccHoch * 1 + ((long)AdWertAccHoch)) / 2L;
     IntegralAccNick += ACC_AMPLIFY * AdWertAccNick;
     IntegralAccRoll += ACC_AMPLIFY * AdWertAccRoll;
+    NaviAccNick    += AdWertAccNick;
+    NaviAccRoll    += AdWertAccRoll;
+    NaviCntAcc++;
     IntegralAccZ    += Aktuell_az - NeutralAccZ;
 // Gier  ++++++++++++++++++++++++++++++++++++++++++++++++
             ErsatzKompass +=  MesswertGier;
             Mess_Integral_Gier +=  MesswertGier;
             Mess_Integral_Gier2 += MesswertGier;
     int tmp_int;
         static int stick_nick,stick_roll;
     ParameterZuordnung();
     stick_nick = (stick_nick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P) / 4;
     stick_nick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D;
-    StickNick = stick_nick - GPS_Nick;
+    StickNick = stick_nick - (GPS_Nick + GPS_Nick2);
 //    StickNick  = (StickNick  * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P) / 4;
     stick_roll = (stick_roll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P) / 4;
     stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D;
-    StickRoll = stick_roll - GPS_Roll;
+    StickRoll = stick_roll - (GPS_Roll + GPS_Roll2);
 //    StickRoll = (StickRoll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P) / 4;
        w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln
        v = abs(IntegralRoll /512);
 //v /= 4;
 //w /= 4;
        if(v > w) w = v; // grösste Neigung ermitteln
-       korrektur = w + 8;
+       korrektur = w + 4;
        if(w < 25 && NeueKompassRichtungMerken && !KompassSignalSchlecht)
+        {
          KompassStartwert = KompassValue;
          NeueKompassRichtungMerken = 0;
+        }
+       fehler = ((540 + KompassValue - (ErsatzKompass/GIER_GRAD_FAKTOR)) % 360) - 180;
+       ErsatzKompass += (fehler * 8);// / korrektur;
+//           DebugOut.Analog[10] = fehler;
        w = (w * Parameter_KompassWirkung) / 64;           // auf die Wirkung normieren
        w = Parameter_KompassWirkung - w;                  // Wirkung ggf drosseln
        if(w > 0)
+        {
           if(!KompassSignalSchlecht)
+          {
-           fehler = ((540 + KompassValue - (ErsatzKompass/GIER_GRAD_FAKTOR)) % 360) - 180;
            GierGyroFehler += fehler;
-//           DebugOut.Analog[10] = fehler;
-           ErsatzKompass += (fehler * 16) / korrektur;
            v = (KompassRichtung * w) / 32;  // nach Kompass ausrichten
            w = Parameter_KompassWirkung;
            if(v > w) v = w; // Begrenzen
            else
            if(v < -w) v = -w;
 //  Debugwerte zuordnen
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
   if(!TimerWerteausgabe--)
+   {
     TimerWerteausgabe = 24;
     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[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[11] = ErsatzKompass / GIER_GRAD_FAKTOR;
     DebugOut.Analog[10] = SenderOkay;
     DebugOut.Analog[16] = Mittelwert_AccHoch;
     DebugOut.Analog[18] = (int)FromNaviCtrl_Value.OsdBar;
     DebugOut.Analog[30] = GPS_Nick;
+    DebugOut.Analog[31] = GPS_Roll;
+/*    DebugOut.Analog[19] += (DebugOut.Analog[1] - Mittelwert_AccRoll)/32;
+    DebugOut.Analog[19] -= DebugOut.Analog[19]/32;
+    if(DebugOut.Analog[19] > 0) DebugOut.Analog[19]--; else DebugOut.Analog[19]++;
+GPS_Roll2 = (1 * GPS_Roll + ((DebugOut.Analog[19] / 16) * Poti3) / 32) / 2;
+    DebugOut.Analog[20] += (DebugOut.Analog[0] - Mittelwert_AccNick)/32;
+    DebugOut.Analog[20] -= DebugOut.Analog[20]/32;
+    if(DebugOut.Analog[20] > 0) DebugOut.Analog[20]--; else DebugOut.Analog[20]++;
+GPS_Nick2 = (1 * GPS_Roll + ((DebugOut.Analog[20] / 16) * Poti3) / 32) / 2;
+*/
+//GPS_Nick = (GPS_Nick + (DebugOut.Analog[20] * Poti3) / 32) / 2;
+//    DebugOut.Analog[19] -= DebugOut.Analog[19]/128;
-    DebugOut.Analog[31] = GPS_Roll;
+//    if(DebugOut.Analog[19] > 0) DebugOut.Analog[19]--; else DebugOut.Analog[19]++;
 /*    DebugOut.Analog[16] = motor_rx[0];
     DebugOut.Analog[17] = motor_rx[1];
     DebugOut.Analog[18] = motor_rx[2];
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Nick-Achse
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
     DiffNick = MesswertNick - StickNick;        // Differenz bestimmen
     if(IntegralFaktor) SummeNick += IntegralNick * IntegralFaktor - StickNick; // I-Anteil bei Winkelregelung
-//    DiffNick = MesswertNick - (StickNick - GPS_Nick); // Differenz bestimmen
-//    if(IntegralFaktor) SummeNick += IntegralNick * IntegralFaktor - (StickNick - GPS_Nick); // I-Anteil bei Winkelregelung
     else  SummeNick += DiffNick; // I-Anteil bei HH
     if(SummeNick >  16000) SummeNick =  16000;
     if(SummeNick < -16000) SummeNick = -16000;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Roll-Achse
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
         DiffRoll = MesswertRoll - StickRoll;    // Differenz bestimmen
     if(IntegralFaktor) SummeRoll += IntegralRoll * IntegralFaktor - StickRoll;// I-Anteil bei Winkelregelung
-//      DiffRoll = MesswertRoll - (StickRoll  - GPS_Roll);      // Differenz bestimmen
-//    if(IntegralFaktor) SummeRoll += IntegralRoll * IntegralFaktor - (StickRoll  - GPS_Roll);// I-Anteil bei Winkelregelung
     else                 SummeRoll += DiffRoll;  // I-Anteil bei HH
     if(SummeRoll >  16000) SummeRoll =  16000;
     if(SummeRoll < -16000) SummeRoll = -16000;

Subversion Repositories FlightCtrl

(root)/branches/V0.82b-Arthur-P/fc.c @ 2121 - Rev 744 → 805