WebSVN - FlightCtrl - Diff - Rev 720 and 723 - /branches/V0.70d CRK HexaLotte/fc.c

         Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L;
         Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagRoll * 2500L;
     ExternHoehenValue = 0;
     ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR;
     GierGyroFehler = 0;
+    SendVersionToNavi = 1;
+}
 //############################################################################
 // Bearbeitet die Messwerte
  if(!NewPpmData-- || Notlandung)
+  {
     int tmp_int;
         static int stick_nick,stick_roll;
     ParameterZuordnung();
-    StickNick = (StickNick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P) / 4;
+    stick_nick = (stick_nick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P) / 4;
-    StickNick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D;
+    stick_nick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D;
+    StickNick = stick_nick - GPS_Nick;
+//    StickNick  = (StickNick  * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P) / 4;
-    StickRoll = (StickRoll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P) / 4;
+    stick_roll = (stick_roll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P) / 4;
-    StickRoll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D;
+    stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D;
+    StickRoll = stick_roll - GPS_Roll;
+//    StickRoll = (StickRoll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P) / 4;
     StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]];
         StickGas  = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120;
-   if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]]) > MaxStickNick)
+/*   if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]]) > MaxStickNick)
      MaxStickNick = abs(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]]); else MaxStickNick--;
-   if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) > MaxStickRoll)
-     MaxStickRoll = abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]); else MaxStickRoll--;
+   if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) > MaxStickRoll)
-   if(Notlandung)  {MaxStickNick = 0; MaxStickRoll = 0;}
+     MaxStickRoll = abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]); else MaxStickRoll--;
 */
     GyroFaktor     = ((float)Parameter_Gyro_P + 10.0) / 256.0;
     IntegralFaktor = ((float) Parameter_Gyro_I) / 44000;
+    }
     if(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor =  0;
     if(GyroFaktor < 0) GyroFaktor = 0;
+    if(IntegralFaktor < 0) IntegralFaktor = 0;
+    if(abs(StickNick) > MaxStickNick) MaxStickNick = abs(StickNick); else MaxStickNick--;
+    if(abs(StickRoll) > MaxStickRoll) MaxStickRoll = abs(StickRoll); else MaxStickRoll--;
+    if(Notlandung)  {MaxStickNick = 0; MaxStickRoll = 0;}
     if(IntegralFaktor < 0) IntegralFaktor = 0;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Looping?
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
   if((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_LINKS)  Looping_Links = 1;
    long tmp_long, tmp_long2;
     tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccNick);
     tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccRoll);
     tmp_long /= 16;
     tmp_long2 /= 16;
-   if((MaxStickNick > 15) || (MaxStickRoll > 15))
+   if((MaxStickNick > 32) || (MaxStickRoll > 32))
+    {
     tmp_long  /= 3;
     tmp_long2 /= 3;
+    }
    if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25)
     ausgleichRoll = IntegralFehlerRoll / EE_Parameter.GyroAccAbgleich;
     LageKorrekturNick = ausgleichNick / ABGLEICH_ANZAHL;
     LageKorrekturRoll = ausgleichRoll / ABGLEICH_ANZAHL;
-   if((MaxStickNick > 15) || (MaxStickRoll > 15) || (abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25))
+   if((MaxStickNick > 32) || (MaxStickRoll > 32) || (abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25))
+    {
      LageKorrekturNick /= 2;
        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 + 3;
+       korrektur = w + 8;
        if(w < 25 && NeueKompassRichtungMerken && !KompassSignalSchlecht)
+        {
          KompassStartwert = KompassValue;
          NeueKompassRichtungMerken = 0;
+        }
           if(!KompassSignalSchlecht)
+          {
            fehler = ((540 + KompassValue - (ErsatzKompass/GIER_GRAD_FAKTOR)) % 360) - 180;
            GierGyroFehler += fehler;
 //           DebugOut.Analog[10] = fehler;
-           ErsatzKompass += (fehler * 128) / korrektur;
+           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;
+    }
     if(Notlandung) SollHoehe = 0;
     h = HoehenWert;
+    if((h > SollHoehe) && HoehenReglerAktiv)      // zu hoch --> drosseln
-    if((h > SollHoehe) && HoehenReglerAktiv)      // zu hoch --> drosseln
+     {
-     {      h = ((h - SollHoehe) * (int) Parameter_Hoehe_P) / 16; // Differenz bestimmen --> P-Anteil
+      h = ((h - SollHoehe) * (int) Parameter_Hoehe_P) / 16; // Differenz bestimmen --> P-Anteil
       h = GasMischanteil - h;         // vom Gas abziehen
       h -= (HoeheD * Parameter_Luftdruck_D)/8;    // D-Anteil
       tmp_int = ((Mess_Integral_Hoch / 512) * (signed long) Parameter_Hoehe_ACC_Wirkung) / 32;
       if(tmp_int > 50) tmp_int = 50;
 //    if(GasMischanteil < 20) GierMischanteil = 0;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Nick-Achse
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+    DiffNick = MesswertNick - StickNick;        // Differenz bestimmen
-// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+    if(IntegralFaktor) SummeNick += IntegralNick * IntegralFaktor - StickNick; // I-Anteil bei Winkelregelung
-    DiffNick = MesswertNick - (StickNick - GPS_Nick);   // Differenz bestimmen
+//    DiffNick = MesswertNick - (StickNick - GPS_Nick); // Differenz bestimmen
+//    if(IntegralFaktor) SummeNick += IntegralNick * IntegralFaktor - (StickNick - GPS_Nick); // I-Anteil bei Winkelregelung
     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;
     pd_ergebnis = DiffNick + Ki * SummeNick; // PI-Regler für Nick
         if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
         Motor_Hinten = motorwert;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Roll-Achse
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+        DiffRoll = MesswertRoll - StickRoll;    // Differenz bestimmen
+    if(IntegralFaktor) SummeRoll += IntegralRoll * IntegralFaktor - StickRoll;// I-Anteil bei Winkelregelung
-        DiffRoll = MesswertRoll - (StickRoll  - GPS_Roll);      // Differenz bestimmen
+//      DiffRoll = MesswertRoll - (StickRoll  - GPS_Roll);      // Differenz bestimmen
-    if(IntegralFaktor) SummeRoll += IntegralRoll * IntegralFaktor - (StickRoll  - GPS_Roll);// I-Anteil bei Winkelregelung
+//    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;
     pd_ergebnis = DiffRoll + Ki * SummeRoll;    // PI-Regler für Roll
     tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64;
     if(pd_ergebnis >  tmp_int) pd_ergebnis =  tmp_int;
     if(pd_ergebnis < -tmp_int) pd_ergebnis = -tmp_int;
     // Motor Links
     motorwert = GasMischanteil + pd_ergebnis - GierMischanteil;
-#define GRENZE Poti1
         if ((motorwert < 0)) motorwert = 0;
         else if(motorwert > MAX_GAS)            motorwert = MAX_GAS;
         if (motorwert < MIN_GAS)            motorwert = MIN_GAS;

Subversion Repositories FlightCtrl

(root)/branches/V0.70d CRK HexaLotte/fc.c @ 957 - Rev 720 → 723