WebSVN - FlightCtrl - Blame - Rev 1168 - /branches/V0.88n_ACC-HH_HR

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
		82	//#######################################################################################
		83	//
		84	SIGNAL(SIG_ADC)
		85	//#######################################################################################
		86	{
		87	static unsigned char kanal=0,state = 0;
1166	hbuss	88	static signed int gier1, roll1, nick1;
		89	static signed long nick_filter, roll_filter;
		90	static signed int accy, accx;
		91	switch(state++)
		92	{
		93	case 0:
		94	nick1 = ADC;
		95	kanal = AD_ROLL;
		96	break;
		97	case 1:
		98	roll1 = ADC;
		99	kanal = AD_GIER;
		100	break;
		101	case 2:
		102	gier1 = ADC;
		103	kanal = AD_ACC_Y;
		104	break;
		105	case 3:
		106	Aktuell_ay = NeutralAccY - ADC;
		107	accy = Aktuell_ay;
		108	kanal = AD_NICK;
		109	break;
		110	case 4:
		111	nick1 += ADC;
		112	kanal = AD_ROLL;
		113	break;
		114	case 5:
		115	roll1 += ADC;
		116	kanal = AD_ACC_Z;
		117	break;
		118	case 6:
		119	AdWertAccHoch = (signed int) ADC - NeutralAccZ;
		120	if(AdWertAccHoch > 1)
		121	{
		122	if(NeutralAccZ < 750)
		123	{
		124	NeutralAccZ += 0.02;
		125	if(modell_fliegt < 500) NeutralAccZ += 0.1;
		126	}
		127	}
		128	else if(AdWertAccHoch < -1)
		129	{
		130	if(NeutralAccZ > 550)
		131	{
		132	NeutralAccZ-= 0.02;
		133	if(modell_fliegt < 500) NeutralAccZ -= 0.1;
		134	}
		135	}
		136	messanzahl_AccHoch = 1;
		137	Aktuell_az = ADC;
		138	Mess_Integral_Hoch += AdWertAccHoch; // Integrieren
		139	Mess_Integral_Hoch -= Mess_Integral_Hoch / 1024; // d�mfen
		140	kanal = AD_NICK;
		141	break;
		142	case 7:
		143	nick1 += ADC;
		144	kanal = AD_ROLL;
		145	break;
		146	case 8:
		147	roll1 += ADC;
		148	kanal = AD_ACC_X;
		149	break;
		150	case 9:
		151	Aktuell_ax = ADC - NeutralAccX;
		152	accx = Aktuell_ax;
		153	kanal = AD_GIER;
		154	break;
		155	case 10:
		156	gier1 += ADC;
		157	kanal = AD_NICK;
		158	break;
		159	case 11:
		160	nick1 += ADC;
		161	kanal = AD_ROLL;
		162	break;
		163	case 12:
		164	roll1 += ADC;
		165	kanal = AD_UBAT;
		166	break;
		167	case 13:
		168	UBat = (3 * UBat + ADC / 3) / 4;//(UBat + ((ADC * 39) / 256) + 19) / 2;
		169	kanal = AD_ACC_Y;
		170	break;
		171	case 14:
		172	Aktuell_ay = NeutralAccY - ADC;
		173	accy += Aktuell_ay;
		174	kanal = AD_NICK;
		175	break;
		176	case 15:
		177	nick1 += ADC;
		178	kanal = AD_ROLL;
		179	break;
		180	case 16:
		181	roll1 += ADC;
		182	kanal = AD_ACC_X;
		183	break;
		184	case 17:
		185	Aktuell_ax = ADC - NeutralAccX;
		186	accx += Aktuell_ax;
		187	kanal = AD_NICK;
		188	break;
		189	case 18:
		190	nick1 += ADC;
		191	kanal = AD_ROLL;
		192	break;
		193	case 19:
		194	roll1 += ADC;
		195	kanal = AD_GIER;
		196	break;
		197	case 20:
		198	gier1 += ADC;
		199	kanal = AD_ACC_Y;
		200	break;
		201	case 21:
		202	Aktuell_ay = NeutralAccY - ADC;
		203	accy += Aktuell_ay;
		204	kanal = AD_NICK;
		205	break;
		206	case 22:
		207	nick1 += ADC;
		208	kanal = AD_ROLL;
		209	break;
		210	case 23:
		211	roll1 += ADC;
		212	kanal = AD_DRUCK;
		213	break;
		214	case 24:
		215	tmpLuftdruck += ADC;
		216	if(++messanzahl_Druck >= 5)
		217	{
		218	MessLuftdruck = ADC;
		219	messanzahl_Druck = 0;
1167	hbuss	220	HoeheD = (7 * HoeheD + (int) Parameter_Luftdruck_D * (int)(255 * ExpandBaro + StartLuftdruck - tmpLuftdruck - HoehenWert)) / 8; // D-Anteil = neuerWert - AlterWert
1166	hbuss	221	Luftdruck = (tmpLuftdruck + 3 * Luftdruck) / 4;
		222	HoehenWert = 255 * ExpandBaro + StartLuftdruck - Luftdruck;
		223	tmpLuftdruck = 0;
		224	}
		225	kanal = AD_NICK;
		226	break;
		227	case 25:
		228	nick1 += ADC;
		229	kanal = AD_ROLL;
		230	break;
		231	case 26:
		232	roll1 += ADC;
		233	kanal = AD_ACC_X;
		234	break;
		235	case 27:
		236	Aktuell_ax = ADC - NeutralAccX;
		237	accx += Aktuell_ax;
		238	kanal = AD_GIER;
		239	break;
		240	case 28:
		241	if(PlatinenVersion == 10) AdWertGier = (ADC + gier1 + 2) / 4;
		242	else
		243	if(PlatinenVersion == 20) AdWertGier = 2047 - (ADC + gier1 + 1) / 2;
		244	else AdWertGier = (ADC + gier1 + 1) / 2;
		245	kanal = AD_NICK;
		246	break;
		247	case 29:
		248	nick1 += ADC;
		249	kanal = AD_ROLL;
		250	break;
		251	case 30:
		252	roll1 += ADC;
		253	kanal = AD_ACC_Y;
		254	break;
		255	case 31:
		256	Aktuell_ay = NeutralAccY - ADC;
		257	AdWertAccRoll = (Aktuell_ay + accy);
		258	kanal = AD_NICK;
		259	break;
		260	case 32:
		261	AdWertNick = (ADC + nick1 + 3) / 5;
		262	nick_filter = (long) (1 * (long) nick_filter + 4 * (long)(ADC + nick1) + 1) / 2;
		263	if(PlatinenVersion == 10) { AdWertNick /= 2;nick_filter /=2;}
		264	HiResNick = nick_filter - 20 * AdNeutralNick;
		265	AdWertNickFilter = (long)(3L * (long)AdWertNickFilter + HiResNick + 2) / 4;
		266	DebugOut.Analog[21] = AdWertNickFilter / 4;
		267	kanal = AD_ROLL;
		268	break;
		269	case 33:
		270	AdWertRoll = (ADC + roll1 + 3) / 5;
		271	roll_filter = (long)(1 * (long)roll_filter + 4 * (long)(ADC + roll1) + 1) / 2;
		272	if(PlatinenVersion == 10) { AdWertRoll /= 2;roll_filter /=2;}
		273	HiResRoll = roll_filter - 20 * AdNeutralRoll;
		274	AdWertRollFilter = (long)(3L * (long)AdWertRollFilter + HiResRoll + 2) / 4;
		275	DebugOut.Analog[22] = AdWertRollFilter / 4;
		276	kanal = AD_ACC_X;
		277	break;
		278	case 34:
		279	Aktuell_ax = ADC - NeutralAccX;
		280	AdWertAccNick = (Aktuell_ax + accx);
		281	kanal = AD_NICK;
		282	state = 0;
		283	AdReady = 1;
		284	ZaehlMessungen++;
		285	break;
		286	default:
		287	kanal = 0;
		288	state = 0;
		289	break;
		290	}
		291	ADMUX = kanal;
		292	if(state != 0) ANALOG_ON;
		293	}
		294

Subversion Repositories FlightCtrl

(root)/branches/V0.88n_ACC-HH_HR_MartinR/analog.c - Rev 1168