WebSVN - FlightCtrl - Blame - Rev 1340

Rev	Author	Line No.	Line
1	ingob	1	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1340	ingob	2	// + Copyright (c) Holger Buss, Ingo Busker
1	ingob	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;
1173	hbuss	199	if(PlatinenVersion == 10) nick1 = 2; else nick1 = 4;
		200	AdWertNick = nick1 / 8;
		201	nick_filter = (nick_filter + nick1) / 2;
		202	HiResNick = nick_filter - AdNeutralNick;
1171	hbuss	203	AdWertNickFilter = (AdWertNickFilter + HiResNick) / 2;
		204	kanal = AD_ROLL;
		205	break;
		206	case 16:
		207	roll1 += ADC;
1173	hbuss	208	if(PlatinenVersion == 10) roll1 = 2; else roll1 = 4;
		209	AdWertRoll = roll1 / 8;
		210	roll_filter = (roll_filter + roll1) / 2;
		211	HiResRoll = roll_filter - AdNeutralRoll;
1171	hbuss	212	AdWertRollFilter = (AdWertRollFilter + HiResRoll) / 2;
		213	kanal = AD_DRUCK;
		214	break;
		215	case 17:
		216	state = 0;
		217	AdReady = 1;
		218	ZaehlMessungen++;
		219	// "break" fehlt hier absichtlich
		220	case 9:
		221	tmpLuftdruck += ADC;
		222	if(++messanzahl_Druck >= 5)
		223	{
		224	tmpLuftdruck /= 2;
		225	MessLuftdruck = ADC;
		226	messanzahl_Druck = 0;
		227	HoeheD = (31 * HoeheD + (int) Parameter_Luftdruck_D * (int)(255 * ExpandBaro + StartLuftdruck - tmpLuftdruck - HoehenWert))/32; // D-Anteil = neuerWert - AlterWert
		228	Luftdruck = (tmpLuftdruck + 7 * Luftdruck + 4) / 8;
		229	HoehenWert = 255 * ExpandBaro + StartLuftdruck - Luftdruck;
		230	tmpLuftdruck /= 2;
		231	}
		232	kanal = AD_NICK;
		233	break;
		234	default:
		235	kanal = 0; state = 0; kanal = AD_NICK;
		236	break;
		237	}
		238	ADMUX = kanal;
		239	if(state != 0) ANALOG_ON;
		240	}
		241
		242
		243
		244	/*
		245	//#######################################################################################
		246	//
		247	SIGNAL(SIG_ADC)
		248	//#######################################################################################
		249	{
		250	static unsigned char kanal=0,state = 0;
1166	hbuss	251	static signed int gier1, roll1, nick1;
		252	static signed long nick_filter, roll_filter;
		253	static signed int accy, accx;
		254	switch(state++)
		255	{
		256	case 0:
		257	nick1 = ADC;
		258	kanal = AD_ROLL;
		259	break;
		260	case 1:
		261	roll1 = ADC;
		262	kanal = AD_GIER;
		263	break;
		264	case 2:
		265	gier1 = ADC;
		266	kanal = AD_ACC_Y;
		267	break;
		268	case 3:
		269	Aktuell_ay = NeutralAccY - ADC;
		270	accy = Aktuell_ay;
		271	kanal = AD_NICK;
		272	break;
		273	case 4:
		274	nick1 += ADC;
		275	kanal = AD_ROLL;
		276	break;
		277	case 5:
		278	roll1 += ADC;
		279	kanal = AD_ACC_Z;
		280	break;
		281	case 6:
		282	AdWertAccHoch = (signed int) ADC - NeutralAccZ;
		283	if(AdWertAccHoch > 1)
		284	{
		285	if(NeutralAccZ < 750)
		286	{
		287	NeutralAccZ += 0.02;
		288	if(modell_fliegt < 500) NeutralAccZ += 0.1;
		289	}
		290	}
		291	else if(AdWertAccHoch < -1)
		292	{
		293	if(NeutralAccZ > 550)
		294	{
		295	NeutralAccZ-= 0.02;
		296	if(modell_fliegt < 500) NeutralAccZ -= 0.1;
		297	}
		298	}
		299	messanzahl_AccHoch = 1;
		300	Aktuell_az = ADC;
		301	Mess_Integral_Hoch += AdWertAccHoch; // Integrieren
		302	Mess_Integral_Hoch -= Mess_Integral_Hoch / 1024; // d�mfen
		303	kanal = AD_NICK;
		304	break;
		305	case 7:
		306	nick1 += ADC;
		307	kanal = AD_ROLL;
		308	break;
		309	case 8:
		310	roll1 += ADC;
		311	kanal = AD_ACC_X;
		312	break;
		313	case 9:
		314	Aktuell_ax = ADC - NeutralAccX;
		315	accx = Aktuell_ax;
		316	kanal = AD_GIER;
		317	break;
		318	case 10:
		319	gier1 += ADC;
		320	kanal = AD_NICK;
		321	break;
		322	case 11:
		323	nick1 += ADC;
		324	kanal = AD_ROLL;
		325	break;
		326	case 12:
		327	roll1 += ADC;
		328	kanal = AD_UBAT;
		329	break;
		330	case 13:
		331	UBat = (3 * UBat + ADC / 3) / 4;//(UBat + ((ADC * 39) / 256) + 19) / 2;
		332	kanal = AD_ACC_Y;
		333	break;
		334	case 14:
		335	Aktuell_ay = NeutralAccY - ADC;
		336	accy += Aktuell_ay;
		337	kanal = AD_NICK;
		338	break;
		339	case 15:
		340	nick1 += ADC;
		341	kanal = AD_ROLL;
		342	break;
		343	case 16:
		344	roll1 += ADC;
		345	kanal = AD_ACC_X;
		346	break;
		347	case 17:
		348	Aktuell_ax = ADC - NeutralAccX;
		349	accx += Aktuell_ax;
		350	kanal = AD_NICK;
		351	break;
		352	case 18:
		353	nick1 += ADC;
		354	kanal = AD_ROLL;
		355	break;
		356	case 19:
		357	roll1 += ADC;
		358	kanal = AD_GIER;
		359	break;
		360	case 20:
		361	gier1 += ADC;
		362	kanal = AD_ACC_Y;
		363	break;
		364	case 21:
		365	Aktuell_ay = NeutralAccY - ADC;
		366	accy += Aktuell_ay;
		367	kanal = AD_NICK;
		368	break;
		369	case 22:
		370	nick1 += ADC;
		371	kanal = AD_ROLL;
		372	break;
		373	case 23:
		374	roll1 += ADC;
		375	kanal = AD_DRUCK;
		376	break;
		377	case 24:
		378	tmpLuftdruck += ADC;
		379	if(++messanzahl_Druck >= 5)
		380	{
		381	MessLuftdruck = ADC;
		382	messanzahl_Druck = 0;
1167	hbuss	383	HoeheD = (7 * HoeheD + (int) Parameter_Luftdruck_D * (int)(255 * ExpandBaro + StartLuftdruck - tmpLuftdruck - HoehenWert)) / 8; // D-Anteil = neuerWert - AlterWert
1166	hbuss	384	Luftdruck = (tmpLuftdruck + 3 * Luftdruck) / 4;
		385	HoehenWert = 255 * ExpandBaro + StartLuftdruck - Luftdruck;
		386	tmpLuftdruck = 0;
		387	}
		388	kanal = AD_NICK;
		389	break;
		390	case 25:
		391	nick1 += ADC;
		392	kanal = AD_ROLL;
		393	break;
		394	case 26:
		395	roll1 += ADC;
		396	kanal = AD_ACC_X;
		397	break;
		398	case 27:
		399	Aktuell_ax = ADC - NeutralAccX;
		400	accx += Aktuell_ax;
		401	kanal = AD_GIER;
		402	break;
		403	case 28:
		404	if(PlatinenVersion == 10) AdWertGier = (ADC + gier1 + 2) / 4;
		405	else
		406	if(PlatinenVersion == 20) AdWertGier = 2047 - (ADC + gier1 + 1) / 2;
		407	else AdWertGier = (ADC + gier1 + 1) / 2;
		408	kanal = AD_NICK;
		409	break;
		410	case 29:
		411	nick1 += ADC;
		412	kanal = AD_ROLL;
		413	break;
		414	case 30:
		415	roll1 += ADC;
		416	kanal = AD_ACC_Y;
		417	break;
		418	case 31:
		419	Aktuell_ay = NeutralAccY - ADC;
		420	AdWertAccRoll = (Aktuell_ay + accy);
		421	kanal = AD_NICK;
		422	break;
		423	case 32:
		424	AdWertNick = (ADC + nick1 + 3) / 5;
		425	nick_filter = (long) (1 * (long) nick_filter + 4 * (long)(ADC + nick1) + 1) / 2;
		426	if(PlatinenVersion == 10) { AdWertNick /= 2;nick_filter /=2;}
		427	HiResNick = nick_filter - 20 * AdNeutralNick;
		428	AdWertNickFilter = (long)(3L * (long)AdWertNickFilter + HiResNick + 2) / 4;
		429	DebugOut.Analog[21] = AdWertNickFilter / 4;
		430	kanal = AD_ROLL;
		431	break;
		432	case 33:
		433	AdWertRoll = (ADC + roll1 + 3) / 5;
		434	roll_filter = (long)(1 * (long)roll_filter + 4 * (long)(ADC + roll1) + 1) / 2;
		435	if(PlatinenVersion == 10) { AdWertRoll /= 2;roll_filter /=2;}
		436	HiResRoll = roll_filter - 20 * AdNeutralRoll;
		437	AdWertRollFilter = (long)(3L * (long)AdWertRollFilter + HiResRoll + 2) / 4;
		438	DebugOut.Analog[22] = AdWertRollFilter / 4;
		439	kanal = AD_ACC_X;
		440	break;
		441	case 34:
		442	Aktuell_ax = ADC - NeutralAccX;
		443	AdWertAccNick = (Aktuell_ax + accx);
		444	kanal = AD_NICK;
		445	state = 0;
		446	AdReady = 1;
		447	ZaehlMessungen++;
		448	break;
		449	default:
		450	kanal = 0;
		451	state = 0;
		452	break;
		453	}
		454	ADMUX = kanal;
		455	if(state != 0) ANALOG_ON;
		456	}
1171	hbuss	457	*/

Subversion Repositories FlightCtrl

(root)/tags/V0.72p/analog.c - Rev 1340