Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 1793 → Rev 1797

/branches/V0.82a_ACC-HH_MartinR/fc.c
73,7 → 73,8
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; //MartinR so war es
long Mess_Integral_Gier = 0; //MartinR: Mess_Integral_Gier2 unbenutzt
long MittelIntegralNick,MittelIntegralRoll,MittelIntegralNick2,MittelIntegralRoll2;
long SummeNick=0,SummeRoll=0;
volatile long Mess_Integral_Hoch = 0;
101,6 → 102,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;
 
152,7 → 155,9
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};
 
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;
long GIER_GRAD_FAKTOR = 1291;
185,6 → 190,10
DebugOut.Analog[13] = Motor[1].SetPoint;
DebugOut.Analog[14] = Motor[2].SetPoint;
DebugOut.Analog[15] = Motor[3].SetPoint;
//DebugOut.Analog[16] = DiffNick; // MartinR: test
//DebugOut.Analog[17] = DiffRoll; // MartinR: test
//DebugOut.Analog[18] = MesswertNick; // MartinR: test
//DebugOut.Analog[19] = MesswertRoll; // MartinR: test
DebugOut.Analog[20] = ServoNickValue;
DebugOut.Analog[22] = Capacity.ActualCurrent;
DebugOut.Analog[23] = Capacity.UsedCapacity;
198,8 → 207,6
//if(Capacity.MinOfMaxPWM < 250/* && modell_fliegt > 500*/) { beeptime = 1000; DebugOut.Analog[25]++; }
}
 
 
 
void Piep(unsigned char Anzahl, unsigned int dauer)
{
if(MotorenEin) return; //auf keinen Fall im Flug!
368,10 → 375,12
static signed long tmpl,tmpl2,tmpl3,tmpl4;
static signed int oldNick, oldRoll, d2Roll, d2Nick;
signed long winkel_nick, winkel_roll;
unsigned char i;
//unsigned char i; // MartinR: wird bei V0.81d nicht mehr benötigt, da Poti-Auswertung verschoben
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;
 
465,11 → 474,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; }
476,23 → 492,55
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;
if(d2Nick > D_LIMIT) d2Nick = D_LIMIT;
else if(d2Nick < -D_LIMIT) d2Nick = -D_LIMIT;
 
d2Roll = HiResRoll - oldRoll;
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;
574,7 → 622,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);
601,6 → 649,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;
 
633,6 → 689,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;
743,6 → 802,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+
}
}
}
785,7 → 846,7
MotorenEin = 1;
sollGier = 0;
Mess_Integral_Gier = 0;
Mess_Integral_Gier2 = 0;
//Mess_Integral_Gier2 = 0; //MartinR: Mess_Integral_Gier2 unbenutzt
Mess_IntegralNick = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccNick;
Mess_IntegralRoll = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccRoll;
Mess_IntegralNick2 = IntegralNick;
829,10 → 890,50
{
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
//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
862,13 → 963,14
StickGas = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120;
 
GyroFaktor = (Parameter_Gyro_P + 10.0);
IntegralFaktor = Parameter_Gyro_I;
// IntegralFaktor = Parameter_Gyro_I; // MartinR: verschoben um Code zu sparen
GyroFaktorGier = (Parameter_Gyro_Gier_P + 10.0);
IntegralFaktorGier = Parameter_Gyro_Gier_I;
 
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//+ 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;
879,7 → 981,7
}
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, verschoben um code zu sparen
 
if(abs(StickNick/STICK_GAIN) > MaxStickNick)
{
969,7 → 1071,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;
983,9 → 1086,29
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: 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
//Integral_Gier = 0; // MartinR: im HH-Modus alle unbenutzten Integratoren = 0
//Mess_Integral_Gier2 = 0; // MartinR: im HH-Modus alle unbenutzten Integratoren = 0
}
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))
if(!Looping_Nick && !Looping_Roll && (Aktuell_az > 512 || MotorenEin)) // MartinR: so war es, wegen Kameraausgleich wieder aktiviert
//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*/)
1042,7 → 1165,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;
1208,7 → 1333,8
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Kompass
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV))
if((KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV)) && !(Parameter_UserParam1 > 50)) // MartinR: bei HH über Schalter wird der Kompass abgeschaltet
//if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV)) // MartinR: so war es
{
int w,v,r,fehler,korrektur;
w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln
1261,8 → 1387,24
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: im HH-Modus alle unbenutzten Integratoren = 0 //neu: nur Kompass wird bei HH deaktiviert
//Integral_Gier = 0; // MartinR: im HH-Modus alle unbenutzten Integratoren = 0
}
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));
}
MesswertGier = (long)(MesswertGier * 2 * (long)GyroFaktorGier) / (256L / STICK_GAIN) + (long)(Integral_Gier * IntegralFaktorGier) / (2 * (44000 / STICK_GAIN));
 
