Subversion Repositories FlightCtrl

Rev

Go to most recent revision | Details | 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;
1650 killagreg 7
volatile unsigned char motor = 0;
8
volatile unsigned char motorread = 0,MissingMotor = 0;
9
volatile unsigned char BLFlags = 0;
1479 killagreg 10
 
11
MotorData_t Motor[MAX_MOTORS];
1648 killagreg 12
BLConfig_t BLConfig[MAX_MOTORS];
1479 killagreg 13
 
1648 killagreg 14
 
1322 hbuss 15
unsigned int I2CError = 0;
1 ingob 16
 
1648 killagreg 17
#define BL_CONF_SYNC_BYTE '#'
18
 
1 ingob 19
//############################################################################
20
//Initzialisieren der I2C (TWI) Schnittstelle
21
void i2c_init(void)
22
//############################################################################
23
{
1648 killagreg 24
        unsigned char i;
25
 
26
        TWSR = 0;
27
        TWBR = ((SYSCLK/SCL_CLOCK)-16)/2;
28
 
29
        for(i=0; i < MAX_MOTORS; i++)
30
        {
31
                Motor[i].Version        = 0;
32
                Motor[i].SetPoint       = 0;
33
                Motor[i].SetPointLowerBits      = 0;
34
                Motor[i].State          = 0;
35
                Motor[i].Current        = 0;
36
                Motor[i].MaxPWM         = 0;
37
                BLConfig[i].SetMask =   MASK_SET_PWM_SCALING|MASK_SET_CURRENT_LIMIT|MASK_SET_TEMP_LIMIT|MASK_SET_CURRENT_SCALING|MASK_SET_BITCONFIG;
38
                BLConfig[i].PwmScaling = 255;           // MaxPWM
39
                BLConfig[i].CurrentLimit = 30;          // Current Limit in A
40
                BLConfig[i].TempLimit = 99;                     // Temperature Limit in °C
41
                BLConfig[i].CurrentScaling = 64;        // Current Scaling
42
                BLConfig[i].BitConfig = 0;                      // BitConfig
43
        }
1 ingob 44
}
45
 
173 holgerb 46
void i2c_reset(void)
1 ingob 47
//############################################################################
173 holgerb 48
{
1648 killagreg 49
        I2C_Stop();
50
        twi_state = 0;
51
        motor = TWDR;
52
        motor = 0;
53
        TWCR = 0x80;
54
        TWAMR = 0;
55
        TWAR = 0;
56
        TWDR = 0;
57
        TWSR = 0;
58
        TWBR = 0;
59
        i2c_init();
60
        I2C_Start();
61
        i2c_write_byte(0);
173 holgerb 62
}
1 ingob 63
 
1648 killagreg 64
 
