WebSVN - FlightCtrl - Blame - Rev 1322 - /branches/Proxy sensor

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

Subversion Repositories FlightCtrl

(root)/branches/Proxy sensor - tempolo/analog.c @ 2758 - Rev 1322