WebSVN - FlightCtrl - Diff - Rev 1651 and 1652 - /branches/Znib/V0.84a/twimaster.c


/*############################################################################
############################################################################*/
 
#include "main.h"
 
volatile unsigned char twi_state = 0;
volatile unsigned char motor = 0;
volatile unsigned char motorread = 0,MissingMotor = 0;
volatile unsigned char BLFlags = 0;
 
MotorData_t Motor[MAX_MOTORS];
BLConfig_t BLConfig[MAX_MOTORS];
 
 
unsigned int I2CError = 0;
 
#define BL_CONF_SYNC_BYTE '#'
 
//############################################################################
//Initzialisieren der I2C (TWI) Schnittstelle
void i2c_init(void)
//############################################################################
{
        unsigned char i;
 
        TWSR = 0;
        TWBR = ((SYSCLK/SCL_CLOCK)-16)/2;
 
        for(i=0; i < MAX_MOTORS; i++)
        {
                Motor[i].Version        = 0;
                Motor[i].SetPoint       = 0;
                Motor[i].SetPointLowerBits      = 0;
                Motor[i].State          = 0;
                Motor[i].Current        = 0;
                Motor[i].MaxPWM         = 0;
                BLConfig[i].SetMask =   MASK_SET_PWM_SCALING|MASK_SET_CURRENT_LIMIT|MASK_SET_TEMP_LIMIT|MASK_SET_CURRENT_SCALING|MASK_SET_BITCONFIG;
                BLConfig[i].PwmScaling = 255;           // MaxPWM
                BLConfig[i].CurrentLimit = 30;          // Current Limit in A
                BLConfig[i].TempLimit = 99;                     // Temperature Limit in °C
                BLConfig[i].CurrentScaling = 64;        // Current Scaling
                BLConfig[i].BitConfig = 0;                      // BitConfig
        }
}
 
