WebSVN - FlightCtrl - Blame - Rev 1153 - /branches/V0.88e_ACC-HH_HR

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;
1051	killagreg	61	int AdNeutralGierBias;
927	hbuss	62	int AdNeutralNick = 0,AdNeutralRoll = 0,AdNeutralGier = 0,StartNeutralRoll = 0,StartNeutralNick = 0;
		63	int Mittelwert_AccNick, Mittelwert_AccRoll,Mittelwert_AccHoch, NeutralAccX=0, NeutralAccY=0;
805	hbuss	64	int NaviAccNick, NaviAccRoll,NaviCntAcc = 0;
1	ingob	65	volatile float NeutralAccZ = 0;
		66	unsigned char CosinusNickWinkel = 0, CosinusRollWinkel = 0;
693	hbuss	67	long IntegralNick = 0,IntegralNick2 = 0;
		68	long IntegralRoll = 0,IntegralRoll2 = 0;
		69	long IntegralAccNick = 0,IntegralAccRoll = 0,IntegralAccZ = 0;
		70	long Integral_Gier = 0;
		71	long Mess_IntegralNick = 0,Mess_IntegralNick2 = 0;
		72	long Mess_IntegralRoll = 0,Mess_IntegralRoll2 = 0;
		73	long Mess_Integral_Gier = 0,Mess_Integral_Gier2 = 0;
		74	long MittelIntegralNick,MittelIntegralRoll,MittelIntegralNick2,MittelIntegralRoll2;
1	ingob	75	volatile long Mess_Integral_Hoch = 0;
1153	hbuss	76	int KompassValue = 0;
		77	int KompassStartwert = 0;
		78	int KompassRichtung = 0;
693	hbuss	79	unsigned int KompassSignalSchlecht = 500;
855	hbuss	80	unsigned char MAX_GAS,MIN_GAS;
1	ingob	81	unsigned char Notlandung = 0;
		82	unsigned char HoehenReglerAktiv = 0;
880	hbuss	83	unsigned char TrichterFlug = 0;
395	hbuss	84	long Umschlag180Nick = 250000L, Umschlag180Roll = 250000L;
693	hbuss	85	long ErsatzKompass;
		86	int ErsatzKompassInGrad; // Kompasswert in Grad
		87	int GierGyroFehler = 0;
1153	hbuss	88	char GyroFaktor;
		89	char IntegralFaktor;
		90	int DiffNick,DiffRoll;
1051	killagreg	91	int Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0;
1	ingob	92	volatile unsigned char Motor_Vorne,Motor_Hinten,Motor_Rechts,Motor_Links, Count;
1111	hbuss	93	volatile unsigned char Motor1, Motor2,Motor3,Motor4,Motor5,Motor6,Motor7,Motor8;
1	ingob	94	volatile unsigned char SenderOkay = 0;
595	hbuss	95	int StickNick = 0,StickRoll = 0,StickGier = 0,StickGas = 0;
1	ingob	96	char MotorenEin = 0;
		97	int HoehenWert = 0;
		98	int SollHoehe = 0;
498	hbuss	99	int LageKorrekturRoll = 0,LageKorrekturNick = 0;
1153	hbuss	100	//float Ki = FAKTOR_I;
		101	int Ki = 10300 / 33;
395	hbuss	102	unsigned char Looping_Nick = 0,Looping_Roll = 0;
		103	unsigned char Looping_Links = 0, Looping_Rechts = 0, Looping_Unten = 0, Looping_Oben = 0;
1	ingob	104
		105	unsigned char Parameter_Luftdruck_D = 48; // Wert : 0-250
		106	unsigned char Parameter_MaxHoehe = 251; // Wert : 0-250
		107	unsigned char Parameter_Hoehe_P = 16; // Wert : 0-32
		108	unsigned char Parameter_Hoehe_ACC_Wirkung = 58; // Wert : 0-250
		109	unsigned char Parameter_KompassWirkung = 64; // Wert : 0-250
1111	hbuss	110	unsigned char Parameter_Gyro_D = 8; // Wert : 0-250
173	holgerb	111	unsigned char Parameter_Gyro_P = 150; // Wert : 10-250
1	ingob	112	unsigned char Parameter_Gyro_I = 150; // Wert : 0-250
		113	unsigned char Parameter_Gier_P = 2; // Wert : 1-20
		114	unsigned char Parameter_I_Faktor = 10; // Wert : 1-20
		115	unsigned char Parameter_UserParam1 = 0;
		116	unsigned char Parameter_UserParam2 = 0;
		117	unsigned char Parameter_UserParam3 = 0;
		118	unsigned char Parameter_UserParam4 = 0;
499	hbuss	119	unsigned char Parameter_UserParam5 = 0;
		120	unsigned char Parameter_UserParam6 = 0;
		121	unsigned char Parameter_UserParam7 = 0;
		122	unsigned char Parameter_UserParam8 = 0;
1	ingob	123	unsigned char Parameter_ServoNickControl = 100;
173	holgerb	124	unsigned char Parameter_LoopGasLimit = 70;
1120	hbuss	125	unsigned char Parameter_AchsKopplung1 = 90;
		126	unsigned char Parameter_AchsKopplung2 = 65;
		127	unsigned char Parameter_CouplingYawCorrection = 64;
		128	//unsigned char Parameter_AchsGegenKopplung1 = 0;
499	hbuss	129	unsigned char Parameter_DynamicStability = 100;
921	hbuss	130	unsigned char Parameter_J16Bitmask; // for the J16 Output
		131	unsigned char Parameter_J16Timing; // for the J16 Output
		132	unsigned char Parameter_J17Bitmask; // for the J17 Output
		133	unsigned char Parameter_J17Timing; // for the J17 Output
		134	unsigned char Parameter_NaviGpsModeControl; // Parameters for the Naviboard
1051	killagreg	135	unsigned char Parameter_NaviGpsGain;
		136	unsigned char Parameter_NaviGpsP;
		137	unsigned char Parameter_NaviGpsI;
		138	unsigned char Parameter_NaviGpsD;
		139	unsigned char Parameter_NaviGpsACC;
993	hbuss	140	unsigned char Parameter_NaviOperatingRadius;
		141	unsigned char Parameter_NaviWindCorrection;
		142	unsigned char Parameter_NaviSpeedCompensation;
921	hbuss	143	unsigned char Parameter_ExternalControl;
1	ingob	144	struct mk_param_struct EE_Parameter;
492	hbuss	145	signed int ExternStickNick = 0,ExternStickRoll = 0,ExternStickGier = 0, ExternHoehenValue = -20;
614	hbuss	146	int MaxStickNick = 0,MaxStickRoll = 0;
871	hbuss	147	unsigned int modell_fliegt = 0;
921	hbuss	148	unsigned char MikroKopterFlags = 0;
1121	hbuss	149	long GIER_GRAD_FAKTOR = 1291;
1153	hbuss	150	signed int KopplungsteilNickRoll,KopplungsteilRollNick;
1	ingob	151
		152	void Piep(unsigned char Anzahl)
		153	{
		154	while(Anzahl--)
		155	{
		156	if(MotorenEin) return; //auf keinen Fall im Flug!
		157	beeptime = 100;
		158	Delay_ms(250);
		159	}
		160	}
		161
		162	//############################################################################
		163	// Nullwerte ermitteln
		164	void SetNeutral(void)
		165	//############################################################################
		166	{
1111	hbuss	167	unsigned char i;
		168	unsigned int gier_neutral=0, nick_neutral=0, roll_neutral=0;
1051	killagreg	169	NeutralAccX = 0;
1	ingob	170	NeutralAccY = 0;
		171	NeutralAccZ = 0;
1051	killagreg	172	AdNeutralNick = 0;
		173	AdNeutralRoll = 0;
1	ingob	174	AdNeutralGier = 0;
927	hbuss	175	AdNeutralGierBias = 0;
395	hbuss	176	Parameter_AchsKopplung1 = 0;
1120	hbuss	177	Parameter_AchsKopplung2 = 0;
1036	hbuss	178	ExpandBaro = 0;
1051	killagreg	179	CalibrierMittelwert();
395	hbuss	180	Delay_ms_Mess(100);
1	ingob	181	CalibrierMittelwert();
		182	if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // H�henregelung aktiviert?
1051	killagreg	183	{
1	ingob	184	if((MessLuftdruck > 950) \|\| (MessLuftdruck < 750)) SucheLuftruckOffset();
		185	}
1111	hbuss	186	for(i=0; i<32; i++)
		187	{
		188	Delay_ms_Mess(10);
		189	gier_neutral += AdWertGier;
		190	nick_neutral += MesswertNick;
		191	roll_neutral += MesswertRoll;
		192	}
		193	AdNeutralNick= nick_neutral / 32;
		194	AdNeutralRoll= roll_neutral / 32;
		195	AdNeutralGier= gier_neutral / 32;
		196	AdNeutralGierBias = AdNeutralGier;
401	hbuss	197	StartNeutralRoll = AdNeutralRoll;
		198	StartNeutralNick = AdNeutralNick;
1051	killagreg	199	if(eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK]) > 4)
513	hbuss	200	{
		201	NeutralAccY = abs(Mittelwert_AccRoll) / ACC_AMPLIFY;
		202	NeutralAccX = abs(Mittelwert_AccNick) / ACC_AMPLIFY;
		203	NeutralAccZ = Aktuell_az;
		204	}
1051	killagreg	205	else
513	hbuss	206	{
		207	NeutralAccX = (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK]) * 256 + (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK+1]);
		208	NeutralAccY = (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL]) * 256 + (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL+1]);
		209	NeutralAccZ = (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_Z]) * 256 + (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_Z+1]);
		210	}
1051	killagreg	211
		212	Mess_IntegralNick = 0;
1	ingob	213	Mess_IntegralNick2 = 0;
1051	killagreg	214	Mess_IntegralRoll = 0;
1	ingob	215	Mess_IntegralRoll2 = 0;
1051	killagreg	216	Mess_Integral_Gier = 0;
1	ingob	217	MesswertNick = 0;
		218	MesswertRoll = 0;
		219	MesswertGier = 0;
1111	hbuss	220	Delay_ms_Mess(100);
1	ingob	221	StartLuftdruck = Luftdruck;
		222	HoeheD = 0;
		223	Mess_Integral_Hoch = 0;
		224	KompassStartwert = KompassValue;
		225	GPS_Neutral();
1051	killagreg	226	beeptime = 50;
882	hbuss	227	Umschlag180Nick = ((long) EE_Parameter.WinkelUmschlagNick * 2500L) + 15000L;
		228	Umschlag180Roll = ((long) EE_Parameter.WinkelUmschlagRoll * 2500L) + 15000L;
492	hbuss	229	ExternHoehenValue = 0;
693	hbuss	230	ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR;
		231	GierGyroFehler = 0;
723	hbuss	232	SendVersionToNavi = 1;
921	hbuss	233	LED_Init();
		234	MikroKopterFlags \|= FLAG_CALIBRATE;
992	hbuss	235	FromNaviCtrl_Value.Kalman_K = -1;
		236	FromNaviCtrl_Value.Kalman_MaxDrift = EE_Parameter.Driftkomp * 16;
		237	FromNaviCtrl_Value.Kalman_MaxFusion = 32;
1	ingob	238	}
		239
		240	//############################################################################
395	hbuss	241	// Bearbeitet die Messwerte
1	ingob	242	void Mittelwert(void)
		243	//############################################################################
