Subversion Repositories FlightCtrl

Rev

Rev 498 | Rev 513 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 498 Rev 499
Line 107... Line 107...
107 
unsigned char Parameter_I_Faktor = 10;          // Wert : 1-20
 107 
unsigned char Parameter_I_Faktor = 10;          // Wert : 1-20
108 
unsigned char Parameter_UserParam1 = 0;
 108 
unsigned char Parameter_UserParam1 = 0;
109 
unsigned char Parameter_UserParam2 = 0;
 109 
unsigned char Parameter_UserParam2 = 0;
110 
unsigned char Parameter_UserParam3 = 0;
 110 
unsigned char Parameter_UserParam3 = 0;
111 
unsigned char Parameter_UserParam4 = 0;
 111 
unsigned char Parameter_UserParam4 = 0;
- 
 
 112 
unsigned char Parameter_UserParam5 = 0;
- 
 
 113 
unsigned char Parameter_UserParam6 = 0;
- 
 
 114 
unsigned char Parameter_UserParam7 = 0;
- 
 
 115 
unsigned char Parameter_UserParam8 = 0;
112 
unsigned char Parameter_ServoNickControl = 100;
 116 
unsigned char Parameter_ServoNickControl = 100;
113 
unsigned char Parameter_LoopGasLimit = 70;
 117 
unsigned char Parameter_LoopGasLimit = 70;
114 
unsigned char Parameter_AchsKopplung1 = 0;
 118 
unsigned char Parameter_AchsKopplung1 = 0;
115 
unsigned char Parameter_AchsGegenKopplung1 = 0;
 119 
unsigned char Parameter_AchsGegenKopplung1 = 0;
- 
 
 120 
unsigned char Parameter_DynamicStability = 100;
116 
struct mk_param_struct EE_Parameter;
 121 
struct mk_param_struct EE_Parameter;
117 
signed int ExternStickNick = 0,ExternStickRoll = 0,ExternStickGier = 0, ExternHoehenValue = -20;
 122 
signed int ExternStickNick = 0,ExternStickRoll = 0,ExternStickGier = 0, ExternHoehenValue = -20;
Line 118... Line 123...
118 
 
 123 
 
119 
void Piep(unsigned char Anzahl)
 124 
void Piep(unsigned char Anzahl)
Line 367... Line 372...
367 
 CHK_POTI(Parameter_I_Faktor,EE_Parameter.I_Faktor,0,255);
 372 
 CHK_POTI(Parameter_I_Faktor,EE_Parameter.I_Faktor,0,255);
368 
 CHK_POTI(Parameter_UserParam1,EE_Parameter.UserParam1,0,255);
 373 
 CHK_POTI(Parameter_UserParam1,EE_Parameter.UserParam1,0,255);
369 
 CHK_POTI(Parameter_UserParam2,EE_Parameter.UserParam2,0,255);
 374 
 CHK_POTI(Parameter_UserParam2,EE_Parameter.UserParam2,0,255);
370 
 CHK_POTI(Parameter_UserParam3,EE_Parameter.UserParam3,0,255);
 375 
 CHK_POTI(Parameter_UserParam3,EE_Parameter.UserParam3,0,255);
371 
 CHK_POTI(Parameter_UserParam4,EE_Parameter.UserParam4,0,255);
 376 
 CHK_POTI(Parameter_UserParam4,EE_Parameter.UserParam4,0,255);
- 
 
 377 
 CHK_POTI(Parameter_UserParam5,EE_Parameter.UserParam5,0,255);
- 
 
 378 
 CHK_POTI(Parameter_UserParam6,EE_Parameter.UserParam6,0,255);
- 
 
 379 
 CHK_POTI(Parameter_UserParam7,EE_Parameter.UserParam7,0,255);
- 
 
 380 
 CHK_POTI(Parameter_UserParam8,EE_Parameter.UserParam8,0,255);
372 
 CHK_POTI(Parameter_ServoNickControl,EE_Parameter.ServoNickControl,0,255);
 381 
 CHK_POTI(Parameter_ServoNickControl,EE_Parameter.ServoNickControl,0,255);
373 
 CHK_POTI(Parameter_LoopGasLimit,EE_Parameter.LoopGasLimit,0,255);
 382 
 CHK_POTI(Parameter_LoopGasLimit,EE_Parameter.LoopGasLimit,0,255);
