Subversion Repositories FlightCtrl

Rev

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

Rev 1211 Rev 1322
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 
unsigned char MotorError[MAX_MOTORS];
- 
 
 12 
unsigned int I2CError = 0;
12 
 
 13 
 
13 
//############################################################################
 14 
//############################################################################
14 
//Initzialisieren der I2C (TWI) Schnittstelle
 15 
//Initzialisieren der I2C (TWI) Schnittstelle
15 
void i2c_init(void)
 16 
void i2c_init(void)
16 
//############################################################################
 17 
//############################################################################
17 
{
 18 
{
18 
  TWSR = 0;
 19 
  TWSR = 0;
19 
  TWBR = ((SYSCLK/SCL_CLOCK)-16)/2;
 20 
  TWBR = ((SYSCLK/SCL_CLOCK)-16)/2;
20 
}
 21 
}
21 
 
 22 
 
22 
//############################################################################
 23 
//############################################################################
23 
//Start I2C
 24 
//Start I2C
24 
void i2c_start(void)
 25 
void i2c_start(void)
25 
//############################################################################
 26 
//############################################################################
26 
{
 27 
{
27 
    TWCR = (1<<TWSTA) | (1<<TWEN) | (1<<TWINT) | (1<<TWIE);
 28 
    TWCR = (1<<TWSTA) | (1<<TWEN) | (1<<TWINT) | (1<<TWIE);
28 
}
 29 
}
29 
 
 30 
 
30 
//############################################################################
 31 
//############################################################################
31 
void i2c_stop(void)
 32 
void i2c_stop(void)
32 
//############################################################################
 33 
//############################################################################
33 
{
 34 
{
34 
    TWCR = (1<<TWEN) | (1<<TWSTO) | (1<<TWINT);
 35 
    TWCR = (1<<TWEN) | (1<<TWSTO) | (1<<TWINT);
35 
}
 36 
}
36 
 
 37 
 
37 
void i2c_reset(void)
 38 
void i2c_reset(void)
38 
//############################################################################
 39 
//############################################################################
39 
{
 40 
{
40 
                 i2c_stop();                
 41 
                 i2c_stop();                
41 
                 twi_state = 0;
 42 
                 twi_state = 0;
42 
                 motor = TWDR;
 43 
                 motor = TWDR;
43 
                 motor = 0;
 44 
                 motor = 0;
44 
                 TWCR = 0x80;
 45 
                 TWCR = 0x80;
45 
                 TWAMR = 0;
 46 
                 TWAMR = 0;
46 
                 TWAR = 0;
 47 
                 TWAR = 0;
47 
                 TWDR = 0;
 48 
                 TWDR = 0;
48 
                 TWSR = 0;
 49 
                 TWSR = 0;
49 
                 TWBR = 0;
 50 
                 TWBR = 0;
50 
                 i2c_init();
 51 
                 i2c_init();
51 
                 i2c_start();
 52 
                 i2c_start();
52 
                 i2c_write_byte(0);
 53 
                 i2c_write_byte(0);
53 
}
 54 
}
54 
 
 55 
 
55 
//############################################################################
 56 
//############################################################################
56 
void i2c_write_byte(char byte)
 57 
void i2c_write_byte(char byte)
57 
//############################################################################
 58 
//############################################################################
58 
{
 59 
{
59 
    TWSR = 0x00;
 60 
    TWSR = 0x00;
60 
    TWDR = byte;
 61 
    TWDR = byte;
61 
    TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE);
 62 
    TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE);
62 
}
 63 
}
63 
 
 64 
 
64 
/****************************************/
 65 
/****************************************/
65 
/*    Write to I2C                      */
 66 
/*    Write to I2C                      */
66 
/****************************************/
 67 
/****************************************/
67 
void I2C_WriteByte(int8_t byte)
 68 
void I2C_WriteByte(int8_t byte)
68 
{
 69 
{
69 
    // move byte to send into TWI Data Register
 70 
    // move byte to send into TWI Data Register
70 
    TWDR = byte;
 71 
    TWDR = byte;
71 
    // clear interrupt flag (TWINT = 1)
 72 
    // clear interrupt flag (TWINT = 1)
72 
    // enable i2c bus (TWEN = 1)
 73 
    // enable i2c bus (TWEN = 1)
73 
    // enable interrupt (TWIE = 1)
 74 
    // enable interrupt (TWIE = 1)
74 
    TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE);
 75 
    TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE);
75 
}
 76 
}
76 
 
 77 
 
77 
/****************************************/
 78 
/****************************************/
78 
/*    Receive byte and send ACK         */
 79 
/*    Receive byte and send ACK         */
79 
/****************************************/
 80 
/****************************************/
80 
void I2C_ReceiveByte(void)
 81 
