WebSVN - FlightCtrl - Diff - Rev 469 and 492 - /branches/V0.72p Code Redesign killagreg/fc.c

 // +  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 // +  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 // +  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 // +  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 // +  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)
+// +  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.
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 unsigned char HoehenReglerAktiv = 0;
 long Umschlag180Nick = 250000L, Umschlag180Roll = 250000L;
 float GyroFaktor;
-float IntegralFaktor;
 float IntegralFaktor;
 volatile int  DiffNick,DiffRoll;
 int  Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0;
 volatile unsigned char Motor_Vorne,Motor_Hinten,Motor_Rechts,Motor_Links, Count;
 unsigned char MotorWert[5];
 unsigned char Parameter_ServoNickControl = 100;
 unsigned char Parameter_LoopGasLimit = 70;
 unsigned char Parameter_AchsKopplung1 = 0;
 unsigned char Parameter_AchsGegenKopplung1 = 0;
 struct mk_param_struct EE_Parameter;
+signed int ExternStickNick = 0,ExternStickRoll = 0,ExternStickGier = 0, ExternHoehenValue = -20;
 void Piep(unsigned char Anzahl)
+{
  while(Anzahl--)
     KompassStartwert = KompassValue;
     GPS_Neutral();
     beeptime = 50;
         Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L;
         Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagRoll * 2500L;
+    ExternHoehenValue = 0;
+}
 //############################################################################
 // Bearbeitet die Messwerte
 //++++++++++++++++++++++++++++++++++++++++++++++++
 // ADC einschalten
     ANALOG_ON;
 //++++++++++++++++++++++++++++++++++++++++++++++++
 DebugOut.Analog[11] = MesswertRoll;
+//DebugOut.Analog[11] = AdWertNick;
     Integral_Gier  = Mess_Integral_Gier;
     IntegralNick = Mess_IntegralNick;
     IntegralRoll = Mess_IntegralRoll;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // neue Werte von der Funke
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  if(!NewPpmData-- || Notlandung)
+  {
+    int tmp_int;
     ParameterZuordnung();
     StickNick = PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P;
     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 += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D;
     StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]];
     GyroFaktor     = ((float)Parameter_Gyro_P + 10.0) / 256.0;
     IntegralFaktor = ((float) Parameter_Gyro_I) / 44000;
+#define KEY_VALUE (Parameter_UserParam1 * 4)  //(Poti3 * 8)
+if(DubWiseKeys[1]) beeptime = 10;
+    if(DubWiseKeys[1] & DUB_KEY_UP)    tmp_int = KEY_VALUE;   else
+    if(DubWiseKeys[1] & DUB_KEY_DOWN)  tmp_int = -KEY_VALUE;  else   tmp_int = 0;
+    ExternStickNick = (ExternStickNick * 7 + tmp_int) / 8;
+    if(DubWiseKeys[1] & DUB_KEY_LEFT)  tmp_int = KEY_VALUE; else
+    if(DubWiseKeys[1] & DUB_KEY_RIGHT) tmp_int = -KEY_VALUE; else tmp_int = 0;
+    ExternStickRoll = (ExternStickRoll * 7 + tmp_int) / 8;
+    if(DubWiseKeys[0] & 8)  ExternStickGier = 50;else
+    if(DubWiseKeys[0] & 4)  ExternStickGier =-50;else ExternStickGier = 0;
+    if(DubWiseKeys[0] & 2)  ExternHoehenValue++;
+    if(DubWiseKeys[0] & 16) ExternHoehenValue--;
+    StickNick += ExternStickNick / 8;
+    StickRoll += ExternStickRoll / 8;
+    StickGier += ExternStickGier;
     if(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor =  0;
     if(GyroFaktor < 0) GyroFaktor = 0;
     if(IntegralFaktor < 0) IntegralFaktor = 0;
     // greift in den Stick ein, um ungewolltes überschlagen zu verhindern
    long tmp_long, tmp_long2;
     tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccNick);
     tmp_long /= 16;
     tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccRoll);
     tmp_long2 /= 16;
- #define AUSGLEICH 32
+ #define AUSGLEICH 32 //(Parameter_UserParam1 / 2)
     if(tmp_long >  AUSGLEICH)  tmp_long  = AUSGLEICH;
     if(tmp_long < -AUSGLEICH)  tmp_long  =-AUSGLEICH;
     if(tmp_long2 > AUSGLEICH)  tmp_long2 = AUSGLEICH;
     if(tmp_long2 <-AUSGLEICH)  tmp_long2 =-AUSGLEICH;
     Mess_IntegralNick -= tmp_long;
     MittelIntegralNick  /= ABGLEICH_ANZAHL;
     MittelIntegralRoll  /= ABGLEICH_ANZAHL;
         IntegralAccNick = (EE_Parameter.GyroAccFaktor * IntegralAccNick) / ABGLEICH_ANZAHL;
         IntegralAccRoll = (EE_Parameter.GyroAccFaktor * IntegralAccRoll) / ABGLEICH_ANZAHL;
     IntegralAccZ    = IntegralAccZ / ABGLEICH_ANZAHL;
