WebSVN - FlightCtrl - Blame - Rev 1171

Rev	Author	Line No.	Line
1	ingob	1	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		2	// + Copyright (c) 04.2007 Holger Buss
		3	// + only for non-profit use
		4	// + www.MikroKopter.com
		5	// + see the File "License.txt" for further Informations
		6	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		7
		8	#include "main.h"
		9	volatile int Aktuell_Nick,Aktuell_Roll,Aktuell_Gier,Aktuell_ax, Aktuell_ay,Aktuell_az, UBat = 100;
1166	hbuss	10	volatile int AdWertNickFilter = 0, AdWertRollFilter = 0, AdWertGierFilter = 0;
		11	volatile int HiResNick = 2500, HiResRoll = 2500;
380	hbuss	12	volatile int AdWertNick = 0, AdWertRoll = 0, AdWertGier = 0;
		13	volatile int AdWertAccRoll = 0,AdWertAccNick = 0,AdWertAccHoch = 0;
1166	hbuss	14	volatile char messanzahl_AccHoch = 0;
1	ingob	15	volatile long Luftdruck = 32000;
		16	volatile int StartLuftdruck;
		17	volatile unsigned int MessLuftdruck = 1023;
		18	unsigned char DruckOffsetSetting;
1036	hbuss	19	signed char ExpandBaro = 0;
1	ingob	20	volatile int HoeheD = 0;
		21	volatile char messanzahl_Druck;
		22	volatile int tmpLuftdruck;
		23	volatile unsigned int ZaehlMessungen = 0;
918	hbuss	24	unsigned char AnalogOffsetNick = 115,AnalogOffsetRoll = 115,AnalogOffsetGier = 115;
		25	unsigned char GyroDefektN = 0,GyroDefektR = 0,GyroDefektG = 0;
1168	hbuss	26	volatile unsigned char AdReady = 1;
1	ingob	27	//#######################################################################################
		28	//
		29	void ADC_Init(void)
		30	//#######################################################################################
		31	{
		32	ADMUX = 0;//Referenz ist extern
1155	hbuss	33	ANALOG_ON;
1	ingob	34	}
		35
		36	void SucheLuftruckOffset(void)
		37	{
		38	unsigned int off;
173	holgerb	39	off = eeprom_read_byte(&EEPromArray[EEPROM_ADR_LAST_OFFSET]);
		40	if(off > 20) off -= 10;
		41	OCR0A = off;
1036	hbuss	42	ExpandBaro = 0;
380	hbuss	43	Delay_ms_Mess(100);
173	holgerb	44	if(MessLuftdruck < 850) off = 0;
		45	for(; off < 250;off++)
1	ingob	46	{
		47	OCR0A = off;
380	hbuss	48	Delay_ms_Mess(50);
1	ingob	49	printf(".");
1036	hbuss	50	if(MessLuftdruck < 850) break;
1	ingob	51	}
173	holgerb	52	eeprom_write_byte(&EEPromArray[EEPROM_ADR_LAST_OFFSET], off);
		53	DruckOffsetSetting = off;
380	hbuss	54	Delay_ms_Mess(300);
1	ingob	55	}
		56
