WebSVN - FlightCtrl - Blame - Rev 1250 - /branches/FC_075b_ACC-HH

Rev	Author	Line No.	Line
1	ingob	1	/*#######################################################################################
		2	Flight Control
		3	#######################################################################################*/
		4	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		5	// + Copyright (c) 04.2007 Holger Buss
		6	// + Nur f�r den privaten Gebrauch
		7	// + www.MikroKopter.com
		8	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1051	killagreg	9	// + Es gilt f�r das gesamte Projekt (Hardware, Software, Bin�rfiles, Sourcecode und Dokumentation),
		10	// + dass eine Nutzung (auch auszugsweise) nur f�r den privaten (nicht-kommerziellen) Gebrauch zul�ssig ist.
		11	// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt
		12	// + bzgl. der Nutzungsbedingungen aufzunehmen.
1	ingob	13	// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Best�ckung und Verkauf von Platinen oder Baus�tzen,
		14	// + Verkauf von Luftbildaufnahmen, usw.
		15	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1051	killagreg	16	// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder ver�ffentlicht,
1	ingob	17	// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright m�ssen dann beiliegen
		18	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		19	// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts
		20	// + auf anderen Webseiten oder sonstigen Medien ver�ffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
		21	// + eindeutig als Ursprung verlinkt werden
		22	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		23	// + Keine Gew�hr auf Fehlerfreiheit, Vollst�ndigkeit oder Funktion
		24	// + Benutzung auf eigene Gefahr
		25	// + Wir �bernehmen keinerlei Haftung f�r direkte oder indirekte Personen- oder Sachsch�den
		26	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1051	killagreg	27	// + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur
1	ingob	28	// + mit unserer Zustimmung zul�ssig
		29	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		30	// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen
		31	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1051	killagreg	32	// + Redistributions of source code (with or without modifications) must retain the above copyright notice,
1	ingob	33	// + this list of conditions and the following disclaimer.
		34	// + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived
		35	// + from this software without specific prior written permission.
1051	killagreg	36	// + * The use of this project (hardware, software, binary files, sources and documentation) is only permittet
1	ingob	37	// + for non-commercial use (directly or indirectly)
1051	killagreg	38	// + Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted
1	ingob	39	// + with our written permission
1051	killagreg	40	// + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be
		41	// + clearly linked as origin
831	hbuss	42	// + * porting to systems other than hardware from www.mikrokopter.de is not allowed
1	ingob	43	// + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
		44	// + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
		45	// + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
		46	// + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
		47	// + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
		48	// + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
		49	// + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
492	hbuss	50	// + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
1	ingob	51	// + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
1051	killagreg	52	// + POSSIBILITY OF SUCH DAMAGE.
1	ingob	53	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		54
		55	#include "main.h"
395	hbuss	56	#include "eeprom.c"
1	ingob	57
		58	unsigned char h,m,s;
173	holgerb	59	volatile unsigned int I2CTimeout = 100;
1153	hbuss	60	int MesswertNick,MesswertRoll,MesswertGier,MesswertGierBias, RohMesswertNick,RohMesswertRoll;
1166	hbuss	61	int TrimNick, TrimRoll;
1051	killagreg	62	int AdNeutralGierBias;
927	hbuss	63	int AdNeutralNick = 0,AdNeutralRoll = 0,AdNeutralGier = 0,StartNeutralRoll = 0,StartNeutralNick = 0;
		64	int Mittelwert_AccNick, Mittelwert_AccRoll,Mittelwert_AccHoch, NeutralAccX=0, NeutralAccY=0;
805	hbuss	65	int NaviAccNick, NaviAccRoll,NaviCntAcc = 0;
1247	MartinR	66	//volatile float NeutralAccZ = 0; // MartinR : so war es
		67	volatile int NeutralAccZ = 0; // MartinR ge�ndert
		68
		69	volatile int NeutralAccZ2 = 0; // MartinR: f�r HP-Filter
		70	signed int h_p,h_d,h_delta,Ziel_d; // MartinR: P,D Anteil beim eigenen H�henregler
		71	signed int SchwebeGas; // MartinR: f�r neuen H�henregler
		72
1	ingob	73	unsigned char CosinusNickWinkel = 0, CosinusRollWinkel = 0;
693	hbuss	74	long IntegralNick = 0,IntegralNick2 = 0;
		75	long IntegralRoll = 0,IntegralRoll2 = 0;
		76	long IntegralAccNick = 0,IntegralAccRoll = 0,IntegralAccZ = 0;
		77	long Integral_Gier = 0;
		78	long Mess_IntegralNick = 0,Mess_IntegralNick2 = 0;
		79	long Mess_IntegralRoll = 0,Mess_IntegralRoll2 = 0;
		80	long Mess_Integral_Gier = 0,Mess_Integral_Gier2 = 0;
		81	long MittelIntegralNick,MittelIntegralRoll,MittelIntegralNick2,MittelIntegralRoll2;
1	ingob	82	volatile long Mess_Integral_Hoch = 0;
1153	hbuss	83	int KompassValue = 0;
		84	int KompassStartwert = 0;
		85	int KompassRichtung = 0;
693	hbuss	86	unsigned int KompassSignalSchlecht = 500;
855	hbuss	87	unsigned char MAX_GAS,MIN_GAS;
1	ingob	88	unsigned char Notlandung = 0;
		89	unsigned char HoehenReglerAktiv = 0;
880	hbuss	90	unsigned char TrichterFlug = 0;
1247	MartinR	91
		92	unsigned char delay_Hoehenregler = 0; // MartinR: zur Begrenzung der Sinkgeschwindigkeit
		93	signed int delay_SchwebeGas = 0; // MartinR: zur Begrenzung der Sinkgeschwindigkeit
		94
395	hbuss	95	long Umschlag180Nick = 250000L, Umschlag180Roll = 250000L;
693	hbuss	96	long ErsatzKompass;
		97	int ErsatzKompassInGrad; // Kompasswert in Grad
		98	int GierGyroFehler = 0;
1211	hbuss	99	char GyroFaktor,GyroFaktorGier;
		100	char IntegralFaktor,IntegralFaktorGier;
1153	hbuss	101	int DiffNick,DiffRoll;
1051	killagreg	102	int Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0;
1	ingob	103	volatile unsigned char SenderOkay = 0;
595	hbuss	104	int StickNick = 0,StickRoll = 0,StickGier = 0,StickGas = 0;
1	ingob	105	char MotorenEin = 0;
1250	MartinR	106	long HoehenWert = 0;
1247	MartinR	107
1250	MartinR	108	long HoehenWertalt = 0; //MartinR: Test
1247	MartinR	109	unsigned char iHoehe; // MartinR: Z�hler f�r HoeheD
		110	signed long HoehenWertSumme = 0; // MartinR: f�r neuen H�henregler D-Anteil
		111
1250	MartinR	112	long SollHoehe = 0;
498	hbuss	113	int LageKorrekturRoll = 0,LageKorrekturNick = 0;
1153	hbuss	114	//float Ki = FAKTOR_I;
		115	int Ki = 10300 / 33;
1247	MartinR	116
		117	int KiHH = 10300 / 33; // MartinR : f�r Ki bei HH �ber Schalter
		118
395	hbuss	119	unsigned char Looping_Nick = 0,Looping_Roll = 0;
		120	unsigned char Looping_Links = 0, Looping_Rechts = 0, Looping_Unten = 0, Looping_Oben = 0;
1	ingob	121
		122	unsigned char Parameter_Luftdruck_D = 48; // Wert : 0-250
		123	unsigned char Parameter_MaxHoehe = 251; // Wert : 0-250
		124	unsigned char Parameter_Hoehe_P = 16; // Wert : 0-32
		125	unsigned char Parameter_Hoehe_ACC_Wirkung = 58; // Wert : 0-250
		126	unsigned char Parameter_KompassWirkung = 64; // Wert : 0-250
1111	hbuss	127	unsigned char Parameter_Gyro_D = 8; // Wert : 0-250
173	holgerb	128	unsigned char Parameter_Gyro_P = 150; // Wert : 10-250
1	ingob	129	unsigned char Parameter_Gyro_I = 150; // Wert : 0-250
		130	unsigned char Parameter_Gier_P = 2; // Wert : 1-20
		131	unsigned char Parameter_I_Faktor = 10; // Wert : 1-20
		132	unsigned char Parameter_UserParam1 = 0;
		133	unsigned char Parameter_UserParam2 = 0;
		134	unsigned char Parameter_UserParam3 = 0;
		135	unsigned char Parameter_UserParam4 = 0;
499	hbuss	136	unsigned char Parameter_UserParam5 = 0;
		137	unsigned char Parameter_UserParam6 = 0;
		138	unsigned char Parameter_UserParam7 = 0;
		139	unsigned char Parameter_UserParam8 = 0;
1	ingob	140	unsigned char Parameter_ServoNickControl = 100;
1232	hbuss	141	unsigned char Parameter_ServoRollControl = 100;
173	holgerb	142	unsigned char Parameter_LoopGasLimit = 70;
1120	hbuss	143	unsigned char Parameter_AchsKopplung1 = 90;
		144	unsigned char Parameter_AchsKopplung2 = 65;
		145	unsigned char Parameter_CouplingYawCorrection = 64;
		146	//unsigned char Parameter_AchsGegenKopplung1 = 0;
499	hbuss	147	unsigned char Parameter_DynamicStability = 100;
921	hbuss	148	unsigned char Parameter_J16Bitmask; // for the J16 Output
		149	unsigned char Parameter_J16Timing; // for the J16 Output
		150	unsigned char Parameter_J17Bitmask; // for the J17 Output
		151	unsigned char Parameter_J17Timing; // for the J17 Output
		152	unsigned char Parameter_NaviGpsModeControl; // Parameters for the Naviboard
1051	killagreg	153	unsigned char Parameter_NaviGpsGain;
		154	unsigned char Parameter_NaviGpsP;
		155	unsigned char Parameter_NaviGpsI;
		156	unsigned char Parameter_NaviGpsD;
		157	unsigned char Parameter_NaviGpsACC;
993	hbuss	158	unsigned char Parameter_NaviOperatingRadius;
		159	unsigned char Parameter_NaviWindCorrection;
		160	unsigned char Parameter_NaviSpeedCompensation;
921	hbuss	161	unsigned char Parameter_ExternalControl;
1	ingob	162	struct mk_param_struct EE_Parameter;
492	hbuss	163	signed int ExternStickNick = 0,ExternStickRoll = 0,ExternStickGier = 0, ExternHoehenValue = -20;
1247	MartinR	164	//int MaxStickNick = 0,MaxStickRoll = 0;MartinR: so war es
		165
		166	int MaxStickNick = 0,MaxStickRoll = 0,stick_nick_neutral = 0,stick_roll_neutral = 0; // MartinR: stick_.._neutral hinzugef�gt
		167
871	hbuss	168	unsigned int modell_fliegt = 0;
1209	hbuss	169	volatile unsigned char MikroKopterFlags = 0;
1121	hbuss	170	long GIER_GRAD_FAKTOR = 1291;
1153	hbuss	171	signed int KopplungsteilNickRoll,KopplungsteilRollNick;
1209	hbuss	172	unsigned char RequiredMotors = 4;
		173	unsigned char Motor[MAX_MOTORS];
		174	signed int tmp_motorwert[MAX_MOTORS];
1155	hbuss	175
1166	hbuss	176	int MotorSmoothing(int neu, int alt)
		177	{
		178	int motor;
		179	if(neu > alt) motor = (1*(int)alt + neu) / 2;
1247	MartinR	180	//else motor = neu - (alt - neu)*1; // MartinR: so war es
		181	else motor = neu; // MartinR: Entsprechen Vorschlag von MartinW ge�ndert
1166	hbuss	182	//if(Poti2 < 20) return(neu);
		183	return(motor);
		184	}
		185
1232	hbuss	186	void Piep(unsigned char Anzahl, unsigned int dauer)
1	ingob	187	{
1232	hbuss	188	if(MotorenEin) return; //auf keinen Fall im Flug!
1	ingob	189	while(Anzahl--)
		190	{
1232	hbuss	191	beeptime = dauer;
		192	while(beeptime);
		193	Delay_ms(dauer * 2);
1	ingob	194	}
		195	}
		196
		197	//############################################################################
		198	// Nullwerte ermitteln
		199	void SetNeutral(void)
		200	//############################################################################
		201	{
1111	hbuss	202	unsigned char i;
		203	unsigned int gier_neutral=0, nick_neutral=0, roll_neutral=0;
1171	hbuss	204	ServoActive = 0; HEF4017R_ON;
1051	killagreg	205	NeutralAccX = 0;
1	ingob	206	NeutralAccY = 0;
		207	NeutralAccZ = 0;
1247	MartinR	208	NeutralAccZ2 = 0; // MartinR
		209
1051	killagreg	210	AdNeutralNick = 0;
		211	AdNeutralRoll = 0;
1	ingob	212	AdNeutralGier = 0;
927	hbuss	213	AdNeutralGierBias = 0;
395	hbuss	214	Parameter_AchsKopplung1 = 0;
1120	hbuss	215	Parameter_AchsKopplung2 = 0;
1036	hbuss	216	ExpandBaro = 0;
1051	killagreg	217	CalibrierMittelwert();
395	hbuss	218	Delay_ms_Mess(100);
1	ingob	219	CalibrierMittelwert();
		220	if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // H�henregelung aktiviert?
1051	killagreg	221	{
1	ingob	222	if((MessLuftdruck > 950) \|\| (MessLuftdruck < 750)) SucheLuftruckOffset();
		223	}
1166	hbuss	224	#define NEUTRAL_FILTER 32
		225	for(i=0; i<NEUTRAL_FILTER; i++)
1111	hbuss	226	{
		227	Delay_ms_Mess(10);
1216	killagreg	228	gier_neutral += AdWertGier;
1166	hbuss	229	nick_neutral += AdWertNick;
		230	roll_neutral += AdWertRoll;
1111	hbuss	231	}
1173	hbuss	232	AdNeutralNick= (nick_neutral+NEUTRAL_FILTER/2) / (NEUTRAL_FILTER / 8);
		233	AdNeutralRoll= (roll_neutral+NEUTRAL_FILTER/2) / (NEUTRAL_FILTER / 8);
		234	AdNeutralGier= (gier_neutral+NEUTRAL_FILTER/2) / (NEUTRAL_FILTER);
1111	hbuss	235	AdNeutralGierBias = AdNeutralGier;
401	hbuss	236	StartNeutralRoll = AdNeutralRoll;
		237	StartNeutralNick = AdNeutralNick;
1051	killagreg	238	if(eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK]) > 4)
513	hbuss	239	{
1171	hbuss	240	NeutralAccY = abs(Mittelwert_AccRoll) / (2*ACC_AMPLIFY);
		241	NeutralAccX = abs(Mittelwert_AccNick) / (2*ACC_AMPLIFY);
513	hbuss	242	NeutralAccZ = Aktuell_az;
1247	MartinR	243	NeutralAccZ2 = NeutralAccZ; // MartinR
513	hbuss	244	}
1051	killagreg	245	else
513	hbuss	246	{
		247	NeutralAccX = (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK]) * 256 + (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK+1]);
		248	NeutralAccY = (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL]) * 256 + (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL+1]);
		249	NeutralAccZ = (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_Z]) * 256 + (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_Z+1]);
		250	}