-#define MAX_I 1//(Poti2/10)
+#define MAX_I 0//(Poti2/10)
 // Nick ++++++++++++++++++++++++++++++++++++++++++++++++
     IntegralFehlerNick = (long)(MittelIntegralNick - (long)IntegralAccNick);
     ausgleichNick = IntegralFehlerNick / EE_Parameter.GyroAccAbgleich;
-    if(labs(MittelIntegralNick_Alt - MittelIntegralNick) < 5000)
+/*    if(labs(MittelIntegralNick_Alt - MittelIntegralNick) < 5000)
+     {
-       tmp_long = IntegralFehlerNick / 700L;
+       tmp_long = IntegralFehlerNick / 256L;
        if(tmp_long > MAX_I) tmp_long = MAX_I;
        if(tmp_long <-MAX_I) tmp_long =-MAX_I;
        I_LageNick = tmp_long;
+     }
      else
+     {
       I_LageNick /= 2;
+     }
+*/
 // Roll ++++++++++++++++++++++++++++++++++++++++++++++++
     IntegralFehlerRoll = (long)(MittelIntegralRoll - (long)IntegralAccRoll);
     ausgleichRoll = IntegralFehlerRoll / EE_Parameter.GyroAccAbgleich;
-    if(labs(MittelIntegralRoll_Alt - MittelIntegralRoll) < 5000)
+/*    if(labs(MittelIntegralRoll_Alt - MittelIntegralRoll) < 5000)
+     {
-      tmp_long2 = IntegralFehlerRoll / 700L;
+      tmp_long2 = IntegralFehlerRoll / 256L;
       if(tmp_long2 > MAX_I) tmp_long2 = MAX_I;
       if(tmp_long2 <-MAX_I) tmp_long2 =-MAX_I;
       I_LageRoll = tmp_long2;
+     }
      else
+     {
       I_LageRoll /=2;
+     }
+*/
-// +++++++++++++++++++++++++++++++++++++++++++++++++++++
-   MittelIntegralNick_Alt = MittelIntegralNick;
-   MittelIntegralRoll_Alt = MittelIntegralRoll;
-// +++++++++++++++++++++++++++++++++++++++++++++++++++++
     Mess_IntegralNick -= ausgleichNick;
     Mess_IntegralRoll -= ausgleichRoll;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Gyro-Drift ermitteln
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
     MittelIntegralNick2 /= ABGLEICH_ANZAHL;
     MittelIntegralRoll2 /= ABGLEICH_ANZAHL;
-    tmp_long  = (long)(MittelIntegralNick2 - (long)IntegralAccNick);
+//    tmp_long  = (long)(MittelIntegralNick2 - (long)IntegralAccNick);
+//    tmp_long2 = (long)(MittelIntegralRoll2 - (long)IntegralAccRoll);
+    tmp_long  = IntegralNick2 - IntegralNick;
-    tmp_long2 = (long)(MittelIntegralRoll2 - (long)IntegralAccRoll);
+    tmp_long2 = IntegralRoll2 - IntegralRoll;
     //DebugOut.Analog[25] = MittelIntegralRoll2 / 26;
     IntegralFehlerNick = tmp_long;
 DebugOut.Analog[22] = MittelIntegralRoll / 26;
 //DebugOut.Analog[28] = ausgleichNick;
 DebugOut.Analog[29] = ausgleichRoll;
 DebugOut.Analog[30] = I_LageRoll * 10;
-#define FEHLER_LIMIT  (ABGLEICH_ANZAHL/2)
+#define FEHLER_LIMIT  (ABGLEICH_ANZAHL * 4)
-#define FEHLER_LIMIT2 (ABGLEICH_ANZAHL*4)
+#define FEHLER_LIMIT2 (ABGLEICH_ANZAHL * 16)
 #define BEWEGUNGS_LIMIT 20000
 // Nick +++++++++++++++++++++++++++++++++++++++++++++++++
+        cnt = 1;// + labs(IntegralFehlerNick) / 4096;
+        if(labs(MittelIntegralNick_Alt - MittelIntegralNick) < BEWEGUNGS_LIMIT)
         cnt = 1;// + labs(IntegralFehlerNick) / 4096;
         if(IntegralFehlerNick >  FEHLER_LIMIT2)
+         {
            if(last_n_p)
+           {
             cnt += labs(IntegralFehlerNick) / FEHLER_LIMIT2;
-            ausgleichNick = IntegralFehlerNick / 12;
+            ausgleichNick = IntegralFehlerNick / 8;
             if(ausgleichNick > 5000) ausgleichNick = 5000;
-//            Mess_IntegralNick -= ausgleichNick;
+            Mess_IntegralNick -= ausgleichNick;
+           }
            else last_n_p = 1;
          } else  last_n_p = 0;
         if(IntegralFehlerNick < -FEHLER_LIMIT2)
