Subversion Repositories FlightCtrl

Rev

Rev 1378 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 1378 Rev 1550
Line 4... Line 4...
4
#include "main.h"
4
#include "main.h"
Line 5... Line 5...
5
 
5
 
6
volatile unsigned char twi_state = 0;
6
volatile unsigned char twi_state = 0;
7
unsigned char motor = 0;
7
unsigned char motor = 0;
8
unsigned char motorread = 0,MissingMotor = 0;
-
 
9
unsigned char motor_rx[16],motor_rx2[16];
-
 
-
 
8
unsigned char motorread = 0,MissingMotor = 0;
10
unsigned char MotorPresent[MAX_MOTORS];
9
 
-
 
10
MotorData_t Motor[MAX_MOTORS];
11
unsigned char MotorError[MAX_MOTORS];
11
 
Line 12... Line 12...
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;
Line 20... Line 20...
20
  TWBR = ((SYSCLK/SCL_CLOCK)-16)/2;
20
  TWBR = ((SYSCLK/SCL_CLOCK)-16)/2;
21
}
21
}
22
 
22
 
23
//############################################################################
23
//############################################################################
24
//Start I2C
24
//Start I2C
25
void i2c_start(void)
25
void i2c_start(void)
26
//############################################################################
26
//############################################################################
Line 36... Line 36...
36
}
36
}
Line 37... Line 37...
37
 
37
 
38
void i2c_reset(void)
38
void i2c_reset(void)
39
//############################################################################
39
//############################################################################
40
{
40
{
41
                 i2c_stop();                
41
                 i2c_stop();
42
                 twi_state = 0;
42
                 twi_state = 0;
43
                 motor = TWDR;
43
                 motor = TWDR;
44
                 motor = 0;
44
                 motor = 0;
45
                 TWCR = 0x80;
45
                 TWCR = 0x80;
Line 54... Line 54...
54
}
54
}
Line 55... Line 55...
55
 
55
 
56
//############################################################################
56
//############################################################################
57
void i2c_write_byte(char byte)
57
void i2c_write_byte(char byte)
58
//############################################################################
58
//############################################################################
59
{
59
{
60
    TWSR = 0x00;
60
    TWSR = 0x00;
61
    TWDR = byte;
61
    TWDR = byte;
62
    TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE);
62
    TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE);
Line 103... Line 103...
103
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
103
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
104
// Writing the Data
104
// Writing the Data
105
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
105
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
106
        case 0:
106
        case 0:
107
                        while(Mixer.Motor[motor][0] <= 0 && motor < MAX_MOTORS) motor++;  // skip if not used
107
                        while(Mixer.Motor[motor][0] <= 0 && motor < MAX_MOTORS) motor++;  // skip if not used
108
                                if(motor == MAX_MOTORS)  // writing finished -> now read 
108
                                if(motor == MAX_MOTORS)  // writing finished -> now read
109
                                {
109
                                {
110
                                 motor = 0;
110
                                 motor = 0;
111
                                 twi_state = 3;
111
                                 twi_state = 3;
112
                                 i2c_write_byte(0x53+(motorread*2));
112
                                 i2c_write_byte(0x53+(motorread*2));
113
                                 }
113
                                 }
114
                                else i2c_write_byte(0x52+(motor*2));
114
                                else i2c_write_byte(0x52+(motor*2));
115
                break;
115
                break;
116
        case 1:
116
        case 1:
117
                i2c_write_byte(Motor[motor++]);
117
                i2c_write_byte(Motor[motor++].SetPoint);
118
                break;
118
                break;
119
        case 2:
119
        case 2:
120
                if(TWSR == 0x30)
120
                if(TWSR == 0x30)