// Maximalwerte abfangen
1354,7 → 1496,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--)
{
1372,7 → 1515,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
1434,6 → 1587,7
FC_StatusFlags &= ~(FC_STATUS_VARIO_TRIM_UP|FC_STATUS_VARIO_TRIM_DOWN);
HeightTrimming = 0;
SollHoehe = HoehenWert; // update setpoint to current height
//MartinR: Sollhöhe sollte sich hier nicht mehr ändern!
if(EE_Parameter.ExtraConfig & CFG2_VARIO_BEEP) beeptime = 500;
if(!StartTrigger && HoehenWert > 50)
{
1447,7 → 1601,9
{
SollHoehe += (HeightTrimming * EE_Parameter.Hoehe_Verstaerkung)/(5 * 512 / 2); // move setpoint
HeightTrimming = 0;
LIMIT_MIN_MAX(SollHoehe, (HoehenWert-1024), (HoehenWert+1024)); // max. 10m Unterschied
//LIMIT_MIN_MAX(SollHoehe, (HoehenWert-1024), (HoehenWert+1024)); // max. 10m Unterschied // MartinR: so war es
LIMIT_MIN_MAX(SollHoehe, (HoehenWert-256), (HoehenWert+256)); // MartinR: geändert
// MartinR: limits eventuell verändern
if(EE_Parameter.ExtraConfig & CFG2_VARIO_BEEP) beeptime = 100;
//update hoover gas stick value when setpoint is shifted
if(!EE_Parameter.Hoehe_StickNeutralPoint)
1459,7 → 1615,7
}
}
if(BaroExpandActive) SollHoehe = HoehenWert; // update setpoint to current altitude if Expanding is active
} //if FCFlags & MKFCFLAG_FLY
} //if FC_StatusFlags & MKFCFLAG_FLY
else
{
SollHoehe = HoehenWert - 400;
1480,11 → 1636,14
else // valid data from air pressure sensor
{
// ------------------------- P-Part ----------------------------
//DebugOut.Analog[16] = SollHoehe; // MartinR: test
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
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, -127 * STICK_GAIN, 127 * 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;
1491,9 → 1650,13
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(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 ------------------------
1500,14 → 1663,19
if(Parameter_Hoehe_ACC_Wirkung)
{
tmp_long = ((Mess_Integral_Hoch / 128L) * (int32_t) Parameter_Hoehe_ACC_Wirkung) / (128L / STICK_GAIN);
LIMIT_MIN_MAX(tmp_long, -32 * STICK_GAIN, 64 * STICK_GAIN);
//LIMIT_MIN_MAX(tmp_long, -32 * STICK_GAIN, 64 * STICK_GAIN); // MartinR: so war es
// MartinR: weshalb unsymmetrisch?
LIMIT_MIN_MAX(tmp_long, -32 * STICK_GAIN, 32 * STICK_GAIN); // MartinR: geändert
GasReduction += tmp_long;
}
// ------------------------ 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
//DebugOut.Analog[17] = GasReduction; // MartinR: test
// ------------------------ ----------------------------------
HCGas -= GasReduction;
// limit deviation from hoover point within the target region
1551,7 → 1719,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
1657,20 → 1826,107
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
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
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);
if(SummeNick < -(16000L * STICK_GAIN)) SummeNick = -(16000L * STICK_GAIN);
 
if(EE_Parameter.Gyro_Stability <= 8) pd_ergebnis_nick = (EE_Parameter.Gyro_Stability * DiffNick) / 8; // PI-Regler für Nick
else pd_ergebnis_nick = ((EE_Parameter.Gyro_Stability / 2) * DiffNick) / 4; // Überlauf verhindern
pd_ergebnis_nick += SummeNick / Ki;
 
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;
1679,18 → 1935,45
// 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);
 
if(EE_Parameter.Gyro_Stability <= 8) pd_ergebnis_roll = (EE_Parameter.Gyro_Stability * DiffRoll) / 8; // PI-Regler für Roll
else pd_ergebnis_roll = ((EE_Parameter.Gyro_Stability / 2) * DiffRoll) / 4; // Überlauf verhindern
pd_ergebnis_roll += SummeRoll / Ki;
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;
*/
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Universal Mixer
1716,7 → 1999,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,(int) MIN_GAS * 4,(int) MAX_GAS * 4);
Motor[i].SetPoint = tmp_int / 4;