WebSVN - FlightCtrl - Blame - Rev 1476 - /branches/V0.76g_ACC-HH

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;
1476	MartinR	156	/* // MartinR: deaktiviert
1246	killagreg	157	if(AdWertAccHoch > 1)
1171	hbuss	158	{
1246	killagreg	159	if(NeutralAccZ < 750)
1171	hbuss	160	{
1246	killagreg	161	NeutralAccZ += 0.02;
		162	if(modell_fliegt < 500) NeutralAccZ += 0.1;
		163	}
		164	}
1171	hbuss	165	else if(AdWertAccHoch < -1)
		166	{
1246	killagreg	167	if(NeutralAccZ > 550)
1171	hbuss	168	{
		169	NeutralAccZ-= 0.02;
1246	killagreg	170	if(modell_fliegt < 500) NeutralAccZ -= 0.1;
		171	}
		172	}
1476	MartinR	173	*/ // MartinR: deaktiviert Ende
1171	hbuss	174	messanzahl_AccHoch = 1;
		175	Aktuell_az = ADC;
		176	Mess_Integral_Hoch += AdWertAccHoch; // Integrieren
		177	Mess_Integral_Hoch -= Mess_Integral_Hoch / 1024; // d�mfen
		178	kanal = AD_DRUCK;
		179	break;
		180	// "case 8:" fehlt hier absichtlich
		181	case 10:
		182	nick1 += ADC;
		183	kanal = AD_ROLL;
		184	break;
		185	case 11:
		186	roll1 += ADC;
		187	kanal = AD_GIER;
		188	break;
		189	case 12:
		190	if(PlatinenVersion == 10) AdWertGier = (ADC + gier1 + 1) / 2;
1246	killagreg	191	else
1171	hbuss	192	if(PlatinenVersion == 20) AdWertGier = 2047 - (ADC + gier1);
		193	else AdWertGier = (ADC + gier1);
		194	kanal = AD_ACC_Y;
		195	break;
		196	case 13:
		197	Aktuell_ay = NeutralAccY - ADC;
		198	AdWertAccRoll = (Aktuell_ay + accy);
		199	kanal = AD_ACC_X;
		200	break;
		201	case 14:
		202	Aktuell_ax = ADC - NeutralAccX;
		203	AdWertAccNick = (Aktuell_ax + accx);
		204	kanal = AD_NICK;
		205	break;
		206	case 15:
		207	nick1 += ADC;
1173	hbuss	208	if(PlatinenVersion == 10) nick1 = 2; else nick1 = 4;
		209	AdWertNick = nick1 / 8;
		210	nick_filter = (nick_filter + nick1) / 2;
		211	HiResNick = nick_filter - AdNeutralNick;
1171	hbuss	212	AdWertNickFilter = (AdWertNickFilter + HiResNick) / 2;
		213	kanal = AD_ROLL;
		214	break;
		215	case 16:
		216	roll1 += ADC;
1173	hbuss	217	if(PlatinenVersion == 10) roll1 = 2; else roll1 = 4;
		218	AdWertRoll = roll1 / 8;
		219	roll_filter = (roll_filter + roll1) / 2;
		220	HiResRoll = roll_filter - AdNeutralRoll;
1171	hbuss	221	AdWertRollFilter = (AdWertRollFilter + HiResRoll) / 2;
		222	kanal = AD_DRUCK;
		223	break;
		224	case 17:
		225	state = 0;
		226	AdReady = 1;
		227	ZaehlMessungen++;
		228	// "break" fehlt hier absichtlich
		229	case 9:
1253	killagreg	230	MessLuftdruck = ADC;
		231	tmpLuftdruck += MessLuftdruck;
		232	if(++messanzahl_Druck >= 18)
		233	{
1266	killagreg	234	Luftdruck = (7 * Luftdruck + tmpLuftdruck - (18 * 523) * (long)ExpandBaro + 4) / 8; // -523.19 counts per 10 counts offset step
1253	killagreg	235	HoehenWert = StartLuftdruck - Luftdruck;
1272	hbuss	236	SummenHoehe -= SummenHoehe/SM_FILTER;
1253	killagreg	237	SummenHoehe += HoehenWert;
1272	hbuss	238	VarioMeter = (15 * VarioMeter + 8 * (int)(HoehenWert - SummenHoehe/SM_FILTER))/16;
		239	tmpLuftdruck /= 2;
		240	messanzahl_Druck = 18/2;
1253	killagreg	241	}
1171	hbuss	242	kanal = AD_NICK;
		243	break;
