Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 1596 → Rev 1604

/branches/V0.78f_ACC-HH_MartinR/fc.c
67,7 → 67,9
int AdNeutralNick = 0,AdNeutralRoll = 0,AdNeutralGier = 0,StartNeutralRoll = 0,StartNeutralNick = 0;
int Mittelwert_AccNick, Mittelwert_AccRoll,Mittelwert_AccHoch, NeutralAccX=0, NeutralAccY=0;
int NaviAccNick, NaviAccRoll,NaviCntAcc = 0;
volatile float NeutralAccZ = 0;
//volatile float NeutralAccZ = 0; // MartinR : so war es
volatile int NeutralAccZ = 0; // MartinR geändert´
 
unsigned char CosinusNickWinkel = 0, CosinusRollWinkel = 0;
long IntegralNick = 0,IntegralNick2 = 0;
long IntegralRoll = 0,IntegralRoll2 = 0;
103,6 → 105,8
int LageKorrekturRoll = 0,LageKorrekturNick = 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 → 156,9
unsigned char Parameter_Servo3,Parameter_Servo4,Parameter_Servo5;
struct mk_param_struct EE_Parameter;
signed int ExternStickNick = 0,ExternStickRoll = 0,ExternStickGier = 0, ExternHoehenValue = -20;
int MaxStickNick = 0,MaxStickRoll = 0;
//int MaxStickNick = 0,MaxStickRoll = 0;MartinR: so war es
int MaxStickNick = 0,MaxStickRoll = 0,stick_nick_neutral = 0,stick_roll_neutral = 0; // MartinR: stick_.._neutral hinzugefügt
 
unsigned int modell_fliegt = 0;
volatile unsigned char FCFlags = 0;
long GIER_GRAD_FAKTOR = 1291;
159,7 → 165,6
signed int KopplungsteilNickRoll,KopplungsteilRollNick;
signed int tmp_motorwert[MAX_MOTORS];
char VarioCharacter = ' ';
 
