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

Subversion Repositories FlightCtrl

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