918	hbuss	57	void SucheGyroOffset(void)
		58	{
		59	unsigned char i, ready = 0;
		60	GyroDefektN = 0; GyroDefektR = 0; GyroDefektG = 0;
		61	for(i=140; i != 0; i--)
		62	{
921	hbuss	63	if(ready == 3 && i > 10) i = 9;
918	hbuss	64	ready = 0;
		65	if(AdWertNick < 1020) AnalogOffsetNick--; else if(AdWertNick > 1030) AnalogOffsetNick++; else ready++;
		66	if(AdWertRoll < 1020) AnalogOffsetRoll--; else if(AdWertRoll > 1030) AnalogOffsetRoll++; else ready++;
		67	if(AdWertGier < 1020) AnalogOffsetGier--; else if(AdWertGier > 1030) AnalogOffsetGier++; else ready++;
		68	twi_state = 8;
		69	i2c_start();
		70	if(AnalogOffsetNick < 10) { GyroDefektN = 1; AnalogOffsetNick = 10;}; if(AnalogOffsetNick > 245) { GyroDefektN = 1; AnalogOffsetNick = 245;};
		71	if(AnalogOffsetRoll < 10) { GyroDefektR = 1; AnalogOffsetRoll = 10;}; if(AnalogOffsetRoll > 245) { GyroDefektR = 1; AnalogOffsetRoll = 245;};
		72	if(AnalogOffsetGier < 10) { GyroDefektG = 1; AnalogOffsetGier = 10;}; if(AnalogOffsetGier > 245) { GyroDefektG = 1; AnalogOffsetGier = 245;};
		73	while(twi_state);
		74	messanzahl_Druck = 0;
		75	ANALOG_ON;
		76	while(messanzahl_Druck == 0);
		77	if(i<10) Delay_ms_Mess(10);
		78	}
921	hbuss	79	Delay_ms_Mess(70);
918	hbuss	80	}
1	ingob	81
1171	hbuss	82	/*
		83
		84	1 r
		85	2 g
		86	3 y
		87	4 x
		88	5 n
		89	6 r
		90	7 u
		91	8 z
		92	9 L
		93	10 n
		94	11 r
		95	12 g
		96	13 y
		97	14 x
		98	15 n
		99	16 r
		100	17 L
		101	*/
		102