#define LIMIT_MIN(value, min) {if(value <= min) value = min;}
#define LIMIT_MAX(value, max) {if(value >= max) value = max;}
#define LIMIT_MIN_MAX(value, min, max) {if(value <= min) value = min; else if(value >= max) value = max;}
168,7 → 173,9
{
int motor;
if(neu > alt) motor = (1*(int)alt + neu) / 2;
else motor = neu - (alt - neu)*1;
//else motor = neu - (alt - neu)*1; // MartinR: so war es
else motor = neu; // MartinR: Entsprechend Vorschlag von MartinW geändert
//if(Poti2 < 20) return(neu);
return(motor);
}
321,8 → 328,12
// Gier ++++++++++++++++++++++++++++++++++++++++++++++++
Mess_Integral_Gier += MesswertGier;
ErsatzKompass += MesswertGier;
// Kopplungsanteil +++++++++++++++++++++++++++++++++++++
if(!Looping_Nick && !Looping_Roll && (EE_Parameter.GlobalConfig & CFG_ACHSENKOPPLUNG_AKTIV))
// Kopplungsanteil +++++++++++++++++++++++++++++++++++++
if((EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) || (Parameter_UserParam1 > 140)) IntegralFaktor = 0; // MartinR: zusätzlich
//if(!Looping_Nick && !Looping_Roll && (EE_Parameter.GlobalConfig & CFG_ACHSENKOPPLUNG_AKTIV)) // MartinR : so war es
if(!Looping_Nick && !Looping_Roll && IntegralFaktor && (EE_Parameter.GlobalConfig & CFG_ACHSENKOPPLUNG_AKTIV)) // MartinR: zusätzlich "&& IntegralFaktor"
{
tmpl3 = (MesswertRoll * winkel_nick) / 2048L;
tmpl3 *= Parameter_AchsKopplung2; //65
391,6 → 402,9
MesswertNick = HiResNick / 8;
MesswertRoll = HiResRoll / 8;
 
// MartinR : so war es Anfang
/*
if(AdWertNick < 15) MesswertNick = -1000; if(AdWertNick < 7) MesswertNick = -2000;
if(PlatinenVersion == 10) { if(AdWertNick > 1010) MesswertNick = +1000; if(AdWertNick > 1017) MesswertNick = +2000; }
else { if(AdWertNick > 2000) MesswertNick = +1000; if(AdWertNick > 2015) MesswertNick = +2000; }
397,6 → 411,35
if(AdWertRoll < 15) MesswertRoll = -1000; if(AdWertRoll < 7) MesswertRoll = -2000;
if(PlatinenVersion == 10) { if(AdWertRoll > 1010) MesswertRoll = +1000; if(AdWertRoll > 1017) MesswertRoll = +2000; }
else { if(AdWertRoll > 2000) MesswertRoll = +1000; if(AdWertRoll > 2015) MesswertRoll = +2000; }
// MartinR : FC 1.0: Sprung von 500 auf 2000 !! FC-ME: Sprung von 1000 auf 2000
*/
// MartinR : so war es Ende
// MartinR : Neu Anfang
if(PlatinenVersion == 10)
{
if(AdWertNick > 1010) MesswertNick = +600;
if(AdWertNick > 1017) MesswertNick = +800;
if(AdWertNick < 15) MesswertNick = -600;
if(AdWertNick < 7) MesswertNick = -800;
if(AdWertRoll > 1010) MesswertRoll = +600;
if(AdWertRoll > 1017) MesswertRoll = +800;
if(AdWertRoll < 15) MesswertRoll = -600;
if(AdWertRoll < 7) MesswertRoll = -800;
}
else
{
if(AdWertNick > 2000) MesswertNick = +1200;
if(AdWertNick > 2015) MesswertNick = +1600;
if(AdWertNick < 15) MesswertNick = -1200;
if(AdWertNick < 7) MesswertNick = -1600;
if(AdWertRoll > 2000) MesswertRoll = +1200;
if(AdWertRoll > 2015) MesswertRoll = +1600;
if(AdWertRoll < 15) MesswertRoll = -1200;
if(AdWertRoll < 7) MesswertRoll = -1600;
}
// MartinR : Neu Ende
 
if(Parameter_Gyro_D)
{
543,6 → 586,12
CHK_POTI(Parameter_DynamicStability,EE_Parameter.DynamicStability);
CHK_POTI(Parameter_ExternalControl,EE_Parameter.ExternalControl);
Ki = 10300 / (Parameter_I_Faktor + 1);
if(Parameter_UserParam1 > 140) 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;
}
555,6 → 604,9
int pd_ergebnis_nick,pd_ergebnis_roll,tmp_int, tmp_int2;
int GierMischanteil,GasMischanteil;
static long SummeNick=0,SummeRoll=0;
static long SummeNickHH=0,SummeRollHH=0; // MartinR: Für ACC-HH Umschaltung
static long sollGier = 0,tmp_long,tmp_long2;
static long IntegralFehlerNick = 0;
static long IntegralFehlerRoll = 0;
749,6 → 801,9
{
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;
StickNick = stick_nick - (GPS_Nick + GPS_Nick2);
756,6 → 811,43
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;
StickRoll = stick_roll - (GPS_Roll + GPS_Roll2);
*/
// MartinR: geändert Anfang
if(Parameter_UserParam1 > 140) // 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
 
StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]];
if(StickGier > 2) StickGier -= 2; else
771,6 → 863,8
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//+ Analoge Steuerung per Seriell
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// MartinR: ToDo: eventuell die Kombination HH und Steuerung per Seriell nicht zulassen??
 
if(ExternControl.Config & 0x01 && Parameter_ExternalControl > 128)
{
StickNick += (int) ExternControl.Nick * (int) EE_Parameter.Stick_P;
781,7 → 875,9
}
if(StickGas < 0) StickGas = 0;
 