1051	killagreg	244	{
1111	hbuss	245	static signed long tmpl,tmpl2,tmpl3,tmpl4;
		246	static signed int oldNick, oldRoll, d2Roll, d2Nick;
1153	hbuss	247	signed int tmp_int;
		248	signed long winkel_nick, winkel_roll;
1111	hbuss	249	MesswertGier = (signed int) AdNeutralGier - AdWertGier;
		250	// MesswertGierBias = (signed int) AdNeutralGierBias - AdWertGier;
395	hbuss	251	MesswertRoll = (signed int) AdWertRoll - AdNeutralRoll;
		252	MesswertNick = (signed int) AdWertNick - AdNeutralNick;
1153	hbuss	253	RohMesswertNick = MesswertNick;
		254	RohMesswertRoll = MesswertRoll;
1120	hbuss	255	//DebugOut.Analog[21] = MesswertNick;
		256	//DebugOut.Analog[22] = MesswertRoll;
1121	hbuss	257	//DebugOut.Analog[22] = Mess_Integral_Gier;
401	hbuss	258
395	hbuss	259	// Beschleunigungssensor ++++++++++++++++++++++++++++++++++++++++++++++++
		260	Mittelwert_AccNick = ((long)Mittelwert_AccNick * 1 + ((ACC_AMPLIFY * (long)AdWertAccNick))) / 2L;
		261	Mittelwert_AccRoll = ((long)Mittelwert_AccRoll * 1 + ((ACC_AMPLIFY * (long)AdWertAccRoll))) / 2L;
		262	Mittelwert_AccHoch = ((long)Mittelwert_AccHoch * 1 + ((long)AdWertAccHoch)) / 2L;
		263	IntegralAccNick += ACC_AMPLIFY * AdWertAccNick;
		264	IntegralAccRoll += ACC_AMPLIFY * AdWertAccRoll;
805	hbuss	265	NaviAccNick += AdWertAccNick;
		266	NaviAccRoll += AdWertAccRoll;
		267	NaviCntAcc++;
1153	hbuss	268	IntegralAccZ += Aktuell_az - NeutralAccZ;
		269
		270	if(Mess_IntegralRoll > 93000L) winkel_roll = 93000L;
		271	else if(Mess_IntegralRoll <-93000L) winkel_roll = -93000L;
		272	else winkel_roll = Mess_IntegralRoll;
		273
		274	if(Mess_IntegralNick > 93000L) winkel_nick = 93000L;
		275	else if(Mess_IntegralNick <-93000L) winkel_nick = -93000L;
		276	else winkel_nick = Mess_IntegralNick;
		277
1120	hbuss	278	// Gier ++++++++++++++++++++++++++++++++++++++++++++++++
1153	hbuss	279	Mess_Integral_Gier += MesswertGier;
		280	ErsatzKompass += MesswertGier;
1111	hbuss	281
395	hbuss	282	// Kopplungsanteil +++++++++++++++++++++++++++++++++++++
		283	if(!Looping_Nick && !Looping_Roll && (EE_Parameter.GlobalConfig & CFG_ACHSENKOPPLUNG_AKTIV))
		284	{
1153	hbuss	285	tmpl3 = (MesswertRoll * winkel_nick) / 2048L;
1120	hbuss	286	tmpl3 *= Parameter_AchsKopplung2; //65
1111	hbuss	287	tmpl3 /= 4096L;
1153	hbuss	288	tmpl4 = (MesswertNick * winkel_roll) / 2048L;
1120	hbuss	289	tmpl4 *= Parameter_AchsKopplung2; //65
1111	hbuss	290	tmpl4 /= 4096L;
1153	hbuss	291	KopplungsteilNickRoll = tmpl3;
		292	KopplungsteilRollNick = tmpl4;
1111	hbuss	293	tmpl4 -= tmpl3;
		294	ErsatzKompass += tmpl4;
		295
1153	hbuss	296	tmpl = ((MesswertGier + tmpl4) * winkel_nick) / 2048L;
1111	hbuss	297	tmpl *= Parameter_AchsKopplung1; // 90
880	hbuss	298	tmpl /= 4096L;
1153	hbuss	299	tmpl2 = ((MesswertGier + tmpl4) * winkel_roll) / 2048L;
395	hbuss	300	tmpl2 *= Parameter_AchsKopplung1;
880	hbuss	301	tmpl2 /= 4096L;
1153	hbuss	302	if(labs(tmpl) > 128 \|\| labs(tmpl2) > 128 /* \|\| abs(KopplungsteilNickRoll) > 128 \|\| abs(KopplungsteilRollNick) > 128)*/) TrichterFlug = 1;
		303	//MesswertGier += (Parameter_CouplingYawCorrection * tmpl4) / 256;
		304	DebugOut.Analog[21] = KopplungsteilNickRoll;
		305	DebugOut.Analog[22] = KopplungsteilRollNick;
395	hbuss	306	}
		307	else tmpl = tmpl2 = 0;
1111	hbuss	308
		309	// Kompasswert begrenzen ++++++++++++++++++++++++++++++++++++++++++++++++
		310	if(ErsatzKompass >= (360L * GIER_GRAD_FAKTOR)) ErsatzKompass -= 360L * GIER_GRAD_FAKTOR; // 360� Umschlag
		311	if(ErsatzKompass < 0) ErsatzKompass += 360L * GIER_GRAD_FAKTOR;
395	hbuss	312	// Roll ++++++++++++++++++++++++++++++++++++++++++++++++
		313	MesswertRoll += tmpl;
1121	hbuss	314	MesswertRoll += tmpl2 / 100L; // War: *5/512
1120	hbuss	315	// MesswertRoll += (tmpl2*Parameter_AchsGegenKopplung1)/512L; //109
395	hbuss	316	Mess_IntegralRoll2 += MesswertRoll;
498	hbuss	317	Mess_IntegralRoll += MesswertRoll - LageKorrekturRoll;
1120	hbuss	318
1051	killagreg	319	if(Mess_IntegralRoll > Umschlag180Roll)
395	hbuss	320	{
882	hbuss	321	Mess_IntegralRoll = -(Umschlag180Roll - 25000L);
395	hbuss	322	Mess_IntegralRoll2 = Mess_IntegralRoll;
1051	killagreg	323	}
395	hbuss	324	if(Mess_IntegralRoll <-Umschlag180Roll)
		325	{
882	hbuss	326	Mess_IntegralRoll = (Umschlag180Roll - 25000L);
395	hbuss	327	Mess_IntegralRoll2 = Mess_IntegralRoll;
1051	killagreg	328	}
395	hbuss	329	if(AdWertRoll < 15) MesswertRoll = -1000;
		330	if(AdWertRoll < 7) MesswertRoll = -2000;
401	hbuss	331	if(PlatinenVersion == 10)
		332	{
		333	if(AdWertRoll > 1010) MesswertRoll = +1000;
		334	if(AdWertRoll > 1017) MesswertRoll = +2000;
		335	}
1051	killagreg	336	else
401	hbuss	337	{
1111	hbuss	338	if(AdWertRoll > 2000) MesswertRoll = +1000;
		339	if(AdWertRoll > 2015) MesswertRoll = +2000;
401	hbuss	340	}
395	hbuss	341	// Nick ++++++++++++++++++++++++++++++++++++++++++++++++
		342	MesswertNick -= tmpl2;
1120	hbuss	343	// MesswertNick -= (tmpl*Parameter_AchsGegenKopplung1)/512L;
		344	MesswertNick -= tmpl / 100L; //109
395	hbuss	345	Mess_IntegralNick2 += MesswertNick;
498	hbuss	346	Mess_IntegralNick += MesswertNick - LageKorrekturNick;
882	hbuss	347
1051	killagreg	348	if(Mess_IntegralNick > Umschlag180Nick)
882	hbuss	349	{
		350	Mess_IntegralNick = -(Umschlag180Nick - 25000L);
		351	Mess_IntegralNick2 = Mess_IntegralNick;
1051	killagreg	352	}
		353	if(Mess_IntegralNick <-Umschlag180Nick)
395	hbuss	354	{
882	hbuss	355	Mess_IntegralNick = (Umschlag180Nick - 25000L);
395	hbuss	356	Mess_IntegralNick2 = Mess_IntegralNick;
1051	killagreg	357	}
395	hbuss	358	if(AdWertNick < 15) MesswertNick = -1000;
		359	if(AdWertNick < 7) MesswertNick = -2000;
401	hbuss	360	if(PlatinenVersion == 10)
		361	{
		362	if(AdWertNick > 1010) MesswertNick = +1000;
		363	if(AdWertNick > 1017) MesswertNick = +2000;
		364	}
1051	killagreg	365	else
401	hbuss	366	{
1111	hbuss	367	if(AdWertNick > 2000) MesswertNick = +1000;
		368	if(AdWertNick > 2015) MesswertNick = +2000;
401	hbuss	369	}
1111	hbuss	370
395	hbuss	371	//++++++++++++++++++++++++++++++++++++++++++++++++
		372	// ADC einschalten
1051	killagreg	373	ANALOG_ON;
395	hbuss	374	//++++++++++++++++++++++++++++++++++++++++++++++++
		375
1	ingob	376	Integral_Gier = Mess_Integral_Gier;
		377	IntegralNick = Mess_IntegralNick;
		378	IntegralRoll = Mess_IntegralRoll;
1051	killagreg	379	IntegralNick2 = Mess_IntegralNick2;
1	ingob	380	IntegralRoll2 = Mess_IntegralRoll2;
1153	hbuss	381	MesswertNick = (RohMesswertNick + 2 * MesswertNick) / 3;
		382	MesswertRoll = (RohMesswertRoll + 2 * MesswertRoll) / 3;
1	ingob	383
1120	hbuss	384	#define D_LIMIT 8
1111	hbuss	385	if(Parameter_Gyro_D)
		386	{
1120	hbuss	387	d2Nick = (((MesswertNick - oldNick)));
1111	hbuss	388	oldNick = MesswertNick;
		389	if(d2Nick > D_LIMIT) d2Nick = D_LIMIT;
		390	else if(d2Nick < -D_LIMIT) d2Nick = -D_LIMIT;
1120	hbuss	391	MesswertNick += (d2Nick * (signed int) Parameter_Gyro_D);
1111	hbuss	392
1120	hbuss	393	d2Roll = (((MesswertRoll - oldRoll)));
1111	hbuss	394	oldRoll = MesswertRoll;
		395	if(d2Roll > D_LIMIT) d2Roll = D_LIMIT;
		396	else if(d2Roll < -D_LIMIT) d2Roll = -D_LIMIT;
1120	hbuss	397	MesswertRoll += (d2Roll * (signed int) Parameter_Gyro_D);
1111	hbuss	398	}
		399
1153	hbuss	400	if(RohMesswertRoll > 0) MesswertRoll += ((long) abs(KopplungsteilNickRoll) * Parameter_CouplingYawCorrection) / 64L;
		401	else MesswertRoll -= ((long) abs(KopplungsteilNickRoll) * Parameter_CouplingYawCorrection) / 64L;
		402	if(RohMesswertNick > 0) MesswertNick += ((long) abs(KopplungsteilRollNick) * Parameter_CouplingYawCorrection) / 64L;
		403	else MesswertNick -= ((long) abs(KopplungsteilRollNick) * Parameter_CouplingYawCorrection) / 64L;
		404
		405	if(EE_Parameter.GlobalConfig & CFG_DREHRATEN_BEGRENZER && !Looping_Nick && !Looping_Roll)
		406	{
		407	if(RohMesswertNick > 256) MesswertNick += 1 * (RohMesswertNick - 256);
		408	else if(RohMesswertNick < -256) MesswertNick += 1 * (RohMesswertNick + 256);
		409	if(RohMesswertRoll > 256) MesswertRoll += 1 * (RohMesswertRoll - 256);
		410	else if(RohMesswertRoll < -256) MesswertRoll += 1 * (RohMesswertRoll + 256);
		411	}
		412
1	ingob	413	if(Poti1 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110 && Poti1) Poti1--;
		414	if(Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110 && Poti2) Poti2--;
		415	if(Poti3 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110 && Poti3) Poti3--;
		416	if(Poti4 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110 && Poti4) Poti4--;
		417	if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255;
		418	if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255;
		419	if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255;
		420	if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255;
		421	}
		422
		423	//############################################################################
		424	// Messwerte beim Ermitteln der Nullage
		425	void CalibrierMittelwert(void)
		426	//############################################################################
