WebSVN - FlightCtrl - Blame - Rev 1581 - /branches/WKN/V0.78b-HEF/analog.c

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

Subversion Repositories FlightCtrl

(root)/branches/WKN/V0.78b-HEF/analog.c - Rev 1581