WebSVN - FlightCtrl - Diff - Rev 1642 and 1643 - /branches/v0.82a MartinW


/*############################################################################
############################################################################*/
 
#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,49,0x00,7,8,9,10};
 static unsigned char missing_motor,send = 0,crc = 0,read_more = 0;
 
     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;
}
 
 

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

Subversion Repositories FlightCtrl

(root)/branches/v0.82a MartinW_Jeti+/twimaster.c @ 2426 - Rev 1642 → 1643