WebSVN - FlightCtrl - Blame - Rev 1660 - /branches/V0.84a_MartinW

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

Subversion Repositories FlightCtrl

(root)/branches/V0.84a_MartinW_Jeti+/analog.c - Rev 1660