WebSVN - FlightCtrl - Diff - Rev 595 and 604 - /branches/V0.88e_ACC-HH_HR

     static signed long tmpl,tmpl2;
     MesswertGier = (signed int) AdNeutralGier - AdWertGier;
     MesswertRoll = (signed int) AdWertRoll - AdNeutralRoll;
     MesswertNick = (signed int) AdWertNick - AdNeutralNick;
+//DebugOut.Analog[26] = MesswertNick;
+DebugOut.Analog[28] = MesswertRoll;
 // Beschleunigungssensor  ++++++++++++++++++++++++++++++++++++++++++++++++
         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;
 // neue Werte von der Funke
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  if(!NewPpmData-- || Notlandung)
+  {
     int tmp_int;
+        static int stick_nick,stick_roll;
     ParameterZuordnung();
-    StickNick = PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P;
+    StickNick = (StickNick * 7 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P) / 8;
     StickNick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D;
-    StickRoll = PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P;
+    StickRoll = (StickRoll * 7 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P) / 8;
     StickRoll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D;
     StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]];
         StickGas  = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120;
     GyroFaktor     = ((float)Parameter_Gyro_P + 10.0) / 256.0;
+    }
     if(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor =  0;
     if(GyroFaktor < 0) GyroFaktor = 0;
+    if(IntegralFaktor < 0) IntegralFaktor = 0;
-    if(IntegralFaktor < 0) IntegralFaktor = 0;
+/*
     // greift in den Stick ein, um ungewolltes überschlagen zu verhindern
     if(!(EE_Parameter.LoopConfig & CFG_LOOP_LINKS) && !(EE_Parameter.LoopConfig & CFG_LOOP_RECHTS))
+     {
       if(IntegralNick >  60000)
+      {
        StickRoll += 8 * EE_Parameter.Stick_P;
        if(IntegralRoll >  80000) StickRoll -= 16 * EE_Parameter.Stick_P;
+      }
+     }
+*/
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Looping?
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
   if((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_LINKS)  Looping_Links = 1;
   else
+  }
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //  Drehgeschwindigkeit und -winkel zu einem Istwert zusammenfassen
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+//DebugOut.Analog[26] = MesswertNick;
+//DebugOut.Analog[28] = MesswertRoll;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
     if(Looping_Nick) MesswertNick = MesswertNick * GyroFaktor;
     else             MesswertNick = IntegralNick * IntegralFaktor + MesswertNick * GyroFaktor;
     if(Looping_Roll) MesswertRoll = MesswertRoll * GyroFaktor;
-    else             MesswertRoll = IntegralRoll * IntegralFaktor + MesswertRoll * GyroFaktor;
-//    MesswertGier = MesswertGier * (GyroFaktor/2) + Integral_Gier * IntegralFaktor;
+    else             MesswertRoll = IntegralRoll * IntegralFaktor + MesswertRoll * GyroFaktor;
-    MesswertGier = MesswertGier * (GyroFaktor) + Integral_Gier * IntegralFaktor/2;
     MesswertGier = MesswertGier * (GyroFaktor) + Integral_Gier * IntegralFaktor/2;
 DebugOut.Analog[28] = MesswertRoll;
+DebugOut.Analog[25] = IntegralRoll * IntegralFaktor;
-DebugOut.Analog[25] = IntegralRoll * IntegralFaktor;
+DebugOut.Analog[31] = StickRoll;// / (26*IntegralFaktor);
-DebugOut.Analog[31] = StickRoll;// / (26*IntegralFaktor);
+DebugOut.Analog[28] = MesswertRoll;
     // Maximalwerte abfangen
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Gier-Anteil
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 #define MUL_G  1.0
     GierMischanteil = MesswertGier - sollGier;     // Regler für Gier
-//GierMischanteil = 0;
+// GierMischanteil = 0;
     if(GierMischanteil > (GasMischanteil / 2)) GierMischanteil = GasMischanteil / 2;
     if(GierMischanteil < -(GasMischanteil / 2)) GierMischanteil = -(GasMischanteil / 2);
     if(GierMischanteil > ((MAX_GAS - GasMischanteil))) GierMischanteil = ((MAX_GAS - GasMischanteil));
     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;
-        Motor_Links = motorwert;
+Motor_Links = motorwert;
+/*
+if(motorwert > (int)Motor_Links + GRENZE)      Motor_Links += GRENZE;
+else if(motorwert < (int)Motor_Links - GRENZE) Motor_Links -= GRENZE;
+else Motor_Links = motorwert;
+*/
     // Motor Rechts
         motorwert = GasMischanteil - pd_ergebnis - GierMischanteil;
         if ((motorwert < 0)) motorwert = 0;
         else if(motorwert > MAX_GAS)            motorwert = MAX_GAS;
         if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
-        Motor_Rechts = motorwert;
+Motor_Rechts = motorwert;
+/*
+if(motorwert > Motor_Rechts + GRENZE)      Motor_Rechts += GRENZE;
+else if(motorwert < Motor_Rechts - GRENZE) Motor_Rechts -= GRENZE;
+else Motor_Rechts = motorwert;
+*/
    // +++++++++++++++++++++++++++++++++++++++++++++++
+}

Subversion Repositories FlightCtrl

(root)/branches/V0.88e_ACC-HH_HR_MartinR/fc.c @ 2165 - Rev 595 → 604