WebSVN - FlightCtrl - Blame - Rev 840 - /branches/KalmanFilter MikeW/KalmanFc.c

Rev	Author	Line No.	Line
838	MikeW	1	/*
		2	Copyright 2008, by Michael Walter
		3
		4	All functions written by Michael Walter are free software and can be redistributed and/or modified under the terms of the GNU Lesser
		5	General Public License as published by the Free Software Foundation. This program is distributed in the hope that it will be useful, but
		6	WITHOUT ANY WARRANTY, without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
		7	See the GNU Lesser General Public License for more details. You should have received a copy of the GNU Lesser General Public
		8	License along with this program. If not, see <http://www.gnu.org/licenses/>.
		9
		10	Please note: The software is based on the framework provided by H. Buss and I. Busker in their Mikrokopter projekt. All functions that
		11	are not written by Michael Walter are under the license by H. Buss and I. Busker (license_buss.txt) published by www.mikrokopter.de
		12	unless it is stated otherwise.
		13	*/
		14
		15	#include "kafi.h"
		16	#include "KalmanFc.h"
		17	#include "main.h"
		18	#include "mm3.h"
		19
		20	#include "main.h"
		21	#include "eeprom.c"
		22
		23	#define MAX_GAS 250
		24	#define MIN_GAS 15
		25
		26	#define sin45 sin(45.F / 180.F * PI)
		27	#define cos45 cos(45.F / 180.F * PI)
		28
		29	extern void GPSupdate(void);
		30
		31	int AdNeutralNick = 0,AdNeutralRoll = 0,AdNeutralGier = 0;
		32	int NeutralAccX = 0, NeutralAccY = 0, NeutralAccZ = 0;
		33	int AverageRoll_X = 0, AverageRoll_Y = 0, AverageRoll_Z = 0;
		34	int AveragerACC_X = 0, AveragerACC_Y = 0, AveragerACC_Z = 0;
		35	int Roll_X_Off = 0, Roll_Y_Off = 0, Roll_Z_Off = 0;
		36
		37	f32_t Roll_X_Offset = 0.F, Roll_Y_Offset = 0.F, Roll_Z_Offset = 0.F;
		38	f32_t ACC_X_Offset = 0.F, ACC_Y_Offset = 0.F, ACC_Z_Offset = 0.F;
		39	f32_t Phi, Theta, sinPhi, cosPhi, tanTheta, secTheta;
		40	f32_t sinPhi, cosPhi, sinTheta, cosTheta;
		41	f32_t sinPhi_P, cosPhi_P, sinTheta_P, cosTheta_P, sinPsi_P, cosPsi_P;
		42
		43	#ifdef USE_Extended_MM3_Measurement_Model
		44	f32_t sinPsi_P, cosPsi_P;
		45	extern f32_t MM3_Hx, MM3_Hy, MM3_Hz;
		46	#endif
		47
		48	int DiffAltitude = 0, DeltaAltitude = 0, CurrentAltitude = 0, LastAltitude = 0, NominalAltitude = 0, InitialAltitude = 0;
		49	int KompassValue = 0, SummeNick=0,SummeRoll=0, StickNick = 0,StickRoll = 0,StickGier = 0, sollGier = 0;
		50	int GierMischanteil;
		51	extern int GasMischanteil;
		52
		53	extern char Notlandung;
		54	extern unsigned long maxDistance;
		55
		56	unsigned char Motor_Vorne,Motor_Hinten,Motor_Rechts,Motor_Links, Count;
		57	unsigned char MotorWert[5];
		58
		59	extern signed int GPS_Nick, GPS_Roll;
		60
		61	int GasMischanteil;
		62	char MotorenEin = 0;
		63	char Notlandung = 0;
		64
		65	unsigned char SenderOkay = 0;
		66	unsigned int I2CTimeout = 100;
		67
		68	struct mk_param_struct EE_Parameter;
		69
		70	/* ****************************************************************************
		71	Functionname: SendMotorData / /!
		72	Description: Senden der Motorwerte per I2C-Bus
		73
		74	@return void
		75	@pre -
		76	@post -
		77	@author H. Buss / I. Busker
		78	**************************************************************************** */
		79	void SendMotorData(void)
		80	{
		81	if(!MotorenEin)
		82	{
		83	Motor_Hinten = 0;
		84	Motor_Vorne = 0;
		85	Motor_Rechts = 0;
		86	Motor_Links = 0;
		87	if(MotorTest[0]) Motor_Vorne = MotorTest[0];
		88	if(MotorTest[1]) Motor_Hinten = MotorTest[1];
		89	if(MotorTest[2]) Motor_Links = MotorTest[2];
		90	if(MotorTest[3]) Motor_Rechts = MotorTest[3];
		91	}
		92
		93	//Start I2C Interrupt Mode
		94	twi_state = 0;
		95	motor = 0;
		96	i2c_start();
		97	}
		98
		99
		100	/* ****************************************************************************
		101	Functionname: MotorControl / /!
		102	Description:
		103
		104	@return void
		105	@pre -
		106	@post -
		107	@author H. Buss / I. Busker
		108	**************************************************************************** */
		109	void MotorControl(void)
		110	{
840	MikeW	111	static unsigned int NotGasZeit;
		112	static unsigned char delay_neutral = 0;
		113	static unsigned char delay_einschalten = 0,delay_ausschalten = 0;
		114	static unsigned int modell_fliegt = 0;
		115
838	MikeW	116	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		117	// Gaswert ermitteln
		118	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		119	GasMischanteil = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120;
		120	if(GasMischanteil < 0)
		121	{
		122	GasMischanteil = 0;
		123	}
		124	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		125	// Emfang schlecht
		126	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		127
		128	if(SenderOkay < 100)
		129	{
		130	if(!PcZugriff)
		131	{
840	MikeW	132	if(BeepMuster == 0xffff)
838	MikeW	133	{
		134	beeptime = 15000;
		135	BeepMuster = 0x0c00;
		136	}
		137	}
		138	if(NotGasZeit)
		139	{
		140	NotGasZeit--;
		141	}
		142	else
		143	{
		144	MotorenEin = 0;
		145	Notlandung = 0;
		146	}
		147	if(modell_fliegt > 2000) // wahrscheinlich in der Luft --> langsam absenken
		148	{
		149	GasMischanteil = 100; //EE_Parameter.NotGas;
		150	Notlandung = 1;
		151	PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] = 0;
		152	PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] = 0;
		153	PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] = 0;
		154	}
		155	else
		156	{
		157	MotorenEin = 0;
		158	}
		159	}
		160	else
840	MikeW	161	{
838	MikeW	162	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
840	MikeW	163	// Emfang gut
838	MikeW	164	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
840	MikeW	165	if(SenderOkay > 140)
		166	{
		167	Notlandung = 0;
		168	NotGasZeit = 200 * 50; //EE_Parameter.NotGasZeit * 50;
		169	if(GasMischanteil > 40)
838	MikeW	170	{
840	MikeW	171	if(modell_fliegt < 0xffff)
		172	{
		173	modell_fliegt++;
		174	}
		175	}
		176
		177	if((GasMischanteil > 200) && MotorenEin == 0)
		178	{
		179	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		180	// auf Nullwerte kalibrieren
		181	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		182	if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75) // Neutralwerte
		183	{
		184	if(++delay_neutral > 50) // nicht sofort
		185	{
		186	MotorenEin = 0;
		187	delay_neutral = 0;
		188	modell_fliegt = 0;
		189	if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // H�henregelung aktiviert?
		190	{
		191	if((AdWertAirPressure_Raw > 950) \|\| (AdWertAirPressure_Raw < 750))
		192	{
		193	SucheLuftruckOffset();
		194	}
		195	}
		196	ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE);
		197	SetNeutral();
		198	}
		199	}
		200	else delay_neutral = 0;
		201	}
