WebSVN - FlightCtrl - Blame - Rev 1650 - /branches/Znib/V0.82a/twimaster.c

Rev	Author	Line No.	Line
1	ingob	1	/*############################################################################
		2	############################################################################*/
		3
		4	#include "main.h"
		5
918	hbuss	6	volatile unsigned char twi_state = 0;
1650	killagreg	7	volatile unsigned char motor = 0;
		8	volatile unsigned char motorread = 0,MissingMotor = 0;
		9	volatile unsigned char BLFlags = 0;
1479	killagreg	10
		11	MotorData_t Motor[MAX_MOTORS];
1648	killagreg	12	BLConfig_t BLConfig[MAX_MOTORS];
1479	killagreg	13
1648	killagreg	14
1322	hbuss	15	unsigned int I2CError = 0;
1	ingob	16
1648	killagreg	17	#define BL_CONF_SYNC_BYTE '#'
		18
1	ingob	19	//############################################################################
		20	//Initzialisieren der I2C (TWI) Schnittstelle
		21	void i2c_init(void)
		22	//############################################################################
		23	{
1648	killagreg	24	unsigned char i;
		25
		26	TWSR = 0;
		27	TWBR = ((SYSCLK/SCL_CLOCK)-16)/2;
		28
		29	for(i=0; i < MAX_MOTORS; i++)
		30	{
		31	Motor[i].Version = 0;
		32	Motor[i].SetPoint = 0;
		33	Motor[i].SetPointLowerBits = 0;
		34	Motor[i].State = 0;
		35	Motor[i].Current = 0;
		36	Motor[i].MaxPWM = 0;
		37	BLConfig[i].SetMask = MASK_SET_PWM_SCALING\|MASK_SET_CURRENT_LIMIT\|MASK_SET_TEMP_LIMIT\|MASK_SET_CURRENT_SCALING\|MASK_SET_BITCONFIG;
		38	BLConfig[i].PwmScaling = 255; // MaxPWM
		39	BLConfig[i].CurrentLimit = 30; // Current Limit in A
		40	BLConfig[i].TempLimit = 99; // Temperature Limit in �C
		41	BLConfig[i].CurrentScaling = 64; // Current Scaling
		42	BLConfig[i].BitConfig = 0; // BitConfig
		43	}
1	ingob	44	}
		45
