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

Subversion Repositories FlightCtrl

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