374 
 CHK_POTI(Parameter_AchsKopplung1,    EE_Parameter.AchsKopplung1,0,255);
 383 
 CHK_POTI(Parameter_AchsKopplung1,    EE_Parameter.AchsKopplung1,0,255);
375 
 CHK_POTI(Parameter_AchsGegenKopplung1,EE_Parameter.AchsGegenKopplung1,0,255);
 384 
 CHK_POTI(Parameter_AchsGegenKopplung1,EE_Parameter.AchsGegenKopplung1,0,255);
- 
 
 385 
 CHK_POTI(Parameter_DynamicStability,EE_Parameter.DynamicStability,0,255);
Line 376... Line 386...
376 
 
 386 
 
377 
 Ki = (float) Parameter_I_Faktor * 0.0001;
 387 
 Ki = (float) Parameter_I_Faktor * 0.0001;
378 
 MAX_GAS = EE_Parameter.Gas_Max;
 388 
 MAX_GAS = EE_Parameter.Gas_Max;
379 
 MIN_GAS = EE_Parameter.Gas_Min;
 389 
 MIN_GAS = EE_Parameter.Gas_Min;
Line 789... Line 799...
789 
//             Mess_IntegralNick -= ausgleichNick;
 799 
//             Mess_IntegralNick -= ausgleichNick;
790 
            }
 800 
            }
791 
           else last_n_n = 1;
 801 
           else last_n_n = 1;
792 
         } else  last_n_n = 0;
 802 
         } else  last_n_n = 0;
793 
        } else cnt = 0;
 803 
        } else cnt = 0;
794 
        if(cnt > 4) cnt = 4;
 - 
 
795 
//        if(cnt > Poti2 / 40) cnt =  Poti2 / 40;
 804 
        if(cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp;
796 
        if(IntegralFehlerNick >  FEHLER_LIMIT)   AdNeutralNick += cnt;
 805 
        if(IntegralFehlerNick >  FEHLER_LIMIT)   AdNeutralNick += cnt;
797 
        if(IntegralFehlerNick < -FEHLER_LIMIT)   AdNeutralNick -= cnt;
 806 
        if(IntegralFehlerNick < -FEHLER_LIMIT)   AdNeutralNick -= cnt;
Line 798... Line 807...
798 
 
 807 
 
799 
// Roll +++++++++++++++++++++++++++++++++++++++++++++++++
 808 
// Roll +++++++++++++++++++++++++++++++++++++++++++++++++
Line 829... Line 838...
829 
        } else
 838 
        } else
830 
        {
 839 
        {
831 
         cnt = 0;
 840 
         cnt = 0;
832 
        }
 841 
        }
833 
DebugOut.Analog[27] = ausgleichRoll;
 842 
DebugOut.Analog[27] = ausgleichRoll;
834 
        if(cnt > 4) cnt = 4;
 - 
 
