Subversion Repositories FlightCtrl

Rev

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

Rev 1400 Rev 1452
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:
Line 212... Line 159...
212 
        case 22:
 159 
        case 22:
213 
                i2c_stop();              
 160 
                i2c_stop();
214 
                I2CTimeout = 10;
 161 
                I2CTimeout = 10;
215 
                twi_state = 0;
 162 
                twi_state = 0;
216 
                break;
 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;
- 
 
 193 
            twi_state = 0;
- 
 
 194 
                break;
217 
        default: twi_state = 0;
 195 
        default: twi_state = 0;
218 
                break;        
 196 
                break;
219 
                }
 197 
                }
220 
 TWCR |= 0x80;
 198 
 TWCR |= 0x80;
221 
}
 199 
}