WebSVN - FlightCtrl - Diff - Rev 1643 and 1644 - /branches/Proxy sensor


/*############################################################################
############################################################################*/
 
#include "main.h"
 
volatile unsigned char twi_state = 0;
unsigned char motor = 0,TransmitBlConfig = 1;
unsigned char motorread = 0,MissingMotor = 0;
unsigned char ReadTemperature = 0;
 
MotorData_t Motor[MAX_MOTORS];
 
unsigned int I2CError = 0;
 
//############################################################################
//Initzialisieren der I2C (TWI) Schnittstelle
void i2c_init(void)
//############################################################################
{
  TWSR = 0;
  TWBR = ((SYSCLK/SCL_CLOCK)-16)/2;
}
 
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;
     switch(twi_state++)
        {
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Writing the Data
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
        case 0:
                        while(Mixer.Motor[motor][0] <= 0 && motor < MAX_MOTORS) motor++;  // skip if not used
                                if(motor == MAX_MOTORS)  // writing finished -> now read
                                {
                                 motor = 0;
                                 twi_state = 3;
                                 i2c_write_byte(0x53+(motorread*2));
                                 }
                                else i2c_write_byte(0x52+(motor*2));
                break;
        case 1:
                i2c_write_byte(Motor[motor++].SetPoint);
                break;
        case 2:
                if(TWSR == 0x30)
                                 {
                                  if(!missing_motor) missing_motor = motor;
                                  if((Motor[motor-1].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[motor-1].State++; // increment error counter and handle overflow
                                 }
                I2C_Stop();
                I2CTimeout = 10;
                twi_state = 0;
                I2C_Start();
                break;
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Reading Data
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
        case 3:
               //Transmit 1st byte for reading
                if(TWSR != 0x40)  // Error?
                 {
                  Motor[motorread].State &= ~MOTOR_STATE_PRESENT_MASK; // clear present bit
                  motorread++;
                  if(motorread >= MAX_MOTORS) motorread = 0;
                  I2C_Stop();
                  twi_state = 0;
                 }
                 else
                 {
                  Motor[motorread].State |= MOTOR_STATE_PRESENT_MASK; // set present bit
                  I2C_ReceiveByte();
                 }
                MissingMotor = missing_motor;
                missing_motor = 0;
                break;
        case 4: //Read 1st byte and transmit 2nd Byte
                Motor[motorread].Current = TWDR;
                I2C_ReceiveLastByte(); //nack
                break;
        case 5:
                //Read 2nd byte
                Motor[motorread].MaxPWM = TWDR;
                                motorread++; // next motor
                if(motorread >= MAX_MOTORS) motorread = 0;
                I2C_Stop();
                twi_state = 0;
                break;
 
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// writing Gyro-Offset
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
        case 8:
                i2c_write_byte(0x98); // Address of the DAC
                break;
        case 9:
                i2c_write_byte(0x10); // Update Channel A
                break;
        case 10:
                i2c_write_byte(AnalogOffsetNick); // Value
                break;
        case 11:
                i2c_write_byte(0x80); // Value
                break;
        case 12:
                I2C_Stop();
                I2CTimeout = 10;
                I2C_Start();
                break;
        case 13:
                i2c_write_byte(0x98); // Address of the DAC
                break;
        case 14:
                i2c_write_byte(0x12); // Update Channel B
                break;
        case 15:
                i2c_write_byte(AnalogOffsetRoll); // Value
                break;
        case 16:
                i2c_write_byte(0x80); // Value
                break;
        case 17:
                I2C_Stop();
                I2CTimeout = 10;
                I2C_Start();
                break;
        case 18:
                i2c_write_byte(0x98); // Address of the DAC
                break;
        case 19:
                i2c_write_byte(0x14); // Update Channel C
                break;
        case 20:
                i2c_write_byte(AnalogOffsetGier); // Value
                break;
        case 21:
                i2c_write_byte(0x80); // Value
                break;
        case 22:
                I2C_Stop();
                I2CTimeout = 10;
                twi_state = 0;
                break;
        default: twi_state = 0;
                break;
                }
 TWCR |= 0x80;
}
*/
 
/*
                if(DataArray[3] & 0x01) Parameter.PwmScaling            = DataArray[4];
                if(DataArray[3] & 0x02) Parameter.MaxStrom                      = DataArray[5];
                if(DataArray[3] & 0x04) Parameter.TemperaturLimiter = DataArray[6];
                if(DataArray[3] & 0x08) Parameter.SkaliereStrom         = DataArray[7];
                if(DataArray[3] & 0x10) Parameter.Bitconfig                     = DataArray[8];
*/
 
//############################################################################
SIGNAL (TWI_vect)
//############################################################################
{                           // 2    3   4   5   6   7   8  9
 unsigned char test[] = {0,0,'#',0x1F,255,30,99,64,0x00,7,8,9,10};
 static unsigned char missing_motor,send = 0,crc = 0,read_more = 0;
J4High;
     switch(twi_state++)
        {
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Writing the Data
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
        case 0:
                        while(Mixer.Motor[motor][0] <= 0 && motor < MAX_MOTORS) motor++;  // skip if not used
                                if(motor == MAX_MOTORS)  // writing finished -> now read
                                {
                                 motor = 0;
                                 twi_state = 4;
                                 i2c_write_byte(0x53+(motorread*2));
                                 }
                                else i2c_write_byte(0x52+(motor*2));
                                send = 0;
                break;
        case 1:
                i2c_write_byte(Motor[motor].SetPoint);
//              if(!(Motor[motor].Version & MOTOR_STATE_NEW_PROTOCOL_MASK) /*|| !Motor[motor].SetPointLowerBits*/)
                if((!(Motor[motor].Version & MOTOR_STATE_NEW_PROTOCOL_MASK) || !Motor[motor].SetPointLowerBits) && !TransmitBlConfig)
                                  twi_state++; // skip
                break;
        case 2:
                if(!send++)
                                 {
                                  i2c_write_byte((Motor[motor].SetPointLowerBits << 1) & 0x07);
                                  crc = 0xAA;
                                 }
                                else
                if(send == 9) i2c_write_byte(crc)
                                else
                                 {
                                  crc += test[send];
                                  i2c_write_byte(test[send]);
                                }
                if(TransmitBlConfig && !MotorenEin && motor == motorread && (Motor[motorread].Version & MOTOR_STATE_NEW_PROTOCOL_MASK))
                                 {
                                  if(send <= 10) twi_state--;
                                 }
 
                break;
        case 3:
                        motor++;
                if(TWSR == 0x30)
                                 {
                                  if(!missing_motor) missing_motor = motor;
                                  if((Motor[motor-1].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[motor-1].State++; // increment error counter and handle overflow
                                 }
                I2C_Stop();
                I2CTimeout = 10;
                twi_state = 0;
                I2C_Start();
                break;
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Reading Data
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
        case 4:
               //Transmit 1st byte for reading
                if(TWSR != 0x40)  // Error?
                 {
                  Motor[motorread].State &= ~MOTOR_STATE_PRESENT_MASK; // clear present bit
                  motorread++;
                  if(motorread >= MAX_MOTORS) { TransmitBlConfig = 0; motorread = 0; ReadTemperature = ++ReadTemperature & 0x0f;}
                  I2C_Stop();
                  twi_state = 0;
                 }
                 else
                 {
                  Motor[motorread].State |= MOTOR_STATE_PRESENT_MASK; // set present bit
                  if(motorread == ReadTemperature || TransmitBlConfig)                         
                                  {
                   read_more = 1;
                   I2C_ReceiveByte();
                                  }
                                  else
                                   {
                                    I2C_ReceiveLastByte();                               
                                        read_more = 0;
                                   }   
                 }
                MissingMotor = missing_motor;
                missing_motor = 0;
                break;
        case 5: //Read 1st byte and transmit 2nd Byte
                Motor[motorread].Current = TWDR;
                if(read_more) I2C_ReceiveByte() //nack
                                else
                                 {
                                        motorread++; // next motor
                                        if(motorread >= MAX_MOTORS) { TransmitBlConfig = 0; motorread = 0; ReadTemperature = ++ReadTemperature & 0x0f;}
                                        I2C_Stop();
                                        twi_state = 0;
                                 }
                break;
        case 6:
                       //Read 2nd byte and transmit 3rd Byte
                Motor[motorread].MaxPWM = TWDR;
                                if(TWDR == 250)
                                 {
                                  if(!MotorenEin) Motor[motorread].Version |= MOTOR_STATE_NEW_PROTOCOL_MASK;
                                 }
                                 else if(TransmitBlConfig) Motor[motorread].Version = 0;
                                I2C_ReceiveLastByte(); //nack
                break;
        case 7: // read next
                                Motor[motorread].Temperature = TWDR;
                                motorread++; // next motor
                if(motorread >= MAX_MOTORS) { TransmitBlConfig = 0; motorread = 0; }
                I2C_Stop();
                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;
}
 
 

Rev 1643	Rev 1644
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,TransmitBlConfig = 1;	7	unsigned char motor = 0,TransmitBlConfig = 1;
8	unsigned char motorread = 0,MissingMotor = 0;	8	unsigned char motorread = 0,MissingMotor = 0;
9	unsigned char ReadTemperature = 0;	9	unsigned char ReadTemperature = 0;
10		10
11	MotorData_t Motor[MAX_MOTORS];	11	MotorData_t Motor[MAX_MOTORS];
12		12
13	unsigned int I2CError = 0;	13	unsigned int I2CError = 0;
14		14
15	//############################################################################	15	//############################################################################
16	//Initzialisieren der I2C (TWI) Schnittstelle	16	//Initzialisieren der I2C (TWI) Schnittstelle
17	void i2c_init(void)	17	void i2c_init(void)
18	//############################################################################	18	//############################################################################
19	{	19	{
20	TWSR = 0;	20	TWSR = 0;
21	TWBR = ((SYSCLK/SCL_CLOCK)-16)/2;	21	TWBR = ((SYSCLK/SCL_CLOCK)-16)/2;
22	}	22	}
23		23
24	void i2c_reset(void)	24	void i2c_reset(void)
25	//############################################################################	25	//############################################################################
26	{	26	{
27	I2C_Stop();	27	I2C_Stop();
28	twi_state = 0;	28	twi_state = 0;
29	motor = TWDR;	29	motor = TWDR;
30	motor = 0;	30	motor = 0;
31	TWCR = 0x80;	31	TWCR = 0x80;
32	TWAMR = 0;	32	TWAMR = 0;
33	TWAR = 0;	33	TWAR = 0;
34	TWDR = 0;	34	TWDR = 0;
35	TWSR = 0;	35	TWSR = 0;
36	TWBR = 0;	36	TWBR = 0;
37	i2c_init();	37	i2c_init();
38	I2C_Start();	38	I2C_Start();
39	i2c_write_byte(0);	39	i2c_write_byte(0);
40	}	40	}
41		41
42	/*	42	/*
43	//############################################################################	43	//############################################################################
44	SIGNAL (TWI_vect)	44	SIGNAL (TWI_vect)
45	//############################################################################	45	//############################################################################
46	{	46	{
47	static unsigned char missing_motor;	47	static unsigned char missing_motor;
48	switch(twi_state++)	48	switch(twi_state++)
49	{	49	{
50	//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	50	//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
51	// Writing the Data	51	// Writing the Data
52	//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	52	//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
53	case 0:	53	case 0:
54	while(Mixer.Motor[motor][0] <= 0 && motor < MAX_MOTORS) motor++; // skip if not used	54	while(Mixer.Motor[motor][0] <= 0 && motor < MAX_MOTORS) motor++; // skip if not used
55	if(motor == MAX_MOTORS) // writing finished -> now read	55	if(motor == MAX_MOTORS) // writing finished -> now read
56	{	56	{
57	motor = 0;	57	motor = 0;
58	twi_state = 3;	58	twi_state = 3;
59	i2c_write_byte(0x53+(motorread*2));	59	i2c_write_byte(0x53+(motorread*2));
60	}	60	}
61	else i2c_write_byte(0x52+(motor*2));	61	else i2c_write_byte(0x52+(motor*2));
62	break;	62	break;
63	case 1:	63	case 1:
64	i2c_write_byte(Motor[motor++].SetPoint);	64	i2c_write_byte(Motor[motor++].SetPoint);
65	break;	65	break;
66	case 2:	66	case 2:
67	if(TWSR == 0x30)	67	if(TWSR == 0x30)
68	{	68	{
69	if(!missing_motor) missing_motor = motor;	69	if(!missing_motor) missing_motor = motor;
70	if((Motor[motor-1].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[motor-1].State++; // increment error counter and handle overflow	70	if((Motor[motor-1].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[motor-1].State++; // increment error counter and handle overflow
71	}	71	}
72	I2C_Stop();	72	I2C_Stop();
73	I2CTimeout = 10;	73	I2CTimeout = 10;
74	twi_state = 0;	74	twi_state = 0;
75	I2C_Start();	75	I2C_Start();
76	break;	76	break;
77	//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	77	//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
78	// Reading Data	78	// Reading Data
79	//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	79	//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
80	case 3:	80	case 3:
81	//Transmit 1st byte for reading	81	//Transmit 1st byte for reading
82	if(TWSR != 0x40) // Error?	82	if(TWSR != 0x40) // Error?
83	{	83	{
84	Motor[motorread].State &= ~MOTOR_STATE_PRESENT_MASK; // clear present bit	84	Motor[motorread].State &= ~MOTOR_STATE_PRESENT_MASK; // clear present bit
85	motorread++;	85	motorread++;
86	if(motorread >= MAX_MOTORS) motorread = 0;	86	if(motorread >= MAX_MOTORS) motorread = 0;
87	I2C_Stop();	87	I2C_Stop();
88	twi_state = 0;	88	twi_state = 0;
89	}	89	}
90	else	90	else
91	{	91	{
92	Motor[motorread].State \|= MOTOR_STATE_PRESENT_MASK; // set present bit	92	Motor[motorread].State \|= MOTOR_STATE_PRESENT_MASK; // set present bit
93	I2C_ReceiveByte();	93	I2C_ReceiveByte();
94	}	94	}
95	MissingMotor = missing_motor;	95	MissingMotor = missing_motor;
96	missing_motor = 0;	96	missing_motor = 0;
97	break;	97	break;
98	case 4: //Read 1st byte and transmit 2nd Byte	98	case 4: //Read 1st byte and transmit 2nd Byte
99	Motor[motorread].Current = TWDR;	99	Motor[motorread].Current = TWDR;
100	I2C_ReceiveLastByte(); //nack	100	I2C_ReceiveLastByte(); //nack
101	break;	101	break;
102	case 5:	102	case 5:
103	//Read 2nd byte	103	//Read 2nd byte
104	Motor[motorread].MaxPWM = TWDR;	104	Motor[motorread].MaxPWM = TWDR;
105	motorread++; // next motor	105	motorread++; // next motor
106	if(motorread >= MAX_MOTORS) motorread = 0;	106	if(motorread >= MAX_MOTORS) motorread = 0;
107	I2C_Stop();	107	I2C_Stop();
108	twi_state = 0;	108	twi_state = 0;
109	break;	109	break;
110		110
111	//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	111	//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
112	// writing Gyro-Offset	112	// writing Gyro-Offset
113	//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	113	//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
114	case 8:	114	case 8:
115	i2c_write_byte(0x98); // Address of the DAC	115	i2c_write_byte(0x98); // Address of the DAC
116	break;	116	break;
117	case 9:	117	case 9:
118	i2c_write_byte(0x10); // Update Channel A	118	i2c_write_byte(0x10); // Update Channel A
119	break;	119	break;
120	case 10:	120	case 10:
121	i2c_write_byte(AnalogOffsetNick); // Value	121	i2c_write_byte(AnalogOffsetNick); // Value
122	break;	122	break;
123	case 11:	123	case 11:
124	i2c_write_byte(0x80); // Value	124	i2c_write_byte(0x80); // Value
125	break;	125	break;
126	case 12:	126	case 12:
127	I2C_Stop();	127	I2C_Stop();
128	I2CTimeout = 10;	128	I2CTimeout = 10;
129	I2C_Start();	129	I2C_Start();
130	break;	130	break;
131	case 13:	131	case 13:
132	i2c_write_byte(0x98); // Address of the DAC	132	i2c_write_byte(0x98); // Address of the DAC
133	break;	133	break;
134	case 14:	134	case 14:
135	i2c_write_byte(0x12); // Update Channel B	135	i2c_write_byte(0x12); // Update Channel B
136	break;	136	break;
137	case 15:	137	case 15:
138	i2c_write_byte(AnalogOffsetRoll); // Value	138	i2c_write_byte(AnalogOffsetRoll); // Value
139	break;	139	break;
140	case 16:	140	case 16:
141	i2c_write_byte(0x80); // Value	141	i2c_write_byte(0x80); // Value
142	break;	142	break;
143	case 17:	143	case 17:
144	I2C_Stop();	144	I2C_Stop();
145	I2CTimeout = 10;	145	I2CTimeout = 10;
146	I2C_Start();	146	I2C_Start();
147	break;	147	break;
148	case 18:	148	case 18:
149	i2c_write_byte(0x98); // Address of the DAC	149	i2c_write_byte(0x98); // Address of the DAC
150	break;	150	break;
151	case 19:	151	case 19:
152	i2c_write_byte(0x14); // Update Channel C	152	i2c_write_byte(0x14); // Update Channel C
153	break;	153	break;
154	case 20:	154	case 20:
155	i2c_write_byte(AnalogOffsetGier); // Value	155	i2c_write_byte(AnalogOffsetGier); // Value
156	break;	156	break;
157	case 21:	157	case 21:
158	i2c_write_byte(0x80); // Value	158	i2c_write_byte(0x80); // Value
159	break;	159	break;
160	case 22:	160	case 22:
161	I2C_Stop();	161	I2C_Stop();
162	I2CTimeout = 10;	162	I2CTimeout = 10;
163	twi_state = 0;	163	twi_state = 0;
164	break;	164	break;
165	default: twi_state = 0;	165	default: twi_state = 0;
166	break;	166	break;
167	}	167	}
168	TWCR \|= 0x80;	168	TWCR \|= 0x80;
169	}	169	}
170	*/	170	*/
171		171
172	/*	172	/*
173	if(DataArray[3] & 0x01) Parameter.PwmScaling = DataArray[4];	173	if(DataArray[3] & 0x01) Parameter.PwmScaling = DataArray[4];
174	if(DataArray[3] & 0x02) Parameter.MaxStrom = DataArray[5];	174	if(DataArray[3] & 0x02) Parameter.MaxStrom = DataArray[5];
175	if(DataArray[3] & 0x04) Parameter.TemperaturLimiter = DataArray[6];	175	if(DataArray[3] & 0x04) Parameter.TemperaturLimiter = DataArray[6];
176	if(DataArray[3] & 0x08) Parameter.SkaliereStrom = DataArray[7];	176	if(DataArray[3] & 0x08) Parameter.SkaliereStrom = DataArray[7];
177	if(DataArray[3] & 0x10) Parameter.Bitconfig = DataArray[8];	177	if(DataArray[3] & 0x10) Parameter.Bitconfig = DataArray[8];
178	*/	178	*/
179		179
180	//############################################################################	180	//############################################################################
181	SIGNAL (TWI_vect)	181	SIGNAL (TWI_vect)
182	//############################################################################	182	//############################################################################
183	{ // 2 3 4 5 6 7 8 9	183	{ // 2 3 4 5 6 7 8 9
184	unsigned char test[] = {0,0,'#',0x1F,255,30,99,49,0x00,7,8,9,10};	184	unsigned char test[] = {0,0,'#',0x1F,255,30,99,64,0x00,7,8,9,10};
185	static unsigned char missing_motor,send = 0,crc = 0,read_more = 0;	185	static unsigned char missing_motor,send = 0,crc = 0,read_more = 0;
186		186	J4High;
187	switch(twi_state++)	187	switch(twi_state++)
188	{	188	{
189	//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	189	//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
190	// Writing the Data	190	// Writing the Data
191	//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	191	//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
192	case 0:	192	case 0:
193	while(Mixer.Motor[motor][0] <= 0 && motor < MAX_MOTORS) motor++; // skip if not used	193	while(Mixer.Motor[motor][0] <= 0 && motor < MAX_MOTORS) motor++; // skip if not used
194	if(motor == MAX_MOTORS) // writing finished -> now read	194	if(motor == MAX_MOTORS) // writing finished -> now read
195	{	195	{
196	motor = 0;	196	motor = 0;
197	twi_state = 4;	197	twi_state = 4;
198	i2c_write_byte(0x53+(motorread*2));	198	i2c_write_byte(0x53+(motorread*2));
199	}	199	}
200	else i2c_write_byte(0x52+(motor*2));	200	else i2c_write_byte(0x52+(motor*2));
201	send = 0;	201	send = 0;
202	break;	202	break;
203	case 1:	203	case 1:
204	i2c_write_byte(Motor[motor].SetPoint);	204	i2c_write_byte(Motor[motor].SetPoint);
205	// if(!(Motor[motor].Version & MOTOR_STATE_NEW_PROTOCOL_MASK) /\|\| !Motor[motor].SetPointLowerBits/)	205	// if(!(Motor[motor].Version & MOTOR_STATE_NEW_PROTOCOL_MASK) /\|\| !Motor[motor].SetPointLowerBits/)
206	if(!(Motor[motor].Version & MOTOR_STATE_NEW_PROTOCOL_MASK) \|\| !Motor[motor].SetPointLowerBits && !TransmitBlConfig)	206	if((!(Motor[motor].Version & MOTOR_STATE_NEW_PROTOCOL_MASK) \|\| !Motor[motor].SetPointLowerBits) && !TransmitBlConfig)
207	twi_state++; // skip	207	twi_state++; // skip
208	break;	208	break;
209	case 2:	209	case 2:
210	if(!send++)	210	if(!send++)
211	{	211	{
212	i2c_write_byte((Motor[motor].SetPointLowerBits << 1) & 0x07);	212	i2c_write_byte((Motor[motor].SetPointLowerBits << 1) & 0x07);
213	crc = 0xAA;	213	crc = 0xAA;
214	}	214	}
215	else	215	else
216	if(send == 9) i2c_write_byte(crc)	216	if(send == 9) i2c_write_byte(crc)
217	else	217	else
218	{	218	{
219	crc += test[send];	219	crc += test[send];
220	i2c_write_byte(test[send]);	220	i2c_write_byte(test[send]);
221	}	221	}
222	if(TransmitBlConfig && !MotorenEin && motor == motorread && (Motor[motorread].Version & MOTOR_STATE_NEW_PROTOCOL_MASK))	222	if(TransmitBlConfig && !MotorenEin && motor == motorread && (Motor[motorread].Version & MOTOR_STATE_NEW_PROTOCOL_MASK))
223	{	223	{
224	if(send <= 10) twi_state--;	224	if(send <= 10) twi_state--;
225	}	225	}
226		-
227	break;	226	break;
228	case 3:	227	case 3:
229	motor++;	228	motor++;
230	if(TWSR == 0x30)	229	if(TWSR == 0x30)
231	{	230	{
232	if(!missing_motor) missing_motor = motor;	231	if(!missing_motor) missing_motor = motor;
233	if((Motor[motor-1].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[motor-1].State++; // increment error counter and handle overflow	232	if((Motor[motor-1].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[motor-1].State++; // increment error counter and handle overflow
234	}	233	}
235	I2C_Stop();	234	I2C_Stop();
236	I2CTimeout = 10;	235	I2CTimeout = 10;
237	twi_state = 0;	236	twi_state = 0;
238	I2C_Start();	237	I2C_Start();
239	break;	238	break;
240	//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	239	//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
241	// Reading Data	240	// Reading Data
242	//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	241	//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
243	case 4:	242	case 4:
244	//Transmit 1st byte for reading	243	//Transmit 1st byte for reading
245	if(TWSR != 0x40) // Error?	244	if(TWSR != 0x40) // Error?
246	{	245	{
247	Motor[motorread].State &= ~MOTOR_STATE_PRESENT_MASK; // clear present bit	246	Motor[motorread].State &= ~MOTOR_STATE_PRESENT_MASK; // clear present bit
248	motorread++;	247	motorread++;
249	if(motorread >= MAX_MOTORS) { TransmitBlConfig = 0; motorread = 0; ReadTemperature = ++ReadTemperature & 0x0f;}	248	if(motorread >= MAX_MOTORS) { TransmitBlConfig = 0; motorread = 0; ReadTemperature = ++ReadTemperature & 0x0f;}
250	I2C_Stop();	249	I2C_Stop();
251	twi_state = 0;	250	twi_state = 0;
252	}	251	}
253	else	252	else
254	{	253	{
255	Motor[motorread].State \|= MOTOR_STATE_PRESENT_MASK; // set present bit	254	Motor[motorread].State \|= MOTOR_STATE_PRESENT_MASK; // set present bit
256	if(motorread == ReadTemperature \|\| TransmitBlConfig)	255	if(motorread == ReadTemperature \|\| TransmitBlConfig)
257	{	256	{
258	read_more = 1;	257	read_more = 1;
259	I2C_ReceiveByte();	258	I2C_ReceiveByte();
260	}	259	}
261	else	260	else
262	{	261	{
263	I2C_ReceiveLastByte();	262	I2C_ReceiveLastByte();
264	read_more = 0;	263	read_more = 0;
265	}	264	}
266	}	265	}
267	MissingMotor = missing_motor;	266	MissingMotor = missing_motor;
268	missing_motor = 0;	267	missing_motor = 0;
269	break;	268	break;
270	case 5: //Read 1st byte and transmit 2nd Byte	269	case 5: //Read 1st byte and transmit 2nd Byte
271	Motor[motorread].Current = TWDR;	270	Motor[motorread].Current = TWDR;
272	if(read_more) I2C_ReceiveByte() //nack	271	if(read_more) I2C_ReceiveByte() //nack
273	else	272	else
274	{	273	{
275	motorread++; // next motor	274	motorread++; // next motor
276	if(motorread >= MAX_MOTORS) { TransmitBlConfig = 0; motorread = 0; ReadTemperature = ++ReadTemperature & 0x0f;}	275	if(motorread >= MAX_MOTORS) { TransmitBlConfig = 0; motorread = 0; ReadTemperature = ++ReadTemperature & 0x0f;}
277	I2C_Stop();	276	I2C_Stop();
278	twi_state = 0;	277	twi_state = 0;
279	}	278	}
280	break;	279	break;
281	case 6:	280	case 6:
282	//Read 2nd byte and transmit 3rd Byte	281	//Read 2nd byte and transmit 3rd Byte
283	Motor[motorread].MaxPWM = TWDR;	282	Motor[motorread].MaxPWM = TWDR;
284	if(TWDR == 250)	283	if(TWDR == 250)
285	{	284	{
286	if(!MotorenEin) Motor[motorread].Version \|= MOTOR_STATE_NEW_PROTOCOL_MASK;	285	if(!MotorenEin) Motor[motorread].Version \|= MOTOR_STATE_NEW_PROTOCOL_MASK;
287	}	286	}
288	else if(TransmitBlConfig) Motor[motorread].Version = 0;	287	else if(TransmitBlConfig) Motor[motorread].Version = 0;
289	I2C_ReceiveLastByte(); //nack	288	I2C_ReceiveLastByte(); //nack
290	break;	289	break;
291	case 7: // read next	290	case 7: // read next
292	Motor[motorread].Temperature = TWDR;	291	Motor[motorread].Temperature = TWDR;
293	motorread++; // next motor	292	motorread++; // next motor
294	if(motorread >= MAX_MOTORS) { TransmitBlConfig = 0; motorread = 0; }	293	if(motorread >= MAX_MOTORS) { TransmitBlConfig = 0; motorread = 0; }
295	I2C_Stop();	294	I2C_Stop();
296	twi_state = 0;	295	twi_state = 0;
297	break;	296	break;
298	//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	297	//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
299	// writing Gyro-Offset	298	// writing Gyro-Offset
300	//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	299	//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
301	case 18:	300	case 18:
302	i2c_write_byte(0x98); // Address of the DAC	301	i2c_write_byte(0x98); // Address of the DAC
303	break;	302	break;
304	case 19:	303	case 19:
305	i2c_write_byte(0x10); // Update Channel A	304	i2c_write_byte(0x10); // Update Channel A
306	break;	305	break;
307	case 20:	306	case 20:
308	i2c_write_byte(AnalogOffsetNick); // Value	307	i2c_write_byte(AnalogOffsetNick); // Value
309	break;	308	break;
310	case 21:	309	case 21:
311	i2c_write_byte(0x80); // Value	310	i2c_write_byte(0x80); // Value
312	break;	311	break;
313	case 22:	312	case 22:
314	I2C_Stop();	313	I2C_Stop();
315	I2CTimeout = 10;	314	I2CTimeout = 10;
316	I2C_Start();	315	I2C_Start();
317	break;	316	break;
318	case 23:	317	case 23:
319	i2c_write_byte(0x98); // Address of the DAC	318	i2c_write_byte(0x98); // Address of the DAC
320	break;	319	break;
321	case 24:	320	case 24:
322	i2c_write_byte(0x12); // Update Channel B	321	i2c_write_byte(0x12); // Update Channel B
323	break;	322	break;
324	case 25:	323	case 25:
325	i2c_write_byte(AnalogOffsetRoll); // Value	324	i2c_write_byte(AnalogOffsetRoll); // Value
326	break;	325	break;
327	case 26:	326	case 26:
328	i2c_write_byte(0x80); // Value	327	i2c_write_byte(0x80); // Value
329	break;	328	break;
330	case 27:	329	case 27:
331	I2C_Stop();	330	I2C_Stop();
332	I2CTimeout = 10;	331	I2CTimeout = 10;
333	I2C_Start();	332	I2C_Start();
334	break;	333	break;
335	case 28:	334	case 28:
336	i2c_write_byte(0x98); // Address of the DAC	335	i2c_write_byte(0x98); // Address of the DAC
337	break;	336	break;
338	case 29:	337	case 29:
339	i2c_write_byte(0x14); // Update Channel C	338	i2c_write_byte(0x14); // Update Channel C
340	break;	339	break;
341	case 30:	340	case 30:
342	i2c_write_byte(AnalogOffsetGier); // Value	341	i2c_write_byte(AnalogOffsetGier); // Value
343	break;	342	break;
344	case 31:	343	case 31:
345	i2c_write_byte(0x80); // Value	344	i2c_write_byte(0x80); // Value
346	break;	345	break;
347	case 32:	346	case 32:
348	I2C_Stop();	347	I2C_Stop();
349	I2CTimeout = 10;	348	I2CTimeout = 10;
350	twi_state = 0;	349	twi_state = 0;
351	break;	350	break;
352	default: twi_state = 0;	351	default: twi_state = 0;
353	break;	352	break;
354	}	353	}
355	TWCR \|= 0x80;	354	TWCR \|= 0x80;
-		355	J4Low;
356	}	356	}
357		357
358		358

Subversion Repositories FlightCtrl

(root)/branches/Proxy sensor - tempolo/twimaster.c @ 2586 - Rev 1643 → 1644