1051	killagreg	251
1	ingob	252	MesswertNick = 0;
		253	MesswertRoll = 0;
		254	MesswertGier = 0;
1111	hbuss	255	Delay_ms_Mess(100);
1174	hbuss	256	Mittelwert_AccNick = ACC_AMPLIFY * (long)AdWertAccNick;
		257	Mittelwert_AccRoll = ACC_AMPLIFY * (long)AdWertAccRoll;
1173	hbuss	258	IntegralNick = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccNick;
		259	IntegralRoll = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccRoll;
		260	Mess_IntegralNick2 = IntegralNick;
		261	Mess_IntegralRoll2 = IntegralRoll;
		262	Mess_Integral_Gier = 0;
1	ingob	263	StartLuftdruck = Luftdruck;
		264	HoeheD = 0;
		265	Mess_Integral_Hoch = 0;
		266	KompassStartwert = KompassValue;
		267	GPS_Neutral();
1051	killagreg	268	beeptime = 50;
882	hbuss	269	Umschlag180Nick = ((long) EE_Parameter.WinkelUmschlagNick * 2500L) + 15000L;
		270	Umschlag180Roll = ((long) EE_Parameter.WinkelUmschlagRoll * 2500L) + 15000L;
492	hbuss	271	ExternHoehenValue = 0;
693	hbuss	272	ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR;
		273	GierGyroFehler = 0;
723	hbuss	274	SendVersionToNavi = 1;
921	hbuss	275	LED_Init();
		276	MikroKopterFlags \|= FLAG_CALIBRATE;
992	hbuss	277	FromNaviCtrl_Value.Kalman_K = -1;
1173	hbuss	278	FromNaviCtrl_Value.Kalman_MaxDrift = 0;
992	hbuss	279	FromNaviCtrl_Value.Kalman_MaxFusion = 32;
1172	hbuss	280	Poti1 = PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110;
		281	Poti2 = PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110;
		282	Poti3 = PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110;
		283	Poti4 = PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110;
1232	hbuss	284	// ServoActive = 1;
1171	hbuss	285	SenderOkay = 100;
1	ingob	286	}
		287
		288	//############################################################################
395	hbuss	289	// Bearbeitet die Messwerte
1	ingob	290	void Mittelwert(void)
		291	//############################################################################
1051	killagreg	292	{
1111	hbuss	293	static signed long tmpl,tmpl2,tmpl3,tmpl4;
		294	static signed int oldNick, oldRoll, d2Roll, d2Nick;
1247	MartinR	295	static signed int oldNick2, oldRoll2 ; //MartinR : f�r ge�nderte d2* Ermittlung
1153	hbuss	296	signed long winkel_nick, winkel_roll;
1171	hbuss	297
1111	hbuss	298	MesswertGier = (signed int) AdNeutralGier - AdWertGier;
		299	// MesswertGierBias = (signed int) AdNeutralGierBias - AdWertGier;
1171	hbuss	300	MesswertNick = (signed int) AdWertNickFilter / 8;
		301	MesswertRoll = (signed int) AdWertRollFilter / 8;
1153	hbuss	302	RohMesswertNick = MesswertNick;
		303	RohMesswertRoll = MesswertRoll;
1120	hbuss	304	//DebugOut.Analog[21] = MesswertNick;
		305	//DebugOut.Analog[22] = MesswertRoll;
1121	hbuss	306	//DebugOut.Analog[22] = Mess_Integral_Gier;
1216	killagreg	307	//DebugOut.Analog[21] = MesswertNick;
		308	//DebugOut.Analog[22] = MesswertRoll;
1166	hbuss	309
395	hbuss	310	// Beschleunigungssensor ++++++++++++++++++++++++++++++++++++++++++++++++
1166	hbuss	311	Mittelwert_AccNick = ((long)Mittelwert_AccNick * 3 + ((ACC_AMPLIFY * (long)AdWertAccNick))) / 4L;
		312	Mittelwert_AccRoll = ((long)Mittelwert_AccRoll * 3 + ((ACC_AMPLIFY * (long)AdWertAccRoll))) / 4L;
		313	Mittelwert_AccHoch = ((long)Mittelwert_AccHoch * 3 + ((long)AdWertAccHoch)) / 4L;
395	hbuss	314	IntegralAccNick += ACC_AMPLIFY * AdWertAccNick;
		315	IntegralAccRoll += ACC_AMPLIFY * AdWertAccRoll;
805	hbuss	316	NaviAccNick += AdWertAccNick;
		317	NaviAccRoll += AdWertAccRoll;
		318	NaviCntAcc++;
1153	hbuss	319	IntegralAccZ += Aktuell_az - NeutralAccZ;
		320
1155	hbuss	321	//++++++++++++++++++++++++++++++++++++++++++++++++
		322	// ADC einschalten
1247	MartinR	323	//ANALOG_ON; // MartinR : so war es
		324	// ANALOG_START; // MartinR verschoben zu timer0
1155	hbuss	325	AdReady = 0;
		326	//++++++++++++++++++++++++++++++++++++++++++++++++
		327
1216	killagreg	328	if(Mess_IntegralRoll > 93000L) winkel_roll = 93000L;
		329	else if(Mess_IntegralRoll <-93000L) winkel_roll = -93000L;
1153	hbuss	330	else winkel_roll = Mess_IntegralRoll;
		331
1216	killagreg	332	if(Mess_IntegralNick > 93000L) winkel_nick = 93000L;
		333	else if(Mess_IntegralNick <-93000L) winkel_nick = -93000L;
1153	hbuss	334	else winkel_nick = Mess_IntegralNick;
		335
1120	hbuss	336	// Gier ++++++++++++++++++++++++++++++++++++++++++++++++
1153	hbuss	337	Mess_Integral_Gier += MesswertGier;
		338	ErsatzKompass += MesswertGier;
395	hbuss	339	// Kopplungsanteil +++++++++++++++++++++++++++++++++++++
1247	MartinR	340
		341	if((EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) \|\| (Parameter_UserParam1 > 140)) IntegralFaktor = 0; // MartinR: zus�tzlich
		342
		343	//if(!Looping_Nick && !Looping_Roll && (EE_Parameter.GlobalConfig & CFG_ACHSENKOPPLUNG_AKTIV)) // MartinR : so war es
		344	if(!Looping_Nick && !Looping_Roll && IntegralFaktor && (EE_Parameter.GlobalConfig & CFG_ACHSENKOPPLUNG_AKTIV)) // MartinR: zus�tzlich "&& IntegralFaktor"
395	hbuss	345	{
1153	hbuss	346	tmpl3 = (MesswertRoll * winkel_nick) / 2048L;
1120	hbuss	347	tmpl3 *= Parameter_AchsKopplung2; //65
1111	hbuss	348	tmpl3 /= 4096L;
1153	hbuss	349	tmpl4 = (MesswertNick * winkel_roll) / 2048L;
1216	killagreg	350	tmpl4 *= Parameter_AchsKopplung2; //65
1111	hbuss	351	tmpl4 /= 4096L;
1153	hbuss	352	KopplungsteilNickRoll = tmpl3;
		353	KopplungsteilRollNick = tmpl4;
1111	hbuss	354	tmpl4 -= tmpl3;
		355	ErsatzKompass += tmpl4;
1166	hbuss	356	if(!Parameter_CouplingYawCorrection) Mess_Integral_Gier -= tmpl4/2; // Gier nachhelfen
1111	hbuss	357
1153	hbuss	358	tmpl = ((MesswertGier + tmpl4) * winkel_nick) / 2048L;
1111	hbuss	359	tmpl *= Parameter_AchsKopplung1; // 90
880	hbuss	360	tmpl /= 4096L;
1153	hbuss	361	tmpl2 = ((MesswertGier + tmpl4) * winkel_roll) / 2048L;
395	hbuss	362	tmpl2 *= Parameter_AchsKopplung1;
880	hbuss	363	tmpl2 /= 4096L;
1225	hbuss	364	if(abs(MesswertGier) > 64) if(labs(tmpl) > 128 \|\| labs(tmpl2) > 128) TrichterFlug = 1;
1153	hbuss	365	//MesswertGier += (Parameter_CouplingYawCorrection * tmpl4) / 256;
395	hbuss	366	}
1166	hbuss	367	else tmpl = tmpl2 = KopplungsteilNickRoll = KopplungsteilRollNick = 0;
1111	hbuss	368
1166	hbuss	369	TrimRoll = tmpl - tmpl2 / 100L;
		370	TrimNick = -tmpl2 + tmpl / 100L;
		371
1111	hbuss	372	// Kompasswert begrenzen ++++++++++++++++++++++++++++++++++++++++++++++++
		373	if(ErsatzKompass >= (360L * GIER_GRAD_FAKTOR)) ErsatzKompass -= 360L * GIER_GRAD_FAKTOR; // 360� Umschlag
		374	if(ErsatzKompass < 0) ErsatzKompass += 360L * GIER_GRAD_FAKTOR;
395	hbuss	375	// Roll ++++++++++++++++++++++++++++++++++++++++++++++++
1166	hbuss	376	Mess_IntegralRoll2 += MesswertRoll + TrimRoll;
		377	Mess_IntegralRoll += MesswertRoll + TrimRoll - LageKorrekturRoll;
1051	killagreg	378	if(Mess_IntegralRoll > Umschlag180Roll)
395	hbuss	379	{
882	hbuss	380	Mess_IntegralRoll = -(Umschlag180Roll - 25000L);
395	hbuss	381	Mess_IntegralRoll2 = Mess_IntegralRoll;
1051	killagreg	382	}
395	hbuss	383	if(Mess_IntegralRoll <-Umschlag180Roll)
		384	{
882	hbuss	385	Mess_IntegralRoll = (Umschlag180Roll - 25000L);
395	hbuss	386	Mess_IntegralRoll2 = Mess_IntegralRoll;
1051	killagreg	387	}
395	hbuss	388	// Nick ++++++++++++++++++++++++++++++++++++++++++++++++
1166	hbuss	389	Mess_IntegralNick2 += MesswertNick + TrimNick;
		390	Mess_IntegralNick += MesswertNick + TrimNick - LageKorrekturNick;
1051	killagreg	391	if(Mess_IntegralNick > Umschlag180Nick)
882	hbuss	392	{
		393	Mess_IntegralNick = -(Umschlag180Nick - 25000L);
		394	Mess_IntegralNick2 = Mess_IntegralNick;
1051	killagreg	395	}
		396	if(Mess_IntegralNick <-Umschlag180Nick)
395	hbuss	397	{
882	hbuss	398	Mess_IntegralNick = (Umschlag180Nick - 25000L);
395	hbuss	399	Mess_IntegralNick2 = Mess_IntegralNick;
1051	killagreg	400	}
1111	hbuss	401
1	ingob	402	Integral_Gier = Mess_Integral_Gier;
		403	IntegralNick = Mess_IntegralNick;
		404	IntegralRoll = Mess_IntegralRoll;
1051	killagreg	405	IntegralNick2 = Mess_IntegralNick2;
1	ingob	406	IntegralRoll2 = Mess_IntegralRoll2;
		407
1166	hbuss	408	#define D_LIMIT 128
		409
1247	MartinR	410	//MesswertNick = HiResNick / 8;// MartinR : so war es
		411	//MesswertRoll = HiResRoll / 8;// MartinR : so war es
		412	MesswertNick = AdWertNickFilter / 8;// MartinR : anstelle HiResNick : AdWertNickFilter
		413	MesswertRoll = AdWertRollFilter / 8;// MartinR : ist im vergleich zu HiRes.. eimal mehr gemittelt
1166	hbuss	414
1247	MartinR	415
		416	// MartinR : so war es Anfang
		417	/*
1167	hbuss	418	if(AdWertNick < 15) MesswertNick = -1000; if(AdWertNick < 7) MesswertNick = -2000;
		419	if(PlatinenVersion == 10) { if(AdWertNick > 1010) MesswertNick = +1000; if(AdWertNick > 1017) MesswertNick = +2000; }
		420	else { if(AdWertNick > 2000) MesswertNick = +1000; if(AdWertNick > 2015) MesswertNick = +2000; }
		421	if(AdWertRoll < 15) MesswertRoll = -1000; if(AdWertRoll < 7) MesswertRoll = -2000;
		422	if(PlatinenVersion == 10) { if(AdWertRoll > 1010) MesswertRoll = +1000; if(AdWertRoll > 1017) MesswertRoll = +2000; }
		423	else { if(AdWertRoll > 2000) MesswertRoll = +1000; if(AdWertRoll > 2015) MesswertRoll = +2000; }
		424
1247	MartinR	425	// MartinR : FC 1.0: Sprung von 500 auf 2000 !! FC-ME: Sprung von 1000 auf 2000
		426	*/
		427	// MartinR : so war es Ende
		428
		429	// MartinR : Neu Anfang
		430	if(PlatinenVersion == 10)
		431	{
		432	if(AdWertNick > 1010) MesswertNick = +600;
		433	if(AdWertNick > 1017) MesswertNick = +800;
		434	if(AdWertNick < 15) MesswertNick = -600;
		435	if(AdWertNick < 7) MesswertNick = -800;
		436	if(AdWertRoll > 1010) MesswertRoll = +600;
		437	if(AdWertRoll > 1017) MesswertRoll = +800;
		438	if(AdWertRoll < 15) MesswertRoll = -600;
		439	if(AdWertRoll < 7) MesswertRoll = -800;
		440	}
		441	else
		442	{
		443	if(AdWertNick > 2000) MesswertNick = +1200;
		444	if(AdWertNick > 2015) MesswertNick = +1600;
		445	if(AdWertNick < 15) MesswertNick = -1200;
		446	if(AdWertNick < 7) MesswertNick = -1600;
		447	if(AdWertRoll > 2000) MesswertRoll = +1200;
		448	if(AdWertRoll > 2015) MesswertRoll = +1600;
		449	if(AdWertRoll < 15) MesswertRoll = -1200;
		450	if(AdWertRoll < 7) MesswertRoll = -1600;
		451	}
		452	// MartinR : Neu Ende
		453
1216	killagreg	454	if(Parameter_Gyro_D)
1111	hbuss	455	{
1247	MartinR	456	/* MartinR: so war es Anfang
		457
1166	hbuss	458	d2Nick = HiResNick - oldNick;
		459	oldNick = (oldNick + HiResNick)/2;
1111	hbuss	460	if(d2Nick > D_LIMIT) d2Nick = D_LIMIT;
		461	else if(d2Nick < -D_LIMIT) d2Nick = -D_LIMIT;
1166	hbuss	462	MesswertNick += (d2Nick * (signed int) Parameter_Gyro_D) / 16;
		463	d2Roll = HiResRoll - oldRoll;
		464	oldRoll = (oldRoll + HiResRoll)/2;
1111	hbuss	465	if(d2Roll > D_LIMIT) d2Roll = D_LIMIT;
		466	else if(d2Roll < -D_LIMIT) d2Roll = -D_LIMIT;
1166	hbuss	467	MesswertRoll += (d2Roll * (signed int) Parameter_Gyro_D) / 16;
		468	HiResNick += (d2Nick * (signed int) Parameter_Gyro_D);
		469	HiResRoll += (d2Roll * (signed int) Parameter_Gyro_D);
1247	MartinR	470
		471	*/ //MartinR: so war es Ende
		472
		473	// MartinR :neu Anfang
		474	d2Nick = MesswertNick - oldNick2;
		475	oldNick2 = oldNick;
		476	oldNick = MesswertNick;
		477	if(d2Nick > D_LIMIT) d2Nick = D_LIMIT;
		478	else if(d2Nick < -D_LIMIT) d2Nick = -D_LIMIT;
		479	if(d2Nick > 0) d2Nick --;
		480	if(d2Nick < 0) d2Nick ++;
		481
		482	MesswertNick += (d2Nick * (signed int) Parameter_Gyro_D) / 16;
		483
		484	d2Roll = MesswertRoll - oldRoll2;
		485	oldRoll2 = oldRoll;
		486	oldRoll = MesswertRoll;
		487	if(d2Roll > D_LIMIT) d2Roll = D_LIMIT;
		488	else if(d2Roll < -D_LIMIT) d2Roll = -D_LIMIT;
		489	if(d2Roll > 0) d2Roll --;
		490	if(d2Roll < 0) d2Roll ++;
		491
		492	MesswertRoll += (d2Roll * (signed int) Parameter_Gyro_D) / 16;
		493	// MartinR :neu Ende
		494
		495
1216	killagreg	496	}
1111	hbuss	497
1166	hbuss	498	if(RohMesswertRoll > 0) TrimRoll += ((long) abs(KopplungsteilNickRoll) * Parameter_CouplingYawCorrection) / 64L;
		499	else TrimRoll -= ((long) abs(KopplungsteilNickRoll) * Parameter_CouplingYawCorrection) / 64L;
		500	if(RohMesswertNick > 0) TrimNick += ((long) abs(KopplungsteilRollNick) * Parameter_CouplingYawCorrection) / 64L;
		501	else TrimNick -= ((long) abs(KopplungsteilRollNick) * Parameter_CouplingYawCorrection) / 64L;
1153	hbuss	502
		503	if(EE_Parameter.GlobalConfig & CFG_DREHRATEN_BEGRENZER && !Looping_Nick && !Looping_Roll)
		504	{
		505	if(RohMesswertNick > 256) MesswertNick += 1 * (RohMesswertNick - 256);
		506	else if(RohMesswertNick < -256) MesswertNick += 1 * (RohMesswertNick + 256);
		507	if(RohMesswertRoll > 256) MesswertRoll += 1 * (RohMesswertRoll - 256);
		508	else if(RohMesswertRoll < -256) MesswertRoll += 1 * (RohMesswertRoll + 256);
		509	}
		510
1	ingob	511	if(Poti1 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110 && Poti1) Poti1--;
		512	if(Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110 && Poti2) Poti2--;
		513	if(Poti3 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110 && Poti3) Poti3--;
		514	if(Poti4 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110 && Poti4) Poti4--;
		515	if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255;
		516	if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255;
		517	if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255;
		518	if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255;
		519	}
		520
		521	//############################################################################
		522	// Messwerte beim Ermitteln der Nullage
		523	void CalibrierMittelwert(void)
		524	//############################################################################