+         {
            if(last_n_n)
+            {
              cnt += labs(IntegralFehlerNick) / FEHLER_LIMIT2;
-             ausgleichNick = IntegralFehlerNick / 12;
+             ausgleichNick = IntegralFehlerNick / 8;
              if(ausgleichNick < -5000) ausgleichNick = -5000;
-//             Mess_IntegralNick -= ausgleichNick;
+             Mess_IntegralNick -= ausgleichNick;
+            }
            else last_n_n = 1;
-         } else  last_n_n = 0;
+         } else  last_n_n = 0;
+        } else cnt = 0;
+        if(cnt > 4) cnt = 4;
-        if(cnt > 5) cnt = 5;
+//        if(cnt > Poti2 / 40) cnt =  Poti2 / 40;
         if(IntegralFehlerNick >  FEHLER_LIMIT)   AdNeutralNick += cnt;
         if(IntegralFehlerNick < -FEHLER_LIMIT)   AdNeutralNick -= cnt;
+// Roll +++++++++++++++++++++++++++++++++++++++++++++++++
-// Roll +++++++++++++++++++++++++++++++++++++++++++++++++
+        cnt = 1;// + labs(IntegralFehlerNick) / 4096;
+ausgleichRoll = 0;
-        cnt = 1;// + labs(IntegralFehlerRoll) / 4096;
+        if(labs(MittelIntegralRoll_Alt - MittelIntegralRoll) < BEWEGUNGS_LIMIT)
 ausgleichRoll = 0;
         if(IntegralFehlerRoll >  FEHLER_LIMIT2)
+         {
            if(last_r_p)
            {
             cnt += labs(IntegralFehlerRoll) / FEHLER_LIMIT2;
-            ausgleichRoll = IntegralFehlerRoll / 12;
+            ausgleichRoll = IntegralFehlerRoll / 8;
+            if(ausgleichRoll > 5000) ausgleichRoll = 5000;
-            if(ausgleichRoll > 5000) ausgleichRoll = 5000;
+            Mess_IntegralRoll -= ausgleichRoll;
-//            Mess_IntegralRoll -= ausgleichRoll;
+//            beeptime = 50;
+           }
            else last_r_p = 1;
          } else  last_r_p = 0;
         if(IntegralFehlerRoll < -FEHLER_LIMIT2)
+         {
            if(last_r_n)
            {
             cnt += labs(IntegralFehlerRoll) / FEHLER_LIMIT2;
-            ausgleichRoll = IntegralFehlerRoll / 12;
+            ausgleichRoll = IntegralFehlerRoll / 8;
+            if(ausgleichRoll < -5000) ausgleichRoll = -5000;
-            if(ausgleichRoll < -5000) ausgleichRoll = -5000;
+            Mess_IntegralRoll -= ausgleichRoll;
-//            Mess_IntegralRoll -= ausgleichRoll;
+//            beeptime = 50;
+           }
+           else last_r_n = 1;
+         } else  last_r_n = 0;
+        } else
+        {
-           else last_r_n = 1;
+         beeptime = 50;
-         } else  last_r_n = 0;
+         cnt = 0;
+        }
+DebugOut.Analog[27] = ausgleichRoll;
+        if(cnt > 4) cnt = 4;
-DebugOut.Analog[27] = ausgleichRoll;
+//        if(cnt > Poti2 / 40) cnt =  Poti2 / 40;
         if(cnt > 5) cnt = 5;
+        if(IntegralFehlerRoll >  FEHLER_LIMIT)   AdNeutralRoll += cnt;
-        if(IntegralFehlerRoll >  FEHLER_LIMIT)   AdNeutralRoll += cnt;
+        if(IntegralFehlerRoll < -FEHLER_LIMIT)   AdNeutralRoll -= cnt;
-        if(IntegralFehlerRoll < -FEHLER_LIMIT)   AdNeutralRoll -= cnt;
+DebugOut.Analog[23] = AdNeutralNick;//10*(AdNeutralNick - StartNeutralNick);
+DebugOut.Analog[24] = 10*(AdNeutralRoll - StartNeutralRoll);
+  }
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++
-DebugOut.Analog[24] = 10*(AdNeutralRoll - StartNeutralRoll);
+   MittelIntegralNick_Alt = MittelIntegralNick;
    MittelIntegralRoll_Alt = MittelIntegralRoll;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++
     IntegralAccNick = 0;
     IntegralAccRoll = 0;
       else
         HoehenReglerAktiv = 1;
+    }
     else
+    {
-     SollHoehe = Parameter_MaxHoehe * EE_Parameter.Hoehe_Verstaerkung - 20;
+     SollHoehe = ((int) ExternHoehenValue + (int) Parameter_MaxHoehe) * (int)EE_Parameter.Hoehe_Verstaerkung - 20;
      HoehenReglerAktiv = 1;
+    }
     if(Notlandung) SollHoehe = 0;

Subversion Repositories FlightCtrl

(root)/branches/V0.72p Code Redesign killagreg/fc.c @ 1180 - Rev 469 → 492