835 
//        if(cnt > Poti2 / 40) cnt =  Poti2 / 40;
 843 
        if(cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp;
836 
//if(cnt > 1) beeptime = 50;
 844 
//if(cnt > 1) beeptime = 50;
837 
        if(IntegralFehlerRoll >  FEHLER_LIMIT)   AdNeutralRoll += cnt;
 845 
        if(IntegralFehlerRoll >  FEHLER_LIMIT)   AdNeutralRoll += cnt;
838 
        if(IntegralFehlerRoll < -FEHLER_LIMIT)   AdNeutralRoll -= cnt;
 846 
        if(IntegralFehlerRoll < -FEHLER_LIMIT)   AdNeutralRoll -= cnt;
839 
DebugOut.Analog[23] = AdNeutralNick;//10*(AdNeutralNick - StartNeutralNick);
 847 
DebugOut.Analog[23] = AdNeutralNick;//10*(AdNeutralNick - StartNeutralNick);
840 
DebugOut.Analog[24] = 10*(AdNeutralRoll - StartNeutralRoll);
 848 
DebugOut.Analog[24] = 10*(AdNeutralRoll - StartNeutralRoll);
Line 1036... Line 1044...
1036 
    if(SummeNick > 0) SummeNick-= (abs(SummeNick)/256 + 1); else SummeNick += abs(SummeNick)/256 + 1;
 1044 
    if(SummeNick > 0) SummeNick-= (abs(SummeNick)/256 + 1); else SummeNick += abs(SummeNick)/256 + 1;
1037 
    if(SummeNick >  16000) SummeNick =  16000;
 1045 
    if(SummeNick >  16000) SummeNick =  16000;
1038 
    if(SummeNick < -16000) SummeNick = -16000;
 1046 
    if(SummeNick < -16000) SummeNick = -16000;
1039 
    pd_ergebnis = DiffNick + Ki * SummeNick; // PI-Regler für Nick                                     
1047 
    pd_ergebnis = DiffNick + Ki * SummeNick; // PI-Regler für Nick                                     
1040 
    // Motor Vorn
 1048 
    // Motor Vorn
1041 
#define MUL  2
 1049 
//#define MUL  2
1042 
    if(pd_ergebnis >  MUL * (GasMischanteil + abs(GierMischanteil))) pd_ergebnis =  MUL * (GasMischanteil + abs(GierMischanteil));
 1050 
//    if(pd_ergebnis >  MUL * (GasMischanteil + abs(GierMischanteil))) pd_ergebnis =  MUL * (GasMischanteil + abs(GierMischanteil));
1043 
    if(pd_ergebnis < -MUL * (GasMischanteil + abs(GierMischanteil))) pd_ergebnis = -MUL * (GasMischanteil + abs(GierMischanteil));
 1051 
//    if(pd_ergebnis < -MUL * (GasMischanteil + abs(GierMischanteil))) pd_ergebnis = -MUL * (GasMischanteil + abs(GierMischanteil));
- 
 
 1052 
    tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64;
- 
 
 1053 
    if(pd_ergebnis >  tmp_int) pd_ergebnis =  tmp_int;
- 
 
 1054 
    if(pd_ergebnis < -tmp_int) pd_ergebnis = -tmp_int;
Line 1044... Line 1055...
1044 
 
 1055 
 
1045 
    motorwert = GasMischanteil + pd_ergebnis + GierMischanteil;   // Mischer
 1056 
    motorwert = GasMischanteil + pd_ergebnis + GierMischanteil;   // Mischer
1046 
        if ((motorwert < 0)) motorwert = 0;
 1057 
        if ((motorwert < 0)) motorwert = 0;
1047 
        else if(motorwert > MAX_GAS)        motorwert = MAX_GAS;
 1058 
        else if(motorwert > MAX_GAS)        motorwert = MAX_GAS;
Line 1061... Line 1072...
1061 
    else                 SummeRoll += DiffRoll;  // I-Anteil bei HH
 1072 
    else                 SummeRoll += DiffRoll;  // I-Anteil bei HH
1062 
    if(SummeRoll > 0) SummeRoll-= (abs(SummeRoll)/256 + 1); else SummeRoll += abs(SummeRoll)/256 + 1;
 1073 
    if(SummeRoll > 0) SummeRoll-= (abs(SummeRoll)/256 + 1); else SummeRoll += abs(SummeRoll)/256 + 1;
1063 
    if(SummeRoll >  16000) SummeRoll =  16000;
 1074 
    if(SummeRoll >  16000) SummeRoll =  16000;
1064 
    if(SummeRoll < -16000) SummeRoll = -16000;
 1075 
    if(SummeRoll < -16000) SummeRoll = -16000;
1065 
    pd_ergebnis = DiffRoll + Ki * SummeRoll;    // PI-Regler für Roll
 1076 
    pd_ergebnis = DiffRoll + Ki * SummeRoll;    // PI-Regler für Roll
1066 
    if(pd_ergebnis >  MUL * (GasMischanteil + abs(GierMischanteil))) pd_ergebnis =  MUL * (GasMischanteil + abs(GierMischanteil));
 1077 
    tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64;
- 
 
 1078 
    if(pd_ergebnis >  tmp_int) pd_ergebnis =  tmp_int;
1067 
    if(pd_ergebnis < -MUL * (GasMischanteil + abs(GierMischanteil))) pd_ergebnis = -MUL * (GasMischanteil + abs(GierMischanteil));
 1079 
    if(pd_ergebnis < -tmp_int) pd_ergebnis = -tmp_int;
1068 
    // Motor Links
 1080 
    // Motor Links
1069 
    motorwert = GasMischanteil + pd_ergebnis - GierMischanteil;
 1081 
    motorwert = GasMischanteil + pd_ergebnis - GierMischanteil;
1070 
        if ((motorwert < 0)) motorwert = 0;
 1082 
        if ((motorwert < 0)) motorwert = 0;
1071 
        else if(motorwert > MAX_GAS)            motorwert = MAX_GAS;
 1083 
        else if(motorwert > MAX_GAS)            motorwert = MAX_GAS;
1072 
        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
 1084 
        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;