1051	killagreg	525	{
1021	hbuss	526	if(PlatinenVersion == 13) SucheGyroOffset();
1	ingob	527	// ADC auschalten, damit die Werte sich nicht w�hrend der Berechnung �ndern
		528	ANALOG_OFF;
395	hbuss	529	MesswertNick = AdWertNick;
		530	MesswertRoll = AdWertRoll;
		531	MesswertGier = AdWertGier;
		532	Mittelwert_AccNick = ACC_AMPLIFY * (long)AdWertAccNick;
		533	Mittelwert_AccRoll = ACC_AMPLIFY * (long)AdWertAccRoll;
		534	Mittelwert_AccHoch = (long)AdWertAccHoch;
		535	// ADC einschalten
1051	killagreg	536	ANALOG_ON;
1	ingob	537	if(Poti1 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110 && Poti1) Poti1--;
		538	if(Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110 && Poti2) Poti2--;
		539	if(Poti3 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110 && Poti3) Poti3--;
		540	if(Poti4 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110 && Poti4) Poti4--;
		541	if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255;
		542	if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255;
		543	if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255;
		544	if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255;
395	hbuss	545
		546	Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L;
720	ingob	547	Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagRoll * 2500L;
1	ingob	548	}
		549
		550	//############################################################################
		551	// Senden der Motorwerte per I2C-Bus
		552	void SendMotorData(void)
		553	//############################################################################
1051	killagreg	554	{
1209	hbuss	555	unsigned char i;
921	hbuss	556	if(!MotorenEin)
1	ingob	557	{
1212	hbuss	558	MikroKopterFlags &= ~(FLAG_MOTOR_RUN \| FLAG_FLY);
1216	killagreg	559	for(i=0;i<MAX_MOTORS;i++)
		560	{
		561	if(!PC_MotortestActive) MotorTest[i] = 0;
1212	hbuss	562	Motor[i] = MotorTest[i];
1216	killagreg	563	}
1212	hbuss	564	if(PC_MotortestActive) PC_MotortestActive--;
1216	killagreg	565	}
1212	hbuss	566	else MikroKopterFlags \|= FLAG_MOTOR_RUN;
1111	hbuss	567
1212	hbuss	568	DebugOut.Analog[12] = Motor[0];
		569	DebugOut.Analog[13] = Motor[1];
		570	DebugOut.Analog[14] = Motor[3];
		571	DebugOut.Analog[15] = Motor[2];
1	ingob	572
		573	//Start I2C Interrupt Mode
		574	twi_state = 0;
		575	motor = 0;
1051	killagreg	576	i2c_start();
1	ingob	577	}
		578
		579
		580
		581	//############################################################################
		582	// Tr�gt ggf. das Poti als Parameter ein
1051	killagreg	583	void ParameterZuordnung(void)
1	ingob	584	//############################################################################
		585	{
921	hbuss	586	#define CHK_POTI_MM(b,a,min,max) { if(a > 250) { if(a == 251) b = Poti1; else if(a == 252) b = Poti2; else if(a == 253) b = Poti3; else if(a == 254) b = Poti4;} else b = a; if(b <= min) b = min; else if(b >= max) b = max;}
		587	#define CHK_POTI(b,a,min,max) { if(a > 250) { if(a == 251) b = Poti1; else if(a == 252) b = Poti2; else if(a == 253) b = Poti3; else if(a == 254) b = Poti4;} else b = a; }
1	ingob	588	CHK_POTI(Parameter_MaxHoehe,EE_Parameter.MaxHoehe,0,255);
921	hbuss	589	CHK_POTI_MM(Parameter_Luftdruck_D,EE_Parameter.Luftdruck_D,0,100);
		590	CHK_POTI_MM(Parameter_Hoehe_P,EE_Parameter.Hoehe_P,0,100);
1	ingob	591	CHK_POTI(Parameter_Hoehe_ACC_Wirkung,EE_Parameter.Hoehe_ACC_Wirkung,0,255);
		592	CHK_POTI(Parameter_KompassWirkung,EE_Parameter.KompassWirkung,0,255);
921	hbuss	593	CHK_POTI_MM(Parameter_Gyro_P,EE_Parameter.Gyro_P,10,255);
1	ingob	594	CHK_POTI(Parameter_Gyro_I,EE_Parameter.Gyro_I,0,255);
1120	hbuss	595	CHK_POTI(Parameter_Gyro_D,EE_Parameter.Gyro_D,0,255);
1051	killagreg	596	CHK_POTI(Parameter_I_Faktor,EE_Parameter.I_Faktor,0,255);
		597	CHK_POTI(Parameter_UserParam1,EE_Parameter.UserParam1,0,255);
		598	CHK_POTI(Parameter_UserParam2,EE_Parameter.UserParam2,0,255);
		599	CHK_POTI(Parameter_UserParam3,EE_Parameter.UserParam3,0,255);
		600	CHK_POTI(Parameter_UserParam4,EE_Parameter.UserParam4,0,255);
		601	CHK_POTI(Parameter_UserParam5,EE_Parameter.UserParam5,0,255);
		602	CHK_POTI(Parameter_UserParam6,EE_Parameter.UserParam6,0,255);
		603	CHK_POTI(Parameter_UserParam7,EE_Parameter.UserParam7,0,255);
		604	CHK_POTI(Parameter_UserParam8,EE_Parameter.UserParam8,0,255);
1	ingob	605	CHK_POTI(Parameter_ServoNickControl,EE_Parameter.ServoNickControl,0,255);
1232	hbuss	606	CHK_POTI(Parameter_ServoRollControl,EE_Parameter.ServoRollControl,0,255);
173	holgerb	607	CHK_POTI(Parameter_LoopGasLimit,EE_Parameter.LoopGasLimit,0,255);
395	hbuss	608	CHK_POTI(Parameter_AchsKopplung1, EE_Parameter.AchsKopplung1,0,255);
1120	hbuss	609	CHK_POTI(Parameter_AchsKopplung2, EE_Parameter.AchsKopplung2,0,255);
		610	CHK_POTI(Parameter_CouplingYawCorrection,EE_Parameter.CouplingYawCorrection,0,255);
		611	// CHK_POTI(Parameter_AchsGegenKopplung1,EE_Parameter.AchsGegenKopplung1,0,255);
499	hbuss	612	CHK_POTI(Parameter_DynamicStability,EE_Parameter.DynamicStability,0,255);
921	hbuss	613	CHK_POTI_MM(Parameter_J16Timing,EE_Parameter.J16Timing,1,255);
		614	CHK_POTI_MM(Parameter_J17Timing,EE_Parameter.J17Timing,1,255);
		615	CHK_POTI(Parameter_ExternalControl,EE_Parameter.ExternalControl,0,255);
1153	hbuss	616	Ki = 10300 / (Parameter_I_Faktor + 1);
1247	MartinR	617
		618	if(Parameter_UserParam1 > 140) KiHH = 10300 / (Parameter_UserParam2 + 1); else KiHH = Ki; // MartinR : f�r HH �ber Schalter
		619	Parameter_NaviGpsModeControl = EE_Parameter.NaviGpsModeControl; //MartinR: Standard: EE_Parameter.NaviGpsModeControl wird �bertragen
		620	if(!IntegralFaktor) Parameter_NaviGpsModeControl= 0; // MartinR: wenn HH dann GPS auf free- Mode
		621	// 0 = AID; 100 = free; 200 = coming home //so war es
		622	// 0 = free; 100 = AID; 200 = coming home //neu
		623
1	ingob	624	MAX_GAS = EE_Parameter.Gas_Max;
		625	MIN_GAS = EE_Parameter.Gas_Min;
		626	}
		627
		628
819	hbuss	629
1	ingob	630	//############################################################################
		631	//
		632	void MotorRegler(void)
		633	//############################################################################
		634	{
1209	hbuss	635	int pd_ergebnis_nick,pd_ergebnis_roll,h,tmp_int;
1	ingob	636	int GierMischanteil,GasMischanteil;
		637	static long SummeNick=0,SummeRoll=0;
1247	MartinR	638
		639	static long SummeNickHH=0,SummeRollHH=0; // MartinR: F�r ACC-HH Umschaltung
		640
1	ingob	641	static long sollGier = 0,tmp_long,tmp_long2;
395	hbuss	642	static long IntegralFehlerNick = 0;
		643	static long IntegralFehlerRoll = 0;
1	ingob	644	static unsigned int RcLostTimer;
		645	static unsigned char delay_neutral = 0;
		646	static unsigned char delay_einschalten = 0,delay_ausschalten = 0;
		647	static int hoehenregler = 0;
		648	static char TimerWerteausgabe = 0;
		649	static char NeueKompassRichtungMerken = 0;
395	hbuss	650	static long ausgleichNick, ausgleichRoll;
1153	hbuss	651	int IntegralNickMalFaktor,IntegralRollMalFaktor;
1209	hbuss	652	unsigned char i;
1051	killagreg	653	Mittelwert();
		654
1	ingob	655	GRN_ON;
1051	killagreg	656	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1	ingob	657	// Gaswert ermitteln
1051	killagreg	658	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		659	GasMischanteil = StickGas;
831	hbuss	660	if(GasMischanteil < MIN_GAS + 10) GasMischanteil = MIN_GAS + 10;
1051	killagreg	661	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
693	hbuss	662	// Empfang schlecht
1051	killagreg	663	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1	ingob	664	if(SenderOkay < 100)
		665	{
1051	killagreg	666	if(!PcZugriff)
173	holgerb	667	{
1051	killagreg	668	if(BeepMuster == 0xffff)
		669	{
173	holgerb	670	beeptime = 15000;
		671	BeepMuster = 0x0c00;
1051	killagreg	672	}
173	holgerb	673	}
1051	killagreg	674	if(RcLostTimer) RcLostTimer--;
		675	else
1	ingob	676	{
		677	MotorenEin = 0;
		678	Notlandung = 0;
1051	killagreg	679	}
1	ingob	680	ROT_ON;
693	hbuss	681	if(modell_fliegt > 1000) // wahrscheinlich in der Luft --> langsam absenken
1	ingob	682	{
		683	GasMischanteil = EE_Parameter.NotGas;
		684	Notlandung = 1;
744	hbuss	685	PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] = 0;
1051	killagreg	686	PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] = 0;
1	ingob	687	PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] = 0;
		688	PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] = 0;
		689	PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] = 0;
173	holgerb	690	}
1	ingob	691	else MotorenEin = 0;
		692	}
1051	killagreg	693	else
		694	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1	ingob	695	// Emfang gut