1	ingob	103	//#######################################################################################
		104	//
		105	SIGNAL(SIG_ADC)
		106	//#######################################################################################
		107	{
		108	static unsigned char kanal=0,state = 0;
1171	hbuss	109	static signed int gier1, roll1, nick1, nick_filter, roll_filter;
		110	static signed int accy, accx;
		111	switch(state++)
		112	{
		113	case 0:
		114	nick1 = ADC;
		115	kanal = AD_ROLL;
		116	break;
		117	case 1:
		118	roll1 = ADC;
		119	kanal = AD_GIER;
		120	break;
		121	case 2:
		122	gier1 = ADC;
		123	kanal = AD_ACC_Y;
		124	break;
		125	case 3:
		126	Aktuell_ay = NeutralAccY - ADC;
		127	accy = Aktuell_ay;
		128	kanal = AD_ACC_X;
		129	break;
		130	case 4:
		131	Aktuell_ax = ADC - NeutralAccX;
		132	accx = Aktuell_ax;
		133	kanal = AD_NICK;
		134	break;
		135	case 5:
		136	nick1 += ADC;
		137	kanal = AD_ROLL;
		138	break;
		139	case 6:
		140	roll1 += ADC;
		141	kanal = AD_UBAT;
		142	break;
		143	case 7:
		144	UBat = (3 * UBat + ADC / 3) / 4;
		145	kanal = AD_ACC_Z;
		146	break;
		147	case 8:
		148	AdWertAccHoch = (signed int) ADC - NeutralAccZ;
		149	if(AdWertAccHoch > 1)
		150	{
		151	if(NeutralAccZ < 750)
		152	{
		153	NeutralAccZ += 0.02;
		154	if(modell_fliegt < 500) NeutralAccZ += 0.1;
		155	}
		156	}
		157	else if(AdWertAccHoch < -1)
		158	{
		159	if(NeutralAccZ > 550)
		160	{
		161	NeutralAccZ-= 0.02;
		162	if(modell_fliegt < 500) NeutralAccZ -= 0.1;
		163	}
		164	}
		165	messanzahl_AccHoch = 1;
		166	Aktuell_az = ADC;
		167	Mess_Integral_Hoch += AdWertAccHoch; // Integrieren
		168	Mess_Integral_Hoch -= Mess_Integral_Hoch / 1024; // d�mfen
		169	kanal = AD_DRUCK;
		170	break;
		171	// "case 8:" fehlt hier absichtlich
		172	case 10:
		173	nick1 += ADC;
		174	kanal = AD_ROLL;
		175	break;
		176	case 11:
		177	roll1 += ADC;
		178	kanal = AD_GIER;
		179	break;
		180	case 12:
		181	if(PlatinenVersion == 10) AdWertGier = (ADC + gier1 + 1) / 2;
		182	else
		183	if(PlatinenVersion == 20) AdWertGier = 2047 - (ADC + gier1);
		184	else AdWertGier = (ADC + gier1);
		185	kanal = AD_ACC_Y;
		186	break;
		187	case 13:
		188	Aktuell_ay = NeutralAccY - ADC;
		189	AdWertAccRoll = (Aktuell_ay + accy);
		190	kanal = AD_ACC_X;
		191	break;
		192	case 14:
		193	Aktuell_ax = ADC - NeutralAccX;
		194	AdWertAccNick = (Aktuell_ax + accx);
		195	kanal = AD_NICK;
		196	break;
		197	case 15:
		198	nick1 += ADC;
		199	AdWertNick = nick1 / 2;
		200	nick_filter = (nick_filter + 4 * nick1) / 2;
		201	if(PlatinenVersion == 10) { AdWertNick /= 2;nick_filter /=2;}
		202	HiResNick = nick_filter - 8 * AdNeutralNick;
		203	AdWertNickFilter = (AdWertNickFilter + HiResNick) / 2;
		204	// DebugOut.Analog[21] = AdWertNickFilter / 4;
		205	kanal = AD_ROLL;
		206	break;
		207	case 16:
		208	roll1 += ADC;
		209	AdWertRoll = roll1 / 2;
		210	roll_filter = (roll_filter + 4 * roll1) / 2;
		211	if(PlatinenVersion == 10) { AdWertRoll /= 2;roll_filter /=2;}
		212	HiResRoll = roll_filter - 8 * AdNeutralRoll;
		213	AdWertRollFilter = (AdWertRollFilter + HiResRoll) / 2;
		214	// DebugOut.Analog[22] = AdWertRollFilter / 4;
		215	kanal = AD_DRUCK;
		216	break;
		217	case 17:
		218	state = 0;
		219	AdReady = 1;
		220	ZaehlMessungen++;
		221	// "break" fehlt hier absichtlich
		222	case 9:
		223	tmpLuftdruck += ADC;
		224	if(++messanzahl_Druck >= 5)
		225	{
		226	tmpLuftdruck /= 2;
		227	MessLuftdruck = ADC;
		228	messanzahl_Druck = 0;
		229	HoeheD = (31 * HoeheD + (int) Parameter_Luftdruck_D * (int)(255 * ExpandBaro + StartLuftdruck - tmpLuftdruck - HoehenWert))/32; // D-Anteil = neuerWert - AlterWert
		230	DebugOut.Analog[24] = 255 * ExpandBaro + StartLuftdruck - tmpLuftdruck;
		231	DebugOut.Analog[23] = HoeheD;
		232	Luftdruck = (tmpLuftdruck + 7 * Luftdruck + 4) / 8;
		233	HoehenWert = 255 * ExpandBaro + StartLuftdruck - Luftdruck;
		234	tmpLuftdruck /= 2;
		235	}
		236	kanal = AD_NICK;
		237	break;
		238	default:
		239	kanal = 0; state = 0; kanal = AD_NICK;
		240	break;
		241	}
		242	ADMUX = kanal;
		243	if(state != 0) ANALOG_ON;
		244	}
		245
		246
		247
		248	/*
		249	//#######################################################################################
		250	//
		251	SIGNAL(SIG_ADC)
		252	//#######################################################################################
		253	{
		254	static unsigned char kanal=0,state = 0;
1166	hbuss	255	static signed int gier1, roll1, nick1;
		256	static signed long nick_filter, roll_filter;
		257	static signed int accy, accx;
		258	switch(state++)
		259	{
		260	case 0:
		261	nick1 = ADC;
		262	kanal = AD_ROLL;
		263	break;
		264	case 1:
		265	roll1 = ADC;
		266	kanal = AD_GIER;
		267	break;
		268	case 2:
		269	gier1 = ADC;
		270	kanal = AD_ACC_Y;
		271	break;
		272	case 3:
		273	Aktuell_ay = NeutralAccY - ADC;
		274	accy = Aktuell_ay;
		275	kanal = AD_NICK;
		276	break;
		277	case 4:
		278	nick1 += ADC;
		279	kanal = AD_ROLL;
		280	break;
		281	case 5:
		282	roll1 += ADC;
		283	kanal = AD_ACC_Z;
		284	break;
		285	case 6:
		286	AdWertAccHoch = (signed int) ADC - NeutralAccZ;
		287	if(AdWertAccHoch > 1)
		288	{
		289	if(NeutralAccZ < 750)
		290	{
		291	NeutralAccZ += 0.02;
		292	if(modell_fliegt < 500) NeutralAccZ += 0.1;
		293	}
		294	}
		295	else if(AdWertAccHoch < -1)
		296	{
		297	if(NeutralAccZ > 550)
		298	{
		299	NeutralAccZ-= 0.02;
		300	if(modell_fliegt < 500) NeutralAccZ -= 0.1;
		301	}
		302	}
		303	messanzahl_AccHoch = 1;
		304	Aktuell_az = ADC;
		305	Mess_Integral_Hoch += AdWertAccHoch; // Integrieren
		306	Mess_Integral_Hoch -= Mess_Integral_Hoch / 1024; // d�mfen
		307	kanal = AD_NICK;
		308	break;
		309	case 7:
		310	nick1 += ADC;
		311	kanal = AD_ROLL;
		312	break;
		313	case 8:
		314	roll1 += ADC;
		315	kanal = AD_ACC_X;
		316	break;
		317	case 9:
		318	Aktuell_ax = ADC - NeutralAccX;
		319	accx = Aktuell_ax;
		320	kanal = AD_GIER;
		321	break;
		322	case 10:
		323	gier1 += ADC;
		324	kanal = AD_NICK;
		325	break;
		326	case 11:
		327	nick1 += ADC;
		328	kanal = AD_ROLL;
		329	break;
		330	case 12:
		331	roll1 += ADC;
		332	kanal = AD_UBAT;
		333	break;
		334	case 13:
		335	UBat = (3 * UBat + ADC / 3) / 4;//(UBat + ((ADC * 39) / 256) + 19) / 2;
		336	kanal = AD_ACC_Y;
		337	break;
		338	case 14:
		339	Aktuell_ay = NeutralAccY - ADC;
		340	accy += Aktuell_ay;
		341	kanal = AD_NICK;
		342	break;
		343	case 15:
		344	nick1 += ADC;
		345	kanal = AD_ROLL;
		346	break;
		347	case 16:
		348	roll1 += ADC;
		349	kanal = AD_ACC_X;
		350	break;
		351	case 17:
		352	Aktuell_ax = ADC - NeutralAccX;
		353	accx += Aktuell_ax;
		354	kanal = AD_NICK;
		355	break;
		356	case 18:
		357	nick1 += ADC;
		358	kanal = AD_ROLL;
		359	break;
		360	case 19:
		361	roll1 += ADC;
		362	kanal = AD_GIER;
		363	break;
		364	case 20:
		365	gier1 += ADC;
		366	kanal = AD_ACC_Y;
		367	break;
		368	case 21:
		369	Aktuell_ay = NeutralAccY - ADC;
		370	accy += Aktuell_ay;
		371	kanal = AD_NICK;
		372	break;
		373	case 22:
		374	nick1 += ADC;
		375	kanal = AD_ROLL;
		376	break;
		377	case 23:
		378	roll1 += ADC;
		379	kanal = AD_DRUCK;
		380	break;
		381	case 24:
		382	tmpLuftdruck += ADC;
		383	if(++messanzahl_Druck >= 5)
		384	{
		385	MessLuftdruck = ADC;
		386	messanzahl_Druck = 0;
1167	hbuss	387	HoeheD = (7 * HoeheD + (int) Parameter_Luftdruck_D * (int)(255 * ExpandBaro + StartLuftdruck - tmpLuftdruck - HoehenWert)) / 8; // D-Anteil = neuerWert - AlterWert
1166	hbuss	388	Luftdruck = (tmpLuftdruck + 3 * Luftdruck) / 4;
		389	HoehenWert = 255 * ExpandBaro + StartLuftdruck - Luftdruck;
		390	tmpLuftdruck = 0;
		391	}
		392	kanal = AD_NICK;
		393	break;
		394	case 25:
		395	nick1 += ADC;
		396	kanal = AD_ROLL;
		397	break;
		398	case 26:
		399	roll1 += ADC;
		400	kanal = AD_ACC_X;
		401	break;
		402	case 27:
		403	Aktuell_ax = ADC - NeutralAccX;
		404	accx += Aktuell_ax;
		405	kanal = AD_GIER;
		406	break;
		407	case 28:
		408	if(PlatinenVersion == 10) AdWertGier = (ADC + gier1 + 2) / 4;
		409	else
		410	if(PlatinenVersion == 20) AdWertGier = 2047 - (ADC + gier1 + 1) / 2;
		411	else AdWertGier = (ADC + gier1 + 1) / 2;
		412	kanal = AD_NICK;
		413	break;
		414	case 29:
		415	nick1 += ADC;
		416	kanal = AD_ROLL;
		417	break;
		418	case 30:
		419	roll1 += ADC;
		420	kanal = AD_ACC_Y;
		421	break;
		422	case 31:
		423	Aktuell_ay = NeutralAccY - ADC;
		424	AdWertAccRoll = (Aktuell_ay + accy);
		425	kanal = AD_NICK;
		426	break;
		427	case 32:
		428	AdWertNick = (ADC + nick1 + 3) / 5;
		429	nick_filter = (long) (1 * (long) nick_filter + 4 * (long)(ADC + nick1) + 1) / 2;
		430	if(PlatinenVersion == 10) { AdWertNick /= 2;nick_filter /=2;}
		431	HiResNick = nick_filter - 20 * AdNeutralNick;
		432	AdWertNickFilter = (long)(3L * (long)AdWertNickFilter + HiResNick + 2) / 4;
		433	DebugOut.Analog[21] = AdWertNickFilter / 4;
		434	kanal = AD_ROLL;
		435	break;
		436	case 33:
		437	AdWertRoll = (ADC + roll1 + 3) / 5;
		438	roll_filter = (long)(1 * (long)roll_filter + 4 * (long)(ADC + roll1) + 1) / 2;
		439	if(PlatinenVersion == 10) { AdWertRoll /= 2;roll_filter /=2;}
		440	HiResRoll = roll_filter - 20 * AdNeutralRoll;
		441	AdWertRollFilter = (long)(3L * (long)AdWertRollFilter + HiResRoll + 2) / 4;
		442	DebugOut.Analog[22] = AdWertRollFilter / 4;
		443	kanal = AD_ACC_X;
		444	break;
		445	case 34:
		446	Aktuell_ax = ADC - NeutralAccX;
		447	AdWertAccNick = (Aktuell_ax + accx);
		448	kanal = AD_NICK;
		449	state = 0;
		450	AdReady = 1;
		451	ZaehlMessungen++;
		452	break;
		453	default:
		454	kanal = 0;
		455	state = 0;
		456	break;
		457	}
		458	ADMUX = kanal;
		459	if(state != 0) ANALOG_ON;
		460	}
1171	hbuss	461	*/

Subversion Repositories FlightCtrl

(root)/tags/V0.74d/analog.c @ 1340 - Rev 1171