1246	killagreg	244	default:
1171	hbuss	245	kanal = 0; state = 0; kanal = AD_NICK;
		246	break;
1246	killagreg	247	}
1171	hbuss	248	ADMUX = kanal;
		249	if(state != 0) ANALOG_ON;
1320	hbuss	250
1171	hbuss	251	}
		252
		253
		254
		255	/*
		256	//#######################################################################################
		257	//
		258	SIGNAL(SIG_ADC)
		259	//#######################################################################################
		260	{
		261	static unsigned char kanal=0,state = 0;
1166	hbuss	262	static signed int gier1, roll1, nick1;
1246	killagreg	263	static signed long nick_filter, roll_filter;
1166	hbuss	264	static signed int accy, accx;
		265	switch(state++)
		266	{
		267	case 0:
		268	nick1 = ADC;
		269	kanal = AD_ROLL;
		270	break;
		271	case 1:
		272	roll1 = ADC;
		273	kanal = AD_GIER;
		274	break;
		275	case 2:
		276	gier1 = ADC;
		277	kanal = AD_ACC_Y;
		278	break;
		279	case 3:
		280	Aktuell_ay = NeutralAccY - ADC;
		281	accy = Aktuell_ay;
		282	kanal = AD_NICK;
		283	break;
		284	case 4:
		285	nick1 += ADC;
		286	kanal = AD_ROLL;
		287	break;
		288	case 5:
		289	roll1 += ADC;
		290	kanal = AD_ACC_Z;
		291	break;
		292	case 6:
		293	AdWertAccHoch = (signed int) ADC - NeutralAccZ;
1246	killagreg	294	if(AdWertAccHoch > 1)
1166	hbuss	295	{
1246	killagreg	296	if(NeutralAccZ < 750)
1166	hbuss	297	{
1246	killagreg	298	NeutralAccZ += 0.02;
		299	if(modell_fliegt < 500) NeutralAccZ += 0.1;
		300	}
		301	}
1166	hbuss	302	else if(AdWertAccHoch < -1)
		303	{
1246	killagreg	304	if(NeutralAccZ > 550)
1166	hbuss	305	{
		306	NeutralAccZ-= 0.02;
1246	killagreg	307	if(modell_fliegt < 500) NeutralAccZ -= 0.1;
		308	}
		309	}
1166	hbuss	310	messanzahl_AccHoch = 1;
		311	Aktuell_az = ADC;
		312	Mess_Integral_Hoch += AdWertAccHoch; // Integrieren
		313	Mess_Integral_Hoch -= Mess_Integral_Hoch / 1024; // d�mfen
		314	kanal = AD_NICK;
		315	break;
		316	case 7:
		317	nick1 += ADC;
		318	kanal = AD_ROLL;
		319	break;
		320	case 8:
		321	roll1 += ADC;
		322	kanal = AD_ACC_X;
		323	break;
		324	case 9:
		325	Aktuell_ax = ADC - NeutralAccX;
		326	accx = Aktuell_ax;
		327	kanal = AD_GIER;
		328	break;
		329	case 10:
		330	gier1 += ADC;
		331	kanal = AD_NICK;
		332	break;
		333	case 11:
		334	nick1 += ADC;
		335	kanal = AD_ROLL;
		336	break;
		337	case 12:
		338	roll1 += ADC;
		339	kanal = AD_UBAT;
		340	break;
		341	case 13:
		342	UBat = (3 * UBat + ADC / 3) / 4;//(UBat + ((ADC * 39) / 256) + 19) / 2;
		343	kanal = AD_ACC_Y;
		344	break;
		345	case 14:
		346	Aktuell_ay = NeutralAccY - ADC;
		347	accy += Aktuell_ay;
		348	kanal = AD_NICK;
		349	break;
		350	case 15:
		351	nick1 += ADC;
		352	kanal = AD_ROLL;
		353	break;
		354	case 16:
		355	roll1 += ADC;
		356	kanal = AD_ACC_X;
		357	break;
		358	case 17:
		359	Aktuell_ax = ADC - NeutralAccX;
		360	accx += Aktuell_ax;
		361	kanal = AD_NICK;
		362	break;
		363	case 18:
		364	nick1 += ADC;
		365	kanal = AD_ROLL;
		366	break;
		367	case 19:
		368	roll1 += ADC;
		369	kanal = AD_GIER;
		370	break;
		371	case 20:
		372	gier1 += ADC;
		373	kanal = AD_ACC_Y;
		374	break;
		375	case 21:
		376	Aktuell_ay = NeutralAccY - ADC;
		377	accy += Aktuell_ay;
		378	kanal = AD_NICK;
		379	break;
		380	case 22:
		381	nick1 += ADC;
		382	kanal = AD_ROLL;
		383	break;
		384	case 23:
		385	roll1 += ADC;
		386	kanal = AD_DRUCK;
		387	break;
		388	case 24:
		389	tmpLuftdruck += ADC;
1246	killagreg	390	if(++messanzahl_Druck >= 5)
1166	hbuss	391	{
		392	MessLuftdruck = ADC;
		393	messanzahl_Druck = 0;
1167	hbuss	394	HoeheD = (7 * HoeheD + (int) Parameter_Luftdruck_D * (int)(255 * ExpandBaro + StartLuftdruck - tmpLuftdruck - HoehenWert)) / 8; // D-Anteil = neuerWert - AlterWert
1166	hbuss	395	Luftdruck = (tmpLuftdruck + 3 * Luftdruck) / 4;
		396	HoehenWert = 255 * ExpandBaro + StartLuftdruck - Luftdruck;
		397	tmpLuftdruck = 0;
1246	killagreg	398	}
1166	hbuss	399	kanal = AD_NICK;
		400	break;
		401	case 25:
		402	nick1 += ADC;
		403	kanal = AD_ROLL;
		404	break;
		405	case 26:
		406	roll1 += ADC;
		407	kanal = AD_ACC_X;
		408	break;
		409	case 27:
		410	Aktuell_ax = ADC - NeutralAccX;
		411	accx += Aktuell_ax;
		412	kanal = AD_GIER;
		413	break;
		414	case 28:
		415	if(PlatinenVersion == 10) AdWertGier = (ADC + gier1 + 2) / 4;
1246	killagreg	416	else
1166	hbuss	417	if(PlatinenVersion == 20) AdWertGier = 2047 - (ADC + gier1 + 1) / 2;
		418	else AdWertGier = (ADC + gier1 + 1) / 2;
		419	kanal = AD_NICK;
		420	break;
		421	case 29:
		422	nick1 += ADC;
		423	kanal = AD_ROLL;
		424	break;
		425	case 30:
		426	roll1 += ADC;
		427	kanal = AD_ACC_Y;
		428	break;
		429	case 31:
		430	Aktuell_ay = NeutralAccY - ADC;
		431	AdWertAccRoll = (Aktuell_ay + accy);
		432	kanal = AD_NICK;
		433	break;
		434	case 32:
		435	AdWertNick = (ADC + nick1 + 3) / 5;
		436	nick_filter = (long) (1 * (long) nick_filter + 4 * (long)(ADC + nick1) + 1) / 2;
		437	if(PlatinenVersion == 10) { AdWertNick /= 2;nick_filter /=2;}
		438	HiResNick = nick_filter - 20 * AdNeutralNick;
		439	AdWertNickFilter = (long)(3L * (long)AdWertNickFilter + HiResNick + 2) / 4;
1246	killagreg	440	DebugOut.Analog[21] = AdWertNickFilter / 4;
1166	hbuss	441	kanal = AD_ROLL;
		442	break;
		443	case 33:
		444	AdWertRoll = (ADC + roll1 + 3) / 5;
		445	roll_filter = (long)(1 * (long)roll_filter + 4 * (long)(ADC + roll1) + 1) / 2;
		446	if(PlatinenVersion == 10) { AdWertRoll /= 2;roll_filter /=2;}
		447	HiResRoll = roll_filter - 20 * AdNeutralRoll;
		448	AdWertRollFilter = (long)(3L * (long)AdWertRollFilter + HiResRoll + 2) / 4;
		449	DebugOut.Analog[22] = AdWertRollFilter / 4;
		450	kanal = AD_ACC_X;
		451	break;
		452	case 34:
		453	Aktuell_ax = ADC - NeutralAccX;
		454	AdWertAccNick = (Aktuell_ax + accx);
		455	kanal = AD_NICK;
		456	state = 0;
		457	AdReady = 1;
		458	ZaehlMessungen++;
		459	break;
1246	killagreg	460	default:
1166	hbuss	461	kanal = 0;
		462	state = 0;
		463	break;
1246	killagreg	464	}
1166	hbuss	465	ADMUX = kanal;
		466	if(state != 0) ANALOG_ON;
		467	}
1171	hbuss	468	*/

Subversion Repositories FlightCtrl

(root)/branches/V0.76g_ACC-HH_MartinR/analog.c - Rev 1476