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

Subversion Repositories FlightCtrl

(root)/branches/V0.84a-test1/twimaster.c @ 2751 - Rev 1652 → 1654