110,6 → 110,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; |
|
161,10 → 163,14 |
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, 11, 13, 14, 15, 15, 16, 16, 16, 15, 14, 13, 11, 10, 8, 6, 4, 2, 0, -2, -4, -6, -8, -10, -11, -13, -14, -15, -15, -16, -16, -16, -15, -15, -14, -13, -11, -10, -8, -6, -4, -2, 0, 2, 4, 6, 8, 10, 11, 13, 14, 15, 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 int modell_fliegt = 0; |
volatile unsigned char FC_StatusFlags = 0, FC_StatusFlags2 = 0; |
long GIER_GRAD_FAKTOR = 1291; |
197,6 → 203,11 |
DebugOut.Analog[13] = Motor[1].SetPoint; |
DebugOut.Analog[14] = Motor[2].SetPoint; |
DebugOut.Analog[15] = Motor[3].SetPoint; |
|
DebugOut.Analog[16] = cosinus; // MartinR: test |
DebugOut.Analog[17] = sinus; // MartinR: test |
DebugOut.Analog[18] = ServoPanValue; // MartinR: test |
DebugOut.Analog[19] = ServoRollValue; // MartinR: test |
DebugOut.Analog[20] = ServoNickValue; |
DebugOut.Analog[22] = Capacity.ActualCurrent; |
DebugOut.Analog[23] = Capacity.UsedCapacity; |
386,8 → 397,10 |
static signed int oldNick, oldRoll, d2Roll, d2Nick; |
signed long winkel_nick, winkel_roll; |
MesswertGier = (signed int) AdNeutralGier - AdWertGier; |
MesswertNick = (signed int) AdWertNickFilter / 8; |
MesswertRoll = (signed int) AdWertRollFilter / 8; |
//MesswertNick = (signed int) AdWertNickFilter / 8; // MartinR: so war es |
//MesswertRoll = (signed int) AdWertRollFilter / 8; // MartinR: so war es |
MesswertNick = (signed int) AdWertNickFilter ; // MartinR die Division /8 erfolgt bereits in der analog.c |
MesswertRoll = (signed int) AdWertRollFilter ; // MartinR die Division /8 erfolgt bereits in der analog.c |
RohMesswertNick = MesswertNick; |
RohMesswertRoll = MesswertRoll; |
|
430,9 → 443,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; |
481,11 → 496,18 |
IntegralNick2 = Mess_IntegralNick2; |
IntegralRoll2 = Mess_IntegralRoll2; |
|
#define D_LIMIT 128 |
//#define D_LIMIT 128 // MartinR: so war es |
#define D_LIMIT 16 |
// MartinR: Änderung war notwendig, da die Division /8 bereits in der analog.c erfolgt |
|
MesswertNick = HiResNick / 8; |
MesswertRoll = HiResRoll / 8; |
//MesswertNick = HiResNick / 8; // MartinR : so war es |
// MesswertRoll = HiResRoll / 8; // MartinR : so war es |
MesswertNick = HiResNick ; // MartinR die Division /8 erfolgt bereits in der analog.c |
MesswertRoll = HiResRoll ; // MartinR die Division /8 erfolgt bereits in der analog.c |
|
// 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; } |
492,8 → 514,39 |
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) |
// MartinR: hier sind Änderungen erforderlich, da u.a. MesswertNick = HiResNick / 8 von der fc.c in die analog.c verschoben wurde |
// Hintergrund: Code einsparen |
{ |
d2Nick = HiResNick - oldNick; |
oldNick = (oldNick + HiResNick)/2; |
504,11 → 557,13 |
oldRoll = (oldRoll + HiResRoll)/2; |
if(d2Roll > D_LIMIT) d2Roll = D_LIMIT; |
else if(d2Roll < -D_LIMIT) d2Roll = -D_LIMIT; |
|
MesswertNick += (d2Nick * (signed int) Parameter_Gyro_D) / 16; |
MesswertRoll += (d2Roll * (signed int) Parameter_Gyro_D) / 16; |
HiResNick += (d2Nick * (signed int) Parameter_Gyro_D); |
HiResRoll += (d2Roll * (signed int) Parameter_Gyro_D); |
|
//MesswertNick += (d2Nick * (signed int) Parameter_Gyro_D) / 16; // MartinR : so war es |
MesswertNick += (d2Nick * (signed int) Parameter_Gyro_D) / 2; // MartinR : geändert |
//MesswertRoll += (d2Roll * (signed int) Parameter_Gyro_D) / 16; // MartinR : so war es |
MesswertRoll += (d2Roll * (signed int) Parameter_Gyro_D) / 2; // MartinR : geändert |
HiResNick += (d2Nick * (signed int) Parameter_Gyro_D * 8); // martinR: *8 hinzugefügt |
HiResRoll += (d2Roll * (signed int) Parameter_Gyro_D * 8); // martinR: *8 hinzugefügt |
} |
|
if(RohMesswertRoll > 0) TrimRoll += ((long) abs(KopplungsteilNickRoll) * Parameter_CouplingYawCorrection) / 64L; |
621,6 → 676,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; |
|
662,6 → 725,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; |
770,6 → 836,8 |
ServoActive = 1; |
DDRD |=0x80; // enable J7 -> Servo signal |
Piep(GetActiveParamSet(),120); |
PPM_in[13] = Parameter_UserParam5 -110; // MartinR: Initialisierungswerte für die seriellen Potis für Jeti+ |
PPM_in[14] = Parameter_UserParam6 -110; // MartinR: Initialisierungswerte für die seriellen Potis für Jeti+ |
} |
} |
} |
858,15 → 926,55 |
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(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]] * stick_p) / 4; |
stick_roll = (stick_roll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * 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 |
//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 |
//StickRoll = stick_roll; // MartinR: GPS nur im ACC-Mode wirksam |
} |
*/ |
// 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; |
876,8 → 984,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]; //MartinR: feinere Auflösung |
//FromNC_Rotate_S = sintab[EE_Parameter.OrientationAngle]; //MartinR: so war es |
FromNC_Rotate_S = sintab[EE_Parameter.OrientationAngle * 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; |
} |
904,6 → 1014,7 |
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
//+ 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; |
1015,7 → 1126,8 |
MittelIntegralNick2 += IntegralNick2; |
MittelIntegralRoll2 += IntegralRoll2; |
|
if(Looping_Nick || Looping_Roll) |
// 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 |
{ |
IntegralAccNick = 0; |
IntegralAccRoll = 0; |
1029,6 → 1141,24 |
LageKorrekturNick = 0; |
LageKorrekturRoll = 0; |
} |
|
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 |
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 |
} |
|
if((EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) || (Parameter_UserParam1 > 50)) IntegralFaktor = 0; // MartinR geändert und verschoben |
else IntegralFaktor = Parameter_Gyro_I; // MartinR: verschoben um Code zu sparen |
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(!Looping_Nick && !Looping_Roll && (Aktuell_az > 512 || MotorenEin)) |
1088,7 → 1218,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; |
1248,14 → 1380,18 |
tmp_int += (EE_Parameter.StickGier_P * StickGier) / 4; |
tmp_int += CompassGierSetpoint; |
sollGier = tmp_int; |
Mess_Integral_Gier -= tmp_int; |
if(Mess_Integral_Gier > 50000) Mess_Integral_Gier = 50000; // begrenzen |
if(Mess_Integral_Gier <-50000) Mess_Integral_Gier =-50000; |
//Mess_Integral_Gier -= tmp_int; // MartinR: so war es |
Mess_Integral_Gier -= (tmp_int * 10) / 8; // MartinR: Test um Zurückschwingen bei hohen I-Faktoren zu verringern |
//if(Mess_Integral_Gier > 50000) Mess_Integral_Gier = 50000; // begrenzen // MartinR: so war es |
//if(Mess_Integral_Gier <-50000) Mess_Integral_Gier =-50000; // MartinR: so war es |
if(Mess_Integral_Gier > 90000) Mess_Integral_Gier = 90000; // begrenzen // MartinR: Begrenzung verändert |
if(Mess_Integral_Gier <-90000) Mess_Integral_Gier =-90000; // MartinR: Begrenzung verändert |
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Kompass |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(KompassValue >= 0 && (Parameter_GlobalConfig & CFG_KOMPASS_AKTIV)) |
//if(KompassValue >= 0 && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV)) // MartinR: so war es |
if((KompassValue >= 0 && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV)) && !(Parameter_UserParam1 > 50)) // MartinR: bei HH über Schalter wird der Kompass abgeschaltet |
{ |
if(CalculateCompassTimer-- == 1) |
{ |
1312,8 → 1448,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 |
1403,7 → 1554,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--) |
{ |
1421,7 → 1573,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 |
1455,6 → 1617,7 |
// PD-Control with respect to hoover point |
// the thrust loss out of horizontal attitude is compensated |
// the setpoint will be fine adjusted with the gas stick position |
HeightDeviation = HoehenWert - SollHoehe; //MartinR: Test |
if(FC_StatusFlags & FC_STATUS_FLY) // trim setpoint only when flying |
{ // gas stick is above hoover point |
if(StickGas > (StickGasHover + HEIGHT_CONTROL_STICKTHRESHOLD) && !BaroAtUpperLimit) |
1462,7 → 1625,8 |
if(FC_StatusFlags & FC_STATUS_VARIO_TRIM_DOWN) |
{ |
FC_StatusFlags &= ~FC_STATUS_VARIO_TRIM_DOWN; |
SollHoehe = HoehenWert; // update setpoint to current heigth |
//SollHoehe = HoehenWert; // update setpoint to current heigth // MartinR: so war es |
SollHoehe = HoehenWert - HeightDeviation / 2; // MartinR: um den Sollwertsprung zu verringern |
} |
FC_StatusFlags |= FC_STATUS_VARIO_TRIM_UP; |
// Limit the maximum Altitude |
1481,7 → 1645,8 |
if(FC_StatusFlags & FC_STATUS_VARIO_TRIM_UP) |
{ |
FC_StatusFlags &= ~FC_STATUS_VARIO_TRIM_UP; |
SollHoehe = HoehenWert; // update setpoint to current heigth |
//SollHoehe = HoehenWert; // update setpoint to current heigth // MartinR: so war es |
SollHoehe = HoehenWert - HeightDeviation / 2; // MartinR: um den Sollwertsprung zu verringern |
} |
FC_StatusFlags |= FC_STATUS_VARIO_TRIM_DOWN; |
AltitudeSetpointTrimming = -abs(StickGas - (StickGasHover - HEIGHT_CONTROL_STICKTHRESHOLD)); |
1502,7 → 1667,8 |
if(FC_StatusFlags & FC_STATUS_VARIO_TRIM_DOWN) // changed from sinking to rising |
{ |
FC_StatusFlags &= ~FC_STATUS_VARIO_TRIM_DOWN; |
SollHoehe = HoehenWert; // update setpoint to current heigth |
//SollHoehe = HoehenWert; // update setpoint to current heigth // MartinR: so war es |
SollHoehe = HoehenWert - HeightDeviation / 2; // MartinR: um den Sollwertsprung zu verringern |
} |
} |
else |
1516,7 → 1682,8 |
if(FC_StatusFlags & FC_STATUS_VARIO_TRIM_UP) // changed from rising to sinking |
{ |
FC_StatusFlags &= ~FC_STATUS_VARIO_TRIM_UP; |
SollHoehe = HoehenWert; // update setpoint to current heigth |
//SollHoehe = HoehenWert; // update setpoint to current heigth // MartinR: so war es |
SollHoehe = HoehenWert - HeightDeviation / 2; // MartinR: um den Sollwertsprung zu verringern |
} |
} |
else |
1585,20 → 1752,55 |
tmp_long = (HoehenWert - SollHoehe); // positive when too high |
LIMIT_MIN_MAX(tmp_long, -32767L, 32767L); // avoid overflov when casting to int16_t |
HeightDeviation = (int)(tmp_long); // positive when too high |
//tmp_long = (tmp_long * (long)Parameter_Hoehe_P) / 32L; // p-part // MartinR: so war es |
// MartinR: geändert Anfang |
if ((SollHoehe > (HoehenWert+64)) || (SollHoehe < (HoehenWert-64))) |
{ |
tmp_long = (tmp_long * (long)Parameter_Hoehe_P) / 24L; // p-part // MartinR P-Part erhoehen |
} |
else |
{ |
tmp_long = (tmp_long * (long)Parameter_Hoehe_P) / 32L; // p-part |
LIMIT_MIN_MAX(tmp_long, -127 * STICK_GAIN, 256 * STICK_GAIN); // more than the full range makes no sense |
} |
// MartinR: geändert Ende |
//LIMIT_MIN_MAX(tmp_long, -127 * STICK_GAIN, 256 * STICK_GAIN); // more than the full range makes no sense // MartinR: so war es |
// MartinR: weshalb unsymmetrisch? |
LIMIT_MIN_MAX(tmp_long, -128 * STICK_GAIN, 128 * STICK_GAIN); // more than the full range makes no sense // MartinR: geändert |
GasReduction = tmp_long; |
// ------------------------- D-Part 1: Vario Meter ---------------------------- |
tmp_int = VarioMeter / 8; |
//tmp_int = VarioMeter / 8; // MartinR: so war es |
// MartinR: geändert Anfang |
tmp_int = VarioMeter / 4; // MartinR: geändert // Variometer: steigen ist positiv |
{ |
if ((SollHoehe > (HoehenWert+512)) || (SollHoehe < (HoehenWert-512))) |
//if ((StickGas > (StickGasHover + 3*HEIGHT_CONTROL_STICKTHRESHOLD)) || (StickGas < (StickGasHover - 3*HEIGHT_CONTROL_STICKTHRESHOLD))) |
{ |
tmp_int = tmp_int + HeightDeviation / 28; |
//tmp_int = tmp_int + Parameter_Hoehe_ACC_Wirkung* HeightDeviation / 64; // reduce d-part while trimming setpoint // MartinR: geändert |
} |
else |
{ |
tmp_int = tmp_int + HeightDeviation / 32; |
//tmp_int = tmp_int + Parameter_Hoehe_ACC_Wirkung* HeightDeviation / 64; |
} |
} |
// MartinR: geändert Ende |
|
LIMIT_MIN_MAX(tmp_int, -127, 128); |
tmp_int = (tmp_int * (long)Parameter_Luftdruck_D) / 4L; // scale to d-gain parameter |
LIMIT_MIN_MAX(tmp_int,-64 * STICK_GAIN, 64 * STICK_GAIN); |
/* // MartinR: so war es Anfang |
if(FC_StatusFlags & (FC_STATUS_VARIO_TRIM_UP|FC_STATUS_VARIO_TRIM_DOWN)) tmp_int /= 4; // reduce d-part while trimming setpoint |
else |
if(Parameter_ExtraConfig & CFG2_HEIGHT_LIMIT) tmp_int /= 8; // reduce d-part in "Deckel" mode |
if(EE_Parameter.ExtraConfig & CFG2_HEIGHT_LIMIT) tmp_int /= 8; // reduce d-part in "Deckel" mode |
*/ // MartinR: so war es Ende |
tmp_int /= 4; // MartinR: geändert: keine veränderung des d-part im "Deckel" mode |
//if(FC_StatusFlags & (FC_STATUS_VARIO_TRIM_UP|FC_STATUS_VARIO_TRIM_DOWN)) tmp_int /= 4; // reduce d-part while trimming setpoint // MartinR: geändert |
|
GasReduction += tmp_int; |
} // EOF no baro range expanding |
// ------------------------ D-Part 2: ACC-Z Integral ------------------------ |
/* // MartinR: deaktiviert Anfang, da statische Ablage bei Schräglage Probleme macht |
if(Parameter_Hoehe_ACC_Wirkung) |
{ |
tmp_long = ((Mess_Integral_Hoch / 128L) * (int32_t) Parameter_Hoehe_ACC_Wirkung) / (128L / STICK_GAIN); |
1605,9 → 1807,12 |
LIMIT_MIN_MAX(tmp_long, -32 * STICK_GAIN, 64 * STICK_GAIN); |
GasReduction += tmp_long; |
} |
*/ // MartinR: deaktiviert Ende |
// ------------------------ D-Part 3: GpsZ ---------------------------------- |
tmp_int = (Parameter_Hoehe_GPS_Z * (int)FromNaviCtrl_Value.GpsZ)/128L; |
LIMIT_MIN_MAX(tmp_int, -32 * STICK_GAIN, 64 * STICK_GAIN); |
//LIMIT_MIN_MAX(tmp_int, -32 * STICK_GAIN, 64 * STICK_GAIN); // MartinR: so war es |
// MartinR: weshalb unsymmetrisch? |
LIMIT_MIN_MAX(tmp_int, -32 * STICK_GAIN, 32 * STICK_GAIN); // MartinR: geändert |
GasReduction += tmp_int; |
GasReduction = (long)((long)GasReduction * HoverGas) / 512; // scale to the gas value |
// ------------------------ ---------------------------------- |
1653,7 → 1858,8 |
LIMIT_MAX(FilterHCGas, GasMischanteil); // nicht mehr als Gas |
GasMischanteil = FilterHCGas; |
} |
else GasMischanteil = FilterHCGas + (GasMischanteil - HoverGas) / 4; // only in Vario-Mode |
//else GasMischanteil = FilterHCGas + (GasMischanteil - HoverGas) / 4; // only in Vario-Mode // MartinR: so war es |
else GasMischanteil = FilterHCGas ; // MartinR: geändert, um Überschwinger bei Höhenänderung zu verringern |
} |
}// EOF height control active |
else // HC not active |
1759,7 → 1965,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 |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1794,6 → 2062,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 |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |