101,6 → 101,8 |
int LageKorrekturRoll = 0,LageKorrekturNick = 0, HoverGas = 0; |
//float Ki = FAKTOR_I; |
int Ki = 10300 / 33; |
int KiHH = 10300 / 33; // MartinR : für Ki bei HH über Schalter |
|
unsigned char Looping_Nick = 0,Looping_Roll = 0; |
unsigned char Looping_Links = 0, Looping_Rechts = 0, Looping_Unten = 0, Looping_Oben = 0; |
|
151,7 → 153,9 |
unsigned char CareFree = 0; |
|
signed int ExternStickNick = 0,ExternStickRoll = 0,ExternStickGier = 0, ExternHoehenValue = -20; |
int MaxStickNick = 0,MaxStickRoll = 0; |
//int MaxStickNick = 0,MaxStickRoll = 0; MartinR: so war es |
int MaxStickNick = 0,MaxStickRoll = 0,stick_nick_neutral = 0,stick_roll_neutral = 0; // MartinR: stick_.._neutral hinzugefügt |
|
unsigned int modell_fliegt = 0; |
volatile unsigned char FCFlags = 0; |
long GIER_GRAD_FAKTOR = 1291; |
398,7 → 402,11 |
Mess_Integral_Gier += MesswertGier; |
ErsatzKompass += MesswertGier; |
// Kopplungsanteil +++++++++++++++++++++++++++++++++++++ |
if(!Looping_Nick && !Looping_Roll && (EE_Parameter.GlobalConfig & CFG_ACHSENKOPPLUNG_AKTIV)) |
if((EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) || (Parameter_UserParam1 > 50)) IntegralFaktor = 0; // MartinR: zusätzlich |
|
//if(!Looping_Nick && !Looping_Roll && (EE_Parameter.GlobalConfig & CFG_ACHSENKOPPLUNG_AKTIV)) // MartinR : so war es |
if(!Looping_Nick && !Looping_Roll && IntegralFaktor && (EE_Parameter.GlobalConfig & CFG_ACHSENKOPPLUNG_AKTIV)) // MartinR: zusätzlich "&& IntegralFaktor" |
|
{ |
tmpl3 = (MesswertRoll * winkel_nick) / 2048L; |
tmpl3 *= Parameter_AchsKopplung2; //65 |
465,6 → 473,9 |
MesswertNick = HiResNick / 8; |
MesswertRoll = HiResRoll / 8; |
|
// MartinR : so war es Anfang |
/* |
|
if(AdWertNick < 15) MesswertNick = -1000; if(AdWertNick < 7) MesswertNick = -2000; |
if(PlatinenVersion == 10) { if(AdWertNick > 1010) MesswertNick = +1000; if(AdWertNick > 1017) MesswertNick = +2000; } |
else { if(AdWertNick > 2000) MesswertNick = +1000; if(AdWertNick > 2015) MesswertNick = +2000; } |
471,6 → 482,35 |
if(AdWertRoll < 15) MesswertRoll = -1000; if(AdWertRoll < 7) MesswertRoll = -2000; |
if(PlatinenVersion == 10) { if(AdWertRoll > 1010) MesswertRoll = +1000; if(AdWertRoll > 1017) MesswertRoll = +2000; } |
else { if(AdWertRoll > 2000) MesswertRoll = +1000; if(AdWertRoll > 2015) MesswertRoll = +2000; } |
|
// MartinR : FC 1.0: Sprung von 500 auf 2000 !! FC-ME: Sprung von 1000 auf 2000 |
*/ |
// MartinR : so war es Ende |
|
// MartinR : Neu Anfang |
if(PlatinenVersion == 10) |
{ |
if(AdWertNick > 1010) MesswertNick = +600; |
if(AdWertNick > 1017) MesswertNick = +800; |
if(AdWertNick < 15) MesswertNick = -600; |
if(AdWertNick < 7) MesswertNick = -800; |
if(AdWertRoll > 1010) MesswertRoll = +600; |
if(AdWertRoll > 1017) MesswertRoll = +800; |
if(AdWertRoll < 15) MesswertRoll = -600; |
if(AdWertRoll < 7) MesswertRoll = -800; |
} |
else |
{ |
if(AdWertNick > 2000) MesswertNick = +1200; |
if(AdWertNick > 2015) MesswertNick = +1600; |
if(AdWertNick < 15) MesswertNick = -1200; |
if(AdWertNick < 7) MesswertNick = -1600; |
if(AdWertRoll > 2000) MesswertRoll = +1200; |
if(AdWertRoll > 2015) MesswertRoll = +1600; |
if(AdWertRoll < 15) MesswertRoll = -1200; |
if(AdWertRoll < 7) MesswertRoll = -1600; |
} |
// MartinR : Neu Ende |
|
if(Parameter_Gyro_D) |
{ |
561,7 → 601,7 |
CHK_POTI(Parameter_Servo4,EE_Parameter.Servo4); |
CHK_POTI(Parameter_Servo5,EE_Parameter.Servo5); |
CHK_POTI(Parameter_MaxHoehe,EE_Parameter.MaxHoehe); |
CHK_POTI(Parameter_MaxHoehe,EE_Parameter.MaxHoehe); |
//CHK_POTI(Parameter_MaxHoehe,EE_Parameter.MaxHoehe); // MartinR: Zeile war doppelt |
CHK_POTI(Parameter_Hoehe_ACC_Wirkung,EE_Parameter.Hoehe_ACC_Wirkung); |
CHK_POTI(Parameter_Hoehe_GPS_Z,EE_Parameter.Hoehe_GPS_Z); |
CHK_POTI(Parameter_KompassWirkung,EE_Parameter.KompassWirkung); |
588,6 → 628,14 |
CHK_POTI(Parameter_DynamicStability,EE_Parameter.DynamicStability); |
CHK_POTI(Parameter_ExternalControl,EE_Parameter.ExternalControl); |
Ki = 10300 / (Parameter_I_Faktor + 1); |
|
|
if(Parameter_UserParam1 > 50) KiHH = 10300 / (Parameter_UserParam2 + 1); else KiHH = Ki; // MartinR : für HH über Schalter |
Parameter_NaviGpsModeControl = EE_Parameter.NaviGpsModeControl; //MartinR: Standard: EE_Parameter.NaviGpsModeControl wird übertragen |
if(!IntegralFaktor) Parameter_NaviGpsModeControl= 0; // MartinR: wenn HH dann GPS auf free- Mode |
// 0 = free; 100 = AID; 200 = coming home //neu |
|
|
MAX_GAS = EE_Parameter.Gas_Max; |
MIN_GAS = EE_Parameter.Gas_Min; |
|
620,6 → 668,9 |
{ |
int pd_ergebnis_nick,pd_ergebnis_roll,tmp_int, tmp_int2; |
int GierMischanteil,GasMischanteil; |
|
static long SummeNickHH=0,SummeRollHH=0; // MartinR: Für ACC-HH Umschaltung Prüfen !!!!!!!!!!!!! |
|
static long sollGier = 0,tmp_long,tmp_long2; |
static long IntegralFehlerNick = 0; |
static long IntegralFehlerRoll = 0; |
815,17 → 866,58 |
{ |
static int stick_nick,stick_roll; |
ParameterZuordnung(); |
// MartinR: original: |
/* |
stick_nick = (stick_nick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P) / 4; |
stick_nick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D; |
stick_roll = (stick_roll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P) / 4; |
stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D; |
|
*/ |
// MartinR: geändert Anfang |
if(Parameter_UserParam1 > 50) // MartinR: zweiter Stick_P Wert nur, wenn HH über Schalter aktiv ist |
{ |
stick_nick = (stick_nick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * Parameter_UserParam3 - stick_nick_neutral) / 4; |
stick_roll = (stick_roll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * Parameter_UserParam3 - stick_roll_neutral) / 4 ; |
//stick_nick = (PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * Parameter_UserParam3 - stick_nick_neutral); |
//stick_roll = (PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * Parameter_UserParam3 - stick_roll_neutral); |
} |
|
else |
{ |
stick_nick = (stick_nick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P) / 4; |
stick_roll = (stick_roll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P) / 4; |
stick_nick_neutral = stick_nick; // beim Umschalten auf HH wird derletzte Stickwert als Neutralposition verwendet, MartinR |
stick_roll_neutral = stick_roll; // beim Umschalten auf HH wird derletzte Stickwert als Neutralposition verwendet, MartinR |
} |
|
if(IntegralFaktor) |
{ |
stick_nick_neutral = stick_nick; // beim Umschalten auf HH wird derletzte Stickwert als Neutralposition verwendet, MartinR |
stick_roll_neutral = stick_roll; // beim Umschalten auf HH wird derletzte Stickwert als Neutralposition verwendet, MartinR |
stick_nick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D; |
stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D; |
|
//StickNick = stick_nick - (GPS_Nick + GPS_Nick2); // MartinR: GPS nur im ACC-Mode wirksam wohin verschoben ?? Prüfen !!!!!!!!!!!!!!!!!!!! |
//StickRoll = stick_roll - (GPS_Roll + GPS_Roll2); // MartinR: GPS nur im ACC-Mode wirksam |
} |
else // wenn HH , MartinR |
{ |
//stick_nick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D; // MartinR: eventuell vor if verschieben |
//stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D; // MartinR: eventuell vor if verschieben |
//StickNick = stick_nick; // MartinR: GPS nur im ACC-Mode wirksam wohin verschoben ?? Prüfen !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! |
//StickRoll = stick_roll; // MartinR: GPS nur im ACC-Mode wirksam |
} |
|
// MartinR: geändert Ende |
|
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// CareFree und freie Wahl der vorderen Richtung |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
signed char sintab[31] = { 0, 2, 4, 6, 7, 8, 8, 8, 7, 6, 4, 2, 0, -2, -4, -6, -7, -8, -8, -8, -7, -6, -4, -2, 0, 2, 4, 6, 7, 8, 8}; |
|
if(CareFree) |
if(CareFree) // MartinR: hier kann vermutlich Code gespart werden |
{ |
signed int nick, roll; |
nick = stick_nick / 4; |
833,7 → 925,7 |
StickNick = ((FromNC_Rotate_C * nick) + (FromNC_Rotate_S * roll)) / (32 / 4); |
StickRoll = ((FromNC_Rotate_C * roll) - (FromNC_Rotate_S * nick)) / (32 / 4); |
} |
else |
else // verrechnung des OrientationAngle |
{ |
FromNC_Rotate_C = sintab[EE_Parameter.OrientationAngle + 6]; |
FromNC_Rotate_S = sintab[EE_Parameter.OrientationAngle]; |
858,6 → 950,8 |
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
//+ Analoge Steuerung per Seriell |
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// MartinR: ToDo: eventuell die Kombination HH und Steuerung per Seriell nicht zulassen?? |
|
if(ExternControl.Config & 0x01 && Parameter_ExternalControl > 128) |
{ |
StickNick += (int) ExternControl.Nick * (int) EE_Parameter.Stick_P; |
868,7 → 962,9 |
} |
if(StickGas < 0) StickGas = 0; |
|
if(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor = 0; |
//if(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor = 0; // MartinR: so war es |
if((EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) || (Parameter_UserParam1 > 50)) IntegralFaktor = 0; // MartinR geändert |
|
|
if(abs(StickNick/STICK_GAIN) > MaxStickNick) |
{ |
958,7 → 1054,9 |
MittelIntegralNick2 += IntegralNick2; |
MittelIntegralRoll2 += IntegralRoll2; |
|
if(Looping_Nick || Looping_Roll) |
//if(Looping_Nick || Looping_Roll) // MartinR: so war es |
if(Looping_Nick || Looping_Roll || !IntegralFaktor) // MartinR: "|| !IntegralFaktor" hinzugefügt |
|
{ |
IntegralAccNick = 0; |
IntegralAccRoll = 0; |
966,6 → 1064,14 |
MittelIntegralRoll = 0; |
MittelIntegralNick2 = 0; |
MittelIntegralRoll2 = 0; |
|
IntegralNick = 0; // MartinR: im HH-Modus alle unbenutzten Integratoren = 0 |
IntegralRoll = 0; // MartinR: im HH-Modus alle unbenutzten Integratoren = 0 |
Mess_IntegralNick = 0; // MartinR: im HH-Modus alle unbenutzten Integratoren = 0 |
Mess_IntegralRoll = 0; // MartinR: im HH-Modus alle unbenutzten Integratoren = 0 |
//Mess_Integral_Gier = 0; // MartinR: im HH-Modus alle unbenutzten Integratoren = 0 |
//Mess_Integral_Gier2 = 0; // MartinR: im HH-Modus alle unbenutzten Integratoren = 0 |
|
Mess_IntegralNick2 = Mess_IntegralNick; |
Mess_IntegralRoll2 = Mess_IntegralRoll; |
ZaehlMessungen = 0; |
974,7 → 1080,9 |
} |
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(!Looping_Nick && !Looping_Roll && (Aktuell_az > 512 || MotorenEin)) |
//if(!Looping_Nick && !Looping_Roll && (Aktuell_az > 512 || MotorenEin)) // MartinR: so war es |
if(!Looping_Nick && !Looping_Roll && (Aktuell_az > 512 || MotorenEin) && IntegralFaktor) // MartinR: "&& IntegralFaktor" hinzugefügt |
|
{ |
long tmp_long, tmp_long2; |
if(FromNaviCtrl_Value.Kalman_K != -1 /*&& !TrichterFlug*/) |
1031,7 → 1139,9 |
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 && !TrichterFlug && EE_Parameter.Driftkomp) |
//if(!Looping_Nick && !Looping_Roll && !TrichterFlug && EE_Parameter.Driftkomp) // MartinR: so war es |
if(!Looping_Nick && !Looping_Roll && !TrichterFlug && EE_Parameter.Driftkomp && IntegralFaktor) // MartinR: "&& IntegralFaktor" hinzugefügt |
|
{ |
MittelIntegralNick /= ABGLEICH_ANZAHL; |
MittelIntegralRoll /= ABGLEICH_ANZAHL; |
1250,8 → 1360,20 |
if(TrimNick > TRIM_MAX) TrimNick = TRIM_MAX; else if(TrimNick <-TRIM_MAX) TrimNick =-TRIM_MAX; |
if(TrimRoll > TRIM_MAX) TrimRoll = TRIM_MAX; else if(TrimRoll <-TRIM_MAX) TrimRoll =-TRIM_MAX; |
|
//MesswertNick = IntegralNickMalFaktor + (long)((long)MesswertNick * GyroFaktor + (long)TrimNick * 128L) / (256L / STICK_GAIN);// MartinR so war es |
//MesswertRoll = IntegralRollMalFaktor + (long)((long)MesswertRoll * GyroFaktor + (long)TrimRoll * 128L) / (256L / STICK_GAIN);// MartinR so war es |
|
if(!IntegralFaktor) // MartinR : hinzugefügt |
{ |
MesswertNick = (long) ((long)MesswertNick * GyroFaktor) / (256L / STICK_GAIN) ; // MartinR : hinzugefügt |
MesswertRoll = (long) ((long)MesswertRoll * GyroFaktor) / (256L / STICK_GAIN) ; // MartinR : hinzugefügt |
} |
else // MartinR so war es |
{ |
MesswertNick = IntegralNickMalFaktor + (long)((long)MesswertNick * GyroFaktor + (long)TrimNick * 128L) / (256L / STICK_GAIN); |
MesswertRoll = IntegralRollMalFaktor + (long)((long)MesswertRoll * GyroFaktor + (long)TrimRoll * 128L) / (256L / STICK_GAIN); |
} |
|
MesswertGier = (long)(MesswertGier * 2 * (long)GyroFaktorGier) / (256L / STICK_GAIN) + (long)(Integral_Gier * IntegralFaktorGier) / (2 * (44000 / STICK_GAIN)); |
|
// Maximalwerte abfangen |
1343,7 → 1465,8 |
// if height control is activated by an rc channel |
if(EE_Parameter.GlobalConfig & CFG_HOEHEN_SCHALTER) // Regler wird über Schalter gesteuert |
{ // check if parameter is less than activation threshold |
if(Parameter_MaxHoehe < 50) // for 3 or 2-state switch height control is disabled in lowest position |
// if(Parameter_MaxHoehe < 50) // for 3 or 2-state switch height control is disabled in lowest position // MartinR :so war es |
if(Parameter_MaxHoehe < 50 || (Parameter_UserParam1 > 140) ) // MartinR: Schalter aus oder HH ohne Höhenregler über UsererParam1 an |
{ //height control not active |
if(!delay--) |
{ |
1361,7 → 1484,17 |
else // no switchable height control |
{ |
SollHoehe = ((int16_t) ExternHoehenValue + (int16_t) Parameter_MaxHoehe) * (int)EE_Parameter.Hoehe_Verstaerkung; |
HoehenReglerAktiv = 1; |
// HoehenReglerAktiv = 1; // MartinR : so war es |
// MartinR : geändert Anfang |
if(Parameter_UserParam1 > 140) // HH über Schalter: HH an + Höhenregler abgeschaltet, Nachführen von Parametern |
{ |
HoehenReglerAktiv = 0; |
} |
else // Höhenregler mit Sollhöhe über Poti aktiv |
{ |
HoehenReglerAktiv = 1; |
} |
// MartinR : geändert Ende |
} |
|
// calculate cos of nick and roll angle used for projection of the vertical hoover gas |
1645,6 → 1778,8 |
// Nick-Achse |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
DiffNick = MesswertNick - StickNick; // Differenz bestimmen |
// MartinR : so war es Anfang |
/* |
if(IntegralFaktor) SummeNick += IntegralNickMalFaktor - StickNick; // I-Anteil bei Winkelregelung |
else SummeNick += DiffNick; // I-Anteil bei HH |
if(SummeNick > (STICK_GAIN * 16000L)) SummeNick = (STICK_GAIN * 16000L); |
1651,6 → 1786,32 |
if(SummeNick < -(16000L * STICK_GAIN)) SummeNick = -(16000L * STICK_GAIN); |
pd_ergebnis_nick = (EE_Parameter.Gyro_Stability * DiffNick) / 8 + SummeNick / Ki; // PI-Regler für Nick |
// Motor Vorn |
|
*/ |
// MartinR : so war es Ende |
|
// MartinR : geändert Anfang |
pd_ergebnis_nick = (EE_Parameter.Gyro_Stability * DiffNick) / 8 ; // Zwischenergebnis um Code zu sparen |
if(IntegralFaktor) // MartinR : ACC-Mode |
{ |
SummeNick += IntegralNickMalFaktor - StickNick; // I-Anteil bei Winkelregelung |
if(SummeNick > (STICK_GAIN * 8000L)) SummeNick = (STICK_GAIN * 8000L); // MartinR : von 16000 auf 8000, da überlauf |
if(SummeNick < -(8000L * STICK_GAIN)) SummeNick = -(8000L * STICK_GAIN); // MartinR : von 16000 auf 8000, da überlauf |
//pd_ergebnis_nick = DiffNick + (SummeNick / Ki); |
pd_ergebnis_nick += (SummeNick / Ki); // PI-Regler für Nick |
SummeNickHH = 0 ; |
} |
else // MartinR : HH-Mode |
{ |
SummeNickHH += DiffNick; // I-Anteil bei HH |
if(SummeNickHH > (STICK_GAIN * 8000L)) SummeNickHH = (STICK_GAIN * 8000L); // MartinR : von 16000 auf 8000, da überlauf |
if(SummeNickHH < -(8000L * STICK_GAIN)) SummeNickHH = -(8000L * STICK_GAIN); // MartinR : von 16000 auf 8000, da überlauf |
pd_ergebnis_nick += SummeNickHH / KiHH; // MartinR: PI-Regler für Nick bei HH |
SummeNick = 0; |
} |
// MartinR : geändert Ende |
|
|
tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64; |
if(pd_ergebnis_nick > tmp_int) pd_ergebnis_nick = tmp_int; |
if(pd_ergebnis_nick < -tmp_int) pd_ergebnis_nick = -tmp_int; |
1659,11 → 1820,40 |
// Roll-Achse |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
DiffRoll = MesswertRoll - StickRoll; // Differenz bestimmen |
// MartinR : so war es Anfang |
/* |
if(IntegralFaktor) SummeRoll += IntegralRollMalFaktor - StickRoll;// I-Anteil bei Winkelregelung |
else SummeRoll += DiffRoll; // I-Anteil bei HH |
if(SummeRoll > (STICK_GAIN * 16000L)) SummeRoll = (STICK_GAIN * 16000L); |
if(SummeRoll < -(16000L * STICK_GAIN)) SummeRoll = -(16000L * STICK_GAIN); |
pd_ergebnis_roll = (EE_Parameter.Gyro_Stability * DiffRoll) / 8 + SummeRoll / Ki; // PI-Regler für Roll |
*/ |
// MartinR : so war es Ende |
|
// MartinR : geändert Anfang |
pd_ergebnis_roll = (EE_Parameter.Gyro_Stability * DiffRoll) / 8; |
if(IntegralFaktor) // MartinR : ACC-Mode |
{ |
SummeRoll += IntegralRollMalFaktor - StickRoll; |
if(SummeRoll > (STICK_GAIN * 8000L)) SummeRoll = (STICK_GAIN * 8000L);// MartinR : von 16000 auf 8000, da überlauf |
if(SummeRoll < -(8000L * STICK_GAIN)) SummeRoll = -(8000L * STICK_GAIN);// MartinR : von 16000 auf 8000, da überlauf |
//tmp_int = SummeRoll / Ki; |
//pd_ergebnis_roll = DiffRoll + tmp_int; // MartinR: PI-Regler im ACC-Mode |
pd_ergebnis_roll += SummeRoll / Ki; // PI-Regler für Roll |
//SummeRollHH = (IntegralRollMalFaktor + tmp_int - stick_roll_neutral + (TrimRoll * STICK_GAIN / 2)) * KiHH;// MartinR: Startwert von SummeRollHH bei Umschaltung auf HH |
// MartinR: Hintergrund: pd_ergebnis_xx soll sich beim Umschalten nicht ändern! |
SummeRollHH = 0; |
} |
else // MartinR : HH-Mode |
{ |
SummeRollHH += DiffRoll; // I-Anteil bei HH |
if(SummeRollHH > (STICK_GAIN * 8000L)) SummeRollHH = (STICK_GAIN * 8000L);// MartinR : von 16000 auf 8000, da überlauf |
if(SummeRollHH < -(8000L * STICK_GAIN)) SummeRollHH = -(8000L * STICK_GAIN);// MartinR : von 16000 auf 8000, da überlauf |
pd_ergebnis_roll += SummeRollHH / KiHH; // MartinR: PI-Regler für Roll bei HH |
SummeRoll = 0; |
} |
// MartinR : geändert Ende |
|
tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64; |
if(pd_ergebnis_roll > tmp_int) pd_ergebnis_roll = tmp_int; |
if(pd_ergebnis_roll < -tmp_int) pd_ergebnis_roll = -tmp_int; |
1692,7 → 1882,8 |
else tmp_int += ((long)GierMischanteil * Mixer.Motor[i][3]) / 64L; |
|
if(tmp_int > tmp_motorwert[i]) tmp_int = (tmp_motorwert[i] + tmp_int) / 2; // MotorSmoothing |
else tmp_int = 2 * tmp_int - tmp_motorwert[i]; // MotorSmoothing |
//else tmp_int = 2 * tmp_int - tmp_motorwert[i]; // MotorSmoothing // MartinR: so war es |
//else tmp_int = tmp_int; // MartinR: Entsprechend Vorschlag von MartinW geändert |
|
LIMIT_MIN_MAX(tmp_int,MIN_GAS * 4,MAX_GAS * 4); |
Motor[i].SetPoint = tmp_int / 4; |