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
102
                        if( (BLFlags & BLFLAG_SEND_CONFIG) && !MotorenEin && (motor == motorread))
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
118
                        }
119
                        else if(byte_counter >= sizeof(BLConfig_t)+1) // last byte?
120
                        {       // send crc byte at the end
121
                                i2c_write_byte(crc);
122
                        }
123
            else // transmit configuration to BLs
124
                        {
125
                                i2c_write_byte(pTxBuff[byte_counter-1]);                // submit next byte
126
                                crc += pTxBuff[byte_counter-1];                         // update crc
127
                                twi_state = 3;                                                          // stay in this state
128
                        }
129
                        byte_counter++; // next byte
130
                        break;
131
        case 4:
132
 
133
                        if(TWSR == 0x30)
134
                        {
135
                                if(!missing_motor) missing_motor = motor + 1;
136
                                if((Motor[motor].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[motor].State++; // increment error counter and handle overflow
137
                        }
138
                        I2C_Stop();
139
                        I2CTimeout = 10;
140
                        motor++;
141
                        twi_state = 0;
142
                        I2C_Start();
143
                        break;
1210 hbuss 144
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
145
// Reading Data
146
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
147
        case 5:
1648 killagreg 148
                        //Transmit 1st byte for reading
149
                        if(TWSR != 0x40)  // Error?
150
                        {
151
                                Motor[motorread].State &= ~MOTOR_STATE_PRESENT_MASK; // clear present bit
152
                                if(++motorread >= MAX_MOTORS)
1638 holgerb 153
                                {
1648 killagreg 154
                                        BLFlags &= ~BLFLAG_SEND_CONFIG;
155
                                        motorread = 0;
156
                                        if(++motorread_temperature >= MAX_MOTORS)
157
                                        {
158
                                                motorread_temperature = 0;
159
                                                BLFlags &= ~BLFLAG_READ_VERSION;
160
                                        }
161
                                }
162
                                I2C_Stop();
163
                                twi_state = 0;
164
                                BLFlags |= BLFLAG_TX_COMPLETE;
165
                        }
166
                        else
167
                        {
168
                                Motor[motorread].State |= MOTOR_STATE_PRESENT_MASK; // set present bit
169
                                if(motorread == motorread_temperature)
170
                                {
171
                                        read_more = 1;
172
                                        I2C_ReceiveByte();
173
                                }
1638 holgerb 174
                                else
1648 killagreg 175
                                {
176
                                        read_more = 0;
177
                                        I2C_ReceiveLastByte();
1642 killagreg 178
                                }
1648 killagreg 179
                        }
180
                        MissingMotor = missing_motor;
181
                        missing_motor = 0;
182
                        break;
183
        case 6: //Read 1st byte and transmit 2nd Byte
1638 holgerb 184
                Motor[motorread].Current = TWDR;
1648 killagreg 185
                if(read_more)
186
                {
187
                                        I2C_ReceiveByte() //ack
188
 
189
                                }
1643 holgerb 190
                                else
1648 killagreg 191
                                {
192
                                        if(++motorread >= MAX_MOTORS)
193
                                        {
194
                                                motorread = 0;          // restart from beginning
195
                                                BLFlags &= ~BLFLAG_SEND_CONFIG;
196
                                                if(++motorread_temperature >= MAX_MOTORS)
197
                                                {
198
                                                        motorread_temperature = 0;
199
                                                        BLFlags &= ~BLFLAG_READ_VERSION;
200
                                                }
201
                                        }
1643 holgerb 202
                                        I2C_Stop();
203
                                        twi_state = 0;
1648 killagreg 204
                                        BLFlags |= BLFLAG_TX_COMPLETE;
205
                                }
1638 holgerb 206
                break;
1648 killagreg 207
        case 7:
1638 holgerb 208
                       //Read 2nd byte and transmit 3rd Byte
209
                Motor[motorread].MaxPWM = TWDR;
1648 killagreg 210
                if(BLFlags & BLFLAG_READ_VERSION)
211
                                {
212
                                        if(TWDR == 250)
213
                                        {
214
                                                if(!MotorenEin) Motor[motorread].Version |= MOTOR_STATE_NEW_PROTOCOL_MASK;
215
                                        }
216
                                        else
217
                                        {
218
                                                Motor[motorread].Version = 0;
219
                                        }
220
                                }
1638 holgerb 221
                                I2C_ReceiveLastByte(); //nack
222
                break;
1648 killagreg 223
        case 8: // read next
1638 holgerb 224
                                Motor[motorread].Temperature = TWDR;
1648 killagreg 225
                                if(++motorread >= MAX_MOTORS)
226
                                {
227
                                        motorread = 0; // restart reading of first motor
228
                                        BLFlags &= ~BLFLAG_SEND_CONFIG;
229
                                        if(++motorread_temperature >= MAX_MOTORS)
230
                                        {
231
                                                motorread_temperature = 0;
232
                                                BLFlags &= ~BLFLAG_READ_VERSION;
233
                                        }
234
                                }
1639 holgerb 235
                I2C_Stop();
1648 killagreg 236
                BLFlags |= BLFLAG_TX_COMPLETE;
1638 holgerb 237
                twi_state = 0;
1643 holgerb 238
                                break;
1638 holgerb 239
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
240
// writing Gyro-Offset
241
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
242
        case 18:
243
                i2c_write_byte(0x98); // Address of the DAC
244
                break;
245
        case 19:
246
                i2c_write_byte(0x10); // Update Channel A
247
                break;
248
        case 20:
249
                i2c_write_byte(AnalogOffsetNick); // Value
250
                break;
251
        case 21:
252
                i2c_write_byte(0x80); // Value
253
                break;
254
        case 22:
1639 holgerb 255
                I2C_Stop();
1638 holgerb 256
                I2CTimeout = 10;
1639 holgerb 257
                I2C_Start();
1638 holgerb 258
                break;
259
        case 23:
260
                i2c_write_byte(0x98); // Address of the DAC
261
                break;
262
        case 24:
263
                i2c_write_byte(0x12); // Update Channel B
264
                break;
265
        case 25:
266
                i2c_write_byte(AnalogOffsetRoll); // Value
267
                break;
268
        case 26:
269
                i2c_write_byte(0x80); // Value
270
                break;
271
        case 27:
1639 holgerb 272
                I2C_Stop();
1638 holgerb 273
                I2CTimeout = 10;
1639 holgerb 274
                I2C_Start();
1638 holgerb 275
                break;
276
        case 28:
277
                i2c_write_byte(0x98); // Address of the DAC
278
                break;
279
        case 29:
280
                i2c_write_byte(0x14); // Update Channel C
281
                break;
282
        case 30:
283
                i2c_write_byte(AnalogOffsetGier); // Value
284
                break;
285
        case 31:
286
                i2c_write_byte(0x80); // Value
287
                break;
288
        case 32:
1639 holgerb 289
                I2C_Stop();
1638 holgerb 290
                I2CTimeout = 10;
291
                twi_state = 0;
292
                break;
293
        default: twi_state = 0;
294
                break;
1648 killagreg 295
        }
296
        TWCR |= 0x80;
297
        J4Low;
1638 holgerb 298
}
1639 holgerb 299
 
1648 killagreg 300
void I2C_SendBLConfig(void)
301
{
302
        unsigned char i;
303
        while(!(BLFlags & BLFLAG_TX_COMPLETE)); //wait for complete transfer
304
        BLFlags |= BLFLAG_SEND_CONFIG; // enable sending of BL config
305
        for(i = 0; i < MAX_MOTORS; i++)
306
        {
307
                Motor[i].SetPoint = 0;
308
                Motor[i].SetPointLowerBits = 0;
309
        }
310
        motorread = 0;
311
        // needs at least MAX_MOTORS loops of 2 ms (12*2ms = 24ms)
312
        do
313
        {
314
                twi_state = 0;
315
                I2C_Start(); // start an i2c transmission
316
                while(!(BLFlags & BLFLAG_TX_COMPLETE)); //wait for complete transfer
317
        }while(BLFlags & BLFLAG_SEND_CONFIG); // repeat until the BL config has been send to all motors
318
}
319
 
320
 
321