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