Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 2380 → Rev 2384

/branches/V2.01a_ACC-HH_MartinR/fc.c
114,6 → 114,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;
 
167,10 → 169,16
unsigned char Parameter_MaximumAltitude;
unsigned char Parameter_Servo3,Parameter_Servo4,Parameter_Servo5;
unsigned char CareFree = 0;
const 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}; // 15° steps
//const 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}; // 15° steps // MartinR: so war es
const signed char sintab[62] = { 0, 2, 4, 6, 8, 10, 12, 13, 14, 15, 16, 16, 16, 16, 16, 15, 14, 13, 12, 10, 8, 6, 4, 2, 0, -2, -4, -6, -8, -10, -12, -13, -14, -15, -16, -16, -16, -16, -16, -15, -14, -13, -12, -10, -8, -6, -4, -2, 0, 2, 4, 6, 8, 10, 12, 13, 14, 15, 16, 16, 16}; // 7,5° steps //MartinR
signed char cosinus, sinus; // MartinR : extern für PAN-Funktion
 
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 char stick_p; // MartinR: Test
unsigned char Parameter_NaviGpsModeControl; // MartinR:
 
unsigned int modell_fliegt = 0;
volatile unsigned char FC_StatusFlags = 0, FC_StatusFlags2 = 0;
long GIER_GRAD_FAKTOR = 1291;
209,6 → 217,12
DebugOut.Analog[13] = Motor[1].SetPoint;
DebugOut.Analog[14] = Motor[2].SetPoint;
DebugOut.Analog[15] = Motor[3].SetPoint;
DebugOut.Analog[16] = cosinus; // MartinR: zur Einstellung der Pan-Funktion
DebugOut.Analog[17] = sinus; // MartinR: test zur Einstellung der Pan-Funktion
DebugOut.Analog[18] = ServoPanValue; // MartinR: zur Einstellung der Pan-Funktion
DebugOut.Analog[19] = ServoRollValue; // MartinR: Test
DebugOut.Analog[20] = ServoNickValue;
DebugOut.Analog[21] = HoverGas;
DebugOut.Analog[22] = Capacity.ActualCurrent;
489,9 → 503,11
KopplungsteilNickRoll = tmpl3;
KopplungsteilRollNick = tmpl4;
tmpl4 -= tmpl3;
if(IntegralFaktor) // MartinR: nur im ACC-Mode
{
ErsatzKompass += tmpl4;
if(!Parameter_CouplingYawCorrection) Mess_Integral_Gier -= tmpl4/2; // Gier nachhelfen
 
}
tmpl = ((MesswertGier + tmpl4) * winkel_nick) / 2048L;
tmpl *= Parameter_AchsKopplung1; // 90
tmpl /= 4096L;
545,6 → 561,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; }
551,6 → 570,40
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
#ifdef NO_GYRO_PROGRESSION // MartinR; keine Begrenzung
#warning : "### NO_GYRO_PROGRESSION ###"
//nichts tun
#else // mit Begrenzung
// 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
#endif
 
