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

Subversion Repositories FlightCtrl

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