118,8 → 118,8 |
int LageKorrekturRoll = 0,LageKorrekturNick = 0, HoverGas = 0; |
//float Ki = FAKTOR_I; |
int Ki = 10300 / 33; |
unsigned char Looping_Nick = 0,Looping_Roll = 0; |
unsigned char Looping_Links = 0, Looping_Rechts = 0, Looping_Unten = 0, Looping_Oben = 0; |
//unsigned char Looping_Nick = 0,Looping_Roll = 0; |
//unsigned char Looping_Links = 0, Looping_Rechts = 0, Looping_Unten = 0, Looping_Oben = 0; |
|
unsigned char Parameter_Luftdruck_D = 48; // Wert : 0-250 |
unsigned char Parameter_HoehenSchalter = 0; // Wert : 0-250 |
604,7 → 604,7 |
Mess_Integral_Gier += MesswertGier; |
ErsatzKompass += MesswertGier; |
// Kopplungsanteil +++++++++++++++++++++++++++++++++++++ |
if(!Looping_Nick && !Looping_Roll) |
// if(!Looping_Nick && !Looping_Roll) |
{ |
tmpl3 = (MesswertRoll * winkel_nick) / 2048L; |
tmpl3 *= Parameter_AchsKopplung2; //65 |
627,7 → 627,7 |
if(abs(MesswertGier) > 64) if(labs(tmpl) > 128 || labs(tmpl2) > 128) TrichterFlug = 1; |
//MesswertGier += (Parameter_CouplingYawCorrection * tmpl4) / 256; |
} |
else tmpl = tmpl2 = KopplungsteilNickRoll = KopplungsteilRollNick = 0; |
// else tmpl = tmpl2 = KopplungsteilNickRoll = KopplungsteilRollNick = 0; |
TrimRoll = tmpl - tmpl2 / 100L; |
TrimNick = -tmpl2 + tmpl / 100L; |
// Kompasswert begrenzen ++++++++++++++++++++++++++++++++++++++++++++++++ |
696,7 → 696,7 |
if(RohMesswertNick > 0) TrimNick += ((long) abs(KopplungsteilRollNick) * Parameter_CouplingYawCorrection) / 64L; |
else TrimNick -= ((long) abs(KopplungsteilRollNick) * Parameter_CouplingYawCorrection) / 64L; |
|
if(Parameter_GlobalConfig & CFG_DREHRATEN_BEGRENZER && !Looping_Nick && !Looping_Roll) |
if(Parameter_GlobalConfig & CFG_DREHRATEN_BEGRENZER) |
{ |
if(RohMesswertNick > 256) MesswertNick += 1 * (RohMesswertNick - 256); |
else if(RohMesswertNick < -256) MesswertNick += 1 * (RohMesswertNick + 256); |
1428,54 → 1428,8 |
} |
else MaxStickRoll--; |
if(FC_StatusFlags & FC_STATUS_EMERGENCY_LANDING) {MaxStickNick = 0; MaxStickRoll = 0;} |
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Looping? |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > EE_Parameter.LoopThreshold) && EE_Parameter.BitConfig & 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.BitConfig & 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.BitConfig & 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.BitConfig & 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 || Looping_Nick) |
{ |
if(GasMischanteil > EE_Parameter.LoopGasLimit) GasMischanteil = EE_Parameter.LoopGasLimit; |
TrichterFlug = 1; |
} |
|
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Bei Empfangsausfall im Flug |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1500,8 → 1454,6 |
IntegralFaktor = 120; |
GyroFaktorGier = 90; |
IntegralFaktorGier = 120; |
Looping_Roll = 0; |
Looping_Nick = 0; |
} |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Integrale auf ACC-Signal abgleichen |
1510,17 → 1462,10 |
|
MittelIntegralNick += IntegralNick; // Für die Mittelwertbildung aufsummieren |
MittelIntegralRoll += IntegralRoll; |
if(Looping_Nick || Looping_Roll) |
{ |
MittelIntegralNick = 0; |
MittelIntegralRoll = 0; |
ZaehlMessungen = 0; |
LageKorrekturNick = 0; |
LageKorrekturRoll = 0; |
} |
|
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(!Looping_Nick && !Looping_Roll && (Aktuell_az > 512 || MotorenEin)) |
if((Aktuell_az > 512 || MotorenEin)) |
{ |
long tmp_long, tmp_long2; |
if(FromNaviCtrl_Value.Kalman_K > 0 /*&& !TrichterFlug*/) |
1689,8 → 1634,8 |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(TrichterFlug) { SummeRoll = 0; SummeNick = 0;}; |
|
if(!Looping_Nick) IntegralNickMalFaktor = (IntegralNick * IntegralFaktor) / (44000 / STICK_GAIN); else IntegralNickMalFaktor = 0; |
if(!Looping_Roll) IntegralRollMalFaktor = (IntegralRoll * IntegralFaktor) / (44000 / STICK_GAIN); else IntegralRollMalFaktor = 0; |
IntegralNickMalFaktor = (IntegralNick * IntegralFaktor) / (44000 / STICK_GAIN); |
IntegralRollMalFaktor = (IntegralRoll * IntegralFaktor) / (44000 / STICK_GAIN); |
|
#define TRIM_MAX 200 |
if(TrimNick > TRIM_MAX) TrimNick = TRIM_MAX; else if(TrimNick <-TRIM_MAX) TrimNick =-TRIM_MAX; |
1755,7 → 1700,7 |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
GasMischanteil *= STICK_GAIN; |
// if height control is activated |
if((Parameter_GlobalConfig & CFG_HOEHENREGELUNG) && !(Looping_Roll || Looping_Nick) && !(VersionInfo.HardwareError[0] & 0x7F)) // Höhenregelung |
if((Parameter_GlobalConfig & CFG_HOEHENREGELUNG) && !(VersionInfo.HardwareError[0] & 0x7F)) // Höhenregelung |
{ |
#define HOVER_GAS_AVERAGE 16384L // 16384 * 2ms = 32s averaging |
#define HC_GAS_AVERAGE 4 // 4 * 2ms= 8ms averaging |