if(Parameter_Gyro_D)
{
698,6 → 751,17
CHK_POTI(Parameter_DynamicStability,EE_Parameter.DynamicStability);
CHK_POTI(Parameter_ExternalControl,EE_Parameter.ExternalControl);
Ki = 10300 / (Parameter_I_Faktor + 1);
//Ki = (10300 / (Parameter_I_Faktor + 4)) + (StickGas /2); // MartinR: Test Gasabhängige Regelung
if(Parameter_UserParam1 > 50) KiHH = 10300 / (Parameter_UserParam2 + 1); else KiHH = Ki; // MartinR : für HH über Schalter
//if(Parameter_UserParam1 > 50) KiHH = (10300 / (Parameter_UserParam2 + 4)) + (StickGas /2); else KiHH = Ki; // MartinR : für HH über Schalter // MartinR: Test Gasabhängige Regelung
//Parameter_NaviGpsModeControl = EE_Parameter.NaviGpsModeControl; //MartinR: Standard: EE_Parameter.NaviGpsModeControl wird übertragen
Parameter_NaviGpsModeControl = GetChannelValue(EE_Parameter.NaviGpsModeChannel); //MartinR: Standard: EE_Parameter.NaviGpsModeChannel 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;
 
745,6 → 809,9
{
int pd_ergebnis_nick,pd_ergebnis_roll,tmp_int, tmp_int2;
int GierMischanteil,GasMischanteil;
static long SummeNickHH=0,SummeRollHH=0; // MartinR: hinzugefügt
static long sollGier = 0,tmp_long,tmp_long2;
static long IntegralFehlerNick = 0;
static long IntegralFehlerRoll = 0;
1038,6 → 1105,12
#if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__))
SpeakHoTT = SPEAK_STARTING;
#endif
// MartinR: hinzugefügt Anfang
stick_nick_neutral = PPM_in[EE_Parameter.Kanalbelegung[K_NICK]]; // aktueller Stickwert wird als Neutralposition im HH verwendet, MartinR
stick_roll_neutral = PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]; // aktueller Stickwert wird als Neutralposition im HH verwendet, MartinR
SummeNickHH = 0 ; // Zurücksetzen der Integratoren
SummeRollHH = 0 ; // Zurücksetzen der Integratoren
// MartinR: hinzugefügt Ende
}
else
{
1118,15 → 1191,46
unsigned char stick_p;
ParameterZuordnung();
stick_p = EE_Parameter.Stick_P;
// MartinR: original:
/*
stick_nick = (stick_nick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * 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]] * stick_p) / 4;
stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D;
*/
// MartinR: geändert Anfang
if(IntegralFaktor) // ACC-Mode
{
stick_nick = (stick_nick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * stick_p) / 4;
stick_roll = (stick_roll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * stick_p) / 4;
stick_nick_neutral = PPM_in[EE_Parameter.Kanalbelegung[K_NICK]]; // beim Umschalten auf HH wird derletzte Stickwert als Neutralposition verwendet, MartinR
stick_roll_neutral = PPM_in[EE_Parameter.Kanalbelegung[K_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;
}
else // HH-Mode
{
if(Parameter_UserParam1 > 49) // MartinR: zweiter Stick_P Wert nur, wenn HH über Schalter aktiv ist
{
stick_nick = ((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] - stick_nick_neutral) * Parameter_UserParam3);
stick_roll = ((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] - stick_roll_neutral) * Parameter_UserParam3);
}
else
{
stick_nick = ((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] - stick_nick_neutral) * stick_p);
stick_roll = ((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] - stick_roll_neutral) * stick_p);
}
}
// MartinR: geändert Ende
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// CareFree und freie Wahl der vorderen Richtung
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(CareFree)
//if(CareFree) // MartinR: so war es
if(CareFree && IntegralFaktor) // MartinR: CareFree nur im ACC-Mode
{
signed int nick, roll;
nick = stick_nick / 4;
1136,8 → 1240,10
}
else
{
FromNC_Rotate_C = sintab[EE_Parameter.OrientationAngle + 6];
FromNC_Rotate_S = sintab[EE_Parameter.OrientationAngle];
//FromNC_Rotate_C = sintab[EE_Parameter.OrientationAngle + 6]; //MartinR: so war es
FromNC_Rotate_C = (sintab[EE_Parameter.OrientationAngle * 2 + 12]) / 2; //MartinR: feinere Auflösung
//FromNC_Rotate_S = sintab[EE_Parameter.OrientationAngle]; //MartinR: so war es
FromNC_Rotate_S = (sintab[EE_Parameter.OrientationAngle * 2]) / 2; //MartinR: feinere Auflösung
StickNick = ((FromNC_Rotate_C * stick_nick) + (FromNC_Rotate_S * stick_roll)) / 8;
StickRoll = ((FromNC_Rotate_C * stick_roll) - (FromNC_Rotate_S * stick_nick)) / 8;
}
1164,7 → 1270,7
StickGas = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 127;
 
GyroFaktor = (Parameter_Gyro_P + 10.0);
IntegralFaktor = Parameter_Gyro_I;
// IntegralFaktor = Parameter_Gyro_I; // MartinR: war mal hier
GyroFaktorGier = (Parameter_Gyro_Gier_P + 10.0);
IntegralFaktorGier = Parameter_Gyro_Gier_I;
 