1051	killagreg	427	{
1021	hbuss	428	if(PlatinenVersion == 13) SucheGyroOffset();
1	ingob	429	// ADC auschalten, damit die Werte sich nicht w�hrend der Berechnung �ndern
		430	ANALOG_OFF;
395	hbuss	431	MesswertNick = AdWertNick;
		432	MesswertRoll = AdWertRoll;
		433	MesswertGier = AdWertGier;
		434	Mittelwert_AccNick = ACC_AMPLIFY * (long)AdWertAccNick;
		435	Mittelwert_AccRoll = ACC_AMPLIFY * (long)AdWertAccRoll;
		436	Mittelwert_AccHoch = (long)AdWertAccHoch;
		437	// ADC einschalten
1051	killagreg	438	ANALOG_ON;
1	ingob	439	if(Poti1 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110 && Poti1) Poti1--;
		440	if(Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110 && Poti2) Poti2--;
		441	if(Poti3 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110 && Poti3) Poti3--;
		442	if(Poti4 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110 && Poti4) Poti4--;
		443	if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255;
		444	if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255;
		445	if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255;
		446	if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255;
395	hbuss	447
		448	Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L;
720	ingob	449	Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagRoll * 2500L;
1	ingob	450	}
		451
		452	//############################################################################
		453	// Senden der Motorwerte per I2C-Bus
		454	void SendMotorData(void)
		455	//############################################################################
1051	killagreg	456	{
921	hbuss	457	if(!MotorenEin)
1	ingob	458	{
1111	hbuss	459	#ifdef OCTO
		460	Motor1 = 0;Motor2 = 0;Motor3 = 0;Motor4 = 0;Motor5 = 0;Motor6 = 0;Motor7 = 0;Motor8 = 0;
		461	if(MotorTest[0]) {Motor1 = MotorTest[0]; Motor2 = MotorTest[0];}
		462	if(MotorTest[3]) {Motor3 = MotorTest[3]; Motor4 = MotorTest[3];}
		463	if(MotorTest[1]) {Motor5 = MotorTest[1]; Motor6 = MotorTest[1];}
		464	if(MotorTest[2]) {Motor7 = MotorTest[2]; Motor8 = MotorTest[2];}
		465	#else
		466	Motor_Hinten = 0; Motor_Vorne = 0; Motor_Rechts = 0; Motor_Links = 0;
1	ingob	467	if(MotorTest[0]) Motor_Vorne = MotorTest[0];
		468	if(MotorTest[1]) Motor_Hinten = MotorTest[1];
		469	if(MotorTest[2]) Motor_Links = MotorTest[2];
		470	if(MotorTest[3]) Motor_Rechts = MotorTest[3];
1111	hbuss	471	#endif
		472
921	hbuss	473	MikroKopterFlags &= ~(FLAG_MOTOR_RUN \| FLAG_FLY);
		474	} else MikroKopterFlags \|= FLAG_MOTOR_RUN;
1	ingob	475
		476	DebugOut.Analog[12] = Motor_Vorne;
		477	DebugOut.Analog[13] = Motor_Hinten;
		478	DebugOut.Analog[14] = Motor_Links;
1051	killagreg	479	DebugOut.Analog[15] = Motor_Rechts;
1	ingob	480
		481	//Start I2C Interrupt Mode
		482	twi_state = 0;
		483	motor = 0;
1051	killagreg	484	i2c_start();
1	ingob	485	}
		486
		487
		488
		489	//############################################################################
		490	// Tr�gt ggf. das Poti als Parameter ein
1051	killagreg	491	void ParameterZuordnung(void)
1	ingob	492	//############################################################################
		493	{
921	hbuss	494	#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;}
		495	#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	496	CHK_POTI(Parameter_MaxHoehe,EE_Parameter.MaxHoehe,0,255);
921	hbuss	497	CHK_POTI_MM(Parameter_Luftdruck_D,EE_Parameter.Luftdruck_D,0,100);
		498	CHK_POTI_MM(Parameter_Hoehe_P,EE_Parameter.Hoehe_P,0,100);
1	ingob	499	CHK_POTI(Parameter_Hoehe_ACC_Wirkung,EE_Parameter.Hoehe_ACC_Wirkung,0,255);
		500	CHK_POTI(Parameter_KompassWirkung,EE_Parameter.KompassWirkung,0,255);
921	hbuss	501	CHK_POTI_MM(Parameter_Gyro_P,EE_Parameter.Gyro_P,10,255);
1	ingob	502	CHK_POTI(Parameter_Gyro_I,EE_Parameter.Gyro_I,0,255);
1120	hbuss	503	CHK_POTI(Parameter_Gyro_D,EE_Parameter.Gyro_D,0,255);
1051	killagreg	504	CHK_POTI(Parameter_I_Faktor,EE_Parameter.I_Faktor,0,255);
		505	CHK_POTI(Parameter_UserParam1,EE_Parameter.UserParam1,0,255);
		506	CHK_POTI(Parameter_UserParam2,EE_Parameter.UserParam2,0,255);
		507	CHK_POTI(Parameter_UserParam3,EE_Parameter.UserParam3,0,255);
		508	CHK_POTI(Parameter_UserParam4,EE_Parameter.UserParam4,0,255);
		509	CHK_POTI(Parameter_UserParam5,EE_Parameter.UserParam5,0,255);
		510	CHK_POTI(Parameter_UserParam6,EE_Parameter.UserParam6,0,255);
		511	CHK_POTI(Parameter_UserParam7,EE_Parameter.UserParam7,0,255);
		512	CHK_POTI(Parameter_UserParam8,EE_Parameter.UserParam8,0,255);
1	ingob	513	CHK_POTI(Parameter_ServoNickControl,EE_Parameter.ServoNickControl,0,255);
173	holgerb	514	CHK_POTI(Parameter_LoopGasLimit,EE_Parameter.LoopGasLimit,0,255);
395	hbuss	515	CHK_POTI(Parameter_AchsKopplung1, EE_Parameter.AchsKopplung1,0,255);
1120	hbuss	516	CHK_POTI(Parameter_AchsKopplung2, EE_Parameter.AchsKopplung2,0,255);
		517	CHK_POTI(Parameter_CouplingYawCorrection,EE_Parameter.CouplingYawCorrection,0,255);
		518
		519	// CHK_POTI(Parameter_AchsGegenKopplung1,EE_Parameter.AchsGegenKopplung1,0,255);
499	hbuss	520	CHK_POTI(Parameter_DynamicStability,EE_Parameter.DynamicStability,0,255);
921	hbuss	521	CHK_POTI_MM(Parameter_J16Timing,EE_Parameter.J16Timing,1,255);
		522	CHK_POTI_MM(Parameter_J17Timing,EE_Parameter.J17Timing,1,255);
993	hbuss	523
		524	// CHK_POTI(Parameter_NaviGpsModeControl,EE_Parameter.NaviGpsModeControl,0,255);
		525	//CHK_POTI(Parameter_NaviGpsGain,EE_Parameter.NaviGpsGain,0,255);
		526	// CHK_POTI(Parameter_NaviGpsP,EE_Parameter.NaviGpsP,0,255);
		527	// CHK_POTI(Parameter_NaviGpsI,EE_Parameter.NaviGpsI,0,255);
		528	// CHK_POTI(Parameter_NaviGpsD,EE_Parameter.NaviGpsD,0,255);
		529	// CHK_POTI(Parameter_NaviGpsACC,EE_Parameter.NaviGpsACC,0,255);
		530	// CHK_POTI_MM(Parameter_NaviOperatingRadius,EE_Parameter.NaviOperatingRadius,10,255);
		531	// CHK_POTI(Parameter_NaviWindCorrection,EE_Parameter.NaviWindCorrection,0,255);
		532	// CHK_POTI(Parameter_NaviSpeedCompensation,EE_Parameter.NaviSpeedCompensation,0,255);
		533
921	hbuss	534	CHK_POTI(Parameter_ExternalControl,EE_Parameter.ExternalControl,0,255);
1	ingob	535
1153	hbuss	536	//Ki = (float) Parameter_I_Faktor * 0.0001;
		537	Ki = 10300 / (Parameter_I_Faktor + 1);
1	ingob	538	MAX_GAS = EE_Parameter.Gas_Max;
		539	MIN_GAS = EE_Parameter.Gas_Min;
		540	}
		541
		542
819	hbuss	543
1	ingob	544	//############################################################################
		545	//
		546	void MotorRegler(void)
		547	//############################################################################
		548	{
1153	hbuss	549	int motorwert,pd_ergebnis, pd_ergebnis_nick,pd_ergebnis_roll,h,tmp_int;
1	ingob	550	int GierMischanteil,GasMischanteil;
		551	static long SummeNick=0,SummeRoll=0;
		552	static long sollGier = 0,tmp_long,tmp_long2;
395	hbuss	553	static long IntegralFehlerNick = 0;
		554	static long IntegralFehlerRoll = 0;
1	ingob	555	static unsigned int RcLostTimer;
		556	static unsigned char delay_neutral = 0;
		557	static unsigned char delay_einschalten = 0,delay_ausschalten = 0;
		558	static int hoehenregler = 0;
		559	static char TimerWerteausgabe = 0;
		560	static char NeueKompassRichtungMerken = 0;
395	hbuss	561	static long ausgleichNick, ausgleichRoll;
1153	hbuss	562	int IntegralNickMalFaktor,IntegralRollMalFaktor;
1051	killagreg	563	Mittelwert();
		564
1	ingob	565	GRN_ON;
1051	killagreg	566	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1	ingob	567	// Gaswert ermitteln
1051	killagreg	568	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		569	GasMischanteil = StickGas;
831	hbuss	570	if(GasMischanteil < MIN_GAS + 10) GasMischanteil = MIN_GAS + 10;
1051	killagreg	571	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
693	hbuss	572	// Empfang schlecht
1051	killagreg	573	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1	ingob	574	if(SenderOkay < 100)
		575	{
1051	killagreg	576	if(!PcZugriff)
173	holgerb	577	{
1051	killagreg	578	if(BeepMuster == 0xffff)
		579	{
173	holgerb	580	beeptime = 15000;
		581	BeepMuster = 0x0c00;
1051	killagreg	582	}
173	holgerb	583	}
1051	killagreg	584	if(RcLostTimer) RcLostTimer--;
		585	else
1	ingob	586	{
		587	MotorenEin = 0;
		588	Notlandung = 0;
1051	killagreg	589	}
1	ingob	590	ROT_ON;
693	hbuss	591	if(modell_fliegt > 1000) // wahrscheinlich in der Luft --> langsam absenken
1	ingob	592	{
		593	GasMischanteil = EE_Parameter.NotGas;
		594	Notlandung = 1;
744	hbuss	595	PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] = 0;
1051	killagreg	596	PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] = 0;
1	ingob	597	PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] = 0;
		598	PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] = 0;
		599	PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] = 0;
173	holgerb	600	}
1	ingob	601	else MotorenEin = 0;
		602	}