1051	killagreg	696	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1	ingob	697	if(SenderOkay > 140)
		698	{
		699	Notlandung = 0;
		700	RcLostTimer = EE_Parameter.NotGasZeit * 50;
921	hbuss	701	if(GasMischanteil > 40 && MotorenEin)
1	ingob	702	{
		703	if(modell_fliegt < 0xffff) modell_fliegt++;
		704	}
871	hbuss	705	if((modell_fliegt < 256))
1	ingob	706	{
		707	SummeNick = 0;
		708	SummeRoll = 0;
1051	killagreg	709	if(modell_fliegt == 250)
918	hbuss	710	{
1051	killagreg	711	NeueKompassRichtungMerken = 1;
918	hbuss	712	sollGier = 0;
1051	killagreg	713	Mess_Integral_Gier = 0;
927	hbuss	714	// Mess_Integral_Gier2 = 0;
1051	killagreg	715	}
921	hbuss	716	} else MikroKopterFlags \|= FLAG_FLY;
1051	killagreg	717
595	hbuss	718	if((PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] > 80) && MotorenEin == 0)
1	ingob	719	{
1051	killagreg	720	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1	ingob	721	// auf Nullwerte kalibrieren
1051	killagreg	722	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1	ingob	723	if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75) // Neutralwerte
		724	{
		725	if(++delay_neutral > 200) // nicht sofort
		726	{
		727	GRN_OFF;
		728	MotorenEin = 0;
		729	delay_neutral = 0;
		730	modell_fliegt = 0;
		731	if(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70 \|\| abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) > 70)
		732	{
304	ingob	733	unsigned char setting=1;
1	ingob	734	if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < 70) setting = 1;
		735	if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 2;
		736	if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 3;
		737	if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] <-70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 4;
		738	if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] <-70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < 70) setting = 5;
1063	killagreg	739	SetActiveParamSetNumber(setting); // aktiven Datensatz merken
1	ingob	740	}
820	hbuss	741	// else
1051	killagreg	742	if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) < 30 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < -70)
		743	{
819	hbuss	744	WinkelOut.CalcState = 1;
		745	beeptime = 1000;
		746	}
		747	else
1	ingob	748	{
819	hbuss	749	ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE);
		750	if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // H�henregelung aktiviert?
		751	{
1	ingob	752	if((MessLuftdruck > 950) \|\| (MessLuftdruck < 750)) SucheLuftruckOffset();
1051	killagreg	753	}
819	hbuss	754	SetNeutral();
1232	hbuss	755	ServoActive = 1;
		756	DDRD \|=0x80; // enable J7 -> Servo signal
		757	Piep(GetActiveParamSetNumber(),120);
819	hbuss	758	}
1051	killagreg	759	}
1	ingob	760	}
1051	killagreg	761	else
513	hbuss	762	if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75) // ACC Neutralwerte speichern
		763	{
		764	if(++delay_neutral > 200) // nicht sofort
		765	{
		766	GRN_OFF;
		767	eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_NICK],0xff); // Werte l�schen
		768	MotorenEin = 0;
		769	delay_neutral = 0;
		770	modell_fliegt = 0;
		771	SetNeutral();
		772	eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_NICK],NeutralAccX / 256); // ACC-NeutralWerte speichern
		773	eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_NICK+1],NeutralAccX % 256); // ACC-NeutralWerte speichern
		774	eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL],NeutralAccY / 256);
		775	eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL+1],NeutralAccY % 256);
		776	eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_Z],(int)NeutralAccZ / 256);
		777	eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_Z+1],(int)NeutralAccZ % 256);
1232	hbuss	778	Piep(GetActiveParamSetNumber(),120);
1051	killagreg	779	}
513	hbuss	780	}
1	ingob	781	else delay_neutral = 0;
		782	}
1051	killagreg	783	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1	ingob	784	// Gas ist unten
1051	killagreg	785	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
595	hbuss	786	if(PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] < 35-120)
1	ingob	787	{
		788	// Starten
		789	if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75)
		790	{
1051	killagreg	791	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1	ingob	792	// Einschalten
1051	killagreg	793	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1	ingob	794	if(++delay_einschalten > 200)
		795	{
		796	delay_einschalten = 200;
		797	modell_fliegt = 1;
		798	MotorenEin = 1;
		799	sollGier = 0;
1051	killagreg	800	Mess_Integral_Gier = 0;
1	ingob	801	Mess_Integral_Gier2 = 0;
1173	hbuss	802	Mess_IntegralNick = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccNick;
		803	Mess_IntegralRoll = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccRoll;
1	ingob	804	Mess_IntegralNick2 = IntegralNick;
		805	Mess_IntegralRoll2 = IntegralRoll;
		806	SummeNick = 0;
		807	SummeRoll = 0;
1247	MartinR	808	SchwebeGas = 0; // MartinR: f�r neuen H�henregler
921	hbuss	809	MikroKopterFlags \|= FLAG_START;
1051	killagreg	810	}
		811	}
1	ingob	812	else delay_einschalten = 0;
		813	//Auf Neutralwerte setzen
1051	killagreg	814	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1	ingob	815	// Auschalten
1051	killagreg	816	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1	ingob	817	if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75)
		818	{
		819	if(++delay_ausschalten > 200) // nicht sofort
		820	{
		821	MotorenEin = 0;
		822	delay_ausschalten = 200;
		823	modell_fliegt = 0;
1051	killagreg	824	}
1	ingob	825	}
		826	else delay_ausschalten = 0;
		827	}
		828	}
		829
1051	killagreg	830	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1	ingob	831	// neue Werte von der Funke
1051	killagreg	832	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1216	killagreg	833
1051	killagreg	834	if(!NewPpmData-- \|\| Notlandung)
1	ingob	835	{
604	hbuss	836	static int stick_nick,stick_roll;
1	ingob	837	ParameterZuordnung();
1247	MartinR	838
		839	// MartinR: original:
		840	/*
1051	killagreg	841	stick_nick = (stick_nick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P) / 4;
723	hbuss	842	stick_nick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D;
805	hbuss	843	StickNick = stick_nick - (GPS_Nick + GPS_Nick2);
604	hbuss	844
723	hbuss	845	stick_roll = (stick_roll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P) / 4;
		846	stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D;
805	hbuss	847	StickRoll = stick_roll - (GPS_Roll + GPS_Roll2);
1247	MartinR	848
		849	*/
723	hbuss	850
1247	MartinR	851	// MartinR: ge�ndert Anfang
		852	if(Parameter_UserParam1 > 140) // MartinR: zweiter Stick_P Wert nur, wenn HH �ber Schalter aktiv ist
		853	{
		854	stick_nick = (stick_nick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * Parameter_UserParam3 - stick_nick_neutral) / 4;
		855	stick_roll = (stick_roll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * Parameter_UserParam3 - stick_roll_neutral) / 4 ;
		856	}
		857
		858	else
		859
		860	stick_nick = (stick_nick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P) / 4;
		861	stick_roll = (stick_roll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P) / 4;
		862
		863
		864	if(IntegralFaktor)
		865	{
		866	stick_nick_neutral = stick_nick; // beim Umschalten auf HH wird derletzte Stickwert als Neutralposition verwendet, MartinR
		867	stick_roll_neutral = stick_roll; // beim Umschalten auf HH wird derletzte Stickwert als Neutralposition verwendet, MartinR
		868	stick_nick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D;
		869	stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D;
		870
		871	StickNick = stick_nick - (GPS_Nick + GPS_Nick2); // MartinR: GPS nur im ACC-Mode wirksam
		872	StickRoll = stick_roll - (GPS_Roll + GPS_Roll2); // MartinR: GPS nur im ACC-Mode wirksam
		873	}
		874	else // wenn HH , MartinR
		875	{
		876	stick_nick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D; // MartinR: eventuell vor if verschieben
		877	stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D; // MartinR: eventuell vor if verschieben
		878	StickNick = stick_nick; // MartinR: GPS nur im ACC-Mode wirksam
		879	StickRoll = stick_roll; // MartinR: GPS nur im ACC-Mode wirksam
		880	}
		881
		882	// MartinR: ge�ndert Ende
		883
		884
1	ingob	885	StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]];
1120	hbuss	886	if(StickGier > 2) StickGier -= 2; else
		887	if(StickGier < -2) StickGier += 2; else StickGier = 0;
		888
595	hbuss	889	StickGas = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120;
1153	hbuss	890	GyroFaktor = (Parameter_Gyro_P + 10.0);
		891	IntegralFaktor = Parameter_Gyro_I;
1211	hbuss	892	GyroFaktorGier = (Parameter_Gyro_P + 10.0);
		893	IntegralFaktorGier = Parameter_Gyro_I;
1	ingob	894
595	hbuss	895	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		896	//+ Analoge Steuerung per Seriell
		897	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1247	MartinR	898	// MartinR: ToDo: eventuell die Kombination HH und Steuerung per Seriell nicht zulassen??
		899
921	hbuss	900	if(ExternControl.Config & 0x01 && Parameter_ExternalControl > 128)
595	hbuss	901	{
		902	StickNick += (int) ExternControl.Nick * (int) EE_Parameter.Stick_P;
		903	StickRoll += (int) ExternControl.Roll * (int) EE_Parameter.Stick_P;
		904	StickGier += ExternControl.Gier;
		905	ExternHoehenValue = (int) ExternControl.Hight * (int)EE_Parameter.Hoehe_Verstaerkung;
		906	if(ExternControl.Gas < StickGas) StickGas = ExternControl.Gas;
		907	}
855	hbuss	908	if(StickGas < 0) StickGas = 0;
1247	MartinR	909
		910	//if(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor = 0; // MartinR: Original
		911	if((EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) \|\| (Parameter_UserParam1 > 140)) IntegralFaktor = 0; // MartinR
492	hbuss	912
1	ingob	913	if(GyroFaktor < 0) GyroFaktor = 0;
		914	if(IntegralFaktor < 0) IntegralFaktor = 0;
723	hbuss	915
1051	killagreg	916	if(abs(StickNick/STICK_GAIN) > MaxStickNick)
928	hbuss	917	{
1051	killagreg	918	MaxStickNick = abs(StickNick)/STICK_GAIN;
928	hbuss	919	if(MaxStickNick > 100) MaxStickNick = 100;
1051	killagreg	920	}
928	hbuss	921	else MaxStickNick--;
1051	killagreg	922	if(abs(StickRoll/STICK_GAIN) > MaxStickRoll)
928	hbuss	923	{
1051	killagreg	924	MaxStickRoll = abs(StickRoll)/STICK_GAIN;
928	hbuss	925	if(MaxStickRoll > 100) MaxStickRoll = 100;
1051	killagreg	926	}
928	hbuss	927	else MaxStickRoll--;
723	hbuss	928	if(Notlandung) {MaxStickNick = 0; MaxStickRoll = 0;}
		929
1051	killagreg	930	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
173	holgerb	931	// Looping?
1051	killagreg	932	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
993	hbuss	933	if((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > EE_Parameter.LoopThreshold) && EE_Parameter.BitConfig & CFG_LOOP_LINKS) Looping_Links = 1;
1051	killagreg	934	else
		935	{
395	hbuss	936	{
1051	killagreg	937	if((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < (EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese))) Looping_Links = 0;
		938	}
		939	}
993	hbuss	940	if((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < -EE_Parameter.LoopThreshold) && EE_Parameter.BitConfig & CFG_LOOP_RECHTS) Looping_Rechts = 1;
1051	killagreg	941	else
395	hbuss	942	{
		943	if(Looping_Rechts) // Hysterese
		944	{
		945	if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > -(EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese)) Looping_Rechts = 0;
		946	}
1051	killagreg	947	}
173	holgerb	948
993	hbuss	949	if((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > EE_Parameter.LoopThreshold) && EE_Parameter.BitConfig & CFG_LOOP_OBEN) Looping_Oben = 1;
1051	killagreg	950	else
		951	{
395	hbuss	952	if(Looping_Oben) // Hysterese
		953	{
1051	killagreg	954	if((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < (EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese))) Looping_Oben = 0;
		955	}
		956	}
993	hbuss	957	if((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < -EE_Parameter.LoopThreshold) && EE_Parameter.BitConfig & CFG_LOOP_UNTEN) Looping_Unten = 1;
1051	killagreg	958	else
395	hbuss	959	{
		960	if(Looping_Unten) // Hysterese
		961	{
		962	if(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > -(EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese)) Looping_Unten = 0;
		963	}
1051	killagreg	964	}
395	hbuss	965
		966	if(Looping_Links \|\| Looping_Rechts) Looping_Roll = 1; else Looping_Roll = 0;
1153	hbuss	967	if(Looping_Oben \|\| Looping_Unten) { Looping_Nick = 1; Looping_Roll = 0; Looping_Links = 0; Looping_Rechts = 0;} else Looping_Nick = 0;
395	hbuss	968	} // Ende neue Funken-Werte
		969
		970	if(Looping_Roll \|\| Looping_Nick)
		971	{
173	holgerb	972	if(GasMischanteil > EE_Parameter.LoopGasLimit) GasMischanteil = EE_Parameter.LoopGasLimit;
1153	hbuss	973	TrichterFlug = 1;
173	holgerb	974	}
		975
1051	killagreg	976
		977	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		978	// Bei Empfangsausfall im Flug
		979	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1	ingob	980	if(Notlandung)
		981	{
		982	StickGier = 0;
		983	StickNick = 0;
		984	StickRoll = 0;
1211	hbuss	985	GyroFaktor = 90;
		986	IntegralFaktor = 120;
		987	GyroFaktorGier = 90;
		988	IntegralFaktorGier = 120;
173	holgerb	989	Looping_Roll = 0;
		990	Looping_Nick = 0;
1051	killagreg	991	}
395	hbuss	992
		993
1051	killagreg	994	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1	ingob	995	// Integrale auf ACC-Signal abgleichen
1051	killagreg	996	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
395	hbuss	997	#define ABGLEICH_ANZAHL 256L
		998
		999	MittelIntegralNick += IntegralNick; // F�r die Mittelwertbildung aufsummieren
		1000	MittelIntegralRoll += IntegralRoll;
		1001	MittelIntegralNick2 += IntegralNick2;
		1002	MittelIntegralRoll2 += IntegralRoll2;
		1003
1247	MartinR	1004	//if(Looping_Nick \|\| Looping_Roll) // MartinR: so war es
		1005	if(Looping_Nick \|\| Looping_Roll \|\| !IntegralFaktor) // MartinR: "\|\| !IntegralFaktor" hinzugef�gt
395	hbuss	1006	{
		1007	IntegralAccNick = 0;
		1008	IntegralAccRoll = 0;
		1009	MittelIntegralNick = 0;
		1010	MittelIntegralRoll = 0;
		1011	MittelIntegralNick2 = 0;
		1012	MittelIntegralRoll2 = 0;
1247	MartinR	1013
		1014	IntegralNick = 0; // MartinR: im HH-Modus alle unbenutzten Integratoren = 0
		1015	IntegralRoll = 0; // MartinR: im HH-Modus alle unbenutzten Integratoren = 0
		1016	Mess_IntegralNick = 0; // MartinR: im HH-Modus alle unbenutzten Integratoren = 0
		1017	Mess_IntegralRoll = 0; // MartinR: im HH-Modus alle unbenutzten Integratoren = 0
		1018	Mess_Integral_Gier = 0; // MartinR: im HH-Modus alle unbenutzten Integratoren = 0
		1019	Mess_Integral_Gier2 = 0; // MartinR: im HH-Modus alle unbenutzten Integratoren = 0
		1020
395	hbuss	1021	Mess_IntegralNick2 = Mess_IntegralNick;
		1022	Mess_IntegralRoll2 = Mess_IntegralRoll;
		1023	ZaehlMessungen = 0;
498	hbuss	1024	LageKorrekturNick = 0;
		1025	LageKorrekturRoll = 0;
395	hbuss	1026	}
		1027
