Subversion Repositories FlightCtrl

Rev

Rev 1639 | Rev 1643 | Go to most recent revision | Details | Compare with Previous | Last modification | View Log | RSS feed

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