1051	killagreg	603	else
		604	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1	ingob	605	// Emfang gut
1051	killagreg	606	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1	ingob	607	if(SenderOkay > 140)
		608	{
		609	Notlandung = 0;
		610	RcLostTimer = EE_Parameter.NotGasZeit * 50;
921	hbuss	611	if(GasMischanteil > 40 && MotorenEin)
1	ingob	612	{
		613	if(modell_fliegt < 0xffff) modell_fliegt++;
		614	}
871	hbuss	615	if((modell_fliegt < 256))
1	ingob	616	{
		617	SummeNick = 0;
		618	SummeRoll = 0;
1051	killagreg	619	if(modell_fliegt == 250)
918	hbuss	620	{
1051	killagreg	621	NeueKompassRichtungMerken = 1;
918	hbuss	622	sollGier = 0;
1051	killagreg	623	Mess_Integral_Gier = 0;
927	hbuss	624	// Mess_Integral_Gier2 = 0;
1051	killagreg	625	}
921	hbuss	626	} else MikroKopterFlags \|= FLAG_FLY;
1051	killagreg	627
595	hbuss	628	if((PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] > 80) && MotorenEin == 0)
1	ingob	629	{
1051	killagreg	630	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1	ingob	631	// auf Nullwerte kalibrieren
1051	killagreg	632	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1	ingob	633	if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75) // Neutralwerte
		634	{
		635	if(++delay_neutral > 200) // nicht sofort
		636	{
		637	GRN_OFF;
		638	MotorenEin = 0;
		639	delay_neutral = 0;
		640	modell_fliegt = 0;
		641	if(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70 \|\| abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) > 70)
		642	{
304	ingob	643	unsigned char setting=1;
1	ingob	644	if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < 70) setting = 1;
		645	if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 2;
		646	if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 3;
		647	if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] <-70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 4;
		648	if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] <-70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < 70) setting = 5;
1063	killagreg	649	SetActiveParamSetNumber(setting); // aktiven Datensatz merken
1	ingob	650	}
820	hbuss	651	// else
1051	killagreg	652	if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) < 30 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < -70)
		653	{
819	hbuss	654	WinkelOut.CalcState = 1;
		655	beeptime = 1000;
		656	}
		657	else
1	ingob	658	{
819	hbuss	659	ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE);
		660	if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // H�henregelung aktiviert?
		661	{
1	ingob	662	if((MessLuftdruck > 950) \|\| (MessLuftdruck < 750)) SucheLuftruckOffset();
1051	killagreg	663	}
819	hbuss	664	SetNeutral();
		665	Piep(GetActiveParamSetNumber());
		666	}
1051	killagreg	667	}
1	ingob	668	}
1051	killagreg	669	else
513	hbuss	670	if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75) // ACC Neutralwerte speichern
		671	{
		672	if(++delay_neutral > 200) // nicht sofort
		673	{
		674	GRN_OFF;
		675	eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_NICK],0xff); // Werte l�schen
		676	MotorenEin = 0;
		677	delay_neutral = 0;
		678	modell_fliegt = 0;
		679	SetNeutral();
		680	eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_NICK],NeutralAccX / 256); // ACC-NeutralWerte speichern
		681	eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_NICK+1],NeutralAccX % 256); // ACC-NeutralWerte speichern
		682	eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL],NeutralAccY / 256);
		683	eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL+1],NeutralAccY % 256);
		684	eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_Z],(int)NeutralAccZ / 256);
		685	eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_Z+1],(int)NeutralAccZ % 256);
		686	Piep(GetActiveParamSetNumber());
1051	killagreg	687	}
513	hbuss	688	}
1	ingob	689	else delay_neutral = 0;
		690	}
1051	killagreg	691	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1	ingob	692	// Gas ist unten
1051	killagreg	693	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
595	hbuss	694	if(PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] < 35-120)
1	ingob	695	{
		696	// Starten
		697	if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75)
		698	{
1051	killagreg	699	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1	ingob	700	// Einschalten
1051	killagreg	701	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1	ingob	702	if(++delay_einschalten > 200)
		703	{
		704	delay_einschalten = 200;
		705	modell_fliegt = 1;
		706	MotorenEin = 1;
		707	sollGier = 0;
1051	killagreg	708	Mess_Integral_Gier = 0;
1	ingob	709	Mess_Integral_Gier2 = 0;
		710	Mess_IntegralNick = 0;
		711	Mess_IntegralRoll = 0;
		712	Mess_IntegralNick2 = IntegralNick;
		713	Mess_IntegralRoll2 = IntegralRoll;
		714	SummeNick = 0;
		715	SummeRoll = 0;
921	hbuss	716	MikroKopterFlags \|= FLAG_START;
1051	killagreg	717	}
		718	}
1	ingob	719	else delay_einschalten = 0;
		720	//Auf Neutralwerte setzen
1051	killagreg	721	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1	ingob	722	// Auschalten
1051	killagreg	723	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1	ingob	724	if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75)
		725	{
		726	if(++delay_ausschalten > 200) // nicht sofort
		727	{
		728	MotorenEin = 0;
		729	delay_ausschalten = 200;
		730	modell_fliegt = 0;
1051	killagreg	731	}
1	ingob	732	}
		733	else delay_ausschalten = 0;
		734	}
		735	}
		736
1051	killagreg	737	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1	ingob	738	// neue Werte von der Funke
1051	killagreg	739	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1153	hbuss	740
1051	killagreg	741	if(!NewPpmData-- \|\| Notlandung)
1	ingob	742	{
492	hbuss	743	int tmp_int;
604	hbuss	744	static int stick_nick,stick_roll;
1	ingob	745	ParameterZuordnung();
1051	killagreg	746	stick_nick = (stick_nick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P) / 4;
723	hbuss	747	stick_nick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D;
805	hbuss	748	StickNick = stick_nick - (GPS_Nick + GPS_Nick2);
604	hbuss	749
723	hbuss	750	stick_roll = (stick_roll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P) / 4;
		751	stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D;
805	hbuss	752	StickRoll = stick_roll - (GPS_Roll + GPS_Roll2);
723	hbuss	753
1	ingob	754	StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]];
1120	hbuss	755	if(StickGier > 2) StickGier -= 2; else
		756	if(StickGier < -2) StickGier += 2; else StickGier = 0;
		757
595	hbuss	758	StickGas = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120;
		759
1051	killagreg	760	/* if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]]) > MaxStickNick)
614	hbuss	761	MaxStickNick = abs(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]]); else MaxStickNick--;
1051	killagreg	762	if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) > MaxStickRoll)
614	hbuss	763	MaxStickRoll = abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]); else MaxStickRoll--;
723	hbuss	764	*/
1153	hbuss	765	// GyroFaktor = ((float)Parameter_Gyro_P + 10.0) / (256.0/STICK_GAIN);
		766	GyroFaktor = (Parameter_Gyro_P + 10.0);
		767	// IntegralFaktor = ((float) Parameter_Gyro_I) / (44000 / STICK_GAIN);
		768	IntegralFaktor = Parameter_Gyro_I;
1	ingob	769
595	hbuss	770	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		771	//+ Analoge Steuerung per Seriell
		772	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
921	hbuss	773	if(ExternControl.Config & 0x01 && Parameter_ExternalControl > 128)
595	hbuss	774	{
		775	StickNick += (int) ExternControl.Nick * (int) EE_Parameter.Stick_P;
		776	StickRoll += (int) ExternControl.Roll * (int) EE_Parameter.Stick_P;
		777	StickGier += ExternControl.Gier;
		778	ExternHoehenValue = (int) ExternControl.Hight * (int)EE_Parameter.Hoehe_Verstaerkung;
		779	if(ExternControl.Gas < StickGas) StickGas = ExternControl.Gas;
		780	}
855	hbuss	781	if(StickGas < 0) StickGas = 0;
492	hbuss	782
1	ingob	783	if(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor = 0;
		784	if(GyroFaktor < 0) GyroFaktor = 0;
		785	if(IntegralFaktor < 0) IntegralFaktor = 0;
723	hbuss	786
1051	killagreg	787	if(abs(StickNick/STICK_GAIN) > MaxStickNick)
928	hbuss	788	{
1051	killagreg	789	MaxStickNick = abs(StickNick)/STICK_GAIN;
928	hbuss	790	if(MaxStickNick > 100) MaxStickNick = 100;
1051	killagreg	791	}
928	hbuss	792	else MaxStickNick--;
1051	killagreg	793	if(abs(StickRoll/STICK_GAIN) > MaxStickRoll)
928	hbuss	794	{
1051	killagreg	795	MaxStickRoll = abs(StickRoll)/STICK_GAIN;
928	hbuss	796	if(MaxStickRoll > 100) MaxStickRoll = 100;
1051	killagreg	797	}
928	hbuss	798	else MaxStickRoll--;
723	hbuss	799	if(Notlandung) {MaxStickNick = 0; MaxStickRoll = 0;}
		800
1051	killagreg	801	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
173	holgerb	802	// Looping?
1051	killagreg	803	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
993	hbuss	804	if((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > EE_Parameter.LoopThreshold) && EE_Parameter.BitConfig & CFG_LOOP_LINKS) Looping_Links = 1;
1051	killagreg	805	else
		806	{
395	hbuss	807	{
1051	killagreg	808	if((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < (EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese))) Looping_Links = 0;
		809	}
		810	}
993	hbuss	811	if((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < -EE_Parameter.LoopThreshold) && EE_Parameter.BitConfig & CFG_LOOP_RECHTS) Looping_Rechts = 1;
1051	killagreg	812	else
395	hbuss	813	{
		814	if(Looping_Rechts) // Hysterese
		815	{
		816	if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > -(EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese)) Looping_Rechts = 0;
		817	}
1051	killagreg	818	}
173	holgerb	819
993	hbuss	820	if((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > EE_Parameter.LoopThreshold) && EE_Parameter.BitConfig & CFG_LOOP_OBEN) Looping_Oben = 1;
1051	killagreg	821	else
		822	{
395	hbuss	823	if(Looping_Oben) // Hysterese
		824	{
1051	killagreg	825	if((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < (EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese))) Looping_Oben = 0;
		826	}
		827	}
993	hbuss	828	if((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < -EE_Parameter.LoopThreshold) && EE_Parameter.BitConfig & CFG_LOOP_UNTEN) Looping_Unten = 1;
1051	killagreg	829	else
395	hbuss	830	{
		831	if(Looping_Unten) // Hysterese
		832	{
		833	if(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > -(EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese)) Looping_Unten = 0;
		834	}
1051	killagreg	835	}
395	hbuss	836
		837	if(Looping_Links \|\| Looping_Rechts) Looping_Roll = 1; else Looping_Roll = 0;
1153	hbuss	838	if(Looping_Oben \|\| Looping_Unten) { Looping_Nick = 1; Looping_Roll = 0; Looping_Links = 0; Looping_Rechts = 0;} else Looping_Nick = 0;
395	hbuss	839	} // Ende neue Funken-Werte
		840
		841	if(Looping_Roll \|\| Looping_Nick)
		842	{
173	holgerb	843	if(GasMischanteil > EE_Parameter.LoopGasLimit) GasMischanteil = EE_Parameter.LoopGasLimit;
1153	hbuss	844	TrichterFlug = 1;
173	holgerb	845	}
		846
1051	killagreg	847
		848	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		849	// Bei Empfangsausfall im Flug
		850	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1	ingob	851	if(Notlandung)
		852	{
		853	StickGier = 0;
		854	StickNick = 0;
		855	StickRoll = 0;
1153	hbuss	856	GyroFaktor = 90;//(float) 100 / (256.0 / STICK_GAIN);
		857	IntegralFaktor = 120;//(float) 120 / (44000 / STICK_GAIN);
173	holgerb	858	Looping_Roll = 0;
		859	Looping_Nick = 0;
1051	killagreg	860	}
395	hbuss	861
		862
