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