1281,7 → 1387,27
MittelIntegralNick2 += IntegralNick2;
MittelIntegralRoll2 += IntegralRoll2;
 
if(Looping_Nick || Looping_Roll)
if((!IntegralFaktor && (Parameter_UserParam1 < 50) && !(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD))) // MartinR:
// nur im Moment des Umschaltens von HH auf ACC erfolgt ein Reset der Integrale, nicht aber bei normalem HH
// um einen im HH-Mode eventuell schwindelig geflogenen ACC_Mode zu resetten!
// bis zur Umschaltung werden die Integrale für den Kameraausgleich verwendet
{
IntegralNick = 0; // MartinR: Reset der Integratoren beim Zurückschalten vom HH- in den ACC-Mode
IntegralRoll = 0; // MartinR: Reset der Integratoren beim Zurückschalten vom HH- in den ACC-Mode
Mess_IntegralNick = 0; // MartinR: Reset der Integratoren beim Zurückschalten vom HH- in den ACC-Mode
Mess_IntegralRoll = 0; // MartinR: Reset der Integratoren beim Zurückschalten vom HH- in den ACC-Mode
Mess_Integral_Gier = 0; // MartinR: Reset der Integratoren beim Zurückschalten vom HH- in den ACC-Mode
sollGier = 0;
Integral_Gier = 0; // MartinR: Reset der Integratoren beim Zurückschalten vom HH- in den ACC-Mode
//Mess_Integral_Gier2 = 0; // MartinR: Reset der Integratoren beim Zurückschalten vom HH- in den ACC-Mode
KompassSollWert = KompassValue; // MartinR: aktuelle Ausrichtung beibehalten
ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR; // MartinR: aktuelle Ausrichtung beibehalten
NeueKompassRichtungMerken = 1; // MartinR: aktuelle Ausrichtung beibehalten
}
 
