Subversion Repositories FlightCtrl

Rev

Rev 1400 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 1400 Rev 1452
Line 15... Line 15...
15 
//Initzialisieren der I2C (TWI) Schnittstelle
 15 
//Initzialisieren der I2C (TWI) Schnittstelle
16 
void i2c_init(void)
 16 
void i2c_init(void)
17 
//############################################################################
 17 
//############################################################################
18 
{
 18 
{
19 
  TWSR = 0;
 19 
  TWSR = 0;
20 
  TWBR = ((SYSCLK/SCL_CLOCK)-16)/2;
 20 
  TWBR = ((SYSCLK/SCL_CLOCK)-16)/2;
21 
}
 21 
}
Line 22... Line 22...
22 
 
 22 
 
23 
//############################################################################
 23 
//############################################################################
24 
//Start I2C
 24 
//Start I2C
25 
void i2c_start(void)
 25 
void i2c_start(void)
26 
//############################################################################
 26 
//############################################################################
27 
{
 27 
{
28 
    TWCR = (1<<TWSTA) | (1<<TWEN) | (1<<TWINT) | (1<<TWIE);
 28 
    TWCR = (1<<TWSTA) | (1<<TWEN) | (1<<TWINT) | (1<<TWIE);
Line 36... Line 36...
36 
}
 36 
}
Line 37... Line 37...
37 
 
 37 
 
38 
void i2c_reset(void)
 38 
void i2c_reset(void)
39 
//############################################################################
 39 
//############################################################################
40 
{
 40 
{
41 
                 i2c_stop();                
 41 
                 i2c_stop();
42 
                 twi_state = 0;
 42 
                 twi_state = 0;
43 
                 motor = TWDR;
 43 
                 motor = TWDR;
44 
                 motor = 0;
 44 
                 motor = 0;
45 
                 TWCR = 0x80;
 45 
                 TWCR = 0x80;
Line 54... Line 54...
54 
}
 54 
}
Line 55... Line 55...
55 
 
 55 
 
56 
//############################################################################
 56 
//############################################################################
57 
void i2c_write_byte(char byte)
 57 
void i2c_write_byte(char byte)
58 
//############################################################################
 58 
//############################################################################
59 
{
 59 
{
60 
    TWSR = 0x00;
 60 
    TWSR = 0x00;
61 
    TWDR = byte;
 61 
    TWDR = byte;
62 
    TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE);
 62 
    TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE);
Line 89... Line 89...
89 
void I2C_ReceiveLastByte(void)
 89 
void I2C_ReceiveLastByte(void)
90 
{
 90 
{
91 
   TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE);
 91 
   TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE);
92 
}
 92 
}
Line -... Line 93...
- 
 
 93 
 
- 
 
 94 
uint8_t calc_crc(uint8_t *buffer, uint8_t l) {
- 
 
 95 
        uint8_t crc = 0xff;
- 
 
 96 
        uint8_t i;
- 
 
 97 
        for(i = 0; i < l; i++) {
- 
 
 98 
                crc = crc ^ buffer[i];
- 
 
 99 
        }
93 
 
 100 
        return crc;
Line 94... Line 101...
94 
 
 101 
}
95 
 
 102 
 
96 
//############################################################################
 103 
//############################################################################
97 
SIGNAL (TWI_vect)
 104 
SIGNAL (TWI_vect)
98 
//############################################################################
 105 
