📄 fc.c

📁 遥控电子玩具飞机用的控制前进后退的程序包
💻 C
📖 第 1 页 / 共 3 页
字号:
上一页 1 23
    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;
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//+ Analoge Steuerung per Seriell
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
   if(ExternControl.Config & 0x01 && Parameter_UserParam1 > 128)
    {
	 StickNick += (int) ExternControl.Nick * (int) EE_Parameter.Stick_P;
	 StickRoll += (int) ExternControl.Roll * (int) EE_Parameter.Stick_P;
	 StickGier += ExternControl.Gier;
     ExternHoehenValue =  (int) ExternControl.Hight * (int)EE_Parameter.Hoehe_Verstaerkung;
     if(ExternControl.Gas < StickGas) StickGas = ExternControl.Gas;
    }

    if(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor =  0;
    if(GyroFaktor < 0) GyroFaktor = 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;
  else 
   { 
     {
      if((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < (EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese))) Looping_Links = 0;  
     }  
   } 
  if((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < -EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_RECHTS) Looping_Rechts = 1;
   else 
   {
   if(Looping_Rechts) // Hysterese
     {
      if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > -(EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese)) Looping_Rechts = 0;
     }
   } 

  if((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_OBEN) Looping_Oben = 1;
  else 
   { 
    if(Looping_Oben)  // Hysterese
     {
      if((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < (EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese))) Looping_Oben = 0;  
     }  
   } 
  if((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < -EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_UNTEN) Looping_Unten = 1;
   else 
   {
    if(Looping_Unten) // Hysterese
     {
      if(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > -(EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese)) Looping_Unten = 0;
     }
   } 

   if(Looping_Links || Looping_Rechts)   Looping_Roll = 1; else Looping_Roll = 0;
   if(Looping_Oben  || Looping_Unten) {Looping_Nick = 1; Looping_Roll = 0; Looping_Links = 0; Looping_Rechts = 0;} else Looping_Nick = 0;
  } // Ende neue Funken-Werte

  if(Looping_Roll) beeptime = 100;
  if(Looping_Roll || Looping_Nick)
   {
    if(GasMischanteil > EE_Parameter.LoopGasLimit) GasMischanteil = EE_Parameter.LoopGasLimit;
   }
    

// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++		
// Bei Empfangsausfall im Flug 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++		
   if(Notlandung)
    {
     StickGier = 0;
     StickNick = 0;
     StickRoll = 0;
     GyroFaktor  = 0.1;
     IntegralFaktor = 0.005;
     Looping_Roll = 0;
     Looping_Nick = 0;
    }  


// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++		
// Integrale auf ACC-Signal abgleichen
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++		
#define ABGLEICH_ANZAHL 256L

 MittelIntegralNick  += IntegralNick;    // F黵 die Mittelwertbildung aufsummieren
 MittelIntegralRoll  += IntegralRoll;
 MittelIntegralNick2 += IntegralNick2;
 MittelIntegralRoll2 += IntegralRoll2;

 if(Looping_Nick || Looping_Roll)
  {
    IntegralAccNick = 0;
    IntegralAccRoll = 0;
    MittelIntegralNick = 0;
    MittelIntegralRoll = 0;
    MittelIntegralNick2 = 0;
    MittelIntegralRoll2 = 0;
    Mess_IntegralNick2 = Mess_IntegralNick;
    Mess_IntegralRoll2 = Mess_IntegralRoll;
    ZaehlMessungen = 0;
    LageKorrekturNick = 0;
    LageKorrekturRoll = 0;
  }

// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++		
  if(!Looping_Nick && !Looping_Roll)
  {
   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))
    {
    tmp_long  /= 3;
    tmp_long2 /= 3;
    }
   if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25)
    {
    tmp_long  /= 3;
    tmp_long2 /= 3;
    }

 #define AUSGLEICH 32
    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;
    Mess_IntegralRoll -= tmp_long2;
  }
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++		

 if(ZaehlMessungen >= ABGLEICH_ANZAHL)
 {
  static int cnt = 0;
  static char last_n_p,last_n_n,last_r_p,last_r_n;
  static long MittelIntegralNick_Alt,MittelIntegralRoll_Alt;
  if(!Looping_Nick && !Looping_Roll)
  {
    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 0//(Poti2/10)
// Nick ++++++++++++++++++++++++++++++++++++++++++++++++
    IntegralFehlerNick = (long)(MittelIntegralNick - (long)IntegralAccNick);
    ausgleichNick = IntegralFehlerNick / EE_Parameter.GyroAccAbgleich;
// Roll ++++++++++++++++++++++++++++++++++++++++++++++++     
    IntegralFehlerRoll = (long)(MittelIntegralRoll - (long)IntegralAccRoll); 
    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))
    {
     LageKorrekturNick /= 2;
     LageKorrekturNick /= 2;
    }

// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++		
// Gyro-Drift ermitteln
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++		
    MittelIntegralNick2 /= ABGLEICH_ANZAHL;
    MittelIntegralRoll2 /= ABGLEICH_ANZAHL;
    tmp_long  = IntegralNick2 - IntegralNick; 
    tmp_long2 = IntegralRoll2 - IntegralRoll; 
    //DebugOut.Analog[25] = MittelIntegralRoll2 / 26;

    IntegralFehlerNick = tmp_long;
    IntegralFehlerRoll = tmp_long2;
    Mess_IntegralNick2 -= IntegralFehlerNick;
    Mess_IntegralRoll2 -= IntegralFehlerRoll;

//    IntegralFehlerNick = (IntegralFehlerNick * 1 + tmp_long) / 2;
//    IntegralFehlerRoll = (IntegralFehlerRoll * 1 + tmp_long2) / 2;


DebugOut.Analog[17] = IntegralAccNick / 26;
DebugOut.Analog[18] = IntegralAccRoll / 26;
DebugOut.Analog[19] = IntegralFehlerNick;// / 26;
DebugOut.Analog[20] = IntegralFehlerRoll;// / 26;
DebugOut.Analog[21] = MittelIntegralNick / 26;
DebugOut.Analog[22] = MittelIntegralRoll / 26;
//DebugOut.Analog[28] = ausgleichNick;
DebugOut.Analog[29] = ausgleichRoll;
DebugOut.Analog[30] = LageKorrekturRoll * 10;

#define FEHLER_LIMIT  (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)
        {
        if(IntegralFehlerNick >  FEHLER_LIMIT2) 
         {
           if(last_n_p) 
           {
            cnt += labs(IntegralFehlerNick) / FEHLER_LIMIT2; 
            ausgleichNick = IntegralFehlerNick / 8;
            if(ausgleichNick > 5000) ausgleichNick = 5000;
            LageKorrekturNick += ausgleichNick / ABGLEICH_ANZAHL;
           } 
           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 / 8;
             if(ausgleichNick < -5000) ausgleichNick = -5000;
             LageKorrekturNick += ausgleichNick / ABGLEICH_ANZAHL;
            } 
           else last_n_n = 1;
         } else  last_n_n = 0;
        } else cnt = 0;
        if(cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp;
        if(IntegralFehlerNick >  FEHLER_LIMIT)   AdNeutralNick += cnt;
        if(IntegralFehlerNick < -FEHLER_LIMIT)   AdNeutralNick -= cnt;

// Roll +++++++++++++++++++++++++++++++++++++++++++++++++
        cnt = 1;// + labs(IntegralFehlerNick) / 4096;

        ausgleichRoll = 0;
        if(labs(MittelIntegralRoll_Alt - MittelIntegralRoll) < BEWEGUNGS_LIMIT)
        {
        if(IntegralFehlerRoll >  FEHLER_LIMIT2) 
         {
           if(last_r_p) 
           {
            cnt += labs(IntegralFehlerRoll) / FEHLER_LIMIT2; 
            ausgleichRoll = IntegralFehlerRoll / 8;
            if(ausgleichRoll > 5000) ausgleichRoll = 5000;
            LageKorrekturRoll += ausgleichRoll / ABGLEICH_ANZAHL;
           } 
           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 / 8;
            if(ausgleichRoll < -5000) ausgleichRoll = -5000;
            LageKorrekturRoll += ausgleichRoll / ABGLEICH_ANZAHL;
           }
           else last_r_n = 1;
         } else  last_r_n = 0;
        } else 
        {
         cnt = 0;
        } 

        if(cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp;
        if(IntegralFehlerRoll >  FEHLER_LIMIT)   AdNeutralRoll += cnt;
        if(IntegralFehlerRoll < -FEHLER_LIMIT)   AdNeutralRoll -= cnt;
DebugOut.Analog[27] = ausgleichRoll;
DebugOut.Analog[23] = AdNeutralNick;//10*(AdNeutralNick - StartNeutralNick);
DebugOut.Analog[24] = 10*(AdNeutralRoll - StartNeutralRoll);
  }
  else 
  {
   LageKorrekturRoll = 0;
   LageKorrekturNick = 0;
  }

  if(!IntegralFaktor) { LageKorrekturRoll = 0; LageKorrekturNick = 0;} // z.B. bei HH
// +++++++++++++++++++++++++++++++++++++++++++++++++++++     
   MittelIntegralNick_Alt = MittelIntegralNick;      
   MittelIntegralRoll_Alt = MittelIntegralRoll;      
// +++++++++++++++++++++++++++++++++++++++++++++++++++++     
    IntegralAccNick = 0;
    IntegralAccRoll = 0;
    IntegralAccZ = 0;
    MittelIntegralNick = 0;
    MittelIntegralRoll = 0;
    MittelIntegralNick2 = 0;
    MittelIntegralRoll2 = 0;
    ZaehlMessungen = 0;
 }
//DebugOut.Analog[31] = StickRoll / (26*IntegralFaktor);

// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++		
//  Gieren
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++		
    if(abs(StickGier) > 20) // war 35 
     {
      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
上一页 1 23
⌨️ 快捷键说明

复制代码 Ctrl + C
搜索代码 Ctrl + F
全屏模式 F11
切换主题 Ctrl + Shift + D
显示快捷键 ?
增大字号 Ctrl + =
减小字号 Ctrl + -