1051	killagreg	863	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1	ingob	864	// Integrale auf ACC-Signal abgleichen
1051	killagreg	865	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
395	hbuss	866	#define ABGLEICH_ANZAHL 256L
		867
		868	MittelIntegralNick += IntegralNick; // F�r die Mittelwertbildung aufsummieren
		869	MittelIntegralRoll += IntegralRoll;
		870	MittelIntegralNick2 += IntegralNick2;
		871	MittelIntegralRoll2 += IntegralRoll2;
		872
		873	if(Looping_Nick \|\| Looping_Roll)
		874	{
		875	IntegralAccNick = 0;
		876	IntegralAccRoll = 0;
		877	MittelIntegralNick = 0;
		878	MittelIntegralRoll = 0;
		879	MittelIntegralNick2 = 0;
		880	MittelIntegralRoll2 = 0;
		881	Mess_IntegralNick2 = Mess_IntegralNick;
		882	Mess_IntegralRoll2 = Mess_IntegralRoll;
		883	ZaehlMessungen = 0;
498	hbuss	884	LageKorrekturNick = 0;
		885	LageKorrekturRoll = 0;
395	hbuss	886	}
		887
1051	killagreg	888	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
498	hbuss	889	if(!Looping_Nick && !Looping_Roll)
469	hbuss	890	{
		891	long tmp_long, tmp_long2;
1120	hbuss	892	if(FromNaviCtrl_Value.Kalman_K != -1 && !TrichterFlug)
992	hbuss	893	{
		894	tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccNick);
1051	killagreg	895	tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccRoll);
		896	tmp_long = (tmp_long * FromNaviCtrl_Value.Kalman_K) / (32 * 16);
		897	tmp_long2 = (tmp_long2 * FromNaviCtrl_Value.Kalman_K) / (32 * 16);
992	hbuss	898	if((MaxStickNick > 64) \|\| (MaxStickRoll > 64))
		899	{
		900	tmp_long /= 2;
		901	tmp_long2 /= 2;
		902	}
		903	if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25)
		904	{
		905	tmp_long /= 3;
		906	tmp_long2 /= 3;
		907	}
		908	if(tmp_long > (long) FromNaviCtrl_Value.Kalman_MaxFusion) tmp_long = (long) FromNaviCtrl_Value.Kalman_MaxFusion;
		909	if(tmp_long < (long)-FromNaviCtrl_Value.Kalman_MaxFusion) tmp_long = (long)-FromNaviCtrl_Value.Kalman_MaxFusion;
		910	if(tmp_long2 > (long) FromNaviCtrl_Value.Kalman_MaxFusion) tmp_long2 = (long) FromNaviCtrl_Value.Kalman_MaxFusion;
		911	if(tmp_long2 < (long)-FromNaviCtrl_Value.Kalman_MaxFusion) tmp_long2 = (long)-FromNaviCtrl_Value.Kalman_MaxFusion;
		912	}
1051	killagreg	913	else
992	hbuss	914	{
		915	tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccNick);
1051	killagreg	916	tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccRoll);
992	hbuss	917	tmp_long /= 16;
		918	tmp_long2 /= 16;
		919	if((MaxStickNick > 64) \|\| (MaxStickRoll > 64))
		920	{
		921	tmp_long /= 3;
		922	tmp_long2 /= 3;
		923	}
1120	hbuss	924	if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25)
992	hbuss	925	{
		926	tmp_long /= 3;
		927	tmp_long2 /= 3;
		928	}
614	hbuss	929	#define AUSGLEICH 32
992	hbuss	930	if(tmp_long > AUSGLEICH) tmp_long = AUSGLEICH;
		931	if(tmp_long < -AUSGLEICH) tmp_long =-AUSGLEICH;
		932	if(tmp_long2 > AUSGLEICH) tmp_long2 = AUSGLEICH;
		933	if(tmp_long2 <-AUSGLEICH) tmp_long2 =-AUSGLEICH;
		934	}
1111	hbuss	935	Mess_IntegralNick -= tmp_long;
		936	Mess_IntegralRoll -= tmp_long2;
469	hbuss	937	}
1051	killagreg	938	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
395	hbuss	939	if(ZaehlMessungen >= ABGLEICH_ANZAHL)
		940	{
		941	static int cnt = 0;
		942	static char last_n_p,last_n_n,last_r_p,last_r_n;
		943	static long MittelIntegralNick_Alt,MittelIntegralRoll_Alt;
1111	hbuss	944	if(!Looping_Nick && !Looping_Roll && !TrichterFlug && (PlatinenVersion < 20))
173	holgerb	945	{
395	hbuss	946	MittelIntegralNick /= ABGLEICH_ANZAHL;
		947	MittelIntegralRoll /= ABGLEICH_ANZAHL;
		948	IntegralAccNick = (EE_Parameter.GyroAccFaktor * IntegralAccNick) / ABGLEICH_ANZAHL;
		949	IntegralAccRoll = (EE_Parameter.GyroAccFaktor * IntegralAccRoll) / ABGLEICH_ANZAHL;
		950	IntegralAccZ = IntegralAccZ / ABGLEICH_ANZAHL;
492	hbuss	951	#define MAX_I 0//(Poti2/10)
395	hbuss	952	// Nick ++++++++++++++++++++++++++++++++++++++++++++++++
		953	IntegralFehlerNick = (long)(MittelIntegralNick - (long)IntegralAccNick);
		954	ausgleichNick = IntegralFehlerNick / EE_Parameter.GyroAccAbgleich;
1051	killagreg	955	// Roll ++++++++++++++++++++++++++++++++++++++++++++++++
		956	IntegralFehlerRoll = (long)(MittelIntegralRoll - (long)IntegralAccRoll);
395	hbuss	957	ausgleichRoll = IntegralFehlerRoll / EE_Parameter.GyroAccAbgleich;
614	hbuss	958
		959	LageKorrekturNick = ausgleichNick / ABGLEICH_ANZAHL;
498	hbuss	960	LageKorrekturRoll = ausgleichRoll / ABGLEICH_ANZAHL;
395	hbuss	961
992	hbuss	962	if(((MaxStickNick > 64) \|\| (MaxStickRoll > 64) \|\| (abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25)) && (FromNaviCtrl_Value.Kalman_K == -1))
614	hbuss	963	{
		964	LageKorrekturNick /= 2;
720	ingob	965	LageKorrekturRoll /= 2;
614	hbuss	966	}
498	hbuss	967
1051	killagreg	968	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
395	hbuss	969	// Gyro-Drift ermitteln
1051	killagreg	970	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
395	hbuss	971	MittelIntegralNick2 /= ABGLEICH_ANZAHL;
		972	MittelIntegralRoll2 /= ABGLEICH_ANZAHL;
1051	killagreg	973	tmp_long = IntegralNick2 - IntegralNick;
		974	tmp_long2 = IntegralRoll2 - IntegralRoll;
395	hbuss	975	//DebugOut.Analog[25] = MittelIntegralRoll2 / 26;
		976
		977	IntegralFehlerNick = tmp_long;
		978	IntegralFehlerRoll = tmp_long2;
		979	Mess_IntegralNick2 -= IntegralFehlerNick;
		980	Mess_IntegralRoll2 -= IntegralFehlerRoll;
		981
		982	// IntegralFehlerNick = (IntegralFehlerNick * 1 + tmp_long) / 2;
		983	// IntegralFehlerRoll = (IntegralFehlerRoll * 1 + tmp_long2) / 2;
1111	hbuss	984	if(EE_Parameter.Driftkomp)
		985	{
927	hbuss	986	if(GierGyroFehler > ABGLEICH_ANZAHL/2) { AdNeutralGier++; AdNeutralGierBias++; }
		987	if(GierGyroFehler <-ABGLEICH_ANZAHL/2) { AdNeutralGier--; AdNeutralGierBias--; }
1111	hbuss	988	}
1153	hbuss	989	//DebugOut.Analog[22] = MittelIntegralRoll / 26;
921	hbuss	990	//DebugOut.Analog[24] = GierGyroFehler;
693	hbuss	991	GierGyroFehler = 0;
720	ingob	992
		993
		994	/*DebugOut.Analog[17] = IntegralAccNick / 26;
395	hbuss	995	DebugOut.Analog[18] = IntegralAccRoll / 26;
		996	DebugOut.Analog[19] = IntegralFehlerNick;// / 26;
		997	DebugOut.Analog[20] = IntegralFehlerRoll;// / 26;
720	ingob	998	*/
		999	//DebugOut.Analog[21] = MittelIntegralNick / 26;
		1000	//MittelIntegralRoll = MittelIntegralRoll;
469	hbuss	1001	//DebugOut.Analog[28] = ausgleichNick;
720	ingob	1002	/*
395	hbuss	1003	DebugOut.Analog[29] = ausgleichRoll;
498	hbuss	1004	DebugOut.Analog[30] = LageKorrekturRoll * 10;
720	ingob	1005	*/
395	hbuss	1006
492	hbuss	1007	#define FEHLER_LIMIT (ABGLEICH_ANZAHL * 4)
		1008	#define FEHLER_LIMIT2 (ABGLEICH_ANZAHL * 16)
		1009	#define BEWEGUNGS_LIMIT 20000
395	hbuss	1010	// Nick +++++++++++++++++++++++++++++++++++++++++++++++++
		1011	cnt = 1;// + labs(IntegralFehlerNick) / 4096;
992	hbuss	1012	if(labs(MittelIntegralNick_Alt - MittelIntegralNick) < BEWEGUNGS_LIMIT \|\| (FromNaviCtrl_Value.Kalman_MaxDrift > 3*16))
492	hbuss	1013	{
1051	killagreg	1014	if(IntegralFehlerNick > FEHLER_LIMIT2)
395	hbuss	1015	{
1051	killagreg	1016	if(last_n_p)
395	hbuss	1017	{
1051	killagreg	1018	cnt += labs(IntegralFehlerNick) / FEHLER_LIMIT2;
492	hbuss	1019	ausgleichNick = IntegralFehlerNick / 8;
401	hbuss	1020	if(ausgleichNick > 5000) ausgleichNick = 5000;
498	hbuss	1021	LageKorrekturNick += ausgleichNick / ABGLEICH_ANZAHL;
1051	killagreg	1022	}
395	hbuss	1023	else last_n_p = 1;
		1024	} else last_n_p = 0;
1051	killagreg	1025	if(IntegralFehlerNick < -FEHLER_LIMIT2)
395	hbuss	1026	{
		1027	if(last_n_n)
1051	killagreg	1028	{
		1029	cnt += labs(IntegralFehlerNick) / FEHLER_LIMIT2;
492	hbuss	1030	ausgleichNick = IntegralFehlerNick / 8;
401	hbuss	1031	if(ausgleichNick < -5000) ausgleichNick = -5000;
498	hbuss	1032	LageKorrekturNick += ausgleichNick / ABGLEICH_ANZAHL;
1051	killagreg	1033	}
395	hbuss	1034	else last_n_n = 1;
		1035	} else last_n_n = 0;
1051	killagreg	1036	}
		1037	else
847	hbuss	1038	{
		1039	cnt = 0;
921	hbuss	1040	KompassSignalSchlecht = 1000;
1051	killagreg	1041	}
499	hbuss	1042	if(cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp;
992	hbuss	1043	if(cnt * 16 > FromNaviCtrl_Value.Kalman_MaxDrift) cnt = FromNaviCtrl_Value.Kalman_MaxDrift/16;
395	hbuss	1044	if(IntegralFehlerNick > FEHLER_LIMIT) AdNeutralNick += cnt;
		1045	if(IntegralFehlerNick < -FEHLER_LIMIT) AdNeutralNick -= cnt;
401	hbuss	1046
395	hbuss	1047	// Roll +++++++++++++++++++++++++++++++++++++++++++++++++
492	hbuss	1048	cnt = 1;// + labs(IntegralFehlerNick) / 4096;
		1049
498	hbuss	1050	ausgleichRoll = 0;
992	hbuss	1051	if(labs(MittelIntegralRoll_Alt - MittelIntegralRoll) < BEWEGUNGS_LIMIT \|\| (FromNaviCtrl_Value.Kalman_MaxDrift > 3*16))
492	hbuss	1052	{
1051	killagreg	1053	if(IntegralFehlerRoll > FEHLER_LIMIT2)
395	hbuss	1054	{
1051	killagreg	1055	if(last_r_p)
395	hbuss	1056	{
1051	killagreg	1057	cnt += labs(IntegralFehlerRoll) / FEHLER_LIMIT2;
492	hbuss	1058	ausgleichRoll = IntegralFehlerRoll / 8;
401	hbuss	1059	if(ausgleichRoll > 5000) ausgleichRoll = 5000;
498	hbuss	1060	LageKorrekturRoll += ausgleichRoll / ABGLEICH_ANZAHL;
1051	killagreg	1061	}
395	hbuss	1062	else last_r_p = 1;
		1063	} else last_r_p = 0;
1051	killagreg	1064	if(IntegralFehlerRoll < -FEHLER_LIMIT2)
395	hbuss	1065	{
1051	killagreg	1066	if(last_r_n)
395	hbuss	1067	{
1051	killagreg	1068	cnt += labs(IntegralFehlerRoll) / FEHLER_LIMIT2;
492	hbuss	1069	ausgleichRoll = IntegralFehlerRoll / 8;
401	hbuss	1070	if(ausgleichRoll < -5000) ausgleichRoll = -5000;
498	hbuss	1071	LageKorrekturRoll += ausgleichRoll / ABGLEICH_ANZAHL;
395	hbuss	1072	}
		1073	else last_r_n = 1;
		1074	} else last_r_n = 0;
1051	killagreg	1075	} else
492	hbuss	1076	{
		1077	cnt = 0;
921	hbuss	1078	KompassSignalSchlecht = 1000;
1051	killagreg	1079	}
499	hbuss	1080	if(cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp;
992	hbuss	1081	if(cnt * 16 > FromNaviCtrl_Value.Kalman_MaxDrift) cnt = FromNaviCtrl_Value.Kalman_MaxDrift/16;
395	hbuss	1082	if(IntegralFehlerRoll > FEHLER_LIMIT) AdNeutralRoll += cnt;
		1083	if(IntegralFehlerRoll < -FEHLER_LIMIT) AdNeutralRoll -= cnt;
173	holgerb	1084	}
1051	killagreg	1085	else
498	hbuss	1086	{
		1087	LageKorrekturRoll = 0;
		1088	LageKorrekturNick = 0;
880	hbuss	1089	TrichterFlug = 0;
498	hbuss	1090	}
1051	killagreg	1091
498	hbuss	1092	if(!IntegralFaktor) { LageKorrekturRoll = 0; LageKorrekturNick = 0;} // z.B. bei HH
1051	killagreg	1093	// +++++++++++++++++++++++++++++++++++++++++++++++++++++
		1094	MittelIntegralNick_Alt = MittelIntegralNick;
		1095	MittelIntegralRoll_Alt = MittelIntegralRoll;
		1096	// +++++++++++++++++++++++++++++++++++++++++++++++++++++
395	hbuss	1097	IntegralAccNick = 0;
		1098	IntegralAccRoll = 0;
		1099	IntegralAccZ = 0;
		1100	MittelIntegralNick = 0;
		1101	MittelIntegralRoll = 0;
		1102	MittelIntegralNick2 = 0;
		1103	MittelIntegralRoll2 = 0;
		1104	ZaehlMessungen = 0;
		1105	}
469	hbuss	1106	//DebugOut.Analog[31] = StickRoll / (26*IntegralFaktor);
395	hbuss	1107
1051	killagreg	1108	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1	ingob	1109	// Gieren
1051	killagreg	1110	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
693	hbuss	1111	// if(GasMischanteil < 35) { if(StickGier > 10) StickGier = 10; else if(StickGier < -10) StickGier = -10;};
1051	killagreg	1112	if(abs(StickGier) > 15) // war 35
1	ingob	1113	{
921	hbuss	1114	KompassSignalSchlecht = 1000;
1051	killagreg	1115	if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX))
		1116	{
		1117	NeueKompassRichtungMerken = 1;
824	hbuss	1118	};
1	ingob	1119	}
395	hbuss	1120	tmp_int = (long) EE_Parameter.Gier_P * ((long)StickGier * abs(StickGier)) / 512L; // expo y = ax + bx�
1051	killagreg	1121	tmp_int += (EE_Parameter.Gier_P * StickGier) / 4;
173	holgerb	1122	sollGier = tmp_int;
1051	killagreg	1123	Mess_Integral_Gier -= tmp_int;
395	hbuss	1124	if(Mess_Integral_Gier > 50000) Mess_Integral_Gier = 50000; // begrenzen
		1125	if(Mess_Integral_Gier <-50000) Mess_Integral_Gier =-50000;