173	holgerb	46	void i2c_reset(void)
1	ingob	47	//############################################################################
173	holgerb	48	{
1648	killagreg	49	I2C_Stop();
		50	twi_state = 0;
		51	motor = TWDR;
		52	motor = 0;
		53	TWCR = 0x80;
		54	TWAMR = 0;
		55	TWAR = 0;
		56	TWDR = 0;
		57	TWSR = 0;
		58	TWBR = 0;
		59	i2c_init();
		60	I2C_Start();
		61	i2c_write_byte(0);
173	holgerb	62	}
1	ingob	63
1648	killagreg	64
1	ingob	65	//############################################################################
		66	SIGNAL (TWI_vect)
		67	//############################################################################
		68	{
1650	killagreg	69	static unsigned char missing_motor = 0, byte_counter = 0, crc = 0, read_more = 0, motorread_temperature = 0;
		70	static unsigned char *pTxBuff;
		71
1648	killagreg	72	J4High;
		73	switch(twi_state++)
		74	{
1210	hbuss	75	//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		76	// Writing the Data
		77	//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1648	killagreg	78	case 0:
		79	while(Mixer.Motor[motor][MIX_GAS] <= 0 && motor < MAX_MOTORS) motor++; // skip if not used
		80	if(motor == MAX_MOTORS) // writing finished -> now read
		81	{
		82	motor = 0;
		83	twi_state = 5;
		84	i2c_write_byte(0x53+(motorread*2));
		85	}
1209	hbuss	86	else i2c_write_byte(0x52+(motor*2));
1648	killagreg	87	break;
		88	case 1:
		89	i2c_write_byte(Motor[motor].SetPoint);
		90	if(!(Motor[motor].Version & MOTOR_STATE_NEW_PROTOCOL_MASK) )
		91	{
		92	twi_state = 4; //jump over sending more data
		93	}
		94	else if(!(Motor[motor].SetPointLowerBits \|\| (BLFlags & BLFLAG_SEND_CONFIG)))
		95	{
		96	twi_state = 4; // skip state
		97	}
		98	break;
		99	case 2: // lower bits of setpoint (higher resolution)
		100	i2c_write_byte((Motor[motor].SetPointLowerBits << 1) & 0x07); // send the lower bits of setpoint
		101	// transmit config only on demand and the motors are not running and only for one motor per round trip
		102	if( (BLFlags & BLFLAG_SEND_CONFIG) && !MotorenEin && (motor == motorread))
		103	{ // prepare sending of configuration
		104	byte_counter = 0; // reset send byte counter
		105	crc = 0xAA; // init checksum
		106	}
		107	else
		108	{ // jump to state for end of transmission for that motor
		109	twi_state = 4;
		110	}
		111	break;
		112	case 3:
		113	if(!byte_counter) // first byte?
		114	{
		115	i2c_write_byte(BL_CONF_SYNC_BYTE);
		116	crc += BL_CONF_SYNC_BYTE; // update crc
		117	pTxBuff = (uint8_t*)&BLConfig[motor]; // select configuration for motor
		118	}
		119	else if(byte_counter >= sizeof(BLConfig_t)+1) // last byte?
		120	{ // send crc byte at the end
		121	i2c_write_byte(crc);
		122	}
		123	else // transmit configuration to BLs
		124	{
		125	i2c_write_byte(pTxBuff[byte_counter-1]); // submit next byte
		126	crc += pTxBuff[byte_counter-1]; // update crc
		127	twi_state = 3; // stay in this state
		128	}
		129	byte_counter++; // next byte
		130	break;
		131	case 4:
		132
		133	if(TWSR == 0x30)
		134	{
		135	if(!missing_motor) missing_motor = motor + 1;
		136	if((Motor[motor].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[motor].State++; // increment error counter and handle overflow
		137	}
		138	I2C_Stop();
		139	I2CTimeout = 10;
		140	motor++;
		141	twi_state = 0;
		142	I2C_Start();
		143	break;
1210	hbuss	144	//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		145	// Reading Data
		146	//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		147	case 5:
1648	killagreg	148	//Transmit 1st byte for reading
		149	if(TWSR != 0x40) // Error?
		150	{
		151	Motor[motorread].State &= ~MOTOR_STATE_PRESENT_MASK; // clear present bit
		152	if(++motorread >= MAX_MOTORS)
1638	holgerb	153	{
1648	killagreg	154	BLFlags &= ~BLFLAG_SEND_CONFIG;
		155	motorread = 0;
		156	if(++motorread_temperature >= MAX_MOTORS)
		157	{
		158	motorread_temperature = 0;
		159	BLFlags &= ~BLFLAG_READ_VERSION;
		160	}
		161	}
		162	I2C_Stop();
		163	twi_state = 0;
		164	BLFlags \|= BLFLAG_TX_COMPLETE;
		165	}
		166	else
		167	{
		168	Motor[motorread].State \|= MOTOR_STATE_PRESENT_MASK; // set present bit
		169	if(motorread == motorread_temperature)
		170	{
		171	read_more = 1;
		172	I2C_ReceiveByte();
		173	}
1638	holgerb	174	else
1648	killagreg	175	{
		176	read_more = 0;
		177	I2C_ReceiveLastByte();
1642	killagreg	178	}
1648	killagreg	179	}
		180	MissingMotor = missing_motor;
		181	missing_motor = 0;
		182	break;
		183	case 6: //Read 1st byte and transmit 2nd Byte
1638	holgerb	184	Motor[motorread].Current = TWDR;
1648	killagreg	185	if(read_more)
		186	{
		187	I2C_ReceiveByte() //ack
		188
		189	}
1643	holgerb	190	else
1648	killagreg	191	{
		192	if(++motorread >= MAX_MOTORS)
		193	{
		194	motorread = 0; // restart from beginning
		195	BLFlags &= ~BLFLAG_SEND_CONFIG;
		196	if(++motorread_temperature >= MAX_MOTORS)
		197	{
		198	motorread_temperature = 0;
		199	BLFlags &= ~BLFLAG_READ_VERSION;
		200	}
		201	}
1643	holgerb	202	I2C_Stop();
		203	twi_state = 0;
1648	killagreg	204	BLFlags \|= BLFLAG_TX_COMPLETE;
		205	}
1638	holgerb	206	break;
1648	killagreg	207	case 7:
1638	holgerb	208	//Read 2nd byte and transmit 3rd Byte
		209	Motor[motorread].MaxPWM = TWDR;
1648	killagreg	210	if(BLFlags & BLFLAG_READ_VERSION)
		211	{
		212	if(TWDR == 250)
		213	{
		214	if(!MotorenEin) Motor[motorread].Version \|= MOTOR_STATE_NEW_PROTOCOL_MASK;
		215	}
		216	else
		217	{
		218	Motor[motorread].Version = 0;
		219	}
		220	}
1638	holgerb	221	I2C_ReceiveLastByte(); //nack
		222	break;
1648	killagreg	223	case 8: // read next
1638	holgerb	224	Motor[motorread].Temperature = TWDR;
1648	killagreg	225	if(++motorread >= MAX_MOTORS)
		226	{
		227	motorread = 0; // restart reading of first motor
		228	BLFlags &= ~BLFLAG_SEND_CONFIG;
		229	if(++motorread_temperature >= MAX_MOTORS)
		230	{
		231	motorread_temperature = 0;
		232	BLFlags &= ~BLFLAG_READ_VERSION;
		233	}
		234	}
1639	holgerb	235	I2C_Stop();
1648	killagreg	236	BLFlags \|= BLFLAG_TX_COMPLETE;
1638	holgerb	237	twi_state = 0;
1643	holgerb	238	break;
1638	holgerb	239	//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		240	// writing Gyro-Offset
		241	//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		242	case 18:
		243	i2c_write_byte(0x98); // Address of the DAC
		244	break;
		245	case 19:
		246	i2c_write_byte(0x10); // Update Channel A
		247	break;
		248	case 20:
		249	i2c_write_byte(AnalogOffsetNick); // Value
		250	break;
		251	case 21:
		252	i2c_write_byte(0x80); // Value
		253	break;
		254	case 22:
1639	holgerb	255	I2C_Stop();
1638	holgerb	256	I2CTimeout = 10;
1639	holgerb	257	I2C_Start();
1638	holgerb	258	break;
		259	case 23:
		260	i2c_write_byte(0x98); // Address of the DAC
		261	break;
		262	case 24:
		263	i2c_write_byte(0x12); // Update Channel B
		264	break;
		265	case 25:
		266	i2c_write_byte(AnalogOffsetRoll); // Value
		267	break;
		268	case 26:
		269	i2c_write_byte(0x80); // Value
		270	break;
		271	case 27:
1639	holgerb	272	I2C_Stop();
1638	holgerb	273	I2CTimeout = 10;
1639	holgerb	274	I2C_Start();
1638	holgerb	275	break;
		276	case 28:
		277	i2c_write_byte(0x98); // Address of the DAC
		278	break;
		279	case 29:
		280	i2c_write_byte(0x14); // Update Channel C
		281	break;
		282	case 30:
		283	i2c_write_byte(AnalogOffsetGier); // Value
		284	break;
		285	case 31:
		286	i2c_write_byte(0x80); // Value
		287	break;
		288	case 32:
1639	holgerb	289	I2C_Stop();
1638	holgerb	290	I2CTimeout = 10;
		291	twi_state = 0;
		292	break;
		293	default: twi_state = 0;
		294	break;
1648	killagreg	295	}
		296	TWCR \|= 0x80;
		297	J4Low;
1638	holgerb	298	}
1639	holgerb	299
1648	killagreg	300	void I2C_SendBLConfig(void)
		301	{
		302	unsigned char i;
		303	while(!(BLFlags & BLFLAG_TX_COMPLETE)); //wait for complete transfer
		304	BLFlags \|= BLFLAG_SEND_CONFIG; // enable sending of BL config
		305	for(i = 0; i < MAX_MOTORS; i++)
		306	{
		307	Motor[i].SetPoint = 0;
		308	Motor[i].SetPointLowerBits = 0;
		309	}
		310	motorread = 0;
		311	// needs at least MAX_MOTORS loops of 2 ms (12*2ms = 24ms)
		312	do
		313	{
		314	twi_state = 0;
		315	I2C_Start(); // start an i2c transmission
		316	while(!(BLFlags & BLFLAG_TX_COMPLETE)); //wait for complete transfer
		317	}while(BLFlags & BLFLAG_SEND_CONFIG); // repeat until the BL config has been send to all motors
		318	}
		319
		320
		321

Subversion Repositories FlightCtrl

(root)/branches/Znib/V0.82a/twimaster.c - Rev 1650