WebSVN - FlightCtrl - Blame - Rev 1320 - /branches/metro/v0.82a metro

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

Subversion Repositories FlightCtrl

(root)/branches/metro/v0.82a metro_Akku+/analog.c - Rev 1320