1051	killagreg	1126
		1127	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1	ingob	1128	// Kompass
1051	killagreg	1129	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
846	hbuss	1130	//DebugOut.Analog[29] = (MaxStickNick + MaxStickRoll);
819	hbuss	1131
1051	killagreg	1132	if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV))
1	ingob	1133	{
819	hbuss	1134	int w,v,r,fehler,korrektur;
1	ingob	1135	w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln
		1136	v = abs(IntegralRoll /512);
		1137	if(v > w) w = v; // gr�sste Neigung ermitteln
824	hbuss	1138	korrektur = w / 8 + 1;
921	hbuss	1139	fehler = ((540 + KompassValue - (ErsatzKompass/GIER_GRAD_FAKTOR)) % 360) - 180;
1051	killagreg	1140	if(NeueKompassRichtungMerken)
1036	hbuss	1141	{
		1142	fehler = 0;
		1143	ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR;
1051	killagreg	1144	}
921	hbuss	1145	if(!KompassSignalSchlecht && w < 25)
		1146	{
		1147	GierGyroFehler += fehler;
1051	killagreg	1148	if(NeueKompassRichtungMerken)
		1149	{
921	hbuss	1150	beeptime = 200;
847	hbuss	1151	// KompassStartwert = KompassValue;
921	hbuss	1152	KompassStartwert = (ErsatzKompass/GIER_GRAD_FAKTOR);
		1153	NeueKompassRichtungMerken = 0;
		1154	}
1	ingob	1155	}
824	hbuss	1156	ErsatzKompass += (fehler * 8) / korrektur;
921	hbuss	1157	w = (w * Parameter_KompassWirkung) / 32; // auf die Wirkung normieren
1	ingob	1158	w = Parameter_KompassWirkung - w; // Wirkung ggf drosseln
921	hbuss	1159	if(w >= 0)
1	ingob	1160	{
1051	killagreg	1161	if(!KompassSignalSchlecht)
693	hbuss	1162	{
1051	killagreg	1163	v = 64 + ((MaxStickNick + MaxStickRoll)) / 8;
847	hbuss	1164	r = ((540 + (ErsatzKompass/GIER_GRAD_FAKTOR) - KompassStartwert) % 360) - 180;
		1165	// r = KompassRichtung;
819	hbuss	1166	v = (r * w) / v; // nach Kompass ausrichten
		1167	w = 3 * Parameter_KompassWirkung;
693	hbuss	1168	if(v > w) v = w; // Begrenzen
1051	killagreg	1169	else
693	hbuss	1170	if(v < -w) v = -w;
		1171	Mess_Integral_Gier += v;
1051	killagreg	1172	}
693	hbuss	1173	if(KompassSignalSchlecht) KompassSignalSchlecht--;
1051	killagreg	1174	}
921	hbuss	1175	else KompassSignalSchlecht = 500; // so lange das Signal taub stellen --> ca. 1 sek
1051	killagreg	1176	}
		1177	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1	ingob	1178
1051	killagreg	1179	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1	ingob	1180	// Debugwerte zuordnen
1051	killagreg	1181	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1	ingob	1182	if(!TimerWerteausgabe--)
		1183	{
395	hbuss	1184	TimerWerteausgabe = 24;
805	hbuss	1185
1	ingob	1186	DebugOut.Analog[0] = IntegralNick / EE_Parameter.GyroAccFaktor;
		1187	DebugOut.Analog[1] = IntegralRoll / EE_Parameter.GyroAccFaktor;
		1188	DebugOut.Analog[2] = Mittelwert_AccNick;
		1189	DebugOut.Analog[3] = Mittelwert_AccRoll;
		1190	DebugOut.Analog[4] = MesswertGier;
		1191	DebugOut.Analog[5] = HoehenWert;
1111	hbuss	1192	DebugOut.Analog[6] = Aktuell_az;//(Mess_Integral_Hoch / 512);
1	ingob	1193	DebugOut.Analog[8] = KompassValue;
297	holgerb	1194	DebugOut.Analog[9] = UBat;
1051	killagreg	1195	DebugOut.Analog[11] = ErsatzKompass / GIER_GRAD_FAKTOR;
297	holgerb	1196	DebugOut.Analog[10] = SenderOkay;
929	hbuss	1197	//DebugOut.Analog[16] = Mittelwert_AccHoch;
1083	killagreg	1198	//DebugOut.Analog[17] = FromNaviCtrl_Value.Distance;
		1199	//DebugOut.Analog[18] = (int)FromNaviCtrl_Value.OsdBar;
819	hbuss	1200	DebugOut.Analog[19] = WinkelOut.CalcState;
854	hbuss	1201	DebugOut.Analog[20] = ServoValue;
1111	hbuss	1202	DebugOut.Analog[24] = MesswertNick/2;
		1203	DebugOut.Analog[25] = MesswertRoll/2;
992	hbuss	1204	DebugOut.Analog[27] = (int)FromNaviCtrl_Value.Kalman_MaxDrift;
		1205	DebugOut.Analog[29] = (int)FromNaviCtrl_Value.Kalman_K;
720	ingob	1206	DebugOut.Analog[30] = GPS_Nick;
		1207	DebugOut.Analog[31] = GPS_Roll;
		1208
805	hbuss	1209
		1210	// DebugOut.Analog[19] -= DebugOut.Analog[19]/128;
		1211	// if(DebugOut.Analog[19] > 0) DebugOut.Analog[19]--; else DebugOut.Analog[19]++;
		1212
173	holgerb	1213	/* DebugOut.Analog[16] = motor_rx[0];
		1214	DebugOut.Analog[17] = motor_rx[1];
		1215	DebugOut.Analog[18] = motor_rx[2];
		1216	DebugOut.Analog[19] = motor_rx[3];
		1217	DebugOut.Analog[20] = motor_rx[0] + motor_rx[1] + motor_rx[2] + motor_rx[3];
		1218	DebugOut.Analog[20] /= 14;
		1219	DebugOut.Analog[21] = motor_rx[4];
		1220	DebugOut.Analog[22] = motor_rx[5];
		1221	DebugOut.Analog[23] = motor_rx[6];
		1222	DebugOut.Analog[24] = motor_rx[7];
		1223	DebugOut.Analog[25] = motor_rx[4] + motor_rx[5] + motor_rx[6] + motor_rx[7];
		1224	*/
		1225	// DebugOut.Analog[9] = MesswertNick;
1	ingob	1226	// DebugOut.Analog[9] = SollHoehe;
		1227	// DebugOut.Analog[10] = Mess_Integral_Gier / 128;
		1228	// DebugOut.Analog[11] = KompassStartwert;
1051	killagreg	1229	// DebugOut.Analog[10] = Parameter_Gyro_I;
		1230	// DebugOut.Analog[10] = EE_Parameter.Gyro_I;
		1231	// DebugOut.Analog[9] = KompassRichtung;
1	ingob	1232	// DebugOut.Analog[10] = GasMischanteil;
		1233	// DebugOut.Analog[3] = HoeheD * 32;
		1234	// DebugOut.Analog[4] = hoehenregler;
		1235	}
		1236