1051	killagreg	1028	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1247	MartinR	1029	if(!Looping_Nick && !Looping_Roll && (Aktuell_az > 512 \|\| MotorenEin) && IntegralFaktor) // MartinR: "&& IntegralFaktor" hinzugef�gt
469	hbuss	1030	{
		1031	long tmp_long, tmp_long2;
1171	hbuss	1032	if(FromNaviCtrl_Value.Kalman_K != -1 /&& !TrichterFlug/)
992	hbuss	1033	{
		1034	tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccNick);
1051	killagreg	1035	tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccRoll);
		1036	tmp_long = (tmp_long * FromNaviCtrl_Value.Kalman_K) / (32 * 16);
		1037	tmp_long2 = (tmp_long2 * FromNaviCtrl_Value.Kalman_K) / (32 * 16);
992	hbuss	1038	if((MaxStickNick > 64) \|\| (MaxStickRoll > 64))
		1039	{
		1040	tmp_long /= 2;
		1041	tmp_long2 /= 2;
		1042	}
		1043	if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25)
		1044	{
		1045	tmp_long /= 3;
		1046	tmp_long2 /= 3;
		1047	}
		1048	if(tmp_long > (long) FromNaviCtrl_Value.Kalman_MaxFusion) tmp_long = (long) FromNaviCtrl_Value.Kalman_MaxFusion;
		1049	if(tmp_long < (long)-FromNaviCtrl_Value.Kalman_MaxFusion) tmp_long = (long)-FromNaviCtrl_Value.Kalman_MaxFusion;
		1050	if(tmp_long2 > (long) FromNaviCtrl_Value.Kalman_MaxFusion) tmp_long2 = (long) FromNaviCtrl_Value.Kalman_MaxFusion;
		1051	if(tmp_long2 < (long)-FromNaviCtrl_Value.Kalman_MaxFusion) tmp_long2 = (long)-FromNaviCtrl_Value.Kalman_MaxFusion;
		1052	}
1051	killagreg	1053	else
992	hbuss	1054	{
		1055	tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccNick);
1051	killagreg	1056	tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccRoll);
992	hbuss	1057	tmp_long /= 16;
		1058	tmp_long2 /= 16;
		1059	if((MaxStickNick > 64) \|\| (MaxStickRoll > 64))
		1060	{
		1061	tmp_long /= 3;
		1062	tmp_long2 /= 3;
1216	killagreg	1063	}
		1064	if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25)
992	hbuss	1065	{
		1066	tmp_long /= 3;
		1067	tmp_long2 /= 3;
		1068	}
1155	hbuss	1069
		1070	#define AUSGLEICH 32
992	hbuss	1071	if(tmp_long > AUSGLEICH) tmp_long = AUSGLEICH;
		1072	if(tmp_long < -AUSGLEICH) tmp_long =-AUSGLEICH;
		1073	if(tmp_long2 > AUSGLEICH) tmp_long2 = AUSGLEICH;
		1074	if(tmp_long2 <-AUSGLEICH) tmp_long2 =-AUSGLEICH;
		1075	}
1166	hbuss	1076
		1077	//if(Poti2 > 20) { tmp_long = 0; tmp_long2 = 0;}
1111	hbuss	1078	Mess_IntegralNick -= tmp_long;
		1079	Mess_IntegralRoll -= tmp_long2;
469	hbuss	1080	}
1051	killagreg	1081	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
395	hbuss	1082	if(ZaehlMessungen >= ABGLEICH_ANZAHL)
		1083	{
		1084	static int cnt = 0;
		1085	static char last_n_p,last_n_n,last_r_p,last_r_n;
		1086	static long MittelIntegralNick_Alt,MittelIntegralRoll_Alt;
1247	MartinR	1087
		1088	if(!Looping_Nick && !Looping_Roll && !TrichterFlug && EE_Parameter.Driftkomp && IntegralFaktor) // MartinR: "&& IntegralFaktor" hinzugef�gt
		1089
173	holgerb	1090	{
395	hbuss	1091	MittelIntegralNick /= ABGLEICH_ANZAHL;
		1092	MittelIntegralRoll /= ABGLEICH_ANZAHL;
		1093	IntegralAccNick = (EE_Parameter.GyroAccFaktor * IntegralAccNick) / ABGLEICH_ANZAHL;
		1094	IntegralAccRoll = (EE_Parameter.GyroAccFaktor * IntegralAccRoll) / ABGLEICH_ANZAHL;
		1095	IntegralAccZ = IntegralAccZ / ABGLEICH_ANZAHL;
492	hbuss	1096	#define MAX_I 0//(Poti2/10)
395	hbuss	1097	// Nick ++++++++++++++++++++++++++++++++++++++++++++++++
		1098	IntegralFehlerNick = (long)(MittelIntegralNick - (long)IntegralAccNick);
		1099	ausgleichNick = IntegralFehlerNick / EE_Parameter.GyroAccAbgleich;
1051	killagreg	1100	// Roll ++++++++++++++++++++++++++++++++++++++++++++++++
		1101	IntegralFehlerRoll = (long)(MittelIntegralRoll - (long)IntegralAccRoll);
395	hbuss	1102	ausgleichRoll = IntegralFehlerRoll / EE_Parameter.GyroAccAbgleich;
614	hbuss	1103
		1104	LageKorrekturNick = ausgleichNick / ABGLEICH_ANZAHL;
498	hbuss	1105	LageKorrekturRoll = ausgleichRoll / ABGLEICH_ANZAHL;
395	hbuss	1106
992	hbuss	1107	if(((MaxStickNick > 64) \|\| (MaxStickRoll > 64) \|\| (abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25)) && (FromNaviCtrl_Value.Kalman_K == -1))
614	hbuss	1108	{
		1109	LageKorrekturNick /= 2;
720	ingob	1110	LageKorrekturRoll /= 2;
614	hbuss	1111	}
498	hbuss	1112
1051	killagreg	1113	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
395	hbuss	1114	// Gyro-Drift ermitteln
1051	killagreg	1115	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
395	hbuss	1116	MittelIntegralNick2 /= ABGLEICH_ANZAHL;
		1117	MittelIntegralRoll2 /= ABGLEICH_ANZAHL;
1051	killagreg	1118	tmp_long = IntegralNick2 - IntegralNick;
		1119	tmp_long2 = IntegralRoll2 - IntegralRoll;
395	hbuss	1120	//DebugOut.Analog[25] = MittelIntegralRoll2 / 26;
		1121
		1122	IntegralFehlerNick = tmp_long;
		1123	IntegralFehlerRoll = tmp_long2;
		1124	Mess_IntegralNick2 -= IntegralFehlerNick;
		1125	Mess_IntegralRoll2 -= IntegralFehlerRoll;
		1126
		1127	// IntegralFehlerNick = (IntegralFehlerNick * 1 + tmp_long) / 2;
		1128	// IntegralFehlerRoll = (IntegralFehlerRoll * 1 + tmp_long2) / 2;
1111	hbuss	1129	if(EE_Parameter.Driftkomp)
		1130	{
927	hbuss	1131	if(GierGyroFehler > ABGLEICH_ANZAHL/2) { AdNeutralGier++; AdNeutralGierBias++; }
		1132	if(GierGyroFehler <-ABGLEICH_ANZAHL/2) { AdNeutralGier--; AdNeutralGierBias--; }
1111	hbuss	1133	}
1153	hbuss	1134	//DebugOut.Analog[22] = MittelIntegralRoll / 26;
921	hbuss	1135	//DebugOut.Analog[24] = GierGyroFehler;
693	hbuss	1136	GierGyroFehler = 0;
720	ingob	1137
		1138
		1139	/*DebugOut.Analog[17] = IntegralAccNick / 26;
395	hbuss	1140	DebugOut.Analog[18] = IntegralAccRoll / 26;
		1141	DebugOut.Analog[19] = IntegralFehlerNick;// / 26;
		1142	DebugOut.Analog[20] = IntegralFehlerRoll;// / 26;
720	ingob	1143	*/
		1144	//DebugOut.Analog[21] = MittelIntegralNick / 26;
		1145	//MittelIntegralRoll = MittelIntegralRoll;
469	hbuss	1146	//DebugOut.Analog[28] = ausgleichNick;
720	ingob	1147	/*
395	hbuss	1148	DebugOut.Analog[29] = ausgleichRoll;
498	hbuss	1149	DebugOut.Analog[30] = LageKorrekturRoll * 10;
720	ingob	1150	*/
395	hbuss	1151
1225	hbuss	1152	#define FEHLER_LIMIT (ABGLEICH_ANZAHL / 2)
		1153	#define FEHLER_LIMIT1 (ABGLEICH_ANZAHL * 2) //4
		1154	#define FEHLER_LIMIT2 (ABGLEICH_ANZAHL * 16) //16
492	hbuss	1155	#define BEWEGUNGS_LIMIT 20000
395	hbuss	1156	// Nick +++++++++++++++++++++++++++++++++++++++++++++++++
		1157	cnt = 1;// + labs(IntegralFehlerNick) / 4096;
1225	hbuss	1158	if(labs(IntegralFehlerNick) > FEHLER_LIMIT1) cnt = 4;
1173	hbuss	1159	if(labs(MittelIntegralNick_Alt - MittelIntegralNick) < BEWEGUNGS_LIMIT \|\| (FromNaviCtrl_Value.Kalman_MaxDrift > 3*8))
492	hbuss	1160	{
1051	killagreg	1161	if(IntegralFehlerNick > FEHLER_LIMIT2)
395	hbuss	1162	{
1051	killagreg	1163	if(last_n_p)
395	hbuss	1164	{
1173	hbuss	1165	cnt += labs(IntegralFehlerNick) / (FEHLER_LIMIT2 / 8);
492	hbuss	1166	ausgleichNick = IntegralFehlerNick / 8;
401	hbuss	1167	if(ausgleichNick > 5000) ausgleichNick = 5000;
498	hbuss	1168	LageKorrekturNick += ausgleichNick / ABGLEICH_ANZAHL;
1051	killagreg	1169	}
395	hbuss	1170	else last_n_p = 1;
		1171	} else last_n_p = 0;
1051	killagreg	1172	if(IntegralFehlerNick < -FEHLER_LIMIT2)
395	hbuss	1173	{
		1174	if(last_n_n)
1051	killagreg	1175	{
1173	hbuss	1176	cnt += labs(IntegralFehlerNick) / (FEHLER_LIMIT2 / 8);
492	hbuss	1177	ausgleichNick = IntegralFehlerNick / 8;
401	hbuss	1178	if(ausgleichNick < -5000) ausgleichNick = -5000;
498	hbuss	1179	LageKorrekturNick += ausgleichNick / ABGLEICH_ANZAHL;
1051	killagreg	1180	}
395	hbuss	1181	else last_n_n = 1;
		1182	} else last_n_n = 0;
1051	killagreg	1183	}
		1184	else
847	hbuss	1185	{
		1186	cnt = 0;
921	hbuss	1187	KompassSignalSchlecht = 1000;
1051	killagreg	1188	}
499	hbuss	1189	if(cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp;
1173	hbuss	1190	if(FromNaviCtrl_Value.Kalman_MaxDrift) if(cnt > FromNaviCtrl_Value.Kalman_MaxDrift) cnt = FromNaviCtrl_Value.Kalman_MaxDrift;
395	hbuss	1191	if(IntegralFehlerNick > FEHLER_LIMIT) AdNeutralNick += cnt;
		1192	if(IntegralFehlerNick < -FEHLER_LIMIT) AdNeutralNick -= cnt;
401	hbuss	1193
395	hbuss	1194	// Roll +++++++++++++++++++++++++++++++++++++++++++++++++
492	hbuss	1195	cnt = 1;// + labs(IntegralFehlerNick) / 4096;
1225	hbuss	1196	if(labs(IntegralFehlerRoll) > FEHLER_LIMIT1) cnt = 4;
498	hbuss	1197	ausgleichRoll = 0;
1173	hbuss	1198	if(labs(MittelIntegralRoll_Alt - MittelIntegralRoll) < BEWEGUNGS_LIMIT \|\| (FromNaviCtrl_Value.Kalman_MaxDrift > 3*8))
492	hbuss	1199	{
1051	killagreg	1200	if(IntegralFehlerRoll > FEHLER_LIMIT2)
395	hbuss	1201	{
1051	killagreg	1202	if(last_r_p)
395	hbuss	1203	{
1173	hbuss	1204	cnt += labs(IntegralFehlerRoll) / (FEHLER_LIMIT2 / 8);
492	hbuss	1205	ausgleichRoll = IntegralFehlerRoll / 8;
401	hbuss	1206	if(ausgleichRoll > 5000) ausgleichRoll = 5000;
498	hbuss	1207	LageKorrekturRoll += ausgleichRoll / ABGLEICH_ANZAHL;
1051	killagreg	1208	}
395	hbuss	1209	else last_r_p = 1;
		1210	} else last_r_p = 0;
1051	killagreg	1211	if(IntegralFehlerRoll < -FEHLER_LIMIT2)
395	hbuss	1212	{
1051	killagreg	1213	if(last_r_n)
395	hbuss	1214	{
1173	hbuss	1215	cnt += labs(IntegralFehlerRoll) / (FEHLER_LIMIT2 / 8);
492	hbuss	1216	ausgleichRoll = IntegralFehlerRoll / 8;
401	hbuss	1217	if(ausgleichRoll < -5000) ausgleichRoll = -5000;
498	hbuss	1218	LageKorrekturRoll += ausgleichRoll / ABGLEICH_ANZAHL;
395	hbuss	1219	}
		1220	else last_r_n = 1;
		1221	} else last_r_n = 0;
1051	killagreg	1222	} else
492	hbuss	1223	{
		1224	cnt = 0;
921	hbuss	1225	KompassSignalSchlecht = 1000;
1051	killagreg	1226	}
499	hbuss	1227	if(cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp;
1173	hbuss	1228	if(FromNaviCtrl_Value.Kalman_MaxDrift) if(cnt > FromNaviCtrl_Value.Kalman_MaxDrift) cnt = FromNaviCtrl_Value.Kalman_MaxDrift;
395	hbuss	1229	if(IntegralFehlerRoll > FEHLER_LIMIT) AdNeutralRoll += cnt;
		1230	if(IntegralFehlerRoll < -FEHLER_LIMIT) AdNeutralRoll -= cnt;
173	holgerb	1231	}
1051	killagreg	1232	else
498	hbuss	1233	{
		1234	LageKorrekturRoll = 0;
		1235	LageKorrekturNick = 0;
880	hbuss	1236	TrichterFlug = 0;
498	hbuss	1237	}
1051	killagreg	1238
498	hbuss	1239	if(!IntegralFaktor) { LageKorrekturRoll = 0; LageKorrekturNick = 0;} // z.B. bei HH
1051	killagreg	1240	// +++++++++++++++++++++++++++++++++++++++++++++++++++++
		1241	MittelIntegralNick_Alt = MittelIntegralNick;
		1242	MittelIntegralRoll_Alt = MittelIntegralRoll;
		1243	// +++++++++++++++++++++++++++++++++++++++++++++++++++++
395	hbuss	1244	IntegralAccNick = 0;
		1245	IntegralAccRoll = 0;
		1246	IntegralAccZ = 0;
		1247	MittelIntegralNick = 0;
		1248	MittelIntegralRoll = 0;
		1249	MittelIntegralNick2 = 0;
		1250	MittelIntegralRoll2 = 0;
		1251	ZaehlMessungen = 0;
1173	hbuss	1252	} // ZaehlMessungen >= ABGLEICH_ANZAHL
395	hbuss	1253
1173	hbuss	1254
1051	killagreg	1255	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1	ingob	1256	// Gieren
1051	killagreg	1257	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
693	hbuss	1258	// if(GasMischanteil < 35) { if(StickGier > 10) StickGier = 10; else if(StickGier < -10) StickGier = -10;};
1051	killagreg	1259	if(abs(StickGier) > 15) // war 35
1	ingob	1260	{
921	hbuss	1261	KompassSignalSchlecht = 1000;
1051	killagreg	1262	if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX))
		1263	{
		1264	NeueKompassRichtungMerken = 1;
824	hbuss	1265	};
1	ingob	1266	}
395	hbuss	1267	tmp_int = (long) EE_Parameter.Gier_P * ((long)StickGier * abs(StickGier)) / 512L; // expo y = ax + bx�
1051	killagreg	1268	tmp_int += (EE_Parameter.Gier_P * StickGier) / 4;
173	holgerb	1269	sollGier = tmp_int;
1051	killagreg	1270	Mess_Integral_Gier -= tmp_int;
395	hbuss	1271	if(Mess_Integral_Gier > 50000) Mess_Integral_Gier = 50000; // begrenzen
		1272	if(Mess_Integral_Gier <-50000) Mess_Integral_Gier =-50000;