void I2C_ReceiveByte(void)
81 
{
 82 
{
82 
   TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE) | (1<<TWEA);
 83 
   TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE) | (1<<TWEA);
83 
}
 84 
}
84 
 
 85 
 
85 
/****************************************/
 86 
/****************************************/
86 
/* I2C receive last byte and send no ACK*/
 87 
/* I2C receive last byte and send no ACK*/
87 
/****************************************/
 88 
/****************************************/
88 
void I2C_ReceiveLastByte(void)
 89 
void I2C_ReceiveLastByte(void)
89 
{
 90 
{
90 
   TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE);
 91 
   TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE);
91 
}
 92 
}
92 
 
 93 
 
93 
 
 94 
 
94 
 
 95 
 
95 
//############################################################################
 96 
//############################################################################
96 
SIGNAL (TWI_vect)
 97 
SIGNAL (TWI_vect)
97 
//############################################################################
 98 
//############################################################################
98 
{
 99 
{
99 
 static unsigned char missing_motor;
 100 
 static unsigned char missing_motor;
100 
     switch(twi_state++)
 101 
     switch(twi_state++)
101 
        {
 102 
        {
102 
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 103 
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
103 
// Writing the Data
 104 
// Writing the Data
104 
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 105 
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
105 
        case 0:
 106 
        case 0:
106 
                        while(Mixer.Motor[motor][0] <= 0 && motor < MAX_MOTORS) motor++;  // skip if not used
 107 
                        while(Mixer.Motor[motor][0] <= 0 && motor < MAX_MOTORS) motor++;  // skip if not used
107 
                                if(motor == MAX_MOTORS)  // writing finished -> now read
108 
                                if(motor == MAX_MOTORS)  // writing finished -> now read
108 
                                {
 109 
                                {
109 
                                 motor = 0;
 110 
                                 motor = 0;
110 
                                 twi_state = 3;
 111 
                                 twi_state = 3;
111 
                                 i2c_write_byte(0x53+(motorread*2));
 112 
                                 i2c_write_byte(0x53+(motorread*2));
112 
                                 }
 113 
                                 }
113 
                                else i2c_write_byte(0x52+(motor*2));
 114 
                                else i2c_write_byte(0x52+(motor*2));
114 
                break;
 115 
                break;
115 
        case 1:
 116 
        case 1:
116 
                i2c_write_byte(Motor[motor++]);
 117 
                i2c_write_byte(Motor[motor++]);
117 
                break;
 118 
                break;
118 
        case 2:
 119 
        case 2:
- 
 
 120 
                if(TWSR == 0x30)
- 
 
 121 
                                 {
- 
 
 122 
                                  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;}
 123 
                                  if(++MotorError[motor-1] == 0) MotorError[motor-1] = 255;
- 
 
 124 
                                 }
120 
                i2c_stop();
 125 
                i2c_stop();
121 
                I2CTimeout = 10;
 126 
                I2CTimeout = 10;
122 
                twi_state = 0;
 127 
                twi_state = 0;
123 
                i2c_start();  
 128 
                i2c_start();  
124 
                break;
 129 
                break;
125 
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 130 
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
126 
// Reading Data
 131 
// Reading Data
127 
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 132 
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
128 
        case 3:
 133 
        case 3:
129 
               //Transmit 1st byte for reading
 134 
               //Transmit 1st byte for reading
130 
                if(TWSR != 0x40)  // Error?
 135 
                if(TWSR != 0x40)  // Error?
131 
                 {
 136 
                 {
132 
                  MotorPresent[motorread] = 0;
 137 
                  MotorPresent[motorread] = 0;
133 
                  motorread++;
 138 
                  motorread++;
134 
                  if(motorread >= MAX_MOTORS) motorread = 0;
 139 
                  if(motorread >= MAX_MOTORS) motorread = 0;
135 
                  i2c_stop();
 140 
                  i2c_stop();
136 
                  twi_state = 0;
 141 
                  twi_state = 0;
137 
                 }
 142 
                 }
138 
                 else
 143 
                 else
139 
                 {
 144 
                 {
140 
                  MotorPresent[motorread] = ('1' - '-') + motorread;
 145 
                  MotorPresent[motorread] = ('1' - '-') + motorread;
141 
                  I2C_ReceiveByte();
 146 
                  I2C_ReceiveByte();
142 
                 }
 147 
                 }
143 
                MissingMotor = missing_motor;
 148 
                MissingMotor = missing_motor;
144 
                missing_motor = 0;
 149 
                missing_motor = 0;
145 
                break;
 150 
                break;
146 
        case 4: //Read 1st byte and transmit 2nd Byte
 151 
        case 4: //Read 1st byte and transmit 2nd Byte
147 
                motor_rx[motorread] = TWDR;
 152 
                motor_rx[motorread] = TWDR;
148 
                I2C_ReceiveLastByte(); //nack
 153 
                I2C_ReceiveLastByte(); //nack
149 
                break;
 154 
                break;
150 
        case 5:
 155 
        case 5:
151 
                //Read 2nd byte
 156 
                //Read 2nd byte
152 
                motor_rx2[motorread++] = TWDR;
 157 
                motor_rx2[motorread++] = TWDR;
153 
                if(motorread >= MAX_MOTORS) motorread = 0;
 158 
                if(motorread >= MAX_MOTORS) motorread = 0;
154 
                i2c_stop();
 159 
                i2c_stop();
155 
                twi_state = 0;
 160 
                twi_state = 0;
156 
                break;
 161 
                break;
157 
                       
 162 
                       
158 
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 163 
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
159 
// writing Gyro-Offset
 164 
// writing Gyro-Offset
160 
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 165 
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
161 
        case 8:
 166 
        case 8:
162 
                i2c_write_byte(0x98); // Address of the DAC
 167 
                i2c_write_byte(0x98); // Address of the DAC
163 
                break;
 168 
                break;
164 
        case 9:
 169 
        case 9:
165 
                i2c_write_byte(0x10); // Update Channel A
 170 
                i2c_write_byte(0x10); // Update Channel A
166 
                break;
 171 
                break;
167 
        case 10:
 172 
        case 10:
168 
                i2c_write_byte(AnalogOffsetNick); // Value
 173 
                i2c_write_byte(AnalogOffsetNick); // Value
169 
                break;
 174 
                break;
170 
        case 11:
 175 
        case 11:
171 
                i2c_write_byte(0x80); // Value
 176 
                i2c_write_byte(0x80); // Value
172 
                break;
 177 
                break;
173 
        case 12:
 178 
        case 12:
174 
                i2c_stop();              
 179 
                i2c_stop();              
175 
                I2CTimeout = 10;
 180 
                I2CTimeout = 10;
176 
                i2c_start();  
 181 
                i2c_start();  
177 
                break;
 182 
                break;
178 
        case 13:
 183 
        case 13:
179 
                i2c_write_byte(0x98); // Address of the DAC
 184 
                i2c_write_byte(0x98); // Address of the DAC
180 
                break;
 185 
                break;
181 
        case 14:
 186 
        case 14:
182 
                i2c_write_byte(0x12); // Update Channel B
 187 
                i2c_write_byte(0x12); // Update Channel B
183 
                break;
 188 
                break;
184 
        case 15:
 189 
        case 15:
185 
                i2c_write_byte(AnalogOffsetRoll); // Value
 190 
                i2c_write_byte(AnalogOffsetRoll); // Value
186 
                break;
 191 
                break;
187 
        case 16:
 192 
        case 16:
188 
                i2c_write_byte(0x80); // Value
 193 
                i2c_write_byte(0x80); // Value
189 
                break;
 194 
                break;
190 
        case 17:
 195 
        case 17:
191 
                i2c_stop();              
 196 
                i2c_stop();              
192 
                I2CTimeout = 10;
 197 
                I2CTimeout = 10;
193 
                i2c_start();  
 198 
                i2c_start();  
194 
                break;
 199 
                break;
195 
        case 18:
 200 
        case 18:
196 
                i2c_write_byte(0x98); // Address of the DAC
 201 
                i2c_write_byte(0x98); // Address of the DAC
197 
                break;
 202 
                break;
198 
        case 19:
 203 
        case 19:
199 
                i2c_write_byte(0x14); // Update Channel C
 204 
                i2c_write_byte(0x14); // Update Channel C
200 
                break;
 205 
                break;
201 
        case 20:
 206 
        case 20:
202 
                i2c_write_byte(AnalogOffsetGier); // Value
 207 
                i2c_write_byte(AnalogOffsetGier); // Value
203 
                break;
 208 
                break;
204 
        case 21:
 209 
        case 21:
205 
                i2c_write_byte(0x80); // Value
 210 
                i2c_write_byte(0x80); // Value
206 
                break;
 211 
                break;
207 
        case 22:
 212 
        case 22:
208 
                i2c_stop();              
 213 
                i2c_stop();              
209 
                I2CTimeout = 10;
 214 
                I2CTimeout = 10;
210 
                twi_state = 0;
 215 
                twi_state = 0;
211 
                break;
 216 
                break;
212 
        default: twi_state = 0;
 217 
        default: twi_state = 0;
213 
                break;        
 218 
                break;        
214 
                }
 219 
                }
215 
 TWCR |= 0x80;
 220 
 TWCR |= 0x80;
216 
}
 221 
}
217 
 
 222