1051	killagreg	1237	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1	ingob	1238	// Drehgeschwindigkeit und -winkel zu einem Istwert zusammenfassen
1051	killagreg	1239	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1153	hbuss	1240	if(TrichterFlug) { SummeRoll = 0; SummeNick = 0;};
604	hbuss	1241
1153	hbuss	1242	if(!Looping_Nick) IntegralNickMalFaktor = (IntegralNick * IntegralFaktor) / (44000 / STICK_GAIN); else IntegralNickMalFaktor = 0;
		1243	if(!Looping_Roll) IntegralRollMalFaktor = (IntegralRoll * IntegralFaktor) / (44000 / STICK_GAIN); else IntegralRollMalFaktor = 0;
		1244
		1245	MesswertNick = IntegralNickMalFaktor + (long)((long)MesswertNick * GyroFaktor) / (256L / STICK_GAIN);
		1246	MesswertRoll = IntegralRollMalFaktor + (long)((long)MesswertRoll * GyroFaktor) / (256L / STICK_GAIN);
		1247
1111	hbuss	1248	#ifdef OCTO
1153	hbuss	1249	MesswertGier = (long)(MesswertGier * 4 * (long)GyroFaktor) / (256L / STICK_GAIN) + (long)(Integral_Gier * IntegralFaktor) / (4 * (44000 / STICK_GAIN));
1111	hbuss	1250	#else
1153	hbuss	1251	MesswertGier = (long)(MesswertGier * 2 * (long)GyroFaktor) / (256L / STICK_GAIN) + (long)(Integral_Gier * IntegralFaktor) / (2 * (44000 / STICK_GAIN));
		1252	#endif
1	ingob	1253
		1254	// Maximalwerte abfangen
1153	hbuss	1255	// #define MAX_SENSOR (4096*STICK_GAIN)
		1256	#define MAX_SENSOR (4096*4)
		1257	if(MesswertNick > MAX_SENSOR) MesswertNick = MAX_SENSOR;
1	ingob	1258	if(MesswertNick < -MAX_SENSOR) MesswertNick = -MAX_SENSOR;
		1259	if(MesswertRoll > MAX_SENSOR) MesswertRoll = MAX_SENSOR;
		1260	if(MesswertRoll < -MAX_SENSOR) MesswertRoll = -MAX_SENSOR;
		1261	if(MesswertGier > MAX_SENSOR) MesswertGier = MAX_SENSOR;
		1262	if(MesswertGier < -MAX_SENSOR) MesswertGier = -MAX_SENSOR;
		1263
1051	killagreg	1264	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1	ingob	1265	// H�henregelung
		1266	// Die H�henregelung schw�cht lediglich das Gas ab, erh�ht es allerdings nicht
1051	killagreg	1267	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1111	hbuss	1268
855	hbuss	1269	GasMischanteil *= STICK_GAIN;
1	ingob	1270	if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // H�henregelung
		1271	{
		1272	int tmp_int;
1036	hbuss	1273	static char delay = 100;
1	ingob	1274	if(EE_Parameter.GlobalConfig & CFG_HOEHEN_SCHALTER) // Regler wird �ber Schalter gesteuert
		1275	{
1036	hbuss	1276	if(((EE_Parameter.BitConfig & CFG_HIGHT_3SWITCH) && ((Parameter_MaxHoehe > 80) && (Parameter_MaxHoehe < 140))) \|\|
993	hbuss	1277	(!(EE_Parameter.BitConfig & CFG_HIGHT_3SWITCH) && (Parameter_MaxHoehe < 50)))
1	ingob	1278	{
1036	hbuss	1279	if(!delay--)
		1280	{
1051	killagreg	1281	if(MessLuftdruck > 1000)
1036	hbuss	1282	{
		1283	ExpandBaro -= 10;
1051	killagreg	1284	OCR0A = DruckOffsetSetting - ExpandBaro;
1036	hbuss	1285	beeptime = 300;
		1286	delay = 250;
		1287	}
		1288	else
1051	killagreg	1289	if(MessLuftdruck < 100)
1036	hbuss	1290	{
		1291	ExpandBaro += 10;
1051	killagreg	1292	OCR0A = DruckOffsetSetting - ExpandBaro;
1036	hbuss	1293	beeptime = 300;
		1294	delay = 250;
		1295	}
		1296	else
1051	killagreg	1297	{
1036	hbuss	1298	SollHoehe = HoehenWert - 20; // Parameter_MaxHoehe ist der PPM-Wert des Schalters
		1299	HoehenReglerAktiv = 0;
		1300	delay = 1;
		1301	}
1051	killagreg	1302	}
1	ingob	1303	}
1051	killagreg	1304	else
		1305	{
1036	hbuss	1306	HoehenReglerAktiv = 1;
		1307	delay = 200;
1051	killagreg	1308	}
1	ingob	1309	}
1051	killagreg	1310	else
1	ingob	1311	{
492	hbuss	1312	SollHoehe = ((int) ExternHoehenValue + (int) Parameter_MaxHoehe) * (int)EE_Parameter.Hoehe_Verstaerkung - 20;
1	ingob	1313	HoehenReglerAktiv = 1;
		1314	}
		1315
		1316	if(Notlandung) SollHoehe = 0;
		1317	h = HoehenWert;
		1318	if((h > SollHoehe) && HoehenReglerAktiv) // zu hoch --> drosseln
1051	killagreg	1319	{
855	hbuss	1320	h = ((h - SollHoehe) * (int) Parameter_Hoehe_P) / (16 / STICK_GAIN); // Differenz bestimmen --> P-Anteil
1	ingob	1321	h = GasMischanteil - h; // vom Gas abziehen
871	hbuss	1322	// h -= (HoeheD * Parameter_Luftdruck_D)/(8/STICK_GAIN); // D-Anteil
		1323	h -= (HoeheD)/(8/STICK_GAIN); // D-Anteil
		1324	tmp_int = ((Mess_Integral_Hoch / 128) * (signed long) Parameter_Hoehe_ACC_Wirkung) / (128 / STICK_GAIN);
		1325	if(tmp_int > 70STICK_GAIN) tmp_int = 70STICK_GAIN;
		1326	else if(tmp_int < -(70STICK_GAIN)) tmp_int = -(70STICK_GAIN);
1	ingob	1327	h -= tmp_int;
1051	killagreg	1328	hoehenregler = (hoehenregler*15 + h) / 16;
855	hbuss	1329	if(hoehenregler < EE_Parameter.Hoehe_MinGas * STICK_GAIN) // nicht unter MIN
1	ingob	1330	{
855	hbuss	1331	if(GasMischanteil >= EE_Parameter.Hoehe_MinGas * STICK_GAIN) hoehenregler = EE_Parameter.Hoehe_MinGas * STICK_GAIN;
		1332	if(GasMischanteil < EE_Parameter.Hoehe_MinGas * STICK_GAIN) hoehenregler = GasMischanteil;
1051	killagreg	1333	}
1	ingob	1334	if(hoehenregler > GasMischanteil) hoehenregler = GasMischanteil; // nicht mehr als Gas
		1335	GasMischanteil = hoehenregler;
1051	killagreg	1336	}
1	ingob	1337	}
855	hbuss	1338	if(GasMischanteil > (MAX_GAS - 20) * STICK_GAIN) GasMischanteil = (MAX_GAS - 20) * STICK_GAIN;
1111	hbuss	1339
1153	hbuss	1340
1111	hbuss	1341	#ifdef OCTO
1051	killagreg	1342	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1	ingob	1343	// + Mischer und PI-Regler
1051	killagreg	1344	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
173	holgerb	1345	DebugOut.Analog[7] = GasMischanteil;
1051	killagreg	1346	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1	ingob	1347	// Gier-Anteil
1051	killagreg	1348	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
188	holgerb	1349	#define MUL_G 1.0
855	hbuss	1350	GierMischanteil = MesswertGier - sollGier * STICK_GAIN; // Regler f�r Gier
604	hbuss	1351	// GierMischanteil = 0;
855	hbuss	1352	#define MIN_GIERGAS (40*STICK_GAIN) // unter diesem Gaswert trotzdem Gieren
693	hbuss	1353	if(GasMischanteil > MIN_GIERGAS)
		1354	{
1051	killagreg	1355	if(GierMischanteil > (GasMischanteil / 2)) GierMischanteil = GasMischanteil / 2;
		1356	if(GierMischanteil < -(GasMischanteil / 2)) GierMischanteil = -(GasMischanteil / 2);
693	hbuss	1357	}
1051	killagreg	1358	else
693	hbuss	1359	{
1051	killagreg	1360	if(GierMischanteil > (MIN_GIERGAS / 2)) GierMischanteil = MIN_GIERGAS / 2;
		1361	if(GierMischanteil < -(MIN_GIERGAS / 2)) GierMischanteil = -(MIN_GIERGAS / 2);
693	hbuss	1362	}
855	hbuss	1363	tmp_int = MAX_GAS*STICK_GAIN;
1051	killagreg	1364	if(GierMischanteil > ((tmp_int - GasMischanteil))) GierMischanteil = ((tmp_int - GasMischanteil));
855	hbuss	1365	if(GierMischanteil < -((tmp_int - GasMischanteil))) GierMischanteil = -((tmp_int - GasMischanteil));
513	hbuss	1366
1051	killagreg	1367	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1	ingob	1368	// Nick-Achse
1051	killagreg	1369	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
723	hbuss	1370	DiffNick = MesswertNick - StickNick; // Differenz bestimmen
1153	hbuss	1371	if(IntegralFaktor) SummeNick += IntegralNickMalFaktor - StickNick; // I-Anteil bei Winkelregelung
1051	killagreg	1372	else SummeNick += DiffNick; // I-Anteil bei HH
855	hbuss	1373	if(SummeNick > (STICK_GAIN * 16000L)) SummeNick = (STICK_GAIN * 16000L);
		1374	if(SummeNick < -(16000L * STICK_GAIN)) SummeNick = -(16000L * STICK_GAIN);
1153	hbuss	1375	pd_ergebnis_nick = DiffNick + SummeNick / Ki; // PI-Regler f�r Nick
1	ingob	1376	// Motor Vorn
499	hbuss	1377	tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64;
1153	hbuss	1378	if(pd_ergebnis_nick > tmp_int) pd_ergebnis_nick = tmp_int;
		1379	if(pd_ergebnis_nick < -tmp_int) pd_ergebnis_nick = -tmp_int;