1051	killagreg	1273
		1274	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1	ingob	1275	// Kompass
1051	killagreg	1276	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1175	hbuss	1277	//DebugOut.Analog[16] = KompassSignalSchlecht;
819	hbuss	1278
1051	killagreg	1279	if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV))
1	ingob	1280	{
819	hbuss	1281	int w,v,r,fehler,korrektur;
1	ingob	1282	w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln
		1283	v = abs(IntegralRoll /512);
		1284	if(v > w) w = v; // gr�sste Neigung ermitteln
824	hbuss	1285	korrektur = w / 8 + 1;
921	hbuss	1286	fehler = ((540 + KompassValue - (ErsatzKompass/GIER_GRAD_FAKTOR)) % 360) - 180;
1171	hbuss	1287	if(abs(MesswertGier) > 128)
1036	hbuss	1288	{
		1289	fehler = 0;
1051	killagreg	1290	}
921	hbuss	1291	if(!KompassSignalSchlecht && w < 25)
		1292	{
		1293	GierGyroFehler += fehler;
1051	killagreg	1294	if(NeueKompassRichtungMerken)
		1295	{
921	hbuss	1296	beeptime = 200;
847	hbuss	1297	// KompassStartwert = KompassValue;
1171	hbuss	1298	ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR;
921	hbuss	1299	KompassStartwert = (ErsatzKompass/GIER_GRAD_FAKTOR);
		1300	NeueKompassRichtungMerken = 0;
		1301	}
1	ingob	1302	}
824	hbuss	1303	ErsatzKompass += (fehler * 8) / korrektur;
921	hbuss	1304	w = (w * Parameter_KompassWirkung) / 32; // auf die Wirkung normieren
1	ingob	1305	w = Parameter_KompassWirkung - w; // Wirkung ggf drosseln
921	hbuss	1306	if(w >= 0)
1	ingob	1307	{
1051	killagreg	1308	if(!KompassSignalSchlecht)
693	hbuss	1309	{
1051	killagreg	1310	v = 64 + ((MaxStickNick + MaxStickRoll)) / 8;
847	hbuss	1311	r = ((540 + (ErsatzKompass/GIER_GRAD_FAKTOR) - KompassStartwert) % 360) - 180;
		1312	// r = KompassRichtung;
819	hbuss	1313	v = (r * w) / v; // nach Kompass ausrichten
		1314	w = 3 * Parameter_KompassWirkung;
693	hbuss	1315	if(v > w) v = w; // Begrenzen
1051	killagreg	1316	else
693	hbuss	1317	if(v < -w) v = -w;
		1318	Mess_Integral_Gier += v;
1051	killagreg	1319	}
693	hbuss	1320	if(KompassSignalSchlecht) KompassSignalSchlecht--;
1051	killagreg	1321	}
921	hbuss	1322	else KompassSignalSchlecht = 500; // so lange das Signal taub stellen --> ca. 1 sek
1051	killagreg	1323	}
		1324	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1	ingob	1325
1051	killagreg	1326	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1	ingob	1327	// Debugwerte zuordnen
1051	killagreg	1328	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1	ingob	1329	if(!TimerWerteausgabe--)
		1330	{
395	hbuss	1331	TimerWerteausgabe = 24;
805	hbuss	1332
1171	hbuss	1333	DebugOut.Analog[0] = IntegralNick / (EE_Parameter.GyroAccFaktor * 4);
		1334	DebugOut.Analog[1] = IntegralRoll / (EE_Parameter.GyroAccFaktor * 4);
		1335	DebugOut.Analog[2] = Mittelwert_AccNick / 4;
		1336	DebugOut.Analog[3] = Mittelwert_AccRoll / 4;
1	ingob	1337	DebugOut.Analog[4] = MesswertGier;
		1338	DebugOut.Analog[5] = HoehenWert;
1224	hbuss	1339	DebugOut.Analog[6] = Aktuell_az;//(Mess_Integral_Hoch / 512);//Aktuell_az;
1	ingob	1340	DebugOut.Analog[8] = KompassValue;
297	holgerb	1341	DebugOut.Analog[9] = UBat;
1051	killagreg	1342	DebugOut.Analog[11] = ErsatzKompass / GIER_GRAD_FAKTOR;
297	holgerb	1343	DebugOut.Analog[10] = SenderOkay;
929	hbuss	1344	//DebugOut.Analog[16] = Mittelwert_AccHoch;
1083	killagreg	1345	//DebugOut.Analog[17] = FromNaviCtrl_Value.Distance;
		1346	//DebugOut.Analog[18] = (int)FromNaviCtrl_Value.OsdBar;
819	hbuss	1347	DebugOut.Analog[19] = WinkelOut.CalcState;
1247	MartinR	1348	// DebugOut.Analog[20] = ServoValue; // MartinR: DebugOut.Analog[20] wird in timer0.c ausgegeben!!
1171	hbuss	1349	// DebugOut.Analog[24] = MesswertNick/2;
		1350	// DebugOut.Analog[25] = MesswertRoll/2;
1173	hbuss	1351	DebugOut.Analog[27] = (int)FromNaviCtrl_Value.Kalman_MaxDrift;
1172	hbuss	1352	// DebugOut.Analog[28] = (int)FromNaviCtrl_Value.Kalman_MaxFusion;
		1353	// DebugOut.Analog[29] = (int)FromNaviCtrl_Value.Kalman_K;
1215	hbuss	1354	DebugOut.Analog[29] = FromNaviCtrl_Value.SerialDataOkay;
1216	killagreg	1355	DebugOut.Analog[30] = GPS_Nick;
720	ingob	1356	DebugOut.Analog[31] = GPS_Roll;
		1357
805	hbuss	1358
		1359	// DebugOut.Analog[19] -= DebugOut.Analog[19]/128;
		1360	// if(DebugOut.Analog[19] > 0) DebugOut.Analog[19]--; else DebugOut.Analog[19]++;
		1361
173	holgerb	1362	/* DebugOut.Analog[16] = motor_rx[0];
		1363	DebugOut.Analog[17] = motor_rx[1];
		1364	DebugOut.Analog[18] = motor_rx[2];
		1365	DebugOut.Analog[19] = motor_rx[3];
		1366	DebugOut.Analog[20] = motor_rx[0] + motor_rx[1] + motor_rx[2] + motor_rx[3];
		1367	DebugOut.Analog[20] /= 14;
		1368	DebugOut.Analog[21] = motor_rx[4];
		1369	DebugOut.Analog[22] = motor_rx[5];
		1370	DebugOut.Analog[23] = motor_rx[6];
		1371	DebugOut.Analog[24] = motor_rx[7];
		1372	DebugOut.Analog[25] = motor_rx[4] + motor_rx[5] + motor_rx[6] + motor_rx[7];
		1373	*/
		1374	// DebugOut.Analog[9] = MesswertNick;
1	ingob	1375	// DebugOut.Analog[9] = SollHoehe;
		1376	// DebugOut.Analog[10] = Mess_Integral_Gier / 128;
		1377	// DebugOut.Analog[11] = KompassStartwert;
1051	killagreg	1378	// DebugOut.Analog[10] = Parameter_Gyro_I;
		1379	// DebugOut.Analog[10] = EE_Parameter.Gyro_I;
		1380	// DebugOut.Analog[9] = KompassRichtung;
1	ingob	1381	// DebugOut.Analog[10] = GasMischanteil;
		1382	// DebugOut.Analog[3] = HoeheD * 32;
		1383	// DebugOut.Analog[4] = hoehenregler;
		1384	}
		1385
1051	killagreg	1386	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1	ingob	1387	// Drehgeschwindigkeit und -winkel zu einem Istwert zusammenfassen
1051	killagreg	1388	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1216	killagreg	1389	if(TrichterFlug) { SummeRoll = 0; SummeNick = 0;};
604	hbuss	1390
1171	hbuss	1391	if(!Looping_Nick) IntegralNickMalFaktor = (IntegralNick * IntegralFaktor) / (44000 / STICK_GAIN); else IntegralNickMalFaktor = 0;
		1392	if(!Looping_Roll) IntegralRollMalFaktor = (IntegralRoll * IntegralFaktor) / (44000 / STICK_GAIN); else IntegralRollMalFaktor = 0;
1153	hbuss	1393
1167	hbuss	1394	#define TRIM_MAX 200
1166	hbuss	1395	if(TrimNick > TRIM_MAX) TrimNick = TRIM_MAX; else if(TrimNick <-TRIM_MAX) TrimNick =-TRIM_MAX;
		1396	if(TrimRoll > TRIM_MAX) TrimRoll = TRIM_MAX; else if(TrimRoll <-TRIM_MAX) TrimRoll =-TRIM_MAX;
1153	hbuss	1397
1247	MartinR	1398	if(!IntegralFaktor) // MartinR : hinzugef�gt
		1399	{
		1400	MesswertNick = (long) ((long)MesswertNick * GyroFaktor) / (256L / STICK_GAIN) ; // MartinR : hinzugef�gt
		1401	MesswertRoll = (long) ((long)MesswertRoll * GyroFaktor) / (256L / STICK_GAIN) ; // MartinR : hinzugef�gt
		1402	}
		1403	else // MartinR so war es
		1404	{
1166	hbuss	1405	MesswertNick = IntegralNickMalFaktor + (long)((long)MesswertNick * GyroFaktor + (long)TrimNick * 128L) / (256L / STICK_GAIN);
		1406	MesswertRoll = IntegralRollMalFaktor + (long)((long)MesswertRoll * GyroFaktor + (long)TrimRoll * 128L) / (256L / STICK_GAIN);
1247	MartinR	1407	}
		1408
1211	hbuss	1409	MesswertGier = (long)(MesswertGier * 2 * (long)GyroFaktorGier) / (256L / STICK_GAIN) + (long)(Integral_Gier * IntegralFaktorGier) / (2 * (44000 / STICK_GAIN));
1166	hbuss	1410
1	ingob	1411	// Maximalwerte abfangen
1247	MartinR	1412	// MartinR : Pr�fen ob Unterscheidung nach Platinenversion erforderlich?
		1413
1153	hbuss	1414	// #define MAX_SENSOR (4096*STICK_GAIN)
		1415	#define MAX_SENSOR (4096*4)
1216	killagreg	1416	if(MesswertNick > MAX_SENSOR) MesswertNick = MAX_SENSOR;
1	ingob	1417	if(MesswertNick < -MAX_SENSOR) MesswertNick = -MAX_SENSOR;
		1418	if(MesswertRoll > MAX_SENSOR) MesswertRoll = MAX_SENSOR;
		1419	if(MesswertRoll < -MAX_SENSOR) MesswertRoll = -MAX_SENSOR;
		1420	if(MesswertGier > MAX_SENSOR) MesswertGier = MAX_SENSOR;
		1421	if(MesswertGier < -MAX_SENSOR) MesswertGier = -MAX_SENSOR;
		1422
1051	killagreg	1423	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1210	hbuss	1424	// all BL-Ctrl connected?
		1425	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1216	killagreg	1426	if(MissingMotor) if(modell_fliegt > 1 && modell_fliegt < 50 && GasMischanteil > 0)
		1427	{
		1428	modell_fliegt = 1;
		1429	GasMischanteil = MIN_GAS;
		1430	}
1210	hbuss	1431
		1432	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1	ingob	1433	// H�henregelung
		1434	// Die H�henregelung schw�cht lediglich das Gas ab, erh�ht es allerdings nicht
1051	killagreg	1435	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
855	hbuss	1436	GasMischanteil *= STICK_GAIN;
1247	MartinR	1437
		1438	Mess_Integral_Hoch += AdWertAccHoch; // Integrieren // MartinR: zu H�henregler in fc.c verschoben
		1439	Mess_Integral_Hoch -= Mess_Integral_Hoch / 1024; // d�mfen // MartinR: zu H�henregler in fc.c verschoben
		1440
		1441	// MartinR: �nderungen HoheD Anfang :
1250	MartinR	1442	//HoehenWert = 255 * ExpandBaro + StartLuftdruck - Luftdruck;// wieder bei analog.c
1247	MartinR	1443	HoehenWertSumme += HoehenWert ;
1250	MartinR	1444	//if (++ iHoehe >= 40) // MartinR: Wartezeit um auch kleine �nderungsgeschwindigkeiten erkennen zu k�nnen
		1445	if (++ iHoehe >= Parameter_UserParam5) // MartinR: Wartezeit einstellbar