if(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor = 0;
//if(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor = 0; // MartinR: so war es
if((EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) || (Parameter_UserParam1 > 140)) IntegralFaktor = 0; // MartinR geändert
//if(GyroFaktor < 0) GyroFaktor = 0;
//if(IntegralFaktor < 0) IntegralFaktor = 0;
 
873,7 → 969,9
MittelIntegralNick2 += IntegralNick2;
MittelIntegralRoll2 += IntegralRoll2;
 
if(Looping_Nick || Looping_Roll)
//if(Looping_Nick || Looping_Roll) // MartinR: so war es
if(Looping_Nick || Looping_Roll || !IntegralFaktor) // MartinR: "|| !IntegralFaktor" hinzugefügt
{
IntegralAccNick = 0;
IntegralAccRoll = 0;
881,6 → 979,14
MittelIntegralRoll = 0;
MittelIntegralNick2 = 0;
MittelIntegralRoll2 = 0;
IntegralNick = 0; // MartinR: im HH-Modus alle unbenutzten Integratoren = 0
IntegralRoll = 0; // MartinR: im HH-Modus alle unbenutzten Integratoren = 0
Mess_IntegralNick = 0; // MartinR: im HH-Modus alle unbenutzten Integratoren = 0
Mess_IntegralRoll = 0; // MartinR: im HH-Modus alle unbenutzten Integratoren = 0
Mess_Integral_Gier = 0; // MartinR: im HH-Modus alle unbenutzten Integratoren = 0
Mess_Integral_Gier2 = 0; // MartinR: im HH-Modus alle unbenutzten Integratoren = 0
Mess_IntegralNick2 = Mess_IntegralNick;
Mess_IntegralRoll2 = Mess_IntegralRoll;
ZaehlMessungen = 0;
889,7 → 995,9
}
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(!Looping_Nick && !Looping_Roll && (Aktuell_az > 512 || MotorenEin))
//if(!Looping_Nick && !Looping_Roll && (Aktuell_az > 512 || MotorenEin)) // MartinR: so war es
if(!Looping_Nick && !Looping_Roll && (Aktuell_az > 512 || MotorenEin) && IntegralFaktor) // MartinR: "&& IntegralFaktor" hinzugefügt
{
long tmp_long, tmp_long2;
if(FromNaviCtrl_Value.Kalman_K != -1 /*&& !TrichterFlug*/)
947,7 → 1055,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;
1216,8 → 1326,20
if(TrimNick > TRIM_MAX) TrimNick = TRIM_MAX; else if(TrimNick <-TRIM_MAX) TrimNick =-TRIM_MAX;
if(TrimRoll > TRIM_MAX) TrimRoll = TRIM_MAX; else if(TrimRoll <-TRIM_MAX) TrimRoll =-TRIM_MAX;
 
//MesswertNick = IntegralNickMalFaktor + (long)((long)MesswertNick * GyroFaktor + (long)TrimNick * 128L) / (256L / STICK_GAIN);// MartinR so war es
//MesswertRoll = IntegralRollMalFaktor + (long)((long)MesswertRoll * GyroFaktor + (long)TrimRoll * 128L) / (256L / STICK_GAIN);// MartinR so war es
if(!IntegralFaktor) // MartinR : hinzugefügt
{
MesswertNick = (long) ((long)MesswertNick * GyroFaktor) / (256L / STICK_GAIN) ; // MartinR : hinzugefügt
MesswertRoll = (long) ((long)MesswertRoll * GyroFaktor) / (256L / STICK_GAIN) ; // MartinR : hinzugefügt
}
else // MartinR so war es
{
MesswertNick = IntegralNickMalFaktor + (long)((long)MesswertNick * GyroFaktor + (long)TrimNick * 128L) / (256L / STICK_GAIN);
MesswertRoll = IntegralRollMalFaktor + (long)((long)MesswertRoll * GyroFaktor + (long)TrimRoll * 128L) / (256L / STICK_GAIN);
}
MesswertGier = (long)(MesswertGier * 2 * (long)GyroFaktorGier) / (256L / STICK_GAIN) + (long)(Integral_Gier * IntegralFaktorGier) / (2 * (44000 / STICK_GAIN));
 
// Maximalwerte abfangen
1306,7 → 1428,9
// 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 über UsererParam1 an
{ //height control not active
if(!delay--)
{
1324,7 → 1448,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: 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
1580,7 → 1714,9
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Nick-Achse
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
DiffNick = MesswertNick - StickNick; // Differenz bestimmen
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);
1587,6 → 1723,28
if(SummeNick < -(16000L * STICK_GAIN)) SummeNick = -(16000L * STICK_GAIN);
pd_ergebnis_nick = DiffNick + SummeNick / Ki; // PI-Regler für Nick
// Motor Vorn
*/
// MartinR : so war es Ende
// MartinR : geändert Anfang
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);
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 = DiffNick + 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;
1594,12 → 1752,39
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Roll-Achse
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
DiffRoll = MesswertRoll - StickRoll; // Differenz bestimmen
DiffRoll = MesswertRoll - StickRoll; // Differenz bestimmen
// MartinR : so war es Anfang
/*
if(IntegralFaktor) SummeRoll += IntegralRollMalFaktor - StickRoll;// I-Anteil bei Winkelregelung
else SummeRoll += DiffRoll; // I-Anteil bei HH
if(SummeRoll > (STICK_GAIN * 16000L)) SummeRoll = (STICK_GAIN * 16000L);
if(SummeRoll < -(16000L * STICK_GAIN)) SummeRoll = -(16000L * STICK_GAIN);
pd_ergebnis_roll = DiffRoll + SummeRoll / Ki; // PI-Regler für Roll
*/
// MartinR : so war es Ende
// MartinR : geändert Anfang
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
//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 = DiffRoll + 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;