838	MikeW	202	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
840	MikeW	203	// Gas ist unten
838	MikeW	204	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
840	MikeW	205	if(GasMischanteil < 35)
838	MikeW	206	{
840	MikeW	207	// Starten
		208	if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75)
		209	{
		210	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		211	// Einschalten
		212	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		213	if(++delay_einschalten > 100)
		214	{
		215	MotorenEin = 1;
		216	modell_fliegt = 1;
		217	delay_einschalten = 100;
		218	}
		219	}
		220	else
		221	{
		222	delay_einschalten = 0;
		223	}
		224	//Auf Neutralwerte setzen
		225	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		226	// Auschalten
		227	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		228	if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75)
		229	{
		230	if(++delay_ausschalten > 50) // nicht sofort
		231	{
		232	MotorenEin = 0;
		233	modell_fliegt = 0;
		234	delay_ausschalten = 50;
		235	}
		236	}
		237	else
		238	{
		239	delay_ausschalten = 0;
		240	}
		241	}
838	MikeW	242	}
840	MikeW	243	}
838	MikeW	244	}
		245
		246
		247	/* ****************************************************************************
		248	Functionname: SetNeutral / /!
		249	Description:
		250
		251	@param[in]
		252
		253	@return void
		254	@pre -
		255	@post -
		256	@author Michael Walter
		257	**************************************************************************** */
		258	void SetNeutral(void)
		259	{
		260	if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // H�henregelung aktiviert?
		261	{
		262	if((AdWertAirPressure_Raw > 950) \|\| (AdWertAirPressure_Raw < 750))
		263	{
		264	SucheLuftruckOffset();
		265	}
		266	}
		267
		268	LastAltitude = CurrentAltitude;
		269	Delay_ms_Mess(300);
		270
		271	ANALOG_OFF;
		272	AdNeutralNick = AdWertNick_Raw;
		273	AdNeutralRoll = AdWertRoll_Raw;
		274	AdNeutralGier = AdWertGier_Raw;
		275	NeutralAccY = AdWertAccRoll_Raw;
		276	NeutralAccX = AdWertAccNick_Raw;
		277	NeutralAccZ = AdWertAccHoch_Raw;
		278
		279	Roll_X_Offset = 0.F;
		280	Roll_Y_Offset = 0.F;
		281	Roll_Z_Offset = 0.F;
		282	ACC_X_Offset = 0.F;
		283	ACC_Y_Offset = 0.F;
		284	ACC_Z_Offset = 0.F;
		285
		286	AccumulatedACC_X = 0;
		287	AccumulatedACC_X_cnt = 0;
		288	AccumulatedACC_Y = 0;
		289	AccumulatedACC_Y_cnt = 0;
		290	AccumulatedACC_Z = 0;
		291	AccumulatedACC_Z_cnt = 0;
		292
		293	AccumulatedRoll_X = 0;
		294	AccumulatedRoll_X_cnt = 0;
		295	AccumulatedRoll_Y = 0;
		296	AccumulatedRoll_Y_cnt = 0;
		297	AccumulatedRoll_Z = 0;
		298	AccumulatedRoll_Z_cnt = 0;
		299
		300	AveragerACC_X = 0;
		301	AveragerACC_Y = 0;
		302	AveragerACC_Z = 0;
		303
		304	AccumulatedACC_X = 0;
		305	AccumulatedACC_X_cnt = 0;
		306	AccumulatedACC_Y = 0;
		307	AccumulatedACC_Y_cnt = 0;
		308	AccumulatedACC_Z = 0;
		309	AccumulatedACC_Z_cnt = 0;
		310	ANALOG_ON;
		311
		312	SummeRoll = 0;
		313	SummeNick = 0;
		314
		315	sollGier = status.iPsi10;
		316	InitialAltitude = AdWertAirPressure_Raw;
		317	beeptime = 50;
		318	}
		319
		320
		321	/* ****************************************************************************
		322	Functionname: CalculateAverage / /!
		323	Description:
		324
		325	@param[in]
		326
		327	@return void
		328	@pre -
		329	@post -
		330	@author Michael Walter
		331	**************************************************************************** */
		332	void CalculateAverage(void)
		333	{
		334	ANALOG_OFF;
		335	AverageRoll_X = AccumulatedRoll_X / AccumulatedRoll_X_cnt;
		336	AverageRoll_Y = AccumulatedRoll_Y / AccumulatedRoll_Y_cnt;
		337	AverageRoll_Z = AccumulatedRoll_Z / AccumulatedRoll_Z_cnt;
		338
		339	/* Get Pressure Differenz */
		340	CurrentAltitude = InitialAltitude - AccumulatedAirPressure / AccumulatedAirPressure_cnt;
		341	AccumulatedAirPressure_cnt = 0;
		342	AccumulatedAirPressure = 0;
		343
		344	static char AirPressureCnt = 0;
		345	if (AirPressureCnt % 25 == 1)
		346	{
		347	DeltaAltitude = CurrentAltitude - LastAltitude;
		348	LastAltitude = CurrentAltitude;
		349	}
		350	AirPressureCnt++;
		351
		352	if ((GPS_Roll == 0 && GPS_Nick == 0) \|\| (maxDistance / 10 > 10))
		353	{
		354	Roll_X_Offset = 0.9995F * Roll_X_Offset + 0.0005F * (float) (MAX(-60, MIN(60,AverageRoll_X)));
		355	Roll_Y_Offset = 0.9995F * Roll_Y_Offset + 0.0005F * (float) (MAX(-60, MIN(60,AverageRoll_Y)));
		356
		357	if (abs(StickGier) < 15 \|\| MotorenEin == 0)
		358	{
		359	Roll_Z_Offset = 0.9998F * Roll_Z_Offset + 0.0002F * (float) ( MAX(-60, MIN(60,AverageRoll_Z)));
		360	}
		361	}
		362
		363	#if 0
		364	DebugOut.Analog[3] = AdWertGier_Raw;
		365	DebugOut.Analog[4] = AdWertRoll_Raw;
		366	DebugOut.Analog[5] = AdWertNick_Raw;
		367	#endif
		368
		369	AverageRoll_X -= Roll_X_Offset;
		370	AverageRoll_Y -= Roll_Y_Offset;
		371	AverageRoll_Z -= Roll_Z_Offset;
		372
		373	AccumulatedRoll_X = 0;
		374	AccumulatedRoll_X_cnt = 0;
		375	AccumulatedRoll_Y = 0;
		376	AccumulatedRoll_Y_cnt = 0;
		377	AccumulatedRoll_Z = 0;
		378	AccumulatedRoll_Z_cnt = 0;
		379
		380	#if 0
		381	ACC_X_Offset = 0.9999F * ACC_X_Offset + 0.0001F * ((float) AccumulatedACC_X / (float) AccumulatedACC_X_cnt);
		382	ACC_Y_Offset = 0.9999F * ACC_Y_Offset + 0.0001F * ((float) AccumulatedACC_Y / (float) AccumulatedACC_Y_cnt);
		383	ACC_Z_Offset = 0.9999F * ACC_Z_Offset + 0.0001F * ((float) AccumulatedACC_Z / (float) AccumulatedACC_Z_cnt);
		384	#endif
		385
		386	AveragerACC_X = AccumulatedACC_X / AccumulatedACC_X_cnt;
		387	AveragerACC_Y = AccumulatedACC_Y / AccumulatedACC_Y_cnt;
		388	AveragerACC_Z = AccumulatedACC_Z / AccumulatedACC_Z_cnt;
		389
		390	AccumulatedACC_X = 0;
		391	AccumulatedACC_X_cnt = 0;
		392	AccumulatedACC_Y = 0;
		393	AccumulatedACC_Y_cnt = 0;
		394	AccumulatedACC_Z = 0;
		395	AccumulatedACC_Z_cnt = 0;
		396
		397	ANALOG_ON;
		398
		399	if (Roll_X_Off > 60)
		400	{
		401	Roll_X_Off = 60;
		402	}
		403	if (Roll_X_Off < -60)
		404	{
		405	Roll_X_Off = -60;
		406	}
		407
		408	if (Roll_Y_Off > 60)
		409	{
		410	Roll_Y_Off = 60;
		411	}
		412	if (Roll_Y_Off < -60)
		413	{
		414	Roll_Y_Off = -60;
		415	}
		416
		417	if (Roll_Z_Off > 60)
		418	{
		419	Roll_Z_Off = 60;
		420	}
		421	if (Roll_Z_Off < -60)
		422	{
		423	Roll_Z_Off = -60;
		424	}
		425	}
		426
		427
		428	/* ****************************************************************************
		429	Functionname: PID_Regler / /!
		430	Description:
		431
		432	@return void
		433	@pre -
		434	@post -
		435	@author Michael Walter
		436	**************************************************************************** */
		437	void PID_Regler(void)
		438	{
		439	int StickNick45,StickRoll45;
		440	int DiffNick,DiffRoll, DiffGier;
		441	int motorwert = 0;
		442	int pd_ergebnis;
		443
		444	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		445	// neue Werte von der Funke
		446	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		447	if(!NewPpmData--)
		448	{
		449	StickNick = (StickNick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P) / 4;
		450	StickRoll = (StickRoll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P) / 4;
		451	StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]];
		452	} // Ende neue Funken-Werte
		453
		454	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		455	// Bei Empfangsausfall im Flug
		456	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		457	if(Notlandung)
		458	{
		459	StickGier = 0;
		460	StickNick = 0;
		461	StickRoll = 0;
		462	}
		463
		464	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		465	// + Mischer und PID-Regler
		466	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		467
		468	/* G P S U p d a t e */
		469	static char CntGPS = 0;
		470	if (CntGPS % 10 == 1)
		471	{
		472	//GPSupdate();
		473	}
		474	CntGPS++;
		475
		476	StickRoll45 = (int) (0.707F * (float)(StickRoll - GPS_Roll) - 0.707F * (float)(StickNick - GPS_Nick));
		477	StickNick45 = (int) (0.707F * (float)(StickRoll - GPS_Roll) + 0.707F * (float)(StickNick - GPS_Nick));
		478
		479	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		480	// Gieren
		481	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		482
		483	if(GasMischanteil < 20)
		484	{
		485	sollGier = status.iPsi10;
		486	SummeRoll = 0;
		487	SummeNick = 0;
		488	}
		489
		490	/* Gier-Anteil */
		491	if (abs(StickGier) > 4)
		492	{
		493	sollGier = status.iPsi10 + 4 * StickGier;
		494	}
		495	DiffGier = (sollGier - status.iPsi10);
		496	GierMischanteil = (int) (-4 * DiffGier - 4* (AdWertGier - AdNeutralGier - Roll_Z_Off)) / 10;
		497
		498	#define MUL_G 0.8
		499	if(GierMischanteil > MUL_G * GasMischanteil)
		500	{
		501	GierMischanteil = MUL_G * GasMischanteil;
		502	}
		503	if(GierMischanteil < -MUL_G * GasMischanteil)
		504	{
		505	GierMischanteil = -MUL_G * GasMischanteil;
		506	}
		507	if(GierMischanteil > 50)
		508	{
		509	GierMischanteil = 50;
		510	}
		511	if(GierMischanteil < -50)
		512	{
		513	GierMischanteil = -50;
		514	}
		515
		516	int scale_p = 7;
		517	int scale_d = 10;
		518
		519	/* N i c k - A c h s e */
		520	DiffNick = -(status.iTheta10 + StickNick45);
		521	/* R o l l - A c h s e */
		522	DiffRoll = -(status.iPhi10 + StickRoll45);
		523
		524	SummeNick += DiffNick;
		525	if(SummeNick > 10000)
		526	{
		527	SummeNick = 10000;
		528	}
		529	if(SummeNick < -10000)
		530	{
		531	SummeNick = -10000;
		532	}
		533
		534	SummeRoll += DiffRoll;
		535	if(SummeRoll > 10000)
		536	{
		537	SummeRoll = 10000;
		538	}
		539	if(SummeRoll < -10000)
		540	{
		541	SummeRoll = -10000;
		542	}
		543
		544	pd_ergebnis = ((scale_p DiffNick + scale_d (AdWertNick - AdNeutralNick - Roll_Y_Off)) / 10) ; // + (int)(SummeNick / 2000);
		545	if(pd_ergebnis > (GasMischanteil + abs(GierMischanteil)))
		546	{
		547	pd_ergebnis = (GasMischanteil + abs(GierMischanteil));
		548	}
		549	if(pd_ergebnis < -(GasMischanteil + abs(GierMischanteil)))
		550	{
		551	pd_ergebnis = -(GasMischanteil + abs(GierMischanteil));
		552	}
		553
		554	/* M o t o r V o r n */
		555	motorwert = GasMischanteil + pd_ergebnis + GierMischanteil;
		556	if ((motorwert < 0))
		557	{
		558	motorwert = 0;
		559	}
		560	else if(motorwert > MAX_GAS)
		561	{
		562	motorwert = MAX_GAS;
		563	}
		564	if (motorwert < MIN_GAS)
		565	{
		566	motorwert = MIN_GAS;
		567	}
		568	Motor_Vorne = motorwert;
		569
		570	/* M o t o r H e c k */
		571	motorwert = GasMischanteil - pd_ergebnis + GierMischanteil;
		572	if ((motorwert < 0))
		573	{
		574	motorwert = 0;
		575	}
		576	else if(motorwert > MAX_GAS)
		577	{
		578	motorwert = MAX_GAS;
		579	}
		580	if (motorwert < MIN_GAS)
		581	{
		582	motorwert = MIN_GAS;
		583	}
		584	Motor_Hinten = motorwert;
		585
		586	pd_ergebnis = ((scale_p * DiffRoll + scale_d * (AdWertRoll - AdNeutralRoll - Roll_X_Off)) / 10) ; //+ (int)(SummeRoll / 2000);
		587	if(pd_ergebnis > (GasMischanteil + abs(GierMischanteil)))
		588	{
		589	pd_ergebnis = (GasMischanteil + abs(GierMischanteil));
		590	}
		591	if(pd_ergebnis < -(GasMischanteil + abs(GierMischanteil)))
		592	{
		593	pd_ergebnis = -(GasMischanteil + abs(GierMischanteil));
		594	}
		595
		596	/* M o t o r L i n k s */
		597	motorwert = GasMischanteil + pd_ergebnis - GierMischanteil;
		598	if(motorwert > MAX_GAS)
		599	{
		600	motorwert = MAX_GAS;
		601	}
		602	if (motorwert < MIN_GAS)
		603	{
		604	motorwert = MIN_GAS;
		605	}
		606	Motor_Links = motorwert;
		607
		608	/* M o t o r R e c h t s */
		609	motorwert = GasMischanteil - pd_ergebnis - GierMischanteil;
		610	if(motorwert > MAX_GAS)
		611	{
		612	motorwert = MAX_GAS;
		613	}
		614	if (motorwert < MIN_GAS)
		615	{
		616	motorwert = MIN_GAS;
		617	}
		618	Motor_Rechts = motorwert;
		619
		620	#if 1
		621	DebugOut.Analog[0] = status.iTheta10;
		622	DebugOut.Analog[1] = status.iPhi10;
		623	DebugOut.Analog[2] = status.iPsi10 / 10;
		624	//DebugOut.Analog[3] = KompassValue;
		625	//DebugOut.Analog[4] = (sollGier - status.iPsi10 ) / 10;
		626	//DebugOut.Analog[5] = AverageRoll_Z;
		627	//DebugOut.Analog[6] = AverageRoll_X;
		628
		629	//DebugOut.Analog[9] = CurrentAltitude;
		630	//DebugOut.Analog[10] = DiffAltitude;
		631	//DebugOut.Analog[8] = AverageRoll_X;
		632	//DebugOut.Analog[9] = AverageRoll_Y;
		633	//DebugOut.Analog[10] = AverageRoll_Z;
		634
		635	//DebugOut.Analog[11] = GasMischanteil;
		636	//DebugOut.Analog[13] = AverageRoll_X;
		637	//DebugOut.Analog[14] = AverageRoll_Y;
		638	//DebugOut.Analog[15] = AdWertAirPressure_Raw;
		639	#endif
		640	}
		641
		642
		643	/*****************************************************************************
		644	VARIABLES
		645	*****************************************************************************/
		646
		647	/* Kalman filter */
		648	kafi_t *p_kafi;
		649	mat_Matrix_t *matP0;
		650	mat_Matrix_t *matQ;
		651	mat_Matrix_t *matR;
		652	mat_Matrix_t matXd; / estimated state */
		653	mat_Matrix_t matXs; / predicted state */
		654	mat_Matrix_t matPhi; / transition matrix to predict new state from current state */
		655	mat_Matrix_t matB; / control matrix to predict new state from ext. control vector U */
		656	mat_Matrix_t matU; / control vector */
		657	mat_Matrix_t matC; / Jacobi matrix */
		658	mat_Matrix_t matY; / real measurements */
		659	mat_Matrix_t matdY; / innovation */
		660	mat_Matrix_t matYs; / predicted measurements */
		661	mat_Matrix_t matP; / error covariance matrix */
		662	mat_Matrix_t matPDiag; / diagonal covariance matrix */
		663	char fYValid; / measurement validity flag */
		664
		665
		666	void Kafi_Init()
		667	{
		668	/* create kalman filter and associated matrizes */
		669	p_kafi = KAFICreate();
		670
		671	KAFIInit(p_kafi);
		672
		673	/* get pointers to all matrices */
		674	KAFIGetP0(p_kafi, &matP0);
		675	KAFIGetQ(p_kafi, &matQ);
		676	KAFIGetR(p_kafi, &matR);
		677	KAFIGetXd(p_kafi, &matXd);
		678	KAFIGetXs(p_kafi, &matXs);
		679	KAFIGetPhi(p_kafi, &matPhi);
		680	KAFIGetB(p_kafi, &matB);
		681	KAFIGetU(p_kafi, &matU);
		682	KAFIGetC(p_kafi, &matC);
		683	KAFIGetY(p_kafi, &matY);
		684	KAFIGetYs(p_kafi, &matYs);
		685	KAFIGetPDiag(p_kafi, &matPDiag);
		686	KAFIGetYValid(p_kafi, &fYValid);
		687	KAFIGetdY(p_kafi,&matdY);
		688
		689	trResetKalmanFilter();
		690	}
		691
		692	/* ****************************************************************************
		693	Functionname: trResetKalmanFilter / /!
		694	Description:
		695
		696	@return void
		697	@pre -
		698	@post -
		699	@author Michael Walter
		700	**************************************************************************** */
		701
		702	void trResetKalmanFilter()
		703	{
		704	/--- (SYMBOLIC) CONSTANTS ---/
		705
		706	/--- VARIABLES ---/
		707	ui32_t i, j;
		708
		709	/* set phi to identity matrix */
		710	for ( j = 0; j < p_kafi->uiDimX; j++ )
		711	{
		712	for ( i = 0; i < p_kafi->uiDimX; i++ )
		713	{
		714	matSetFull(matPhi, j, i, (j == i) ? 1.F : 0.F);
		715	}
		716	}
		717
		718	/* set diagonal of measurement noise covariance R */
		719	matSetDiagonal(matR, _ax, _ax, 10.0F);
		720	matSetDiagonal(matR, _ay, _ay, 10.0F);
		721	matSetDiagonal(matR, _az, _az, 10.0F);
		722	#ifdef USE_Extended_MM3_Measurement_Model
		723	matSetDiagonal(matR, _mx, _mx, 100.0F);
		724	matSetDiagonal(matR, _my, _my, 100.0F);
		725	matSetDiagonal(matR, _mz, _mz, 100.0F);
		726	#else
		727	matSetDiagonal(matR, _compass, _compass, 5.0F);
		728	#endif
		729
		730	/* set diagonal of system noise covariance Q */
		731	matSetDiagonal(matQ, _Phi, _Phi, 0.00001F);
		732	matSetDiagonal(matQ, _Theta, _Theta, 0.00001F);
		733	matSetDiagonal(matQ, _Psi, _Psi, 0.00005F);
		734
		735	/* set diagonal init. estimation error (covariance) P0 */
		736	matSetDiagonal(matP0, _Phi, _Phi_, 0.00001F);
		737	matSetDiagonal(matP0, _Theta, _Theta, 0.00001F);
		738	matSetDiagonal(matP0, _Psi, _Psi, 0.00001F);
		739
		740	matSetFull(matXd, _Phi, 0, 0.0F);
		741	matSetFull(matXd, _Theta, 0, 0.0F);
		742	matSetFull(matXd, _Psi, 0, 0.0F);
		743
		744	matSetFull(matXs, _Phi, 0, 0.0F);
		745	matSetFull(matXs, _Theta, 0, 0.0F);
		746	matSetFull(matXs, _Psi, 0, 0.0F);
		747	memset( &status, 0, sizeof(status) );
		748
		749	KAFISetP0(p_kafi);
		750	}
		751
		752
		753
		754
		755	/* ****************************************************************************
		756	Functionname: trInnovate / /!
		757	Description: Update the current System State based on the current Observation
		758
		759
		760	@param[in]
		761
		762	@return void
		763	@pre -
		764	@post -
		765	@author Michael Walter
		766	**************************************************************************** */
		767	void trInnovate()
		768	{
		769	/--- (SYMBOLIC) CONSTANTS ---/
		770
		771	/--- VARIABLES ---/
		772
		773	/estimate the new system state (Xd), nominal case /
		774	KAFIInnovation(p_kafi);
		775	KAFIUpdatePDiag(p_kafi);
		776	KAFIUpdateP(p_kafi);
		777	return;
		778	}
		779
		780
		781
		782	/* ****************************************************************************
		783	Functionname: trUpdatePhi / /!
		784	Description: Setup matPhi, used to calculate the predicted state
		785	from the current state
		786
		787
		788	@param[in]
		789	@param[in] fCycleTime
		790
		791	@return void
		792	@pre -
		793	@post -
		794	@author Michael Walter
		795	**************************************************************************** */
		796	void trUpdatePhi()
		797	{
		798	/--- (SYMBOLIC) CONSTANTS ---/
		799
		800	/--- VARIABLES ---/
		801	matSetFull(matPhi, _Phi, _Phi, 1.0F);
		802	matSetFull(matPhi, _Theta, _Theta, 1.0F);
		803	matSetFull(matPhi, _Psi, _Psi, 1.0F);
		804	}
		805
		806
		807
		808	#ifdef USE_Extended_MM3_Measurement_Model
		809	/* ****************************************************************************
		810	Functionname: trUpdateJacobiMatrix / /!
		811	Description:
		812
		813
		814	@param[in] void
		815
		816	@return void
		817	@pre -
		818	@post -
		819	@author Michael Walter
		820	*****************************************************************************/
		821	void trUpdateJacobiMatrix(void)
		822	{
		823	/--- (SYMBOLIC) CONSTANTS ---/
		824	/*
		825	Simplified Version
		826	Pre_ax = du - sinTheta * g;
		827	Pre_ay = dv + cosTheta_sinPhi * g;
		828	Pre_az = dw + cosTheta_cosPhi * g;
		829	*/
		830
		831	/--- VARIABLES ---/
		832	f32_t Phi, Theta, Psi;
		833	f32_t sinPsi, cosPsi;
		834
		835	f32_t g = 9.81F;
		836	f32_t Pre_Hx = 4.78F; /* horizontal field component at the current location */
		837	f32_t Pre_Hz = 8.96F; /* vertical field component at the current location */
		838
		839	Phi = status.Phi;
		840	Theta = status.Theta;
		841	Psi = status.Psi;
		842
		843	sinPhi = sin(Phi);
		844	cosPhi = cos(Phi);
		845	sinTheta = sin(Theta);
		846	cosTheta = cos(Theta);
		847	sinPsi = sin(Psi);
		848	cosPsi = cos(Psi);
		849
		850	//Pre_ax = du - sinTheta * g;
		851	//matSetFull(matC, _ax, _Phi , 0);
		852	matSetFull(matC, _ax, _Theta, -cosTheta * g);
		853	//matSetFull(matC, _ax, _Psi , 0);
		854
		855	//Pre_ay = dv + cosTheta_sinPhi * g;
		856	matSetFull(matC, _ay, _Phi, cosTheta * cosPhi * g);
		857	matSetFull(matC, _ay, _Theta , -sinTheta * sinPhi * g);
		858	//matSetFull(matC, _ay, _Psi , 0);
		859
		860	//Pre_az = dw + cosTheta_cosPhi * g;
		861	matSetFull(matC, _az, _Phi , -cosTheta * sinPhi * g);
		862	matSetFull(matC, _az, _Theta , -sinTheta * cosPhi * g);
		863	//matSetFull(matC, _az, _Psi , 0);
		864
		865	//Pre_mx = (cos45 * (cosTheta * cosPsi) + sin45 * (-cosPhi * sinPsi + sinPhi * sinTheta * cosPsi) )* Pre_Hx + (-cos45 * sinTheta + sin45 * sinPhi * cosTheta) * Pre_Hz;
		866	//matSetFull(matC, _mx, _Phi , (sin45 * (sinPhi * sinPsi + cosPhi * sinTheta * cosPsi) )* Pre_Hx + ( sin45 * cosPhi * cosTheta) * Pre_Hz );
		867	//matSetFull(matC, _mx, _Theta, (cos45 * (-sinTheta * cosPsi) + sin45 * (sinPhi * cosTheta * cosPsi) )* Pre_Hx + (-cos45 * cosTheta - sin45 * sinPhi * sinTheta) * Pre_Hz);
		868	//matSetFull(matC, _mx, _Psi , (-cos45 * (cosTheta * sinPsi) + sin45 * (-cosPhi * cosPsi - sinPhi * sinTheta * sinPsi) )* Pre_Hx );
		869
		870	//Pre_my = (-sin45 * (cosTheta * cosPsi) + cos45 * (-cosPhi * sinPsi + sinPhi * sinTheta * cosPsi)) * Pre_Hx + (sin45 * sinTheta + cos45 * sinPhi * cosTheta) * Pre_Hz;
		871	//matSetFull(matC, _my, _Phi , (cos45 * (sinPhi * sinPsi + cosPhi * sinTheta * cosPsi)) * Pre_Hx + ( cos45 * cosPhi * cosTheta) * Pre_Hz );
		872	//matSetFull(matC, _my, _Theta, (sin45 * (sinTheta * cosPsi) + cos45 * (sinPhi * cosTheta * cosPsi)) * Pre_Hx + (sin45 * cosTheta - cos45 * sinPhi * sinTheta) * Pre_Hz );
		873	//matSetFull(matC, _my, _Psi , (sin45 * (cosTheta * sinPsi) + cos45 * (-cosPhi * cosPsi - sinPhi * sinTheta * sinPsi)) * Pre_Hx );
		874
		875	//Pre_mz = (sinPhi_P * sinPsi_P + cosPhi_P * sinTheta_P * cosPsi_P) * Pre_Hx + cosPhi_P * cosTheta_P * Pre_Hz;
		876	//matSetFull(matC, _mz, _Phi , (cosPhi * sinPsi - sinPhi * sinTheta * cosPsi) * Pre_Hx - sinPhi * cosTheta * Pre_Hz);
		877	//matSetFull(matC, _mz, _Theta, (cosPhi * cosTheta * cosPsi) * Pre_Hx - cosPhi * sinTheta * Pre_Hz );
		878	//matSetFull(matC, _mz, _Psi , (sinPhi * cosPsi - cosPhi * sinTheta * sinPsi) * Pre_Hx );
		879
		880	/* Runtime Optimised Version */
		881	f32_t cos45_sinTheta_cosPsi = cos45 * (sinTheta * cosPsi) ;
		882	f32_t sin45_sinPhi_sinTheta = sin45 * sinPhi * sinTheta;
		883	f32_t sin45_cosPhi_cosTheta = sin45 * cosPhi * cosTheta;
		884	f32_t cos45_sinPhi_sinPsi_cosPhi_sinTheta_cosPsi = cos45 * (sinPhi * sinPsi + cosPhi * sinTheta * cosPsi);
		885	f32_t cos45_cosTheta_sinPsi = cos45 * (cosTheta * sinPsi) ;
		886	f32_t sin45_cosPhi_cosPsi_sinPhi_sinTheta_sinPsi = sin45 * (-cosPhi * cosPsi - sinPhi * sinTheta * sinPsi);
		887	f32_t sin45_sinPhi_cosTheta_cosPsi = sin45 * (sinPhi * cosTheta * cosPsi);
		888	f32_t tmp1= (cos45_sinPhi_sinPsi_cosPhi_sinTheta_cosPsi)* Pre_Hx + (sin45_cosPhi_cosTheta) * Pre_Hz;
		889
		890	//Pre_mx = (cos45 * (cosTheta * cosPsi) + sin45 * (-cosPhi * sinPsi + sinPhi * sinTheta * cosPsi) )* Pre_Hx + (-cos45 * sinTheta + sin45 * sinPhi * cosTheta) * Pre_Hz;
		891	matSetFull(matC, _mx, _Phi , tmp1);
		892	matSetFull(matC, _mx, _Theta, (-cos45_sinTheta_cosPsi + sin45_sinPhi_cosTheta_cosPsi)* Pre_Hx + (-cos45 * cosTheta - sin45_sinPhi_sinTheta) * Pre_Hz);
		893	matSetFull(matC, _mx, _Psi , (-cos45_cosTheta_sinPsi+ sin45_cosPhi_cosPsi_sinPhi_sinTheta_sinPsi)* Pre_Hx );
		894
		895	//Pre_my = (-sin45 * (cosTheta * cosPsi) + cos45 * (-cosPhi * sinPsi + sinPhi * sinTheta * cosPsi)) * Pre_Hx + (sin45 * sinTheta + cos45 * sinPhi * cosTheta) * Pre_Hz;
		896	matSetFull(matC, _my, _Phi , tmp1);
		897	matSetFull(matC, _my, _Theta, (cos45_sinTheta_cosPsi + sin45_sinPhi_cosTheta_cosPsi) * Pre_Hx + (sin45 * cosTheta - sin45_sinPhi_sinTheta) * Pre_Hz );
		898	matSetFull(matC, _my, _Psi , (cos45_cosTheta_sinPsi + sin45_cosPhi_cosPsi_sinPhi_sinTheta_sinPsi) * Pre_Hx );
		899
		900	//Pre_mz = (sinPhi_P * sinPsi_P + cosPhi_P * sinTheta_P * cosPsi_P) * Pre_Hx + cosPhi_P * cosTheta_P * Pre_Hz;
		901	matSetFull(matC, _mz, _Phi , (cosPhi * sinPsi - sinPhi * sinTheta * cosPsi) * Pre_Hx - sinPhi * cosTheta * Pre_Hz);
		902	matSetFull(matC, _mz, _Theta, (cosPhi * cosTheta * cosPsi) * Pre_Hx - cosPhi * sinTheta * Pre_Hz );
		903	matSetFull(matC, _mz, _Psi , (sinPhi * cosPsi - cosPhi * sinTheta * sinPsi) * Pre_Hx );
		904	}
		905	#else
		906	/* ****************************************************************************
		907	Functionname: trUpdateJacobiMatrix / /!
		908	Description:
		909
		910
		911	@param[in] void
		912
		913	@return void
		914	@pre -
		915	@post -
		916	@author Michael Walter
		917	*****************************************************************************/
		918	void trUpdateJacobiMatrix(void)
		919	{
		920	/--- (SYMBOLIC) CONSTANTS ---/
		921	/*
		922	Simplified Version
		923	Pre_ax = du - sinTheta * g;
		924	Pre_ay = dv + cosTheta_sinPhi * g;
		925	Pre_az = dw + cosTheta_cosPhi * g;
		926	*/
		927
		928	/--- VARIABLES ---/
		929	f32_t Phi, Theta, Psi;
		930	f32_t g = 9.81F;
		931
		932	Phi = status.Phi;
		933	Theta = status.Theta;
		934	Psi = status.Psi;
		935
		936	sinPhi = sin(Phi);
		937	cosPhi = cos(Phi);
		938	sinTheta = sin(Theta);
		939	cosTheta = cos(Theta);
		940
		941	//Pre_ax = du - sinTheta * g;
		942	//matSetFull(matC, _ax, _Phi , 0);
		943	matSetFull(matC, _ax, _Theta, -cosTheta * g);
		944	//matSetFull(matC, _ax, _Psi , 0);
		945
		946	//Pre_ay = dv + cosTheta_sinPhi * g;
		947	matSetFull(matC, _ay, _Phi, cosTheta * cosPhi * g);
		948	matSetFull(matC, _ay, _Theta , -sinTheta * sinPhi * g);
		949	//matSetFull(matC, _ay, _Psi , 0);
		950
		951	//Pre_az = dw + cosTheta_cosPhi * g;
		952	matSetFull(matC, _az, _Phi , -cosTheta * sinPhi * g);
		953	matSetFull(matC, _az, _Theta , -sinTheta * cosPhi * g);
		954	//matSetFull(matC, _az, _Psi , 0);
		955
		956	matSetFull(matC, _compass, _Psi , 1.0F);
		957	}
		958	#endif
		959
		960
		961	#ifdef USE_Extended_MM3_Measurement_Model
		962	/*****************************************************************************
		963	Functionname: trPredictMeasurement / /!
		964	Description: Predict Measurements: AX, AY, AZ, HX, HY, HZ
		965
		966
		967	@param[in] void
		968
		969	@return void
		970	@pre -
		971	@post -
		972	@author Michael Walter
		973	*****************************************************************************/
		974	void trPredictMeasurement(void)
		975	{
		976	/--- (SYMBOLIC) CONSTANTS ---/
		977
		978	/*
		979	Simplified Version
		980	Pre_ax = du - sinTheta * g;
		981	Pre_ay = dv + cosTheta_sinPhi * g;
		982	Pre_az = dw + cosTheta_cosPhi * g;
		983	*/
		984
		985	/--- VARIABLES ---/
		986	f32_t g = 9.81F;
		987	f32_t Pre_ax, Pre_ay, Pre_az;
		988	f32_t Phi, Theta, Psi;
		989
		990	f32_t Pre_mx;
		991	f32_t Pre_my;
		992	f32_t Pre_mz;
		993
		994	f32_t Pre_Hx = 4.78F; /* horizontal field component at the current location */
		995	f32_t Pre_Hz = 8.96F; /* vertical field component at the current location */
		996
		997	f32_t cosTheta_cosPsi, sinPhi_cosTheta, cosPhi_sinPsi, sinPhi_sinTheta_cosPsi, cos45_cosTheta_cosPsi;
		998	f32_t cos45_cosPhi_sinPsi_sinPhi_sinTheta_cosPsi, cos45_sinPhi_cosTheta, cos45_sinTheta;
		999
		1000	matGetFull(matXs, _Phi, 0, &Phi);
		1001	matGetFull(matXs, _Theta, 0, &Theta);
		1002	matGetFull(matXs, _Psi, 0, &Psi);
		1003
		1004	sinPhi_P = sin(Phi);
		1005	cosPhi_P = cos(Phi);
		1006	sinTheta_P = sin(Theta);
		1007	cosTheta_P = cos(Theta);
		1008	sinPsi_P = sin(Psi);
		1009	cosPsi_P = cos(Psi);
		1010
		1011	cosTheta_cosPsi = cosTheta_P * cosPsi_P;
		1012	sinPhi_cosTheta = sinPhi_P * cosTheta_P;
		1013	cosPhi_sinPsi = cosPhi_P * sinPsi_P;
		1014	sinPhi_sinTheta_cosPsi = sinPhi_P * sinTheta_P * cosPsi_P;
		1015
		1016	cos45_cosTheta_cosPsi = cos45 * (cosTheta_cosPsi);
		1017	cos45_cosPhi_sinPsi_sinPhi_sinTheta_cosPsi = cos45 * (-cosPhi_sinPsi + sinPhi_sinTheta_cosPsi);
		1018	cos45_sinPhi_cosTheta = cos45 * sinPhi_cosTheta;
		1019	cos45_sinTheta = cos45 * sinTheta_P;
		1020
		1021	/* Simplified Version */
		1022	Pre_ax = -sinTheta_P * g;
		1023	Pre_ay = cosTheta_P * sinPhi_P * g;
		1024	Pre_az = cosTheta_P * cosPhi_P * g;
		1025
		1026	matSetFull(matYs, _ax, 0, Pre_ax);
		1027	matSetFull(matYs, _ay, 0, Pre_ay);
		1028	matSetFull(matYs, _az, 0, Pre_az);
		1029
		1030	//Pre_mx = (cos45 * (cosTheta_P * cosPsi_P) + sin45 * (-cosPhi_P * sinPsi_P + sinPhi_P * sinTheta_P * cosPsi_P) )* Pre_Hx + (-cos45 * sinTheta_P + sin45 * sinPhi_P * cosTheta_P) * Pre_Hz;
		1031	//Pre_my = (-sin45 * (cosTheta_P * cosPsi_P) + cos45 * (-cosPhi_P * sinPsi_P + sinPhi_P * sinTheta_P * cosPsi_P)) * Pre_Hx + (sin45 * sinTheta_P + cos45 * sinPhi_P * cosTheta_P) * Pre_Hz;
		1032	//Pre_mz = (sinPhi_P * sinPsi_P + cosPhi_P * sinTheta_P * cosPsi_P) * Pre_Hx + cosPhi_P * cosTheta_P * Pre_Hz;
		1033
		1034	Pre_mx = (cos45_cosTheta_cosPsi + cos45_cosPhi_sinPsi_sinPhi_sinTheta_cosPsi)* Pre_Hx + (-cos45_sinTheta + cos45_sinPhi_cosTheta) * Pre_Hz;
		1035	Pre_my = (-cos45_cosTheta_cosPsi + cos45_cosPhi_sinPsi_sinPhi_sinTheta_cosPsi) * Pre_Hx + (cos45_sinTheta + cos45_sinPhi_cosTheta) * Pre_Hz;
		1036	Pre_mz = (sinPhi_P * sinPsi_P + cosPhi_P * sinTheta_P * cosPsi_P) * Pre_Hx + cosPhi_P * cosTheta_P * Pre_Hz;
		1037
		1038	//DebugOut.Analog[3] = Pre_mx * 100;
		1039	//DebugOut.Analog[4] = Pre_my * 100;
		1040	//DebugOut.Analog[5] = Pre_mz * 100;
		1041
		1042	matSetFull(matYs, _mx, 0, Pre_mx);
		1043	matSetFull(matYs, _my, 0, Pre_my);
		1044	matSetFull(matYs, _mz, 0, Pre_mz);
		1045	}
		1046
		1047	#else
		1048	/*****************************************************************************
		1049	Functionname: trPredictMeasurement / /!
		1050	Description: Predict Measurements: AX, AY, AZ, Compass
		1051
		1052
		1053	@param[in] void
		1054
		1055	@return void
		1056	@pre -
		1057	@post -
		1058	@author Michael Walter
		1059	*****************************************************************************/
		1060	void trPredictMeasurement(void)
		1061	{
		1062	/--- (SYMBOLIC) CONSTANTS ---/
		1063
		1064	/*
		1065	Simplified Version
		1066	Pre_ax = du - sinTheta * g;
		1067	Pre_ay = dv + cosTheta_sinPhi * g;
		1068	Pre_az = dw + cosTheta_cosPhi * g;
		1069	*/
		1070
		1071	/--- VARIABLES ---/
		1072	f32_t g = 9.81F;
		1073	f32_t Pre_ax, Pre_ay, Pre_az;
		1074	f32_t Phi, Theta, Psi;
		1075
		1076	matGetFull(matXs, _Phi, 0, &Phi);
		1077	matGetFull(matXs, _Theta, 0, &Theta);
		1078	matGetFull(matXs, _Psi, 0, &Psi);
		1079
		1080	sinPhi_P = sin(Phi);
		1081	cosPhi_P = cos(Phi);
		1082	sinTheta_P = sin(Theta);
		1083	cosTheta_P = cos(Theta);
		1084
		1085	/* Simplified Version */
		1086	Pre_ax = -sinTheta_P * g;
		1087	Pre_ay = cosTheta_P * sinPhi_P * g;
		1088	Pre_az = cosTheta_P * cosPhi_P * g;
		1089
		1090	matSetFull(matYs, _ax, 0, Pre_ax);
		1091	matSetFull(matYs, _ay, 0, Pre_ay);
		1092	matSetFull(matYs, _az, 0, Pre_az);
		1093	matSetFull(matYs, _compass, 0, Psi);
		1094	}
		1095	#endif
		1096
		1097	#ifdef USE_Extended_MM3_Measurement_Model
		1098	/* ****************************************************************************
		1099	Functionname: trMeasure / /!
		1100	Description:
		1101
		1102	@param[in]
		1103
		1104	@return void
		1105	@pre -
		1106	@post -
		1107	@author Michael Walter
		1108	**************************************************************************** */
		1109	void trMeasure()
		1110	{
		1111	/--- (SYMBOLIC) CONSTANTS ---/
		1112
		1113	/--- VARIABLES ---/
		1114
		1115	f32_t ADC_ax = 0.0F;
		1116	f32_t ADC_ay = 0.0F;
		1117	f32_t ADC_az = 0.0F;
		1118
		1119	fYValid[_ax] = 1;
		1120	fYValid[_ay] = 1;
		1121	fYValid[_az] = 1;
		1122
		1123	ADC_ax = AdWertAccNick * 0.047F;
		1124	ADC_ay = -AdWertAccRoll * 0.047F;
		1125	ADC_az = AdWertAccHoch * 0.047F;
		1126
		1127	matSetFull(matY, _ax, 0, ADC_ax);
		1128	matSetFull(matY, _ay, 0, ADC_ay);
		1129	matSetFull(matY, _az, 0, ADC_az);
		1130
		1131	//if (MM3_Ready == 1)
		1132	if (1)
		1133	{
		1134	MM3_Heading();
		1135	matSetFull(matY, _mx, 0, MM3_Hx);
		1136	matSetFull(matY, _my, 0, MM3_Hy);
		1137	matSetFull(matY, _mz, 0, MM3_Hz);
		1138
		1139	fYValid[_mx] = 1;
		1140	fYValid[_my] = 1;
		1141	fYValid[_mz] = 1;
		1142	//MM3_Ready = 0;
		1143
		1144	DebugOut.Analog[13] = MM3_Hx * 100;
		1145	DebugOut.Analog[14] = MM3_Hy * 100;
		1146	DebugOut.Analog[15] = MM3_Hz * 100;
		1147	}
		1148	else
		1149	{
		1150	fYValid[_mx] = 0;
		1151	fYValid[_my] = 0;
		1152	fYValid[_mz] = 0;
		1153	}
		1154	}
		1155	#else
		1156	/* ****************************************************************************
		1157	Functionname: trMeasure / /!
		1158	Description:
		1159
		1160	@param[in]
		1161
		1162	@return void
		1163	@pre -
		1164	@post -
		1165	@author Michael Walter
		1166	**************************************************************************** */
		1167	void trMeasure()
		1168	{
		1169	/--- (SYMBOLIC) CONSTANTS ---/
		1170
		1171	/--- VARIABLES ---/
		1172
		1173	f32_t ADC_ax = 0.0F;
		1174	f32_t ADC_ay = 0.0F;
		1175	f32_t ADC_az = 0.0F;
		1176
		1177	fYValid[_ax] = 1;
		1178	fYValid[_ay] = 1;
		1179	fYValid[_az] = 1;
		1180
		1181	ADC_ax = AdWertAccNick * 0.047F;
		1182	ADC_ay = -AdWertAccRoll * 0.047F;
		1183	ADC_az = AdWertAccHoch * 0.047F;
		1184
		1185	matSetFull(matY, _ax, 0, ADC_ax);
		1186	matSetFull(matY, _ay, 0, ADC_ay);
		1187	matSetFull(matY, _az, 0, ADC_az);
		1188
		1189	static uint8_t updCompass = 0;
		1190	f32_t Psi, Compass;
		1191
		1192	if (!updCompass--)
		1193	{
		1194	updCompass = 10;
		1195	KompassValue = MM3_Heading();
		1196	}
		1197
		1198	Compass = KompassValue / 180.F * PI;
		1199
		1200	matGetFull(matXs, _Psi, 0, &Psi);
		1201
		1202	if (fabs(Compass + 2*PI - Psi) < fabs(Compass - Psi))
		1203	{
		1204	Compass += 2 * PI;
		1205	}
		1206	else if (fabs(Compass - 2*PI - Psi) < fabs(Compass - Psi))
		1207	{
		1208	Compass -= 2 * PI;
		1209	}
		1210
		1211	matSetFull(matY, _compass, 0, Compass);
		1212
		1213	/* Roll and Yaw angle are smaller 8 Deg*/
		1214	if ((abs(status.iPhi10) < 80 ) && (abs(status.iTheta10) < 80))
		1215	{
		1216	fYValid[_compass] = 1;
		1217	}
		1218	else
		1219	{
		1220	fYValid[_compass] = 0;
		1221	}
		1222	}
		1223	#endif
		1224
		1225	/* ****************************************************************************
		1226	Functionname: trUpdateBU / /!
		1227	Description: Update control matrix B and vector U
		1228
		1229
		1230	@param[in]
		1231
		1232	@return void
		1233	@pre -
		1234	@post -
		1235	@author Michael Walter
		1236	**************************************************************************** */
		1237	void trUpdateBU()
		1238	{
		1239	/--- (SYMBOLIC) CONSTANTS ---/
		1240
		1241	/--- VARIABLES ---/
		1242	/*
		1243	dPhi \| 1 sinPhi_tanTheta cosPhi_tanTheta \| p
		1244	dTheta = \| 0 cosPhi -sinPhi \| * q
		1245	dPsi \| 0 sinPhi_secTheta cosPhi_secTheta \| w
		1246	*/
		1247
		1248	f32_t ADC_p = 0.0F;
		1249	f32_t ADC_q = 0.0F;
		1250	f32_t ADC_r = 0.0F;
		1251
		1252	/* store predicted state vector in current state */
		1253	matGetFull(matXd, _Phi , 0, &Phi);
		1254	matGetFull(matXd, _Theta , 0, &Theta);
		1255
		1256	sinPhi = sin(Phi);
		1257	cosPhi = cos(Phi);
		1258
		1259	tanTheta = tan(Theta);
		1260	secTheta = 1.0F / cos(Theta);
		1261
		1262	matSetFull(matB, _Phi, _p, 1.0F);
		1263	matSetFull(matB, _Phi, _q, sinPhi * tanTheta );
		1264	matSetFull(matB, _Phi, _r, cosPhi * tanTheta );
		1265
		1266	//matSetFull(matB, _Theta, _p, 0.0F);
		1267	matSetFull(matB, _Theta, _q, cosPhi );
		1268	matSetFull(matB, _Theta, _r, -sinPhi );
		1269
		1270	//matSetFull(matB, _Psi, _p, 0.0F);
		1271	matSetFull(matB, _Psi, _q, sinPhi * secTheta );
		1272	matSetFull(matB, _Psi, _r, cosPhi * secTheta );
		1273
		1274	ADC_p = -(float) AverageRoll_X * 0.00001F * fCycleTime;
		1275	ADC_q = -(float) AverageRoll_Y * 0.00001F * fCycleTime;
		1276	ADC_r = -(float) AverageRoll_Z * 0.00001F * fCycleTime;
		1277
		1278	matSetFull(matU, _p, 0, ADC_p );
		1279	matSetFull(matU, _q, 0, ADC_q);
		1280	matSetFull(matU, _r, 0, ADC_r);
		1281	}
		1282
		1283
		1284	void AttitudeEstimation()
		1285	{
		1286	trPredictMeasurement();
		1287
		1288	trUpdateJacobiMatrix();
		1289
		1290	/* Extract measurements and store them in vector matY.
		1291	Measurement validity is indicated by fYValid[] */
		1292	trMeasure();
		1293
		1294	/* Innovation: calculate Xd, and Pd */
		1295	trInnovate();
		1296
		1297	/* Update transition matrix Phi */
		1298	trUpdatePhi();
		1299
		1300	/* Update control matrix B */
		1301	trUpdateBU();
		1302
		1303	/* Predict new state Xs and Ps */
		1304	KAFIPrediction(p_kafi);
		1305
		1306	/* store innovated state vector in current state */
		1307	matGetFull(matXs, _Phi , 0, &status.Phi);
		1308	matGetFull(matXs, _Theta , 0, &status.Theta);
		1309	matGetFull(matXs, _Psi , 0, &status.Psi);
		1310
		1311	trLimitAngles();
		1312	}
		1313
		1314
		1315
		1316	/* ****************************************************************************
		1317	Functionname: trEstimateVelocity / /!
		1318	Description:
		1319
		1320	@param[in]
		1321
		1322	@return void
		1323	@pre -
		1324	@post -
		1325	@author
		1326	**************************************************************************** */
		1327	void trEstimateVelocity()
		1328	{
		1329	/--- (SYMBOLIC) CONSTANTS ---/
		1330
		1331	/--- VARIABLES ---/
		1332
		1333	}
		1334
		1335
		1336	/* ****************************************************************************
		1337	Functionname: trLimitAngles / /!
		1338	Description:
		1339
		1340	@param[in]
		1341
		1342	@return void
		1343	@pre -
		1344	@post -
		1345	@author
		1346	**************************************************************************** */
		1347	void trLimitAngles()
		1348	{
		1349	/--- (SYMBOLIC) CONSTANTS ---/
		1350
		1351	/--- VARIABLES ---/
		1352	f32_t Psi;
		1353
		1354	if (status.Phi > 2.0F * PI)
		1355	{
		1356	status.Phi -= 2.0F * PI;
		1357	matSetFull(matXs, _Phi, 0, status.Phi);
		1358	}
		1359	if (status.Phi < -2.0F * PI)
		1360	{
		1361	status.Phi += 2.0F * PI;
		1362	matSetFull(matXs, _Phi, 0, status.Phi);
		1363	}
		1364	if (status.Theta > 2.0F * PI)
		1365	{
		1366	status.Theta -= 2.0F * PI;
		1367	matSetFull(matXs, _Theta, 0, status.Theta);
		1368	}
		1369	if (status.Theta < -2.0F * PI)
		1370	{
		1371	status.Theta += 2.0F * PI;
		1372	matSetFull(matXs, _Theta, 0, status.Theta);
		1373	}
		1374	if (status.Psi > 2.0F * PI)
		1375	{
		1376	status.Psi -= 2.0F * PI;
		1377	sollGier -= 3600;
		1378
		1379	matGetFull(matXs, _Psi, 0, &Psi);
		1380	matSetFull(matXs, _Psi , 0, Psi - 2.0F * PI);
		1381	matGetFull(matXd, _Psi, 0, &Psi);
		1382	matSetFull(matXd, _Psi , 0, Psi - 2.0F * PI);
		1383	}
		1384	if (status.Psi < 0.0F * PI)
		1385	{
		1386	status.Psi += 2.0F * PI;
		1387	sollGier += 3600;
		1388
		1389	matGetFull(matXs, _Psi, 0, &Psi);
		1390	matSetFull(matXs, _Psi , 0, Psi + 2.0F * PI);
		1391	matGetFull(matXd, _Psi, 0, &Psi);
		1392	matSetFull(matXd, _Psi , 0, Psi + 2.0F * PI);
		1393	}
		1394
		1395	status.iPhi10 = (int) (status.Phi * 573.0F);
		1396	status.iTheta10 = (int) (status.Theta * 573.0F);
		1397	status.iPsi10 = (int) (status.Psi * 573.0F);
		1398
		1399	if ((sollGier - status.iPsi10) > 3600)
		1400	{
		1401	sollGier -= 3600;
		1402	}
		1403
		1404	if ((sollGier - status.iPsi10) < -3600)
		1405	{
		1406	sollGier += 3600;
		1407	}
		1408	}
		1409
		1410

Subversion Repositories FlightCtrl

(root)/branches/KalmanFilter MikeW/KalmanFc.c - Rev 840