58,6 → 58,36 |
#include "mymath.h" |
#include "isqrt.h" |
|
|
//MartinW; added vars |
unsigned char loop1, loop2, loop3; |
unsigned char settingdest = 5; |
char keynumber=-7; |
unsigned char jetistepstep[] = {1,5,10,25,50}, jetistep = 0; |
|
|
unsigned short CurrentOffset = 0;/// |
|
unsigned char Motors[8]; |
unsigned char Motorsmax[8]; |
unsigned short MotorsTmax; |
unsigned char updatemotors=2; |
|
//Panorama Trigger; |
int degreeold =0; |
int degreedivold =0; |
int degreediv =0; |
unsigned int panograd=0; |
unsigned char panotrigger=0; |
unsigned char calibration_done = 0; |
|
unsigned char jetibeepcode[] = {130,'E','I','S','H'}; |
|
|
///MartinW; added vars END |
|
|
|
unsigned char h,m,s; |
unsigned int BaroExpandActive = 0; |
int MesswertNick,MesswertRoll,MesswertGier,MesswertGierBias, RohMesswertNick,RohMesswertRoll; |
74,7 → 104,9 |
long Integral_Gier = 0; |
long Mess_IntegralNick = 0,Mess_IntegralNick2 = 0; |
long Mess_IntegralRoll = 0,Mess_IntegralRoll2 = 0; |
long Mess_Integral_Gier = 0,Mess_Integral_Gier2 = 0; |
//long Mess_Integral_Gier = 0,Mess_Integral_Gier2 = 0; ///MartinW so war es |
long Mess_Integral_Gier = 0; ///MartinW: Mess_Integral_Gier2 unbenutzt |
|
long MittelIntegralNick,MittelIntegralRoll,MittelIntegralNick2,MittelIntegralRoll2; |
long SummeNick=0,SummeRoll=0; |
volatile long Mess_Integral_Hoch = 0; |
113,6 → 145,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; |
|
164,10 → 198,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; |
201,9 → 239,21 |
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[22] = Capacity.ActualCurrent; |
#ifdef WITH_REMAINCAPACITY // only include functions if DEBUG is defined in main.h |
|
#warning : "### with REMAIN CAPACITY ###" |
DebugOut.Analog[23] = Capacity.RemainCapacity; |
#else |
DebugOut.Analog[23] = Capacity.UsedCapacity; |
#endif |
|
DebugOut.Analog[24] = SollHoehe/5; |
// DebugOut.Analog[22] = FromNaviCtrl_Value.GpsZ; |
// DebugOut.Analog[29] = FromNaviCtrl_Value.SerialDataOkay; |
337,7 → 387,7 |
Mittelwert_AccRoll = ACC_AMPLIFY * AdWertAccRoll; |
IntegralNick = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccNick; |
IntegralRoll = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccRoll; |
Mess_IntegralNick = IntegralNick; |
Mess_IntegralNick = IntegralNick; |
Mess_IntegralRoll = IntegralRoll; |
Mess_Integral_Gier = 0; |
StartLuftdruck = Luftdruck; |
374,7 → 424,8 |
if((NeutralAccY < 300) || (NeutralAccY > 750)) { VersionInfo.HardwareError[0] |= FC_ERROR0_ACC_ROLL; }; |
if((NeutralAccZ < 512) || (NeutralAccZ > 850)) { VersionInfo.HardwareError[0] |= FC_ERROR0_ACC_TOP; }; |
carefree_old = 70; |
#if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__)) |
#if ((defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__)) && defined(WITH_HOTTMENU)) |
#warning : "### with Hottmenu ###" |
LIBFC_HoTT_Clear(); |
#endif |
} |
389,8 → 440,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; |
|
433,9 → 486,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; |
484,11 → 539,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; } |
495,8 → 557,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; |
507,11 → 600,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; |
632,6 → 727,15 |
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 |
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; |
|
674,6 → 778,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; |
788,7 → 895,10 |
CalibrationDone = 1; |
ServoActive = 1; |
DDRD |=0x80; // enable J7 -> Servo signal |
JetiBeep = jetibeepcode[GetActiveParamSet()-1]; |
Piep(GetActiveParamSet(),120); |
PPM_in[13] = Parameter_UserParam5 -127 ; // MartinR: Initialisierungswerte für die seriellen Potis für Jeti+ |
PPM_in[14] = Parameter_UserParam6 -127 ; // MartinR: Initialisierungswerte für die seriellen Potis für Jeti+ |
} |
} |
} |
830,7 → 940,7 |
MotorenEin = 1; |
sollGier = 0; |
Mess_Integral_Gier = 0; |
Mess_Integral_Gier2 = 0; |
// Mess_Integral_Gier2 = 0; |
Mess_IntegralNick = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccNick; |
Mess_IntegralRoll = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccRoll; |
Mess_IntegralNick2 = IntegralNick; |
877,15 → 987,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; |
895,8 → 1045,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; |
} |
923,6 → 1075,9 |
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
//+ Analoge Steuerung per Seriell |
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
#ifdef WITH_ExternControl /// MartinW memorysaving |
#warning : "### with ExternControl ###" |
|
if(ExternControl.Config & 0x01 && Parameter_ExternalControl > 128) |
{ |
StickNick += (int) ExternControl.Nick * (int) EE_Parameter.Stick_P; |
948,6 → 1103,10 |
} |
else MaxStickRoll--; |
if(FC_StatusFlags & FC_STATUS_EMERGENCY_LANDING) {MaxStickNick = 0; MaxStickRoll = 0;} |
|
#else |
#warning : "### without ExternControl ###" |
#endif |
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Looping? |
1034,7 → 1193,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; |
1048,6 → 1208,26 |
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 |
KompassSollWert = KompassValue; // MartinR: aktuelle Ausrichtung beibehalten |
ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR; // MartinR: aktuelle Ausrichtung beibehalten |
} |
|
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)) |
1107,7 → 1287,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; |
1267,14 → 1449,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) |
{ |
1331,8 → 1517,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 |
1422,7 → 1623,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--) |
{ |
1440,7 → 1642,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 |
1474,6 → 1686,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) |
1481,7 → 1694,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 |
1500,7 → 1714,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)); |
1521,7 → 1736,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 |
1535,7 → 1751,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 |
1604,20 → 1821,57 |
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 |
LIMIT_MIN_MAX(tmp_long, -127 * STICK_GAIN, 256 * STICK_GAIN); // more than the full range makes no sense |
//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 |
tmp_long = (tmp_long * (long)Parameter_Hoehe_P) / 70L; // p-part // MartinR Anpassung an Standardwert |
} |
else |
{ |
//tmp_long = (tmp_long * (long)Parameter_Hoehe_P) / 32L; // p-part |
tmp_long = (tmp_long * (long)Parameter_Hoehe_P) / 90L; // MartinR Anpassung an Standardwert |
} |
// 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); |
1624,9 → 1878,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 |
// ------------------------ ---------------------------------- |
1672,7 → 1929,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 |
1786,7 → 2044,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 |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1821,6 → 2141,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 |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1844,7 → 2167,8 |
else if(Mixer.Motor[i][3] == -64) tmp_int -= GierMischanteil; |
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 |
//if(tmp_int > tmp_motorwert[i]) tmp_int = (tmp_motorwert[i] + tmp_int) / 2; // MotorSmoothing // MartinR: so war es |
if(tmp_int > tmp_motorwert[i]) tmp_int = ((2* tmp_motorwert[i] + tmp_int) / 2) + 1; // MartinR: evtl. stärkere Filterung um Hüpfen bei der Landung zu verringern |
// else tmp_int = 2 * tmp_int - tmp_motorwert[i]; // original MotorSmoothing |
else |
{ |