1247	MartinR	1446	{
		1447	HoehenWertSumme = HoehenWertSumme / iHoehe ; // MartinR: Mittelwert w�hrend der Wartezeit
		1448	HoeheD = (int) Parameter_Luftdruck_D * (HoehenWert - HoehenWertalt) ;
		1449	if (HoeheD < 0) HoeheD += Parameter_Luftdruck_D ; // MartinR: Digitalisierungsrauschen abmindern
		1450	if (HoeheD > 0) HoeheD -= Parameter_Luftdruck_D ; // MartinR: entspricht - 1 Bit vom H�henwert
		1451
		1452	iHoehe = 0 ;
		1453	HoehenWertalt = HoehenWertSumme ;
		1454	HoehenWertSumme = 0 ;
		1455	}
		1456	// MartinR: �nderungen HoheD Ende
		1457
		1458
		1459	// MartinR: bisheriger H�henregler
		1460
		1461	/*
1	ingob	1462	if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // H�henregelung
		1463	{
		1464	int tmp_int;
1036	hbuss	1465	static char delay = 100;
1	ingob	1466	if(EE_Parameter.GlobalConfig & CFG_HOEHEN_SCHALTER) // Regler wird �ber Schalter gesteuert
		1467	{
1216	killagreg	1468	if(Parameter_MaxHoehe < 50)
1	ingob	1469	{
1036	hbuss	1470	if(!delay--)
		1471	{
1225	hbuss	1472	if((MessLuftdruck > 1000) && OCR0A < 254)
1036	hbuss	1473	{
1225	hbuss	1474	if(OCR0A < 244)
		1475	{
		1476	ExpandBaro -= 10;
		1477	OCR0A = DruckOffsetSetting - ExpandBaro;
		1478	}
		1479	else OCR0A = 254;
		1480	beeptime = 300;
		1481	delay = 250;
1036	hbuss	1482	}
1225	hbuss	1483	else
		1484	if((MessLuftdruck < 100) && OCR0A > 1)
1036	hbuss	1485	{
1225	hbuss	1486	if(OCR0A > 10)
		1487	{
		1488	ExpandBaro += 10;
		1489	OCR0A = DruckOffsetSetting - ExpandBaro;
		1490	}
		1491	else OCR0A = 1;
1036	hbuss	1492	beeptime = 300;
		1493	delay = 250;
		1494	}
		1495	else
1051	killagreg	1496	{
1036	hbuss	1497	SollHoehe = HoehenWert - 20; // Parameter_MaxHoehe ist der PPM-Wert des Schalters
		1498	HoehenReglerAktiv = 0;
		1499	delay = 1;
		1500	}
1051	killagreg	1501	}
1	ingob	1502	}
1051	killagreg	1503	else
		1504	{
1036	hbuss	1505	HoehenReglerAktiv = 1;
		1506	delay = 200;
1051	killagreg	1507	}
1	ingob	1508	}
1051	killagreg	1509	else
1	ingob	1510	{
492	hbuss	1511	SollHoehe = ((int) ExternHoehenValue + (int) Parameter_MaxHoehe) * (int)EE_Parameter.Hoehe_Verstaerkung - 20;
1	ingob	1512	HoehenReglerAktiv = 1;
		1513	}
		1514
		1515	if(Notlandung) SollHoehe = 0;
		1516	h = HoehenWert;
		1517	if((h > SollHoehe) && HoehenReglerAktiv) // zu hoch --> drosseln
1051	killagreg	1518	{
855	hbuss	1519	h = ((h - SollHoehe) * (int) Parameter_Hoehe_P) / (16 / STICK_GAIN); // Differenz bestimmen --> P-Anteil
1	ingob	1520	h = GasMischanteil - h; // vom Gas abziehen
871	hbuss	1521	h -= (HoeheD)/(8/STICK_GAIN); // D-Anteil
		1522	tmp_int = ((Mess_Integral_Hoch / 128) * (signed long) Parameter_Hoehe_ACC_Wirkung) / (128 / STICK_GAIN);
		1523	if(tmp_int > 70STICK_GAIN) tmp_int = 70STICK_GAIN;
		1524	else if(tmp_int < -(70STICK_GAIN)) tmp_int = -(70STICK_GAIN);
1	ingob	1525	h -= tmp_int;
1051	killagreg	1526	hoehenregler = (hoehenregler*15 + h) / 16;
855	hbuss	1527	if(hoehenregler < EE_Parameter.Hoehe_MinGas * STICK_GAIN) // nicht unter MIN
1	ingob	1528	{
855	hbuss	1529	if(GasMischanteil >= EE_Parameter.Hoehe_MinGas * STICK_GAIN) hoehenregler = EE_Parameter.Hoehe_MinGas * STICK_GAIN;
		1530	if(GasMischanteil < EE_Parameter.Hoehe_MinGas * STICK_GAIN) hoehenregler = GasMischanteil;
1051	killagreg	1531	}
1	ingob	1532	if(hoehenregler > GasMischanteil) hoehenregler = GasMischanteil; // nicht mehr als Gas
		1533	GasMischanteil = hoehenregler;
1051	killagreg	1534	}
1	ingob	1535	}
855	hbuss	1536	if(GasMischanteil > (MAX_GAS - 20) * STICK_GAIN) GasMischanteil = (MAX_GAS - 20) * STICK_GAIN;
1247	MartinR	1537
		1538	*/
		1539	// MartinR: Ende bisheriger H�henregler
1111	hbuss	1540
1247	MartinR	1541	// MartinR: Neuer H�henregler von MartinR: Anfang
		1542
		1543	if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // H�henregelung
		1544	{
1250	MartinR	1545
		1546	/*
		1547	// Vorschlag Killagreg Anfang
		1548	#define HOOVER_GAS_AVERAGE 4192 // 4192 * 2ms = 8.2s averaging
		1549	#define HEIGHT_CONTROL_GAS_AVERAGE 16
		1550	int16_t CosNick, CosRoll;
		1551	int16_t HeightControlGas;
		1552	static int16_t FilterHeightControlGas = 0;
		1553	static uint32_t HooverGasEstimation = 0;
		1554	//static uint8_t delay = 100;
		1555	// Ende Vorschlag Killagreg
		1556	*/
		1557
		1558
1247	MartinR	1559	int tmp_int;
		1560	static char delay = 100;
		1561	if(EE_Parameter.GlobalConfig & CFG_HOEHEN_SCHALTER) // Regler wird �ber Schalter gesteuert
		1562	{
		1563	//if(Parameter_MaxHoehe < 50) // MartinR :so war es
1250	MartinR	1564
		1565
		1566	if(Parameter_MaxHoehe < 50 \|\| (Parameter_UserParam1 > 140) ) // MartinR: Schalter aus oder HH �ber UsererParam1 an
1247	MartinR	1567	{
1250	MartinR	1568	// H�henregler nicht aktiv
		1569	if(!delay--)
1247	MartinR	1570	{
1250	MartinR	1571	#define OPA_OFFSET_STEP 10
		1572	if((MessLuftdruck > 1000) && (OCR0A < (255 - OPA_OFFSET_STEP)) )
1247	MartinR	1573	{
1250	MartinR	1574	ExpandBaro -= 1;
		1575	OCR0A = DruckOffsetSetting - OPA_OFFSET_STEP * ExpandBaro;
1247	MartinR	1576	beeptime = 300;
		1577	delay = 250;
		1578	}
1250	MartinR	1579	else if((MessLuftdruck < 100) && (OCR0A > OPA_OFFSET_STEP))
		1580	{
		1581	ExpandBaro += 1;
		1582	OCR0A = DruckOffsetSetting - OPA_OFFSET_STEP * ExpandBaro;
		1583	beeptime = 300;
		1584	delay = 250;
		1585	}
1247	MartinR	1586	else
		1587	if((MessLuftdruck < 100) && OCR0A > 1)
		1588	{
		1589	if(OCR0A > 10)
		1590	{
		1591	ExpandBaro += 10;
		1592	OCR0A = DruckOffsetSetting - ExpandBaro;
		1593	}
		1594	else OCR0A = 1;
		1595	beeptime = 300;
		1596	delay = 250;
		1597	}
		1598	else
1250	MartinR	1599	{
1247	MartinR	1600	//SollHoehe = HoehenWert - 20; // Parameter_MaxHoehe ist der PPM-Wert des Schalters // MartinR : so war es
		1601	SollHoehe = HoehenWert; // MartinR : ge�ndert
		1602	HoehenReglerAktiv = 0;
		1603	delay = 1;
		1604	}
		1605	}
		1606	SollHoehe = HoehenWert; // MartinR: SollHoehe wird nachgef�hrt bis HoehenreglerAktiv = 1
		1607	SchwebeGas = GasMischanteil; // MartinR: SchwebeGas wird nachgef�hrt bis HoehenreglerAktiv = 1
		1608	HoehenReglerAktiv = 0; // MartinR: zur Sicherheit
		1609	}
		1610	else // Schalter an und kein HH �ber Schalter
		1611	{
		1612	HoehenReglerAktiv = 1;
		1613	// Schwebegas ist der letzte GasMischanteil vor dem Aktivieren der H�henregelung
		1614	delay = 200;
		1615	}
		1616	}
		1617	else // Sollh�he �ber Poti
		1618	{
		1619	//SollHoehe = ((int) ExternHoehenValue + (int) Parameter_MaxHoehe) * (int)EE_Parameter.Hoehe_Verstaerkung - 20; // MartinR : so war es
		1620	//HoehenReglerAktiv = 1; // MartinR : so war es
		1621	if(Parameter_UserParam1 > 140) // HH �ber Schalter: H�henregler abgeschaltet, Nachf�hren von Parametern
		1622	{
		1623	SollHoehe = HoehenWert; // SollHoehe wird nachgef�hrt bis HoehenreglerAktiv = 1
		1624	HoehenReglerAktiv = 0;
		1625	}
		1626	else // H�henregler mit Sollh�he �ber Poti aktiv
		1627	{
		1628	SchwebeGas = GasMischanteil; // SchwebeGas zun�chst = Gasmischanteil bis bessere L�sung
		1629	HoehenReglerAktiv = 1;
		1630	// MartinR: Versuch H�henregler zu verlangsamen. Steig- / Sinkrate im MK-Tool �ber UserParam4 einstellbar
		1631	if (((int) ExternHoehenValue + (int) Parameter_MaxHoehe) * (int)EE_Parameter.Hoehe_Verstaerkung - 20 > SollHoehe)
		1632	{
		1633	if(++delay_Hoehenregler > (Parameter_UserParam4 / 4))
		1634	{
		1635	delay_Hoehenregler = 0;
		1636	SollHoehe ++ ; //MartinR: H�hen�nderung begrenzen.
		1637	}
		1638	}
		1639	else
		1640	{
		1641	if (((int) ExternHoehenValue + (int) Parameter_MaxHoehe) * (int)EE_Parameter.Hoehe_Verstaerkung - 20 < SollHoehe)
		1642	{
		1643	if(++delay_Hoehenregler > (Parameter_UserParam4 / 4))
		1644	{
		1645	delay_Hoehenregler = 0;
		1646	SollHoehe -- ; //MartinR: H�hen�nderung begrenzen.
		1647	}
		1648	}
		1649	}
		1650	}
		1651
		1652
		1653	}
		1654
		1655	if(Notlandung) SollHoehe = 0;
1250	MartinR	1656	// Vorschlag von Killagerg:
		1657	/*
		1658	CosNick = IntegralNick/GIER_GRAD_FAKTOR; // nick angle in deg // =IntegralNick/GIER_GRAD_FAKTOR
		1659	LIMIT_MIN_MAX(CosNick, -60, 60); // limit nick angle
		1660	CosNick = c_cos_8192(CosNick);
		1661	CosRoll = IntegralRoll/GIER_GRAD_FAKTOR; // roll angle in deg
		1662	LIMIT_MIN_MAX(CosRoll, -60, 60); // limit roll angle
		1663	CosRoll = c_cos_8192(CosRoll);
		1664	*/
		1665	/*
		1666	// calculate cos of nick and roll angle used for projection of the vertical hoover gas
		1667	CosNick = IntegralGyroNick/GYRO_DEG_FACTOR; // nick angle in deg // =IntegralNick/GIER_GRAD_FAKTOR
		1668	LIMIT_MIN_MAX(CosNick, -60, 60); // limit nick angle
		1669	CosNick = c_cos_8192(CosNick);
		1670	CosRoll = IntegralGyroRoll/GYRO_DEG_FACTOR; // roll angle in deg
		1671	LIMIT_MIN_MAX(CosRoll, -60, 60); // limit roll angle
		1672	CosRoll = c_cos_8192(CosRoll);
		1673	if(HeightControlActive)
		1674	{
		1675	if(HooverGasEstimation == 0) HeightControlGas = GasMixFraction; // take stick gas
		1676	else HeightControlGas = (int16_t)(HooverGasEstimation/HOOVER_GAS_AVERAGE); // take hoover point
		1677	HeightControlGas -= ((ReadingHeight - SetPointHeight) * (int16_t) FCParam.HeightP) / (16 / STICK_GAIN); // p-part
		1678	HeightControlGas -= (HeightD) / (8 / STICK_GAIN); // d-part
		1679	// acceleration sensor effect
		1680	tmp_int = ((ReadingIntegralTop / 128) * (int32_t) FCParam.Height_ACC_Effect) / (128 / STICK_GAIN);
		1681	LIMIT_MIN_MAX(tmp_int, -(70 * STICK_GAIN), 70 * STICK_GAIN); // limit acceleration sensor effect
		1682	HeightControlGas -= tmp_int; // acc-part
		1683
		1684	// strech by actual attitude projection
		1685	tmp_long3 = (int32_t)HeightControlGas;
		1686	tmp_long3 *= 8192; tmp_long3 /= CosNick;
		1687	tmp_long3 *= 8192; tmp_long3 /= CosRoll;
		1688	HeightControlGas = (int16_t)tmp_long3;
		1689	// update height control gas averaging
		1690	FilterHeightControlGas = (FilterHeightControlGas * (HEIGHT_CONTROL_GAS_AVERAGE - 1) + HeightControlGas) / HEIGHT_CONTROL_GAS_AVERAGE;
		1691	// limit gas reduction
		1692	if(FilterHeightControlGas < ParamSet.HeightMinGas * STICK_GAIN)
		1693	{
		1694	if(GasMixFraction >= ParamSet.HeightMinGas * STICK_GAIN) FilterHeightControlGas = ParamSet.HeightMinGas * STICK_GAIN;
		1695	// allows landing also if gas stick is reduced below min gas on height control
		1696	if(GasMixFraction < ParamSet.HeightMinGas * STICK_GAIN) FilterHeightControlGas = GasMixFraction;
		1697	}
		1698	// limit gas to stick setting
		1699	LIMIT_MAX(FilterHeightControlGas ,GasMixFraction);
		1700	// set GasMixFraction to HeightControlGasFilter
		1701	GasMixFraction = FilterHeightControlGas;
		1702	} // EOF height control active
		1703	// Hoover gas estimation by averaging gas control output on small z-velocities
		1704	// this is done only if height contol option is selected in global config and aircraft is flying
		1705	if(MKFlags & MKFLAG_FLY)
		1706	{
		1707	if(HooverGasEstimation == 0) HooverGasEstimation = HOOVER_GAS_AVERAGE * (uint32_t)GasMixFraction; // init estimation
		1708	if(abs((HeightD * 20) / FCParam.HeightD) < 5)
		1709	{
		1710	tmp_long3 = (int32_t)GasMixFraction; // take current thrust
		1711	tmp_long3 *= CosNick; // apply nick projection
		1712	tmp_long3 /= 8192; tmp_long3 *= CosRoll; // apply roll projection
		1713	tmp_long3 /= 8129; // average vertical projected thrust
		1714	HooverGasEstimation -= HooverGasEstimation/HOOVER_GAS_AVERAGE;
		1715	HooverGasEstimation += tmp_long3; DebugOut.Analog[16] = HooverGasEstimation/HOOVER_GAS_AVERAGE;
		1716	}
		1717	}
		1718	}// EOF ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL
		1719
		1720
		1721	*/
		1722
		1723
		1724
1247	MartinR	1725	// MartinR: so war es Anfang
		1726	/*
		1727	h = HoehenWert;
		1728	if((h > SollHoehe) && HoehenReglerAktiv) // zu hoch --> drosseln
		1729	{
		1730	h = ((h - SollHoehe) * (int) Parameter_Hoehe_P) / (16 / STICK_GAIN); // Differenz bestimmen --> P-Anteil
		1731	h = GasMischanteil - h; // vom Gas abziehen
		1732	h -= (HoeheD)/(8/STICK_GAIN); // D-Anteil
		1733	tmp_int = ((Mess_Integral_Hoch / 128) * (signed long) Parameter_Hoehe_ACC_Wirkung) / (128 / STICK_GAIN);
		1734	if(tmp_int > 70STICK_GAIN) tmp_int = 70STICK_GAIN;
		1735	else if(tmp_int < -(70STICK_GAIN)) tmp_int = -(70STICK_GAIN);
		1736	h -= tmp_int;
		1737	hoehenregler = (hoehenregler*15 + h) / 16;
		1738	if(hoehenregler < EE_Parameter.Hoehe_MinGas * STICK_GAIN) // nicht unter MIN
		1739	{
		1740	if(GasMischanteil >= EE_Parameter.Hoehe_MinGas * STICK_GAIN) hoehenregler = EE_Parameter.Hoehe_MinGas * STICK_GAIN;
		1741	if(GasMischanteil < EE_Parameter.Hoehe_MinGas * STICK_GAIN) hoehenregler = GasMischanteil;
		1742	}
		1743	if(hoehenregler > GasMischanteil) hoehenregler = GasMischanteil; // nicht mehr als Gas
		1744	GasMischanteil = hoehenregler;
		1745	}
		1746	}
		1747	*/ // MartinR: so war es Ende
		1748
		1749	// MartinR: neuer H�henregler Anfang
		1750
		1751	if(HoehenReglerAktiv && !(Parameter_UserParam1 > 140)) // kein HH
		1752	{
		1753	h_delta = (int) SollHoehe - (int) HoehenWert; // positiver Wert: MK ist unterhalb der Sollh�he
		1754
		1755
		1756	h_p = (h_delta * (int) Parameter_Hoehe_P) / (16 / STICK_GAIN); // Differenz bestimmen --> P-Anteil
		1757
		1758	h_d = (int) (HoeheD)/(64/STICK_GAIN); // D-Anteil (8*8=64)
		1759	// ACC-D-Anteil
		1760	tmp_int = ((Mess_Integral_Hoch / 128) * (signed long) Parameter_Hoehe_ACC_Wirkung) / (128 / STICK_GAIN);
		1761	if(tmp_int > 50STICK_GAIN) tmp_int = 50STICK_GAIN;
		1762	else if(tmp_int < -(50STICK_GAIN)) tmp_int = -(50STICK_GAIN);
		1763	h_d += tmp_int; // positiver D-Anteil bedeutet Steigen
		1764
		1765	h = SchwebeGas + h_p - h_d ;
		1766
		1767	hoehenregler = (hoehenregler*7 + h) / 8;
		1768
		1769	if(hoehenregler < EE_Parameter.Hoehe_MinGas * STICK_GAIN) // nicht unter MIN
		1770	{
		1771	if(GasMischanteil >= EE_Parameter.Hoehe_MinGas * STICK_GAIN) hoehenregler = EE_Parameter.Hoehe_MinGas * STICK_GAIN;
		1772	if(GasMischanteil < EE_Parameter.Hoehe_MinGas * STICK_GAIN) hoehenregler = GasMischanteil;
		1773	}
		1774	if(hoehenregler > GasMischanteil) hoehenregler = GasMischanteil; // nicht mehr als Gas
		1775	//else hoehenregler += (GasMischanteil-hoehenregler)/16 ; // um die H�he noch leicht mit Gas korrigieren zu k�nnen
		1776	//if(SchwebeGas < 20) SchwebeGas = 20; // nicht unter 20
		1777
		1778	GasMischanteil = hoehenregler;
		1779	}
		1780	else // Integratoren auf Null halten
		1781	{
		1782	Mess_Integral_Hoch = 0;
		1783	hoehenregler = GasMischanteil;
		1784	h_p = 0 ;
		1785	h_d = 0 ;
		1786	}
		1787	}
		1788	// MartinR: neuer H�henregler Ende
		1789
		1790	if(GasMischanteil > (MAX_GAS - 20) * STICK_GAIN) GasMischanteil = (MAX_GAS - 20) * STICK_GAIN;
		1791
		1792	// MartinR: Ende neuer H�henregler
		1793
1051	killagreg	1794	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1	ingob	1795	// + Mischer und PI-Regler
1051	killagreg	1796	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
173	holgerb	1797	DebugOut.Analog[7] = GasMischanteil;
1051	killagreg	1798	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1	ingob	1799	// Gier-Anteil
1051	killagreg	1800	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
188	holgerb	1801	#define MUL_G 1.0
855	hbuss	1802	GierMischanteil = MesswertGier - sollGier * STICK_GAIN; // Regler f�r Gier
604	hbuss	1803	// GierMischanteil = 0;
855	hbuss	1804	#define MIN_GIERGAS (40*STICK_GAIN) // unter diesem Gaswert trotzdem Gieren
693	hbuss	1805	if(GasMischanteil > MIN_GIERGAS)
		1806	{
1051	killagreg	1807	if(GierMischanteil > (GasMischanteil / 2)) GierMischanteil = GasMischanteil / 2;
		1808	if(GierMischanteil < -(GasMischanteil / 2)) GierMischanteil = -(GasMischanteil / 2);
693	hbuss	1809	}
1051	killagreg	1810	else
693	hbuss	1811	{
1051	killagreg	1812	if(GierMischanteil > (MIN_GIERGAS / 2)) GierMischanteil = MIN_GIERGAS / 2;
		1813	if(GierMischanteil < -(MIN_GIERGAS / 2)) GierMischanteil = -(MIN_GIERGAS / 2);
693	hbuss	1814	}
855	hbuss	1815	tmp_int = MAX_GAS*STICK_GAIN;
1051	killagreg	1816	if(GierMischanteil > ((tmp_int - GasMischanteil))) GierMischanteil = ((tmp_int - GasMischanteil));
855	hbuss	1817	if(GierMischanteil < -((tmp_int - GasMischanteil))) GierMischanteil = -((tmp_int - GasMischanteil));
513	hbuss	1818
1051	killagreg	1819	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1	ingob	1820	// Nick-Achse
1051	killagreg	1821	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
723	hbuss	1822	DiffNick = MesswertNick - StickNick; // Differenz bestimmen
1247	MartinR	1823
		1824	// MartinR : so war es Anfang
		1825	/*
1153	hbuss	1826	if(IntegralFaktor) SummeNick += IntegralNickMalFaktor - StickNick; // I-Anteil bei Winkelregelung
1051	killagreg	1827	else SummeNick += DiffNick; // I-Anteil bei HH
855	hbuss	1828	if(SummeNick > (STICK_GAIN * 16000L)) SummeNick = (STICK_GAIN * 16000L);
		1829	if(SummeNick < -(16000L * STICK_GAIN)) SummeNick = -(16000L * STICK_GAIN);
1153	hbuss	1830	pd_ergebnis_nick = DiffNick + SummeNick / Ki; // PI-Regler f�r Nick
1	ingob	1831	// Motor Vorn
1247	MartinR	1832	*/
		1833	// MartinR : so war es Ende
		1834
		1835	// MartinR : ge�ndert Anfang
		1836
		1837	if(IntegralFaktor) // MartinR : ACC-Mode
		1838	{
		1839	SummeNick += IntegralNickMalFaktor - StickNick; // I-Anteil bei Winkelregelung
		1840	if(SummeNick > (STICK_GAIN * 8000L)) SummeNick = (STICK_GAIN * 8000L); // MartinR : von 16000 auf 8000, da �berlauf
		1841	if(SummeNick < -(8000L * STICK_GAIN)) SummeNick = -(8000L * STICK_GAIN); // MartinR : von 16000 auf 8000, da �berlauf
		1842	pd_ergebnis_nick = DiffNick + (SummeNick / Ki);
		1843	SummeNickHH = 0 ;
		1844	}
		1845	else // MartinR : HH-Mode
		1846	{
		1847	SummeNickHH += DiffNick; // I-Anteil bei HH
		1848	if(SummeNickHH > (STICK_GAIN * 8000L)) SummeNickHH = (STICK_GAIN * 8000L); // MartinR : von 16000 auf 8000, da �berlauf
		1849	if(SummeNickHH < -(8000L * STICK_GAIN)) SummeNickHH = -(8000L * STICK_GAIN); // MartinR : von 16000 auf 8000, da �berlauf
		1850	pd_ergebnis_nick = DiffNick + SummeNickHH / KiHH; // MartinR: PI-Regler f�r Nick bei HH
		1851	SummeNick = 0;
		1852	}
		1853
		1854	// MartinR : ge�ndert Ende
		1855
499	hbuss	1856	tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64;
1153	hbuss	1857	if(pd_ergebnis_nick > tmp_int) pd_ergebnis_nick = tmp_int;
		1858	if(pd_ergebnis_nick < -tmp_int) pd_ergebnis_nick = -tmp_int;
173	holgerb	1859
1247	MartinR	1860
		1861
1153	hbuss	1862	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		1863	// Roll-Achse
		1864	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		1865	DiffRoll = MesswertRoll - StickRoll; // Differenz bestimmen
1247	MartinR	1866
		1867	// MartinR : so war es Anfang
		1868	/*
1153	hbuss	1869	if(IntegralFaktor) SummeRoll += IntegralRollMalFaktor - StickRoll;// I-Anteil bei Winkelregelung
		1870	else SummeRoll += DiffRoll; // I-Anteil bei HH
		1871	if(SummeRoll > (STICK_GAIN * 16000L)) SummeRoll = (STICK_GAIN * 16000L);
		1872	if(SummeRoll < -(16000L * STICK_GAIN)) SummeRoll = -(16000L * STICK_GAIN);
		1873	pd_ergebnis_roll = DiffRoll + SummeRoll / Ki; // PI-Regler f�r Roll
1247	MartinR	1874	*/
		1875	// MartinR : so war es Ende
		1876
		1877	// MartinR : ge�ndert Anfang
		1878
		1879	if(IntegralFaktor) // MartinR : ACC-Mode
		1880	{
		1881	SummeRoll += IntegralRollMalFaktor - StickRoll;
		1882	if(SummeRoll > (STICK_GAIN * 8000L)) SummeRoll = (STICK_GAIN * 8000L);// MartinR : von 16000 auf 8000, da �berlauf
		1883	if(SummeRoll < -(8000L * STICK_GAIN)) SummeRoll = -(8000L * STICK_GAIN);// MartinR : von 16000 auf 8000, da �berlauf
		1884	tmp_int = SummeRoll / Ki;
		1885	pd_ergebnis_roll = DiffRoll + tmp_int; // MartinR: PI-Regler im ACC-Mode
		1886	//SummeRollHH = (IntegralRollMalFaktor + tmp_int - stick_roll_neutral + (TrimRoll * STICK_GAIN / 2)) * KiHH;// MartinR: Startwert von SummeRollHH bei Umschaltung auf HH
		1887	// MartinR: Hintergrund: pd_ergebnis_xx soll sich beim Umschalten nicht �ndern!
		1888	SummeRollHH = 0;
		1889	}
		1890	else // MartinR : HH-Mode
		1891	{
		1892	SummeRollHH += DiffRoll; // I-Anteil bei HH
		1893	if(SummeRollHH > (STICK_GAIN * 8000L)) SummeRollHH = (STICK_GAIN * 8000L);// MartinR : von 16000 auf 8000, da �berlauf
		1894	if(SummeRollHH < -(8000L * STICK_GAIN)) SummeRollHH = -(8000L * STICK_GAIN);// MartinR : von 16000 auf 8000, da �berlauf
		1895	pd_ergebnis_roll = DiffRoll + SummeRollHH / KiHH; // MartinR: PI-Regler f�r Roll bei HH
		1896	SummeRoll = 0;
		1897	}
		1898
		1899	// MartinR : ge�ndert Ende
		1900
		1901
1153	hbuss	1902	tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64;
		1903	if(pd_ergebnis_roll > tmp_int) pd_ergebnis_roll = tmp_int;
		1904	if(pd_ergebnis_roll < -tmp_int) pd_ergebnis_roll = -tmp_int;
		1905
		1906	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1209	hbuss	1907	// Universal Mixer
1155	hbuss	1908	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1216	killagreg	1909	for(i=0; i<MAX_MOTORS; i++)
1209	hbuss	1910	{
		1911	signed int tmp_int;
		1912	if(Mixer.Motor[i][0] > 0)
		1913	{
		1914	tmp_int = ((long)GasMischanteil * Mixer.Motor[i][0]) / 64L;
		1915	tmp_int += ((long)pd_ergebnis_nick * Mixer.Motor[i][1]) / 64L;
		1916	tmp_int += ((long)pd_ergebnis_roll * Mixer.Motor[i][2]) / 64L;
		1917	tmp_int += ((long)GierMischanteil * Mixer.Motor[i][3]) / 64L;
		1918	tmp_motorwert[i] = MotorSmoothing(tmp_int,tmp_motorwert[i]); // Filter
1216	killagreg	1919	tmp_int = tmp_motorwert[i] / STICK_GAIN;
1209	hbuss	1920	CHECK_MIN_MAX(tmp_int,MIN_GAS,MAX_GAS);
1216	killagreg	1921	Motor[i] = tmp_int;
1209	hbuss	1922	}
		1923	else Motor[i] = 0;
		1924	}
1167	hbuss	1925	/*
1153	hbuss	1926	if(Poti1 > 20) Motor1 = 0;
		1927	if(Poti1 > 90) Motor6 = 0;
		1928	if(Poti1 > 140) Motor2 = 0;
1175	hbuss	1929	//if(Poti1 > 200) Motor7 = 0;
1167	hbuss	1930	*/
1111	hbuss	1931	}

Subversion Repositories FlightCtrl

(root)/branches/FC_075b_ACC-HH_MartinR/fc.c - Rev 1250