WebSVN - FlightCtrl - Blame - Rev 1246 - /branches/MartinW

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

Subversion Repositories FlightCtrl

(root)/branches/MartinW_Jeti+/analog.c @ 2089 - Rev 1246