Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 1226 → Rev 1286

/branches/V0.74d_ACC-HH_MartinR/fc.c
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;