WebSVN - FlightCtrl - Diff - Rev 1639 and 1642 - /branches/V0.84a_var


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

Subversion Repositories FlightCtrl

(root)/branches/V0.84a_var_motorsmoothing/twimaster.c @ 1948 - Rev 1639 → 1642