1 ingob 65
//############################################################################
66
SIGNAL (TWI_vect)
67
//############################################################################
68
{
1650 killagreg 69
        static unsigned char missing_motor = 0, byte_counter = 0, crc = 0, read_more = 0, motorread_temperature = 0;
70
        static unsigned char *pTxBuff;
1648 killagreg 71
J4High;
72
        switch(twi_state++)
73
        {
1210 hbuss 74
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
75
// Writing the Data
76
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1648 killagreg 77
                case 0:
78
                        while(Mixer.Motor[motor][MIX_GAS] <= 0 && motor < MAX_MOTORS) motor++;  // skip if not used
79
                        if(motor == MAX_MOTORS)  // writing finished -> now read
80
                        {
81
                                motor = 0;
82
                                twi_state = 5;
83
                                i2c_write_byte(0x53+(motorread*2));
84
                        }
1209 hbuss 85
                                else i2c_write_byte(0x52+(motor*2));
1648 killagreg 86
                        break;
87
                case 1:
88
                        i2c_write_byte(Motor[motor].SetPoint);
1652 holgerb 89
                        if(!(Motor[motor].Version & MOTOR_STATE_NEW_PROTOCOL_MASK) || RequiredMotors > 6)
1648 killagreg 90
                        {
91
                                twi_state = 4; //jump over sending more data
92
                        }
93
                        else if(!(Motor[motor].SetPointLowerBits || (BLFlags & BLFLAG_SEND_CONFIG)))
94
                        {
95
                                twi_state = 4; // skip state
96
                        }
97
                        break;
98
                case 2: // lower bits of setpoint (higher resolution)
99
                        i2c_write_byte((Motor[motor].SetPointLowerBits << 1) & 0x07); // send the lower bits of setpoint
100
                        // transmit config only on demand and the motors are not running and only for one motor per round trip
1651 killagreg 101
                        if( (BLFlags & BLFLAG_SEND_CONFIG) && (motor == motorread))
1648 killagreg 102
                        {       // prepare sending of configuration
103
                                byte_counter = 0;       // reset send byte counter
104
                                crc = 0xAA; // init checksum
105
                        }
106
                        else
107
                        {       // jump to state for end of transmission for that motor
108
                                twi_state = 4;
109
                        }
110
                        break;
111
                case 3:
112
                        if(!byte_counter) // first byte?
113
                        {
114
                                i2c_write_byte(BL_CONF_SYNC_BYTE);
115
                                crc += BL_CONF_SYNC_BYTE; // update crc
116
                                pTxBuff = (uint8_t*)&BLConfig[motor]; // select configuration for motor
1651 killagreg 117
                                twi_state = 3; // keep state 3
1648 killagreg 118
                        }
1651 killagreg 119
                        else if(byte_counter == sizeof(BLConfig_t) + 1)
1648 killagreg 120
                        {       // send crc byte at the end
121
                                i2c_write_byte(crc);
1651 killagreg 122
                                twi_state = 3; // keep state 3
1648 killagreg 123
                        }
1651 killagreg 124
                        else if(byte_counter > sizeof(BLConfig_t) + 1)
125
                        {
126
                                i2c_write_byte(0);
127
                                // jump to case 4
128
                        }
1648 killagreg 129
            else // transmit configuration to BLs
130
                        {
1651 killagreg 131
                                i2c_write_byte(pTxBuff[byte_counter-1]);        // submit next byte
1648 killagreg 132
                                crc += pTxBuff[byte_counter-1];                         // update crc
1651 killagreg 133
                                twi_state = 3;// keep state 3
1648 killagreg 134
                        }
135
                        byte_counter++; // next byte
136
                        break;
137
        case 4:
138
 
139
                        if(TWSR == 0x30)
140
                        {
141
                                if(!missing_motor) missing_motor = motor + 1;
142
                                if((Motor[motor].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[motor].State++; // increment error counter and handle overflow
143
                        }
144
                        I2C_Stop();
145
                        I2CTimeout = 10;
146
                        motor++;
147
                        twi_state = 0;
148
                        I2C_Start();
149
                        break;
1210 hbuss 150
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
151
// Reading Data
152
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
153
        case 5:
1648 killagreg 154
                        //Transmit 1st byte for reading
155
                        if(TWSR != 0x40)  // Error?
156
                        {
157
                                Motor[motorread].State &= ~MOTOR_STATE_PRESENT_MASK; // clear present bit
158
                                if(++motorread >= MAX_MOTORS)
1638 holgerb 159
                                {
1648 killagreg 160
                                        BLFlags &= ~BLFLAG_SEND_CONFIG;
161
                                        motorread = 0;
162
                                        if(++motorread_temperature >= MAX_MOTORS)
163
                                        {
164
                                                motorread_temperature = 0;
165
                                                BLFlags &= ~BLFLAG_READ_VERSION;
166
                                        }
167
                                }
168
                                I2C_Stop();
169
                                twi_state = 0;
170
                                BLFlags |= BLFLAG_TX_COMPLETE;
171
                        }
172
                        else
173
                        {
174
                                Motor[motorread].State |= MOTOR_STATE_PRESENT_MASK; // set present bit
175
                                if(motorread == motorread_temperature)
176
                                {
177
                                        read_more = 1;
178
                                        I2C_ReceiveByte();
179
                                }
1638 holgerb 180
                                else
1648 killagreg 181
                                {
182
                                        read_more = 0;
183
                                        I2C_ReceiveLastByte();
1642 killagreg 184
                                }
1648 killagreg 185
                        }
186
                        MissingMotor = missing_motor;
187
                        missing_motor = 0;
188
                        break;
189
        case 6: //Read 1st byte and transmit 2nd Byte
1638 holgerb 190
                Motor[motorread].Current = TWDR;
1648 killagreg 191
                if(read_more)
192
                {
193
                                        I2C_ReceiveByte() //ack
194
 
195
                                }
1643 holgerb 196
                                else
1648 killagreg 197
                                {
198
                                        if(++motorread >= MAX_MOTORS)
199
                                        {
200
                                                motorread = 0;          // restart from beginning
201
                                                BLFlags &= ~BLFLAG_SEND_CONFIG;
202
                                                if(++motorread_temperature >= MAX_MOTORS)
203
                                                {
204
                                                        motorread_temperature = 0;
205
                                                        BLFlags &= ~BLFLAG_READ_VERSION;
206
                                                }
207
                                        }
1643 holgerb 208
                                        I2C_Stop();
209
                                        twi_state = 0;
1648 killagreg 210
                                        BLFlags |= BLFLAG_TX_COMPLETE;
211
                                }
1638 holgerb 212
                break;
1648 killagreg 213
        case 7:
1638 holgerb 214
                       //Read 2nd byte and transmit 3rd Byte
215
                Motor[motorread].MaxPWM = TWDR;
1648 killagreg 216
                if(BLFlags & BLFLAG_READ_VERSION)
217
                                {
218
                                        if(TWDR == 250)
219
                                        {
220
                                                if(!MotorenEin) Motor[motorread].Version |= MOTOR_STATE_NEW_PROTOCOL_MASK;
221
                                        }
222
                                        else
223
                                        {
224
                                                Motor[motorread].Version = 0;
225
                                        }
226
                                }
1638 holgerb 227
                                I2C_ReceiveLastByte(); //nack
228
                break;
1648 killagreg 229
        case 8: // read next
1638 holgerb 230
                                Motor[motorread].Temperature = TWDR;
1648 killagreg 231
                                if(++motorread >= MAX_MOTORS)
232
                                {
233
                                        motorread = 0; // restart reading of first motor
234
                                        BLFlags &= ~BLFLAG_SEND_CONFIG;
235
                                        if(++motorread_temperature >= MAX_MOTORS)
236
                                        {
237
                                                motorread_temperature = 0;
238
                                                BLFlags &= ~BLFLAG_READ_VERSION;
239
                                        }
240
                                }
1639 holgerb 241
                I2C_Stop();
1648 killagreg 242
                BLFlags |= BLFLAG_TX_COMPLETE;
1638 holgerb 243
                twi_state = 0;
1643 holgerb 244
                                break;
1638 holgerb 245
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
246
// writing Gyro-Offset
247
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
248
        case 18:
249
                i2c_write_byte(0x98); // Address of the DAC
250
                break;
251
        case 19:
252
                i2c_write_byte(0x10); // Update Channel A
253
                break;
254
        case 20:
255
                i2c_write_byte(AnalogOffsetNick); // Value
256
                break;
257
        case 21:
258
                i2c_write_byte(0x80); // Value
259
                break;
260
        case 22:
1639 holgerb 261
                I2C_Stop();
1638 holgerb 262
                I2CTimeout = 10;
1639 holgerb 263
                I2C_Start();
1638 holgerb 264
                break;
265
        case 23:
266
                i2c_write_byte(0x98); // Address of the DAC
267
                break;
268
        case 24:
269
                i2c_write_byte(0x12); // Update Channel B
270
                break;
271
        case 25:
272
                i2c_write_byte(AnalogOffsetRoll); // Value
273
                break;
274
        case 26:
275
                i2c_write_byte(0x80); // Value
276
                break;
277
        case 27:
1639 holgerb 278
                I2C_Stop();
1638 holgerb 279
                I2CTimeout = 10;
1639 holgerb 280
                I2C_Start();
1638 holgerb 281
                break;
282
        case 28:
283
                i2c_write_byte(0x98); // Address of the DAC
284
                break;
285
        case 29:
286
                i2c_write_byte(0x14); // Update Channel C
287
                break;
288
        case 30:
289
                i2c_write_byte(AnalogOffsetGier); // Value
290
                break;
291
        case 31:
292
                i2c_write_byte(0x80); // Value
293
                break;
294
        case 32:
1639 holgerb 295
                I2C_Stop();
1638 holgerb 296
                I2CTimeout = 10;
297
                twi_state = 0;
298
                break;
299
        default: twi_state = 0;
300
                break;
1648 killagreg 301
        }
302
        TWCR |= 0x80;
303
        J4Low;
1638 holgerb 304
}
1639 holgerb 305
 
1648 killagreg 306
void I2C_SendBLConfig(void)
307
{
308
        unsigned char i;
1651 killagreg 309
        if(MotorenEin) return;
1648 killagreg 310
        while(!(BLFlags & BLFLAG_TX_COMPLETE)); //wait for complete transfer
311
        BLFlags |= BLFLAG_SEND_CONFIG; // enable sending of BL config
1651 killagreg 312
        // setpoints should be zero
1648 killagreg 313
        for(i = 0; i < MAX_MOTORS; i++)
314
        {
315
                Motor[i].SetPoint = 0;
316
                Motor[i].SetPointLowerBits = 0;
317
        }
318
        motorread = 0;
319
        // needs at least MAX_MOTORS loops of 2 ms (12*2ms = 24ms)
320
        do
321
        {
322
                twi_state = 0;
323
                I2C_Start(); // start an i2c transmission
324
                while(!(BLFlags & BLFLAG_TX_COMPLETE)); //wait for complete transfer
325
        }while(BLFlags & BLFLAG_SEND_CONFIG); // repeat until the BL config has been send to all motors
326
}
327
 
328
 
329