void i2c_reset(void)
//############################################################################
{
        I2C_Stop();
        twi_state = 0;
        motor = TWDR;
        motor = 0;
        TWCR = 0x80;
        TWAMR = 0;
        TWAR = 0;
        TWDR = 0;
        TWSR = 0;
        TWBR = 0;
        i2c_init();
        I2C_Start();
        i2c_write_byte(0);
}
 
 
//############################################################################
SIGNAL (TWI_vect)
//############################################################################
{
        static unsigned char missing_motor = 0, byte_counter = 0, crc = 0, read_more = 0, motorread_temperature = 0;
        static unsigned char *pTxBuff;
 
J4High;
        switch(twi_state++)
        {
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Writing the Data
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
                case 0:
                        while(Mixer.Motor[motor][MIX_GAS] <= 0 && motor < MAX_MOTORS) motor++;  // skip if not used
                        if(motor == MAX_MOTORS)  // writing finished -> now read
                        {
                                motor = 0;
                                twi_state = 5;
                                i2c_write_byte(0x53+(motorread*2));
                        }
                                else i2c_write_byte(0x52+(motor*2));
                        break;
                case 1:
                        i2c_write_byte(Motor[motor].SetPoint);
                        if(!(Motor[motor].Version & MOTOR_STATE_NEW_PROTOCOL_MASK) || RequiredMotors > 6)
                        {
                                twi_state = 4; //jump over sending more data
                        }
                        else if(!(Motor[motor].SetPointLowerBits || (BLFlags & BLFLAG_SEND_CONFIG)))
                        {
                                twi_state = 4; // skip state
                        }
                        break;
                case 2: // lower bits of setpoint (higher resolution)
                        i2c_write_byte((Motor[motor].SetPointLowerBits << 1) & 0x07); // send the lower bits of setpoint
                        // transmit config only on demand and the motors are not running and only for one motor per round trip
                        if( (BLFlags & BLFLAG_SEND_CONFIG) && (motor == motorread))
                        {       // prepare sending of configuration
                                byte_counter = 0;       // reset send byte counter
                                crc = 0xAA; // init checksum
                        }
                        else
                        {       // jump to state for end of transmission for that motor
                                twi_state = 4;
                        }
                        break;
                case 3:
                        if(!byte_counter) // first byte?
                        {
                                i2c_write_byte(BL_CONF_SYNC_BYTE);
                                crc += BL_CONF_SYNC_BYTE; // update crc
                                pTxBuff = (uint8_t*)&BLConfig[motor]; // select configuration for motor
                                twi_state = 3; // keep state 3
                        }
                        else if(byte_counter == sizeof(BLConfig_t) + 1)
                        {       // send crc byte at the end
                                i2c_write_byte(crc);
                                twi_state = 3; // keep state 3
                        }
                        else if(byte_counter > sizeof(BLConfig_t) + 1)
                        {
                                i2c_write_byte(0);
                                // jump to case 4
                        }
            else // transmit configuration to BLs
                        {
                                i2c_write_byte(pTxBuff[byte_counter-1]);        // submit next byte
                                crc += pTxBuff[byte_counter-1];                         // update crc
                                twi_state = 3;// keep state 3
                        }
                        byte_counter++; // next byte
                        break;
        case 4:
 
                        if(TWSR == 0x30)
                        {
                                if(!missing_motor) missing_motor = motor + 1;
                                if((Motor[motor].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[motor].State++; // increment error counter and handle overflow
                        }
                        I2C_Stop();
                        I2CTimeout = 10;
                        motor++;
                        twi_state = 0;
                        I2C_Start();
                        break;
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Reading Data
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
        case 5:
                        //Transmit 1st byte for reading
                        if(TWSR != 0x40)  // Error?
                        {
                                Motor[motorread].State &= ~MOTOR_STATE_PRESENT_MASK; // clear present bit
                                if(++motorread >= MAX_MOTORS)
                                {
                                        BLFlags &= ~BLFLAG_SEND_CONFIG;
                                        motorread = 0;
                                        if(++motorread_temperature >= MAX_MOTORS)
                                        {
                                                motorread_temperature = 0;
                                                BLFlags &= ~BLFLAG_READ_VERSION;
                                        }
                                }
                                I2C_Stop();
                                twi_state = 0;
                                BLFlags |= BLFLAG_TX_COMPLETE;
                        }
                        else
                        {
                                Motor[motorread].State |= MOTOR_STATE_PRESENT_MASK; // set present bit
                                if(motorread == motorread_temperature)
                                {
                                        read_more = 1;
                                        I2C_ReceiveByte();
                                }
                                else
                                {
                                        read_more = 0;
                                        I2C_ReceiveLastByte();
                                }
                        }
                        MissingMotor = missing_motor;
                        missing_motor = 0;
                        break;
        case 6: //Read 1st byte and transmit 2nd Byte
                Motor[motorread].Current = TWDR;
                if(read_more)
                {
                                        I2C_ReceiveByte() //ack
 
                                }
                                else
                                {
                                        if(++motorread >= MAX_MOTORS)
                                        {
                                                motorread = 0;          // restart from beginning
                                                BLFlags &= ~BLFLAG_SEND_CONFIG;
                                                if(++motorread_temperature >= MAX_MOTORS)
                                                {
                                                        motorread_temperature = 0;
                                                        BLFlags &= ~BLFLAG_READ_VERSION;
                                                }
                                        }
                                        I2C_Stop();
                                        twi_state = 0;
                                        BLFlags |= BLFLAG_TX_COMPLETE;
                                }
                break;
        case 7:
                       //Read 2nd byte and transmit 3rd Byte
                Motor[motorread].MaxPWM = TWDR;
                if(BLFlags & BLFLAG_READ_VERSION)
                                {
                                        if(TWDR == 250)
                                        {
                                                if(!MotorenEin) Motor[motorread].Version |= MOTOR_STATE_NEW_PROTOCOL_MASK;
                                        }
                                        else
                                        {
                                                Motor[motorread].Version = 0;
                                        }
                                }
                                I2C_ReceiveLastByte(); //nack
                break;
        case 8: // read next
                                Motor[motorread].Temperature = TWDR;
                                if(++motorread >= MAX_MOTORS)
                                {
                                        motorread = 0; // restart reading of first motor
                                        BLFlags &= ~BLFLAG_SEND_CONFIG;
                                        if(++motorread_temperature >= MAX_MOTORS)
                                        {
                                                motorread_temperature = 0;
                                                BLFlags &= ~BLFLAG_READ_VERSION;
                                        }
                                }
                I2C_Stop();
                BLFlags |= BLFLAG_TX_COMPLETE;
                twi_state = 0;
                                break;
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// writing Gyro-Offset
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
        case 18:
                i2c_write_byte(0x98); // Address of the DAC
                break;
        case 19:
                i2c_write_byte(0x10); // Update Channel A
                break;
        case 20:
                i2c_write_byte(AnalogOffsetNick); // Value
                break;
        case 21:
                i2c_write_byte(0x80); // Value
                break;
        case 22:
                I2C_Stop();
                I2CTimeout = 10;
                I2C_Start();
                break;
        case 23:
                i2c_write_byte(0x98); // Address of the DAC
                break;
        case 24:
                i2c_write_byte(0x12); // Update Channel B
                break;
        case 25:
                i2c_write_byte(AnalogOffsetRoll); // Value
                break;
        case 26:
                i2c_write_byte(0x80); // Value
                break;
        case 27:
                I2C_Stop();
                I2CTimeout = 10;
                I2C_Start();
                break;
        case 28:
                i2c_write_byte(0x98); // Address of the DAC
                break;
        case 29:
                i2c_write_byte(0x14); // Update Channel C
                break;
        case 30:
                i2c_write_byte(AnalogOffsetGier); // Value
                break;
        case 31:
                i2c_write_byte(0x80); // Value
                break;
        case 32:
                I2C_Stop();
                I2CTimeout = 10;
                twi_state = 0;
                break;
        default: twi_state = 0;
                break;
        }
        TWCR |= 0x80;
        J4Low;
}
 
void I2C_SendBLConfig(void)
{
        unsigned char i;
        if(MotorenEin) return;
        while(!(BLFlags & BLFLAG_TX_COMPLETE)); //wait for complete transfer
        BLFlags |= BLFLAG_SEND_CONFIG; // enable sending of BL config
        // setpoints should be zero
        for(i = 0; i < MAX_MOTORS; i++)
        {
                Motor[i].SetPoint = 0;
                Motor[i].SetPointLowerBits = 0;
        }
        motorread = 0;
        // needs at least MAX_MOTORS loops of 2 ms (12*2ms = 24ms)
        do
        {
                twi_state = 0;
                I2C_Start(); // start an i2c transmission
                while(!(BLFlags & BLFLAG_TX_COMPLETE)); //wait for complete transfer
        }while(BLFlags & BLFLAG_SEND_CONFIG); // repeat until the BL config has been send to all motors
}
 
 
 
 

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

Subversion Repositories FlightCtrl

(root)/branches/Znib/V0.84a/twimaster.c @ 2058 - Rev 1651 → 1652