WebSVN - FlightCtrl - Blame - Rev 966 - /branches/KalmanFilter MikeW/twimaster.c

Rev	Author	Line No.	Line
838	MikeW	1	/*############################################################################
		2	############################################################################*/
		3
		4	#include "main.h"
		5	#include "kafi.h"
		6
		7	unsigned char twi_state = 0;
		8	unsigned char motor = 0;
		9	unsigned char motorread = 0;
		10	unsigned char motor_rx[8];
		11
		12	unsigned int TriggerMagneticHeading = 0;
		13	unsigned int TriggerSonicReading = 0;
		14
		15
		16	//############################################################################
		17	//Initzialisieren der I2C (TWI) Schnittstelle
		18	void i2c_init(void)
		19	//############################################################################
		20	{
966	MikeW	21	TWSR = 0;
		22	TWBR = ((SYSCLK/SCL_CLOCK)-16)/2;
838	MikeW	23	}
		24
		25	//############################################################################
		26	//Start I2C
		27	char i2c_start(void)
		28	//############################################################################
		29	{
966	MikeW	30	TWCR = (1<<TWSTA) \| (1<<TWEN) \| (1<<TWINT) \| (1<<TWIE);
		31	return(0);
838	MikeW	32	}
		33
		34	//############################################################################
		35	//Start I2C
		36	void i2c_stop(void)
		37	//############################################################################
		38	{
966	MikeW	39	TWCR = (1<<TWEN) \| (1<<TWSTO) \| (1<<TWINT);
838	MikeW	40	}
		41
		42	void i2c_reset(void)
		43	//############################################################################
		44	{
966	MikeW	45	i2c_stop();
		46	twi_state = 0;
		47	motor = TWDR;
		48	motor = 0;
		49	TWCR = 0x80;
		50	TWAMR = 0;
		51	TWAR = 0;
		52	TWDR = 0;
		53	TWSR = 0;
		54	TWBR = 0;
		55	i2c_init();
		56	i2c_start();
		57	i2c_write_byte(0);
838	MikeW	58	}
		59
		60	//############################################################################
		61	//Start I2C
		62	char i2c_write_byte(char byte)
		63	//############################################################################
		64	{
966	MikeW	65	TWSR = 0x00;
		66	TWDR = byte;
		67	TWCR = (1<<TWINT) \| (1<<TWEN) \| (1<<TWIE);
		68
		69	return(0);
838	MikeW	70	}
		71
		72	//############################################################################
		73	//Start I2C
		74	SIGNAL (TWI_vect)
		75	//############################################################################
		76	{
966	MikeW	77	#if 0
		78	int motorwert = 0;
		79	int scale_p = (int) (MAX(0, (PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 170)) / 4.F) / 6;
		80	int scale_d = (int) (MAX(0, (PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 170)) / 4.F) / 6;
838	MikeW	81	#endif
966	MikeW	82	switch (twi_state++)
		83	{
		84	case 0:
		85	i2c_write_byte(0x52+(motor*2));
		86	break;
		87	case 1:
		88	switch(motor++)
		89	{
		90	case 0:
		91	#if 0
		92	pd_ergebnis = (int) (scale_p* DiffNick + scale_d * (AdWertNick - AdNeutralNick - Roll_Y_Off)) / 10;
		93	if(pd_ergebnis > (GasMischanteil + abs(GierMischanteil)))
		94	{
		95	pd_ergebnis = (GasMischanteil + abs(GierMischanteil));
		96	}
		97	if(pd_ergebnis < -(GasMischanteil + abs(GierMischanteil)))
		98	{
		99	pd_ergebnis = -(GasMischanteil + abs(GierMischanteil));
		100	}
		101	/* M o t o r V o r n */
		102	motorwert = GasMischanteil + pd_ergebnis + GierMischanteil;
		103	if ((motorwert < 0))
		104	{
		105	motorwert = 0;
		106	}
		107	else if(motorwert > MAX_GAS)
		108	{
		109	motorwert = MAX_GAS;
		110	}
		111	if (motorwert < MIN_GAS)
		112	{
		113	motorwert = MIN_GAS;
		114	}
		115	Motor_Vorne = motorwert;
		116	if(!MotorenEin)
		117	{
		118	Motor_Vorne = 0;
		119	if(MotorTest[0]) Motor_Vorne = MotorTest[0];
		120	}
		121	#endif
		122	i2c_write_byte(Motor_Vorne);
		123	break;
		124	case 1:
		125	#if 0
		126	pd_ergebnis = (scale_p * DiffNick + scale_d * (AdWertNick - AdNeutralNick - Roll_Y_Off)) / 10;
		127	if(pd_ergebnis > (GasMischanteil + abs(GierMischanteil)))
		128	{
		129	pd_ergebnis = (GasMischanteil + abs(GierMischanteil));
		130	}
		131	if(pd_ergebnis < -(GasMischanteil + abs(GierMischanteil)))
		132	{
		133	pd_ergebnis = -(GasMischanteil + abs(GierMischanteil));
		134	}
		135	/* M o t o r H e c k */
		136	motorwert = GasMischanteil - pd_ergebnis + GierMischanteil;
		137	if ((motorwert < 0))
		138	{
		139	motorwert = 0;
		140	}
		141	else if(motorwert > MAX_GAS)
		142	{
		143	motorwert = MAX_GAS;
		144	}
		145	if (motorwert < MIN_GAS)
		146	{
		147	motorwert = MIN_GAS;
		148	}
		149	Motor_Hinten = motorwert;
		150
		151	if(!MotorenEin)
		152	{
		153	Motor_Hinten = 0;
		154	if(MotorTest[1]) Motor_Hinten = MotorTest[1];
		155	}
		156	#endif
		157	i2c_write_byte(Motor_Hinten);
		158	break;
		159	case 2:
		160	#if 0
		161	pd_ergebnis = (scale_p * DiffRoll + scale_d * (AdWertRoll - AdNeutralRoll - Roll_X_Off)) / 10;
		162	if(pd_ergebnis > (GasMischanteil + abs(GierMischanteil)))
		163	{
		164	pd_ergebnis = (GasMischanteil + abs(GierMischanteil));
		165	}
		166	if(pd_ergebnis < -(GasMischanteil + abs(GierMischanteil)))
		167	{
		168	pd_ergebnis = -(GasMischanteil + abs(GierMischanteil));
		169	}
		170	/* M o t o r R e c h t s */
		171	motorwert = GasMischanteil - pd_ergebnis - GierMischanteil;
		172	if (motorwert < 0)
		173	{
		174	motorwert = 0;
		175	}
		176	else if(motorwert > MAX_GAS)
		177	{
		178	motorwert = MAX_GAS;
		179	}
		180	if (motorwert < MIN_GAS)
		181	{
		182	motorwert = MIN_GAS;
		183	}
		184	Motor_Rechts = motorwert;
		185	if(!MotorenEin)
		186	{
		187	Motor_Rechts = 0;
		188	if(MotorTest[3]) Motor_Rechts = MotorTest[3];
		189	}
		190	#endif
		191	i2c_write_byte(Motor_Rechts);
		192	break;
		193	case 3:
838	MikeW	194	#if 0
966	MikeW	195	pd_ergebnis = (scale_p* DiffRoll + scale_d * (AdWertRoll - AdNeutralRoll - Roll_X_Off)) / 10;
		196	if(pd_ergebnis > (GasMischanteil + abs(GierMischanteil)))
		197	{
		198	pd_ergebnis = (GasMischanteil + abs(GierMischanteil));
		199	}
		200	if(pd_ergebnis < -(GasMischanteil + abs(GierMischanteil)))
		201	{
		202	pd_ergebnis = -(GasMischanteil + abs(GierMischanteil));
		203	}
		204	/* M o t o r L i n k s */
		205	motorwert = GasMischanteil + pd_ergebnis - GierMischanteil;
		206	if (motorwert < 0)
		207	{
		208	motorwert = 0;
		209	}
		210	else if(motorwert > MAX_GAS)
		211	{
		212	motorwert = MAX_GAS;
		213	}
		214	if (motorwert < MIN_GAS)
		215	{
		216	motorwert = MIN_GAS;
		217	}
		218	Motor_Links = motorwert;
		219	if(!MotorenEin)
		220	{
		221	Motor_Links = 0;
		222	if(MotorTest[2]) Motor_Links = MotorTest[2];
		223	}
		224	#endif
		225	i2c_write_byte(Motor_Links);
		226	break;
		227	}
		228	break;
		229	case 2:
		230	i2c_stop();
		231	if (motor<4) twi_state = 0;
		232	else motor = 0;
		233	i2c_start();
		234	break;
		235
		236	//Liest Daten von Motor
		237	case 3:
		238	i2c_write_byte(0x53+(motorread*2));
		239	break;
		240	case 4:
		241	switch(motorread)
		242	{
		243	case 0:
		244	i2c_write_byte(Motor_Vorne);
		245	break;
		246	case 1:
		247	i2c_write_byte(Motor_Hinten);
		248	break;
		249	case 2:
		250	i2c_write_byte(Motor_Rechts);
		251	break;
		252	case 3:
		253	i2c_write_byte(Motor_Links);
		254	break;
		255	}
		256	break;
		257	case 5: //1 Byte vom Motor lesen
		258	motor_rx[motorread] = TWDR;
		259
		260	case 6:
		261	switch(motorread)
		262	{
		263	case 0:
		264	i2c_write_byte(Motor_Vorne);
		265	break;
		266	case 1:
		267	i2c_write_byte(Motor_Hinten);
		268	break;
838	MikeW	269	case 2:
966	MikeW	270	i2c_write_byte(Motor_Rechts);
		271	break;
838	MikeW	272	case 3:
966	MikeW	273	i2c_write_byte(Motor_Links);
		274	break;
		275	}
		276	break;
		277	case 7: //2 Byte vom Motor lesen
		278	motor_rx[motorread+4] = TWDR;
		279	motorread++;
		280	if (motorread>3) motorread=0;
		281	i2c_stop();
		282	I2CTimeout = 10;
		283	twi_state = 0;
		284	}
		285	#if 0
		286	break;
		287	TriggerMagneticHeading++;
		288	TriggerMagneticHeading %= 50;
		289	TriggerSonicReading++;
		290	TriggerSonicReading %= 200;
		291
		292	if (TriggerMagneticHeading == 1)
		293	{
		294	// Get Magnetic Heading
		295	i2c_start();
		296	twi_state = 8;
		297	break;
		298	}
		299
		300	if (TriggerSonicReading == 1)
		301	{
		302	// Get Ultrasonic Reading
		303	i2c_start();
		304	twi_state = 20;
		305	break;
		306	}
		307	else if (TriggerSonicReading == 190)
		308	{
		309	// Get Ultrasonic Reading
		310	i2c_start();
		311	twi_state = 30;
		312	break;
		313	}
		314	else
		315	{
		316	I2CTimeout = 10;
		317	twi_state = 0;
		318	break;
		319	}
		320
		321	case 8:
		322	i2c_write_byte(0xC0);
		323	break;
		324	case 9:
		325	i2c_write_byte(0x02);
		326	break;
		327	case 10:
		328	i2c_start();
		329	break;
		330	case 11:
		331	i2c_write_byte(0xC1);
		332	break;
		333	case 12:
		334	i2c_write_byte(MagneticHeading);
		335	break;
		336	case 13:
		337	MagneticHeading = (0x0F & TWDR) << 8;
		338	i2c_stop();
		339	i2c_start();
		340	break;
		341	case 14:
		342	i2c_write_byte(0xC0);
		343	break;
		344	case 15:
		345	i2c_write_byte(0x03);
		346	break;
		347	case 16:
		348	i2c_start();
		349	break;
		350	case 17:
		351	i2c_write_byte(0xC1);
		352	break;
		353	case 18:
		354	i2c_write_byte(MagneticHeading);
		355	break;
		356	case 19:
		357	MagneticHeading += TWDR;
		358	i2c_stop();
		359	I2CTimeout = 22;
		360	twi_state = 0;
		361	break;
		362
		363	/* Trigger Ultrasonic Ping */
		364	case 20: /* Ping */
		365	i2c_write_byte(0xE0); /* Send I2C Adress */
		366	break;
		367	case 21:
		368	i2c_write_byte(0x00); /* Send I2C Register */
		369	break;
		370	case 22:
		371	i2c_write_byte(82); /* Send I2C Value */
		372	break;
		373	case 24:
		374	VersionID = TWDR;
		375	i2c_stop();
		376	I2CTimeout = 22;
		377	twi_state = 0;
		378	UltrasonicPingCnt++;
		379	break;
		380	case 30:
		381	i2c_write_byte(0xE0);/* Send I2C Adress */
		382	break;
		383	case 31:
		384	i2c_write_byte(0x02); /* Send I2C Register */
		385	break;
		386	case 32:
		387	i2c_start();
		388	break;
		389	case 33:
		390	i2c_write_byte(0xE1);
		391	break;
		392	case 34:
		393	i2c_write_byte(255);
		394	break;
		395	case 35:
		396	UltrasonicRange = TWDR << 8;/* Read I2C Value */
		397	UltrasonicRangeHigh = TWDR;
		398	i2c_stop();
		399	i2c_start();
		400	break;
		401	case 36:
		402	i2c_write_byte(0xE0);
		403	break;
		404	case 37:
		405	i2c_write_byte(0x03);
		406	break;
		407	case 38:
		408	i2c_start();
		409	break;
		410	case 39:
		411	i2c_write_byte(0xE1);
		412	break;
		413	case 40:
		414	i2c_write_byte(UltrasonicRangeLow);
		415	break;
		416	case 41:
		417	UltrasonicRange += TWDR;
		418	UltrasonicRangeLow = TWDR;
		419	i2c_stop();
		420	I2CTimeout = 22;
		421	twi_state = 0;
		422	break;
838	MikeW	423	}
		424	#endif
966	MikeW	425	TWCR \|= 0x80;
838	MikeW	426	}

Subversion Repositories FlightCtrl

(root)/branches/KalmanFilter MikeW/twimaster.c - Rev 966