54,6 → 54,8 |
|
#include "main.h" |
#include "eeprom.c" |
#include "mymath.c" // MartinR: für Killagreg´s Erweiterung |
#include "mymath.h" // MartinR: für Killagreg´s Erweiterung |
|
unsigned char h,m,s; |
volatile unsigned int I2CTimeout = 100; |
63,7 → 65,13 |
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 |
|
volatile int NeutralAccZ2 = 0; // MartinR: für HP-Filter |
signed int h_p,h_d,h_delta,Ziel_d; // MartinR: P,D Anteil beim eigenen Höhenregler |
signed int SchwebeGas; // MartinR: für neuen Höhenregler |
|
unsigned char CosinusNickWinkel = 0, CosinusRollWinkel = 0; |
long IntegralNick = 0,IntegralNick2 = 0; |
long IntegralRoll = 0,IntegralRoll2 = 0; |
82,6 → 90,10 |
unsigned char Notlandung = 0; |
unsigned char HoehenReglerAktiv = 0; |
unsigned char TrichterFlug = 0; |
|
unsigned char delay_Hoehenregler = 0; // MartinR: zur Begrenzung der Sinkgeschwindigkeit |
signed int delay_SchwebeGas = 0; // MartinR: zur Begrenzung der Sinkgeschwindigkeit |
|
long Umschlag180Nick = 250000L, Umschlag180Roll = 250000L; |
long ErsatzKompass; |
int ErsatzKompassInGrad; // Kompasswert in Grad |
93,11 → 105,22 |
volatile unsigned char SenderOkay = 0; |
int StickNick = 0,StickRoll = 0,StickGier = 0,StickGas = 0; |
char MotorenEin = 0; |
int HoehenWert = 0; |
int SollHoehe = 0; |
//int HoehenWert = 0; //MartinR: so war es |
long HoehenWert = 0; //MartinR: geändert |
|
long HoehenWertalt = 0; //MartinR: Test |
unsigned char iHoehe; // MartinR: Zähler für HoeheD |
signed long HoehenWertSumme = 0; // MartinR: für neuen Höhenregler D-Anteil |
|
//int SollHoehe = 0;//MartinR: so war es |
long SollHoehe = 0;//MartinR: geändert |
|
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; |
|
142,7 → 165,10 |
unsigned char Parameter_ExternalControl; |
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 MikroKopterFlags = 0; |
long GIER_GRAD_FAKTOR = 1291; |
155,7 → 181,8 |
{ |
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); |
} |
182,6 → 209,8 |
NeutralAccX = 0; |
NeutralAccY = 0; |
NeutralAccZ = 0; |
NeutralAccZ2 = 0; // MartinR |
|
AdNeutralNick = 0; |
AdNeutralRoll = 0; |
AdNeutralGier = 0; |
215,6 → 244,7 |
NeutralAccY = abs(Mittelwert_AccRoll) / (2*ACC_AMPLIFY); |
NeutralAccX = abs(Mittelwert_AccNick) / (2*ACC_AMPLIFY); |
NeutralAccZ = Aktuell_az; |
NeutralAccZ2 = NeutralAccZ; // MartinR |
} |
else |
{ |
266,6 → 296,7 |
{ |
static signed long tmpl,tmpl2,tmpl3,tmpl4; |
static signed int oldNick, oldRoll, d2Roll, d2Nick; |
static signed int oldNick2, oldRoll2 ; //MartinR : für geänderte d2* Ermittlung |
signed long winkel_nick, winkel_roll; |
|
MesswertGier = (signed int) AdNeutralGier - AdWertGier; |
289,11 → 320,13 |
NaviAccNick += AdWertAccNick; |
NaviAccRoll += AdWertAccRoll; |
NaviCntAcc++; |
IntegralAccZ += Aktuell_az - NeutralAccZ; |
//IntegralAccZ += Aktuell_az - NeutralAccZ; // MartinR: so war es |
IntegralAccZ += AdWertAccHoch;// MartinR: AdWertAccHoch = Aktuell_az - NeutralAccZ; in analog.c |
|
//++++++++++++++++++++++++++++++++++++++++++++++++ |
// ADC einschalten |
ANALOG_ON; |
//ANALOG_ON; // MartinR : so war es |
// ANALOG_START; // MartinR verschoben zu timer0 |
AdReady = 0; |
//++++++++++++++++++++++++++++++++++++++++++++++++ |
|
309,7 → 342,11 |
Mess_Integral_Gier += MesswertGier; |
ErsatzKompass += MesswertGier; |
// Kopplungsanteil +++++++++++++++++++++++++++++++++++++ |
if(!Looping_Nick && !Looping_Roll && (EE_Parameter.GlobalConfig & CFG_ACHSENKOPPLUNG_AKTIV)) |
|
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 |
375,9 → 412,14 |
|
#define D_LIMIT 128 |
|
MesswertNick = HiResNick / 8; |
MesswertRoll = HiResRoll / 8; |
//MesswertNick = HiResNick / 8;// MartinR : so war es |
//MesswertRoll = HiResRoll / 8;// MartinR : so war es |
MesswertNick = AdWertNickFilter / 8;// MartinR : anstelle HiResNick : AdWertNickFilter |
MesswertRoll = AdWertRollFilter / 8;// MartinR : ist im vergleich zu HiRes.. eimal mehr gemittelt |
|
|
// 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; } |
384,9 → 426,40 |
if(AdWertRoll < 15) MesswertRoll = -1000; if(AdWertRoll < 7) MesswertRoll = -2000; |
if(PlatinenVersion == 10) { if(AdWertRoll > 1010) MesswertRoll = +1000; if(AdWertRoll > 1017) MesswertRoll = +2000; } |
else { if(AdWertRoll > 2000) MesswertRoll = +1000; if(AdWertRoll > 2015) MesswertRoll = +2000; } |
|
// MartinR : FC 1.0: Sprung von 500 auf 2000 !! FC-ME: Sprung von 1000 auf 2000 |
*/ |
// MartinR : so war es Ende |
|
// 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: so war es Anfang |
|
d2Nick = HiResNick - oldNick; |
oldNick = (oldNick + HiResNick)/2; |
if(d2Nick > D_LIMIT) d2Nick = D_LIMIT; |
399,6 → 472,31 |
MesswertRoll += (d2Roll * (signed int) Parameter_Gyro_D) / 16; |
HiResNick += (d2Nick * (signed int) Parameter_Gyro_D); |
HiResRoll += (d2Roll * (signed int) Parameter_Gyro_D); |
|
*/ //MartinR: so war es Ende |
|
// MartinR :neu Anfang |
d2Nick = MesswertNick - oldNick2; |
oldNick2 = oldNick; |
oldNick = MesswertNick; |
if(d2Nick > D_LIMIT) d2Nick = D_LIMIT; |
else if(d2Nick < -D_LIMIT) d2Nick = -D_LIMIT; |
if(d2Nick > 0) d2Nick --; |
if(d2Nick < 0) d2Nick ++; |
|
MesswertNick += (d2Nick * (signed int) Parameter_Gyro_D) / 16; |
|
d2Roll = MesswertRoll - oldRoll2; |
oldRoll2 = oldRoll; |
oldRoll = MesswertRoll; |
if(d2Roll > D_LIMIT) d2Roll = D_LIMIT; |
else if(d2Roll < -D_LIMIT) d2Roll = -D_LIMIT; |
if(d2Roll > 0) d2Roll --; |
if(d2Roll < 0) d2Roll ++; |
|
MesswertRoll += (d2Roll * (signed int) Parameter_Gyro_D) / 16; |
// MartinR :neu Ende |
|
} |
|
if(RohMesswertRoll > 0) TrimRoll += ((long) abs(KopplungsteilNickRoll) * Parameter_CouplingYawCorrection) / 64L; |
519,6 → 617,14 |
CHK_POTI_MM(Parameter_J17Timing,EE_Parameter.J17Timing,1,255); |
CHK_POTI(Parameter_ExternalControl,EE_Parameter.ExternalControl,0,255); |
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 = AID; 100 = free; 200 = coming home //so war es |
// 0 = free; 100 = AID; 200 = coming home //neu |
|
|
MAX_GAS = EE_Parameter.Gas_Max; |
MIN_GAS = EE_Parameter.Gas_Min; |
} |
533,6 → 639,9 |
int pd_ergebnis_nick,pd_ergebnis_roll,h,tmp_int; |
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; |
698,6 → 807,7 |
Mess_IntegralRoll2 = IntegralRoll; |
SummeNick = 0; |
SummeRoll = 0; |
SchwebeGas = 0; // MartinR: für neuen Höhenregler |
MikroKopterFlags |= FLAG_START; |
} |
} |
727,6 → 837,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); |
734,7 → 847,42 |
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 ; |
} |
|
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; |
|
|
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 |
if(StickGier < -2) StickGier += 2; else StickGier = 0; |
748,6 → 896,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; |
758,7 → 908,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: Original |
if((EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) || (Parameter_UserParam1 > 140)) IntegralFaktor = 0; // MartinR |
|
if(GyroFaktor < 0) GyroFaktor = 0; |
if(IntegralFaktor < 0) IntegralFaktor = 0; |
|
850,7 → 1002,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) // MartinR: "|| !IntegralFaktor" hinzugefügt |
{ |
IntegralAccNick = 0; |
IntegralAccRoll = 0; |
858,6 → 1011,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; |
866,7 → 1027,7 |
} |
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(!Looping_Nick && !Looping_Roll && (Aktuell_az > 512 || MotorenEin)) |
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*/) |
924,7 → 1085,7 |
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 && IntegralFaktor) // MartinR: "&& IntegralFaktor" hinzugefügt |
{ |
MittelIntegralNick /= ABGLEICH_ANZAHL; |
MittelIntegralRoll /= ABGLEICH_ANZAHL; |
1183,7 → 1344,7 |
//DebugOut.Analog[17] = FromNaviCtrl_Value.Distance; |
//DebugOut.Analog[18] = (int)FromNaviCtrl_Value.OsdBar; |
DebugOut.Analog[19] = WinkelOut.CalcState; |
DebugOut.Analog[20] = ServoValue; |
//DebugOut.Analog[20] = ServoValue; // MartinR: DebugOut.Analog[20] wird in timer0.c ausgegeben!! |
// DebugOut.Analog[24] = MesswertNick/2; |
// DebugOut.Analog[25] = MesswertRoll/2; |
DebugOut.Analog[27] = (int)FromNaviCtrl_Value.Kalman_MaxDrift; |
1232,12 → 1393,22 |
#define TRIM_MAX 200 |
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; |
|
|
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 |
// MartinR : Prüfen ob Unterscheidung nach Platinenversion erforderlich? |
// #define MAX_SENSOR (4096*STICK_GAIN) |
#define MAX_SENSOR (4096*4) |
if(MesswertNick > MAX_SENSOR) MesswertNick = MAX_SENSOR; |
1261,6 → 1432,9 |
// Die Höhenregelung schwächt lediglich das Gas ab, erhöht es allerdings nicht |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
GasMischanteil *= STICK_GAIN; |
|
// MartinR : so war es: |
/* |
if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung |
{ |
int tmp_int; |
1336,7 → 1510,253 |
} |
} |
if(GasMischanteil > (MAX_GAS - 20) * STICK_GAIN) GasMischanteil = (MAX_GAS - 20) * STICK_GAIN; |
*/ |
|
// MartinR: Neuer Höhenregler von MartinR: Anfang |
|
if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung |
{ |
int tmp_int; |
static char delay = 100; |
// Definitionen Vorschlag Killagreg Anfang |
#define HOOVER_GAS_AVERAGE 4192 // 4192 * 2ms = 8.2s averaging |
#define HEIGHT_CONTROL_GAS_AVERAGE 16 |
int16_t CosNick, CosRoll; |
//int16_t HeightControlGas; // MartinR: Schwebegas |
//static int16_t FilterHeightControlGas = 0; // MartinR: SchwebegasFilter |
static uint32_t HooverGasEstimation = 0; |
//static uint8_t delay = 100; |
|
int32_t tmp_long3; |
// Ende Definitionen Vorschlag Killagreg |
|
// calculate cos of nick and roll angle used for projection of the vertical hoover gas // Vorschlag von Killagerg: |
CosNick = IntegralNick/GIER_GRAD_FAKTOR; // nick angle in deg // =IntegralNick/GIER_GRAD_FAKTOR |
CHECK_MIN_MAX(CosNick, -60, 60); // limit nick angle |
CosNick = c_cos_8192(CosNick); |
CosRoll = IntegralRoll/GIER_GRAD_FAKTOR; // roll angle in deg |
CHECK_MIN_MAX(CosRoll, -60, 60); // limit roll angle |
CosRoll = c_cos_8192(CosRoll); |
|
|
// Neigungskorrektur ACCHoch: |
tmp_long3 = (int32_t) (NeutralAccZ-(Parameter_UserParam6*4)); // take current thrust |
tmp_long3 *= CosNick; // apply nick projection |
tmp_long3 /= 8192; tmp_long3 *= CosRoll; // apply roll projection |
tmp_long3 /= 8129; // average vertical projected thrust |
|
//Mess_Integral_Hoch += AdWertAccHoch; // Integrieren // MartinR: zu Höhenregler in fc.c verschoben |
//Mess_Integral_Hoch += tmp_long3; // Integrieren // MartinR: mit Neigungskorrektur // Test |
|
tmp_int = (int16_t) (AdWertAccHoch + NeutralAccZ - tmp_long3 - (Parameter_UserParam6*4)); |
if (tmp_int < 0) tmp_int ++ ; // MartinR: Digitalisierungsrauschen abmindern |
if (tmp_int > 0) tmp_int -- ; // MartinR: entspricht - 1 Bit |
|
Mess_Integral_Hoch += tmp_int; // Integrieren // MartinR: mit Neigungskorrektur |
Mess_Integral_Hoch -= Mess_Integral_Hoch / 256; // dämfen // MartinR: zu Höhenregler in fc.c verschoben |
CHECK_MIN_MAX(Mess_Integral_Hoch, -16000, 16000); // limit |
|
|
// MartinR: Änderungen HoheD und D-Anteil Höhenregler (h_d) Anfang : |
HoehenWertSumme += HoehenWert ; |
//if (++ iHoehe >= 40) // MartinR: Wartezeit um auch kleine Änderungsgeschwindigkeiten erkennen zu können |
if (++ iHoehe >= Parameter_UserParam5) // MartinR: Wartezeit einstellbar |
{ |
HoehenWertSumme = HoehenWertSumme / iHoehe ; // MartinR: Mittelwert während der Wartezeit |
HoeheD = (int) Parameter_Luftdruck_D * (HoehenWert - HoehenWertalt) ; |
if (HoeheD < 0) HoeheD += Parameter_Luftdruck_D ; // MartinR: Digitalisierungsrauschen abmindern |
if (HoeheD > 0) HoeheD -= Parameter_Luftdruck_D ; // MartinR: entspricht - 1 Bit vom Höhenwert |
|
iHoehe = 0 ; |
HoehenWertalt = HoehenWertSumme ; |
HoehenWertSumme = 0 ; |
} |
// D-Anteil berechnen: |
h_d = (int) (HoeheD)/(64/STICK_GAIN); // D-Anteil (8*8=64) |
// ACC-D-Anteil |
tmp_int = ((Mess_Integral_Hoch / 128) * (signed long) Parameter_Hoehe_ACC_Wirkung) / (128 / STICK_GAIN); |
if(tmp_int > 50*STICK_GAIN) tmp_int = 50*STICK_GAIN; |
else if(tmp_int < -(50*STICK_GAIN)) tmp_int = -(50*STICK_GAIN); |
h_d += tmp_int; // positiver D-Anteil bedeutet Steigen |
|
// MartinR: Änderungen HoheD und D-Anteil Höhenregler (h_d) Ende |
|
|
if(EE_Parameter.GlobalConfig & CFG_HOEHEN_SCHALTER) // Regler wird über Schalter gesteuert |
{ |
//if(Parameter_MaxHoehe < 50) // MartinR :so war es |
|
|
if(Parameter_MaxHoehe < 50 || (Parameter_UserParam1 > 140) ) // MartinR: Schalter aus oder HH über UsererParam1 an |
{ |
// Höhenregler nicht aktiv |
if(!delay--) |
{ |
#define OPA_OFFSET_STEP 10 |
if((MessLuftdruck > 1000) && (OCR0A < (255 - OPA_OFFSET_STEP)) ) |
{ |
ExpandBaro -= 1; |
OCR0A = DruckOffsetSetting - OPA_OFFSET_STEP * ExpandBaro; |
beeptime = 300; |
delay = 250; |
} |
else if((MessLuftdruck < 100) && (OCR0A > OPA_OFFSET_STEP)) |
{ |
ExpandBaro += 1; |
OCR0A = DruckOffsetSetting - OPA_OFFSET_STEP * ExpandBaro; |
beeptime = 300; |
delay = 250; |
} |
else |
if((MessLuftdruck < 100) && OCR0A > 1) |
{ |
if(OCR0A > 10) |
{ |
ExpandBaro += 10; |
OCR0A = DruckOffsetSetting - ExpandBaro; |
} |
else OCR0A = 1; |
beeptime = 300; |
delay = 250; |
} |
else |
{ |
//SollHoehe = HoehenWert - 20; // Parameter_MaxHoehe ist der PPM-Wert des Schalters // MartinR : so war es |
SollHoehe = HoehenWert; // MartinR : geändert |
HoehenReglerAktiv = 0; |
delay = 1; |
} |
} |
SollHoehe = HoehenWert; // MartinR: SollHoehe wird nachgeführt bis HoehenreglerAktiv = 1 |
//SchwebeGas = GasMischanteil; // MartinR: SchwebeGas wird nachgeführt bis HoehenreglerAktiv = 1 |
// Deaktiv wegen HooverGasEstimation/HOOVER_GAS_AVERAGE |
|
HoehenReglerAktiv = 0; // MartinR: zur Sicherheit |
} |
else // Schalter an und kein HH über Schalter |
{ |
HoehenReglerAktiv = 1; |
// Schwebegas ist der letzte GasMischanteil vor dem Aktivieren der Höhenregelung |
delay = 200; |
} |
} |
else // Sollhöhe über Poti |
{ |
//SollHoehe = ((int) ExternHoehenValue + (int) Parameter_MaxHoehe) * (int)EE_Parameter.Hoehe_Verstaerkung - 20; // MartinR : so war es |
//HoehenReglerAktiv = 1; // MartinR : so war es |
if(Parameter_UserParam1 > 140) // HH über Schalter: Höhenregler abgeschaltet, Nachführen von Parametern |
{ |
SollHoehe = HoehenWert; // SollHoehe wird nachgeführt bis HoehenreglerAktiv = 1 |
HoehenReglerAktiv = 0; |
} |
else // Höhenregler mit Sollhöhe über Poti aktiv |
{ |
// SchwebeGas = GasMischanteil; // SchwebeGas zunächst = Gasmischanteil bis bessere Lösung |
// Deaktiv wegen HooverGasEstimation/HOOVER_GAS_AVERAGE |
HoehenReglerAktiv = 1; |
// MartinR: Versuch Höhenregler zu verlangsamen. Steig- / Sinkrate im MK-Tool über UserParam4 einstellbar |
if (((int) ExternHoehenValue + (int) Parameter_MaxHoehe) * (int)EE_Parameter.Hoehe_Verstaerkung - 20 > SollHoehe) |
{ |
if(++delay_Hoehenregler > (Parameter_UserParam4 / 4)) |
{ |
delay_Hoehenregler = 0; |
SollHoehe ++ ; //MartinR: Höhenänderung begrenzen. |
} |
} |
else |
{ |
if (((int) ExternHoehenValue + (int) Parameter_MaxHoehe) * (int)EE_Parameter.Hoehe_Verstaerkung - 20 < SollHoehe) |
{ |
if(++delay_Hoehenregler > (Parameter_UserParam4 / 4)) |
{ |
delay_Hoehenregler = 0; |
SollHoehe -- ; //MartinR: Höhenänderung begrenzen. |
} |
} |
} |
} |
} |
|
if(Notlandung) SollHoehe = 0; |
|
if(HoehenReglerAktiv && !(Parameter_UserParam1 > 140)) // kein HH |
{ |
h_delta = (int) SollHoehe - (int) HoehenWert; // positiver Wert: MK ist unterhalb der Sollhöhe |
|
h_p = (h_delta * (int) Parameter_Hoehe_P) / (16 / STICK_GAIN); // Differenz bestimmen --> P-Anteil |
|
|
/* an Anfang Höhenregler verschoben, da h_d für Schwebegas- Abschätzung verwendet wird |
h_d = (int) (HoeheD)/(64/STICK_GAIN); // D-Anteil (8*8=64) |
// ACC-D-Anteil |
tmp_int = ((Mess_Integral_Hoch / 128) * (signed long) Parameter_Hoehe_ACC_Wirkung) / (128 / STICK_GAIN); |
if(tmp_int > 50*STICK_GAIN) tmp_int = 50*STICK_GAIN; |
else if(tmp_int < -(50*STICK_GAIN)) tmp_int = -(50*STICK_GAIN); |
h_d += tmp_int; // positiver D-Anteil bedeutet Steigen |
*/ |
|
|
//if(HooverGasEstimation == 0) HeightControlGas = GasMixFraction; // take stick gas // MartinR: SchwebeGas = GasMischanteil; |
if(HooverGasEstimation == 0) SchwebeGas = GasMischanteil; |
else SchwebeGas = (int16_t)(HooverGasEstimation/HOOVER_GAS_AVERAGE); // take hoover point |
// MartinR: SchwebeGas = HooverGasEstimation/HOOVER_GAS_AVERAGE |
|
h = SchwebeGas + h_p - h_d ; |
|
// strech by actual attitude projection // Neigungskorrektur |
tmp_long3 = (int32_t)h; // MartinR: HeightControlGas = h |
tmp_long3 *= 8192; tmp_long3 /= CosNick; |
tmp_long3 *= 8192; tmp_long3 /= CosRoll; |
h = (int16_t)tmp_long3; |
|
hoehenregler = (hoehenregler*7 + h) / 8; |
|
if(hoehenregler < EE_Parameter.Hoehe_MinGas * STICK_GAIN) // nicht unter MIN |
{ |
if(GasMischanteil >= EE_Parameter.Hoehe_MinGas * STICK_GAIN) hoehenregler = EE_Parameter.Hoehe_MinGas * STICK_GAIN; |
if(GasMischanteil < EE_Parameter.Hoehe_MinGas * STICK_GAIN) hoehenregler = GasMischanteil; |
} |
if(hoehenregler > GasMischanteil) hoehenregler = GasMischanteil; // nicht mehr als Gas |
//else hoehenregler += (GasMischanteil-hoehenregler)/16 ; // um die Höhe noch leicht mit Gas korrigieren zu können |
//if(SchwebeGas < 20) SchwebeGas = 20; // nicht unter 20 |
|
GasMischanteil = hoehenregler; |
} |
else // Integratoren auf Null halten |
{ |
Mess_Integral_Hoch = 0; |
hoehenregler = GasMischanteil; |
// h_p = 0 ; |
// h_d = 0 ; |
} |
// Hoover gas estimation by averaging gas control output on small z-velocities |
// this is done only if height contol option is selected in global config and aircraft is flying |
if(MikroKopterFlags & FLAG_FLY) //MikroKopterFlags FLAG_FLY |
{ |
if(HooverGasEstimation == 0) HooverGasEstimation = HOOVER_GAS_AVERAGE * (uint32_t)GasMischanteil; // init estimation |
// HOOVER_GAS_AVERAGE= 4192 |
if(abs(h_d) < 8) |
{ |
tmp_long3 = (int32_t)GasMischanteil; // take current thrust |
tmp_long3 *= CosNick; // apply nick projection |
tmp_long3 /= 8192; tmp_long3 *= CosRoll; // apply roll projection |
tmp_long3 /= 8129; // average vertical projected thrust |
HooverGasEstimation -= HooverGasEstimation/HOOVER_GAS_AVERAGE; |
HooverGasEstimation += tmp_long3; |
} |
} |
|
DebugOut.Analog[16] = HooverGasEstimation/HOOVER_GAS_AVERAGE; |
//DebugOut.Analog[16] = Mess_Integral_Hoch; |
DebugOut.Analog[17] = h_d; |
|
} // EOF: EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG |
|
|
if(GasMischanteil > (MAX_GAS - 20) * STICK_GAIN) GasMischanteil = (MAX_GAS - 20) * STICK_GAIN; |
|
// MartinR: Ende neuer Höhenregler |
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Mischer und PI-Regler |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1366,6 → 1786,9 |
// 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); |
1372,6 → 1795,29 |
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; |
1380,11 → 1826,40 |
// 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); |
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; |