//############################################################################
99 
{
 106 
{
100 
 static unsigned char missing_motor;
 107 
 static unsigned char missing_motor;
101 
     switch(twi_state++)
 108 
     switch(twi_state++)
102 
        {
 - 
 
103 
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 - 
 
104 
// Writing the Data
 - 
 
105 
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 - 
 
106 
        case 0:
 - 
 
107 
                        while(Mixer.Motor[motor][0] <= 0 && motor < MAX_MOTORS) motor++;  // skip if not used
 - 
 
108 
                                if(motor == MAX_MOTORS)  // writing finished -> now read
- 
 
109 
                                {
 - 
 
110 
                                 motor = 0;
 - 
 
111 
                                 twi_state = 3;
 - 
 
112 
                                 i2c_write_byte(0x53+(motorread*2));
 - 
 
113 
                                 }
 - 
 
114 
                                else i2c_write_byte(0x52+(motor*2));
 - 
 
115 
                break;
 - 
 
116 
        case 1:
 - 
 
117 
                i2c_write_byte(Motor[motor++]);
 - 
 
118 
                break;
 - 
 
119 
        case 2:
 - 
 
120 
                if(TWSR == 0x30)
 - 
 
121 
                                 {
 - 
 
122 
                                  if(!missing_motor) missing_motor = motor;
 - 
 
123 
                                  if(++MotorError[motor-1] == 0) MotorError[motor-1] = 255;
 - 
 
124 
                                 }
 - 
 
125 
                i2c_stop();
 - 
 
126 
                I2CTimeout = 10;
 - 
 
127 
                twi_state = 0;
 - 
 
128 
                i2c_start();  
 - 
 
129 
                break;
 - 
 
130 
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 - 
 
131 
// Reading Data
 - 
 
132 
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 - 
 
133 
        case 3:
 - 
 
134 
               //Transmit 1st byte for reading
 - 
 
135 
                if(TWSR != 0x40)  // Error?
 - 
 
136 
                 {
 - 
 
137 
                  MotorPresent[motorread] = 0;
 - 
 
138 
                  motorread++;
 - 
 
139 
                  if(motorread >= MAX_MOTORS) motorread = 0;
 - 
 
140 
                  i2c_stop();
 - 
 
141 
                  twi_state = 0;
 - 
 
142 
                 }
 - 
 
143 
                 else
 - 
 
144 
                 {
 - 
 
145 
                  MotorPresent[motorread] = ('1' - '-') + motorread;
 - 
 
146 
                  I2C_ReceiveByte();
 - 
 
147 
                 }
 - 
 
148 
                MissingMotor = missing_motor;
 - 
 
149 
                missing_motor = 0;
 - 
 
150 
                break;
 - 
 
151 
        case 4: //Read 1st byte and transmit 2nd Byte
 - 
 
152 
                motor_rx[motorread] = TWDR;
 - 
 
153 
                I2C_ReceiveLastByte(); //nack
 - 
 
154 
                break;
 - 
 
155 
        case 5:
 - 
 
156 
                //Read 2nd byte
 - 
 
157 
                motor_rx2[motorread++] = TWDR;
 - 
 
158 
                if(motorread >= MAX_MOTORS) motorread = 0;
 - 
 
159 
                i2c_stop();
 - 
 
160 
                twi_state = 0;
 - 
 
161 
                break;
 - 
 
162 
                       
 109 
        {
163 
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 110 
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
164 
// writing Gyro-Offset
 111 
// writing Gyro-Offset
165 
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 112 
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
166 
        case 8:
 113 
        case 8:
167 
                i2c_write_byte(0x98); // Address of the DAC
 114 
                i2c_write_byte(0x98); // Address of the DAC
168 
                break;
 115 
                break;
169 
        case 9:
 116 
        case 9:
170 
                i2c_write_byte(0x10); // Update Channel A
 117 
                i2c_write_byte(0x10); // Update Channel A
171 
                break;
 118 
                break;
172 
        case 10:
 119 
        case 10:
173 
                i2c_write_byte(AnalogOffsetNick); // Value
 120 
                i2c_write_byte(AnalogOffsetNick); // Value
174 
                break;
 121 
                break;
175 
        case 11:
 122 
        case 11:
176 
                i2c_write_byte(0x80); // Value
 123 
                i2c_write_byte(0x80); // Value
177 
                break;
 124 
                break;
178 
        case 12:
 125 
        case 12:
179 
                i2c_stop();              
 126 
                i2c_stop();
180 
                I2CTimeout = 10;
 127 
                I2CTimeout = 10;
181 
                i2c_start();  
 128 
                i2c_start();
182 
                break;
 129 
                break;
183 
        case 13:
 130 
        case 13:
184 
                i2c_write_byte(0x98); // Address of the DAC
 131 
                i2c_write_byte(0x98); // Address of the DAC
185 
                break;
 132 
                break;
186 
        case 14:
 133 
        case 14:
187 
                i2c_write_byte(0x12); // Update Channel B
 134 
                i2c_write_byte(0x12); // Update Channel B
188 
                break;
 135 
                break;
189 
        case 15:
 136 
        case 15:
190 
                i2c_write_byte(AnalogOffsetRoll); // Value
 137 
                i2c_write_byte(AnalogOffsetRoll); // Value
191 
                break;
 138 
                break;
192 
        case 16:
 139 
        case 16:
193 
                i2c_write_byte(0x80); // Value
 140 
                i2c_write_byte(0x80); // Value
194 
                break;
 141 
                break;
195 
        case 17:
 142 
        case 17:
196 
                i2c_stop();              
 143 
                i2c_stop();
197 
                I2CTimeout = 10;
 144 
                I2CTimeout = 10;
198 
                i2c_start();  
 145 
                i2c_start();
199 
                break;
 146 
                break;
200 
        case 18:
 147 
        case 18:
201 
                i2c_write_byte(0x98); // Address of the DAC
 148 
                i2c_write_byte(0x98); // Address of the DAC
202 
                break;
 149 
                break;
203 
        case 19:
 150 
        case 19:
204 
                i2c_write_byte(0x14); // Update Channel C
 151 
                i2c_write_byte(0x14); // Update Channel C
205 
                break;
 152 
                break;
206 
        case 20:
 153 
        case 20:
207 
                i2c_write_byte(AnalogOffsetGier); // Value
 154 
                i2c_write_byte(AnalogOffsetGier); // Value
208 
                break;
 155 
                break;
209 
        case 21:
 156 
        case 21:
210 
                i2c_write_byte(0x80); // Value
 157 
                i2c_write_byte(0x80); // Value
211 
                break;
 158 
                break;
212 
        case 22:
 159 
        case 22:
213 
                i2c_stop();              
 160 
                i2c_stop();
214 
                I2CTimeout = 10;
 161 
                I2CTimeout = 10;
- 
 
 162 
                twi_state = 0;
- 
 
 163 
                break;
- 
 
 164 
 
- 
 
 165 
                case 0:
- 
 
 166 
                i2c_write_byte(0x82); // servo board
- 
 
 167 
            twi_state = 100;
- 
 
 168 
                break;
- 
 
 169 
        case 100: // servo 1
- 
 
 170 
                        i2c_write_byte(servoValues[0]);
- 
 
 171 
                break;
- 
 
 172 
        case 101: // servo 2
- 
 
 173 
                        i2c_write_byte(servoValues[1]);
- 
 
 174 
                break;
- 
 
 175 
        case 102: // servo 3
- 
 
 176 
                        i2c_write_byte(servoValues[2]);
- 
 
 177 
                break;
- 
 
 178 
        case 103: // servo 4
- 
 
 179 
                        i2c_write_byte(servoValues[3]);
- 
 
 180 
                break;
- 
 
 181 
        case 104: // servo 5
- 
 
 182 
                        i2c_write_byte(servoValues[4]);
- 
 
 183 
                break;
- 
 
 184 
        case 105: // servo 6
- 
 
 185 
                        i2c_write_byte(servoValues[5]);
- 
 
 186 
                        break;
- 
 
 187 
        case 106:
- 
 
 188 
                        i2c_write_byte(calc_crc(&servoValues, 6));
- 
 
 189 
                        break;
- 
 
 190 
                case 107:
- 
 
 191 
            i2c_stop();
- 
 
 192 
            I2CTimeout = 10;
215 
                twi_state = 0;
 193 
            twi_state = 0;
216 
                break;
 194 
                break;
217 
        default: twi_state = 0;
 195 
        default: twi_state = 0;
218 
                break;        
 196 
                break;
219 
                }
 197 
                }