121
                                 {
121
                                 {
122
                                  if(!missing_motor) missing_motor = motor;
122
                                  if(!missing_motor) missing_motor = motor;
123
                                  if(++MotorError[motor-1] == 0) MotorError[motor-1] = 255;
123
                                  if((Motor[motor-1].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[motor-1].State++; // increment error counter and handle overflow
124
                                 }
124
                                 }
125
                i2c_stop();
125
                i2c_stop();
126
                I2CTimeout = 10;
126
                I2CTimeout = 10;
127
                twi_state = 0;
127
                twi_state = 0;
128
                i2c_start();  
128
                i2c_start();
129
                break;
129
                break;
130
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
130
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
131
// Reading Data
131
// Reading Data
132
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
132
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
133
        case 3:
133
        case 3:
134
               //Transmit 1st byte for reading
134
               //Transmit 1st byte for reading
135
                if(TWSR != 0x40)  // Error?
135
                if(TWSR != 0x40)  // Error?
136
                 {
136
                 {
137
                  MotorPresent[motorread] = 0;
137
                  Motor[motorread].State &= ~MOTOR_STATE_PRESENT_MASK; // clear present bit
138
                  motorread++;
138
                  motorread++;
139
                  if(motorread >= MAX_MOTORS) motorread = 0;
139
                  if(motorread >= MAX_MOTORS) motorread = 0;
140
                  i2c_stop();
140
                  i2c_stop();
141
                  twi_state = 0;
141
                  twi_state = 0;
142
                 }
142
                 }
143
                 else
143
                 else
144
                 {
144
                 {
145
                  MotorPresent[motorread] = ('1' - '-') + motorread;
145
                  Motor[motorread].State |= MOTOR_STATE_PRESENT_MASK; // set present bit
146
                  I2C_ReceiveByte();
146
                  I2C_ReceiveByte();
147
                 }
147
                 }
148
                MissingMotor = missing_motor;
148
                MissingMotor = missing_motor;
149
                missing_motor = 0;
149
                missing_motor = 0;
150
                break;
150
                break;
151
        case 4: //Read 1st byte and transmit 2nd Byte
151
        case 4: //Read 1st byte and transmit 2nd Byte
152
                motor_rx[motorread] = TWDR;
152
                Motor[motorread].Current = TWDR;
153
                I2C_ReceiveLastByte(); //nack
153
                I2C_ReceiveLastByte(); //nack
154
                break;
154
                break;
155
        case 5:
155
        case 5:
156
                //Read 2nd byte
156
                //Read 2nd byte
157
                motor_rx2[motorread++] = TWDR;
157
                Motor[motorread].MaxPWM = TWDR;
-
 
158
                                motorread++; // next motor
158
                if(motorread >= MAX_MOTORS) motorread = 0;
159
                if(motorread >= MAX_MOTORS) motorread = 0;
159
                i2c_stop();
160
                i2c_stop();
160
                twi_state = 0;
161
                twi_state = 0;
161
                break;
162
                break;
162
                       
163
 
163
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
164
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
164
// writing Gyro-Offset
165
// writing Gyro-Offset
165
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
166
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
166
        case 8:
167
        case 8:
167
                i2c_write_byte(0x98); // Address of the DAC
168
                i2c_write_byte(0x98); // Address of the DAC
168
                break;
169
                break;
169
        case 9:
170
        case 9:
170
                i2c_write_byte(0x10); // Update Channel A
171
                i2c_write_byte(0x10); // Update Channel A
171
                break;
172
                break;
172
        case 10:
173
        case 10:
173
                i2c_write_byte(AnalogOffsetNick); // Value
174
                i2c_write_byte(AnalogOffsetNick); // Value
174
                break;
175
                break;
175
        case 11:
176
        case 11:
176
                i2c_write_byte(0x80); // Value
177
                i2c_write_byte(0x80); // Value
177
                break;
178
                break;
178
        case 12:
179
        case 12:
179
                i2c_stop();              
180
                i2c_stop();
180
                I2CTimeout = 10;
181
                I2CTimeout = 10;
181
                i2c_start();  
182
                i2c_start();
182
                break;
183
                break;
183
        case 13:
184
        case 13:
184
                i2c_write_byte(0x98); // Address of the DAC
185
                i2c_write_byte(0x98); // Address of the DAC
185
                break;
186
                break;
186
        case 14:
187
        case 14:
187
                i2c_write_byte(0x12); // Update Channel B
188
                i2c_write_byte(0x12); // Update Channel B
188
                break;
189
                break;
189
        case 15:
190
        case 15:
190
                i2c_write_byte(AnalogOffsetRoll); // Value
191
                i2c_write_byte(AnalogOffsetRoll); // Value
191
                break;
192
                break;
192
        case 16:
193
        case 16:
193
                i2c_write_byte(0x80); // Value
194
                i2c_write_byte(0x80); // Value
194
                break;
195
                break;
195
        case 17:
196
        case 17:
196
                i2c_stop();              
197
                i2c_stop();
197
                I2CTimeout = 10;
198
                I2CTimeout = 10;
198
                i2c_start();  
199
                i2c_start();
199
                break;
200
                break;
200
        case 18:
201
        case 18:
201
                i2c_write_byte(0x98); // Address of the DAC
202
                i2c_write_byte(0x98); // Address of the DAC
202
                break;
203
                break;
203
        case 19:
204
        case 19:
204
                i2c_write_byte(0x14); // Update Channel C
205
                i2c_write_byte(0x14); // Update Channel C
205
                break;
206
                break;
206
        case 20:
207
        case 20:
207
                i2c_write_byte(AnalogOffsetGier); // Value
208
                i2c_write_byte(AnalogOffsetGier); // Value
208
                break;
209
                break;
209
        case 21:
210
        case 21:
210
                i2c_write_byte(0x80); // Value
211
                i2c_write_byte(0x80); // Value
211
                break;
212
                break;
212
        case 22:
213
        case 22:
213
                i2c_stop();              
214
                i2c_stop();
214
                I2CTimeout = 10;
215
                I2CTimeout = 10;
215
                twi_state = 0;
216
                twi_state = 0;
216
                break;
217
                break;
217
        default: twi_state = 0;
218
        default: twi_state = 0;
218
                break;        
219
                break;
219
                }
220
                }
220
 TWCR |= 0x80;
221
 TWCR |= 0x80;
221
}
222
}