Subversion Repositories FlightCtrl

Rev

Rev 1210 | Go to most recent revision | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

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

            217