173	holgerb	1380
1153	hbuss	1381	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		1382	// Roll-Achse
		1383	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		1384	DiffRoll = MesswertRoll - StickRoll; // Differenz bestimmen
		1385	if(IntegralFaktor) SummeRoll += IntegralRollMalFaktor - StickRoll;// I-Anteil bei Winkelregelung
		1386	else SummeRoll += DiffRoll; // I-Anteil bei HH
		1387	if(SummeRoll > (STICK_GAIN * 16000L)) SummeRoll = (STICK_GAIN * 16000L);
		1388	if(SummeRoll < -(16000L * STICK_GAIN)) SummeRoll = -(16000L * STICK_GAIN);
		1389	pd_ergebnis_roll = DiffRoll + SummeRoll / Ki; // PI-Regler f�r Roll
		1390	tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64;
		1391	if(pd_ergebnis_roll > tmp_int) pd_ergebnis_roll = tmp_int;
		1392	if(pd_ergebnis_roll < -tmp_int) pd_ergebnis_roll = -tmp_int;
		1393
		1394	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		1395	// Octo-Mischer
		1396	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		1397	motorwert = GasMischanteil + pd_ergebnis_nick + pd_ergebnis_roll + GierMischanteil;
855	hbuss	1398	motorwert /= STICK_GAIN;
173	holgerb	1399	if ((motorwert < 0)) motorwert = 0;
1	ingob	1400	else if(motorwert > MAX_GAS) motorwert = MAX_GAS;
1051	killagreg	1401	if (motorwert < MIN_GAS) motorwert = MIN_GAS;
1111	hbuss	1402	Motor1 = motorwert;
		1403
1153	hbuss	1404	motorwert = GasMischanteil + pd_ergebnis_nick - pd_ergebnis_roll - GierMischanteil;
1111	hbuss	1405	motorwert /= STICK_GAIN;
		1406	if ((motorwert < 0)) motorwert = 0;
		1407	else if(motorwert > MAX_GAS) motorwert = MAX_GAS;
		1408	if (motorwert < MIN_GAS) motorwert = MIN_GAS;
		1409	Motor2 = motorwert;
		1410
1153	hbuss	1411	motorwert = GasMischanteil + pd_ergebnis_nick - pd_ergebnis_roll + GierMischanteil;
1111	hbuss	1412	motorwert /= STICK_GAIN;
		1413	if ((motorwert < 0)) motorwert = 0;
1153	hbuss	1414	else if(motorwert > MAX_GAS) motorwert = MAX_GAS;
1111	hbuss	1415	if (motorwert < MIN_GAS) motorwert = MIN_GAS;
1153	hbuss	1416	Motor3 = motorwert;
1111	hbuss	1417
1153	hbuss	1418	motorwert = GasMischanteil - pd_ergebnis_nick - pd_ergebnis_roll - GierMischanteil;
1111	hbuss	1419	motorwert /= STICK_GAIN;
		1420	if ((motorwert < 0)) motorwert = 0;
1153	hbuss	1421	else if(motorwert > MAX_GAS) motorwert = MAX_GAS;
1111	hbuss	1422	if (motorwert < MIN_GAS) motorwert = MIN_GAS;
1153	hbuss	1423	Motor4 = motorwert;
1111	hbuss	1424
1153	hbuss	1425	motorwert = GasMischanteil - pd_ergebnis_nick - pd_ergebnis_roll + GierMischanteil;
1111	hbuss	1426	motorwert /= STICK_GAIN;
		1427	if ((motorwert < 0)) motorwert = 0;
1153	hbuss	1428	else if(motorwert > MAX_GAS) motorwert = MAX_GAS;
1111	hbuss	1429	if (motorwert < MIN_GAS) motorwert = MIN_GAS;
1153	hbuss	1430	Motor5 = motorwert;
1111	hbuss	1431
1153	hbuss	1432	motorwert = GasMischanteil - pd_ergebnis_nick + pd_ergebnis_roll - GierMischanteil;
1111	hbuss	1433	motorwert /= STICK_GAIN;
		1434	if ((motorwert < 0)) motorwert = 0;
1153	hbuss	1435	else if(motorwert > MAX_GAS) motorwert = MAX_GAS;
1111	hbuss	1436	if (motorwert < MIN_GAS) motorwert = MIN_GAS;
1153	hbuss	1437	Motor6 = motorwert;
1111	hbuss	1438
1153	hbuss	1439	motorwert = GasMischanteil - pd_ergebnis_nick + pd_ergebnis_roll + GierMischanteil;
1111	hbuss	1440	motorwert /= STICK_GAIN;
		1441	if ((motorwert < 0)) motorwert = 0;
1153	hbuss	1442	else if(motorwert > MAX_GAS) motorwert = MAX_GAS;
1111	hbuss	1443	if (motorwert < MIN_GAS) motorwert = MIN_GAS;
1153	hbuss	1444	Motor7 = motorwert;
1111	hbuss	1445
1153	hbuss	1446	motorwert = GasMischanteil + pd_ergebnis_nick + pd_ergebnis_roll - GierMischanteil;
1111	hbuss	1447	motorwert /= STICK_GAIN;
		1448	if ((motorwert < 0)) motorwert = 0;
1153	hbuss	1449	else if(motorwert > MAX_GAS) motorwert = MAX_GAS;
1111	hbuss	1450	if (motorwert < MIN_GAS) motorwert = MIN_GAS;
1153	hbuss	1451	Motor8 = motorwert;
1111	hbuss	1452	// +++++++++++++++++++++++++++++++++++++++++++++++
1153	hbuss	1453	/*
		1454	if(Poti1 > 20) Motor1 = 0;
		1455	if(Poti1 > 90) Motor6 = 0;
		1456	if(Poti1 > 140) Motor2 = 0;
		1457	if(Poti1 > 200) Motor7 = 0;
		1458	*/
1111	hbuss	1459	}
		1460	#else
		1461
		1462	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		1463	// + Mischer und PI-Regler
		1464	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		1465	DebugOut.Analog[7] = GasMischanteil;
		1466	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		1467	// Gier-Anteil
		1468	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		1469	#define MUL_G 1.0
		1470	GierMischanteil = MesswertGier - sollGier * STICK_GAIN; // Regler f�r Gier
		1471	//GierMischanteil = 0;
		1472	#define MIN_GIERGAS (40*STICK_GAIN) // unter diesem Gaswert trotzdem Gieren
		1473	if(GasMischanteil > MIN_GIERGAS)
		1474	{
		1475	if(GierMischanteil > (GasMischanteil / 2)) GierMischanteil = GasMischanteil / 2;
		1476	if(GierMischanteil < -(GasMischanteil / 2)) GierMischanteil = -(GasMischanteil / 2);
		1477	}
		1478	else
		1479	{
		1480	if(GierMischanteil > (MIN_GIERGAS / 2)) GierMischanteil = MIN_GIERGAS / 2;
		1481	if(GierMischanteil < -(MIN_GIERGAS / 2)) GierMischanteil = -(MIN_GIERGAS / 2);
		1482	}
		1483	tmp_int = MAX_GAS*STICK_GAIN;
		1484	if(GierMischanteil > ((tmp_int - GasMischanteil))) GierMischanteil = ((tmp_int - GasMischanteil));
		1485	if(GierMischanteil < -((tmp_int - GasMischanteil))) GierMischanteil = -((tmp_int - GasMischanteil));
		1486
		1487	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		1488	// Nick-Achse
		1489	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		1490	DiffNick = MesswertNick - StickNick; // Differenz bestimmen
1153	hbuss	1491	if(IntegralFaktor) SummeNick += IntegralNickMalFaktor - StickNick; // I-Anteil bei Winkelregelung
1111	hbuss	1492	else SummeNick += DiffNick; // I-Anteil bei HH
		1493	if(SummeNick > (STICK_GAIN * 16000L)) SummeNick = (STICK_GAIN * 16000L);
		1494	if(SummeNick < -(16000L * STICK_GAIN)) SummeNick = -(16000L * STICK_GAIN);
1153	hbuss	1495	pd_ergebnis = DiffNick + SummeNick / Ki; // PI-Regler f�r Nick
1111	hbuss	1496	// Motor Vorn
		1497	tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64;
		1498	if(pd_ergebnis > tmp_int) pd_ergebnis = tmp_int;
		1499	if(pd_ergebnis < -tmp_int) pd_ergebnis = -tmp_int;
		1500
		1501	motorwert = GasMischanteil + pd_ergebnis + GierMischanteil; // Mischer
		1502	motorwert /= STICK_GAIN;
		1503	if ((motorwert < 0)) motorwert = 0;
		1504	else if(motorwert > MAX_GAS) motorwert = MAX_GAS;
		1505	if (motorwert < MIN_GAS) motorwert = MIN_GAS;
1051	killagreg	1506	Motor_Vorne = motorwert;
1	ingob	1507	// Motor Heck
		1508	motorwert = GasMischanteil - pd_ergebnis + GierMischanteil;
855	hbuss	1509	motorwert /= STICK_GAIN;
173	holgerb	1510	if ((motorwert < 0)) motorwert = 0;
1	ingob	1511	else if(motorwert > MAX_GAS) motorwert = MAX_GAS;
		1512	if (motorwert < MIN_GAS) motorwert = MIN_GAS;
1051	killagreg	1513	Motor_Hinten = motorwert;
		1514	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1	ingob	1515	// Roll-Achse
1051	killagreg	1516	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
723	hbuss	1517	DiffRoll = MesswertRoll - StickRoll; // Differenz bestimmen
1153	hbuss	1518	if(IntegralFaktor) SummeRoll += IntegralRollMalFaktor - StickRoll;// I-Anteil bei Winkelregelung
498	hbuss	1519	else SummeRoll += DiffRoll; // I-Anteil bei HH
918	hbuss	1520	if(SummeRoll > (STICK_GAIN * 16000L)) SummeRoll = (STICK_GAIN * 16000L);
		1521	if(SummeRoll < -(16000L * STICK_GAIN)) SummeRoll = -(16000L * STICK_GAIN);
1153	hbuss	1522	pd_ergebnis = DiffRoll + SummeRoll / Ki; // PI-Regler f�r Roll
499	hbuss	1523	tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64;
1051	killagreg	1524	if(pd_ergebnis > tmp_int) pd_ergebnis = tmp_int;
		1525	if(pd_ergebnis < -tmp_int) pd_ergebnis = -tmp_int;
1	ingob	1526	// Motor Links
		1527	motorwert = GasMischanteil + pd_ergebnis - GierMischanteil;
855	hbuss	1528	motorwert /= STICK_GAIN;
173	holgerb	1529	if ((motorwert < 0)) motorwert = 0;
1	ingob	1530	else if(motorwert > MAX_GAS) motorwert = MAX_GAS;
		1531	if (motorwert < MIN_GAS) motorwert = MIN_GAS;
1051	killagreg	1532	Motor_Links = motorwert;
1	ingob	1533	// Motor Rechts
		1534	motorwert = GasMischanteil - pd_ergebnis - GierMischanteil;
855	hbuss	1535	motorwert /= STICK_GAIN;
173	holgerb	1536	if ((motorwert < 0)) motorwert = 0;
1	ingob	1537	else if(motorwert > MAX_GAS) motorwert = MAX_GAS;
1051	killagreg	1538	if (motorwert < MIN_GAS) motorwert = MIN_GAS;
		1539	Motor_Rechts = motorwert;
1	ingob	1540	// +++++++++++++++++++++++++++++++++++++++++++++++
1111	hbuss	1541	//Motor_Hinten = 0;
		1542	//Motor_Vorne = 0;
1153	hbuss	1543	//Motor_Links = 0;
		1544	//Motor_Rechts = 0;
1	ingob	1545	}
		1546
1111	hbuss	1547	#endif

Subversion Repositories FlightCtrl

(root)/branches/V0.88e_ACC-HH_HR_MartinR/fc.c @ 2161 - Rev 1153