// if(Looping_Nick || Looping_Roll) // MartinR: so war es
if(Looping_Nick || Looping_Roll || (!IntegralFaktor && (Parameter_UserParam1 < 50) && !(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD))) // MartinR: erweitert
// MartinR: beim ACC-Loop oder beim zurückschalten von HH auf ACC
{
IntegralAccNick = 0;
IntegralAccRoll = 0;
1295,6 → 1421,9
LageKorrekturNick = 0;
LageKorrekturRoll = 0;
}
if((EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) || (Parameter_UserParam1 > 49)) IntegralFaktor = 0; // MartinR geändert und verschoben
else IntegralFaktor = Parameter_Gyro_I; // MartinR: geändert
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(!Looping_Nick && !Looping_Roll && (Aktuell_az > 512 || MotorenEin))
1349,7 → 1478,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;
1517,7 → 1648,8
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Kompass
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(KompassValue >= 0 && (Parameter_GlobalConfig & CFG_KOMPASS_AKTIV))
//if(KompassValue >= 0 && (Parameter_GlobalConfig & CFG_KOMPASS_AKTIV)) // MartinR: so war es
if((KompassValue >= 0 && (Parameter_GlobalConfig & CFG_KOMPASS_AKTIV)) && !(Parameter_UserParam1 > 50)) // MartinR: bei HH über Schalter wird der Kompass abgeschaltet
{
if(CalculateCompassTimer-- == 1)
{
1574,8 → 1706,23
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 : HH-Mode hinzugefügt
{
MesswertNick = (long) ((long)MesswertNick * GyroFaktor) / (256L / STICK_GAIN) ; // MartinR : hinzugefügt
MesswertRoll = (long) ((long)MesswertRoll * GyroFaktor) / (256L / STICK_GAIN) ; // MartinR : hinzugefügt
//MesswertGier = (long)(MesswertGier * 2 * (long)GyroFaktorGier) / (256L / STICK_GAIN);
//Mess_Integral_Gier = 0; // MartinR: nur Kompass wird bei HH deaktiviert
//Integral_Gier = 0; // MartinR: nur Kompass wird bei HH deaktiviert
}
else // MartinR: ACC-Mode 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
1592,6 → 1739,10
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(UBat > BattLowVoltageWarning) GasMischanteil = ((unsigned int)GasMischanteil * BattLowVoltageWarning) / UBat; // Gas auf das aktuelle Spannungvieveau beziehen
GasMischanteil *= STICK_GAIN;
//MartinR: Höhenregler nur mit 1284er Prozessor:
#if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__))
 
// if height control is activated
if((Parameter_GlobalConfig & CFG_HOEHENREGELUNG) && !(Looping_Roll || Looping_Nick) && !(VersionInfo.HardwareError[0] & 0x7F)) // Höhenregelung
{
1662,7 → 1813,8
// if height control is activated by an rc channel
if(Parameter_GlobalConfig & CFG_HOEHEN_SCHALTER) // Regler wird über Schalter gesteuert
{ // check if parameter is less than activation threshold
if(Parameter_HoehenSchalter < 50) // for 3 or 2-state switch height control is disabled in lowest position
// if(Parameter_HoehenSchalter < 50) // for 3 or 2-state switch height control is disabled in lowest position // MartinR :so war es
if((Parameter_HoehenSchalter < 50) || (Parameter_UserParam1 > 140) ) // MartinR: Schalter aus oder HH ohne Höhenregler über UsererParam1 an
{ //height control not active
if(!delay--)
{
1687,7 → 1839,17
else // no switchable height control
{
SollHoehe = ((int16_t) ExternHoehenValue + (int16_t) Parameter_HoehenSchalter) * (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
tmp_int = (int)(IntegralNick/GIER_GRAD_FAKTOR); // nick angle in deg
2047,6 → 2209,9
FC_StatusFlags |= (FC_STATUS_VARIO_TRIM_UP|FC_STATUS_VARIO_TRIM_DOWN);
} // EOF no height control
 
// MartinR: Höhenregler nur noch mit 1284er Prozessor
#endif
 
// Limits the maximum gas in case of "Out of Range emergency landing"
if(NC_To_FC_Flags & NC_TO_FC_EMERGENCY_LANDING)
{
2090,7 → 2255,69
tmp_int = MAX_GAS*STICK_GAIN;
if(GierMischanteil > ((tmp_int - GasMischanteil))) GierMischanteil = ((tmp_int - GasMischanteil));
if(GierMischanteil < -((tmp_int - GasMischanteil))) GierMischanteil = -((tmp_int - GasMischanteil));
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Nick / Roll-Achse // MartinR: um Code zu sparen wurde Nick und Roll zusammengefasst
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
DiffNick = MesswertNick - StickNick; // Differenz bestimmen
DiffRoll = MesswertRoll - StickRoll; // Differenz bestimmen
 
// PI-Regler für Nick und Roll
if(EE_Parameter.Gyro_Stability <= 8)
{
pd_ergebnis_nick = (EE_Parameter.Gyro_Stability * DiffNick) / 8 ; // Zwischenergebnis um Code zu sparen
pd_ergebnis_roll = (EE_Parameter.Gyro_Stability * DiffRoll) / 8;
}
else
{
pd_ergebnis_nick = ((EE_Parameter.Gyro_Stability / 2) * DiffNick) / 4; // Überlauf verhindern
pd_ergebnis_roll = ((EE_Parameter.Gyro_Stability / 2) * DiffRoll) / 4; // Überlauf verhindern
}
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 += (SummeNick / Ki); // PI-Regler für Nick
SummeNickHH = 0 ;
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
pd_ergebnis_roll += (SummeRoll / Ki); // PI-Regler für Roll
SummeRollHH = 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;
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_nick > tmp_int) pd_ergebnis_nick = tmp_int;
if(pd_ergebnis_nick < -tmp_int) pd_ergebnis_nick = -tmp_int;
 
//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;
// MartinR: alt
/*
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Nick-Achse
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2125,6 → 2352,9
if(pd_ergebnis_roll > tmp_int) pd_ergebnis_roll = tmp_int;
if(pd_ergebnis_roll < -tmp_int) pd_ergebnis_roll = -tmp_int;
 
*/
// MartinR: alt Ende
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Universal Mixer
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++