17,12 → 17,12 |
//############################################################################ |
{ |
TWSR = 0; |
TWBR = ((SYSCLK/SCL_CLOCK)-16)/2; |
TWBR = ((SYSCLK/SCL_CLOCK)-16)/2; |
} |
|
//############################################################################ |
//Start I2C |
void i2c_start(void) |
void i2c_start(void) |
//############################################################################ |
{ |
TWCR = (1<<TWSTA) | (1<<TWEN) | (1<<TWINT) | (1<<TWIE); |
38,7 → 38,7 |
void i2c_reset(void) |
//############################################################################ |
{ |
i2c_stop(); |
i2c_stop(); |
twi_state = 0; |
motor = TWDR; |
motor = 0; |
56,7 → 56,7 |
//############################################################################ |
void i2c_write_byte(char byte) |
//############################################################################ |
{ |
{ |
TWSR = 0x00; |
TWDR = byte; |
TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE); |
91,8 → 91,15 |
TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE); |
} |
|
uint8_t calc_crc(uint8_t *buffer, uint8_t l) { |
uint8_t crc = 0xff; |
uint8_t i; |
for(i = 0; i < l; i++) { |
crc = crc ^ buffer[i]; |
} |
return crc; |
} |
|
|
//############################################################################ |
SIGNAL (TWI_vect) |
//############################################################################ |
101,121 → 108,92 |
switch(twi_state++) |
{ |
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Writing the Data |
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
case 0: |
while(Mixer.Motor[motor][0] <= 0 && motor < MAX_MOTORS) motor++; // skip if not used |
if(motor == MAX_MOTORS) // writing finished -> now read |
{ |
motor = 0; |
twi_state = 3; |
i2c_write_byte(0x53+(motorread*2)); |
} |
else i2c_write_byte(0x52+(motor*2)); |
break; |
case 1: |
i2c_write_byte(Motor[motor++]); |
break; |
case 2: |
if(TWSR == 0x30) |
{ |
if(!missing_motor) missing_motor = motor; |
if(++MotorError[motor-1] == 0) MotorError[motor-1] = 255; |
} |
i2c_stop(); |
I2CTimeout = 10; |
twi_state = 0; |
i2c_start(); |
break; |
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Reading Data |
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
case 3: |
//Transmit 1st byte for reading |
if(TWSR != 0x40) // Error? |
{ |
MotorPresent[motorread] = 0; |
motorread++; |
if(motorread >= MAX_MOTORS) motorread = 0; |
i2c_stop(); |
twi_state = 0; |
} |
else |
{ |
MotorPresent[motorread] = ('1' - '-') + motorread; |
I2C_ReceiveByte(); |
} |
MissingMotor = missing_motor; |
missing_motor = 0; |
break; |
case 4: //Read 1st byte and transmit 2nd Byte |
motor_rx[motorread] = TWDR; |
I2C_ReceiveLastByte(); //nack |
break; |
case 5: |
//Read 2nd byte |
motor_rx2[motorread++] = TWDR; |
if(motorread >= MAX_MOTORS) motorread = 0; |
i2c_stop(); |
twi_state = 0; |
break; |
|
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// writing Gyro-Offset |
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
case 8: |
case 8: |
i2c_write_byte(0x98); // Address of the DAC |
break; |
case 9: |
case 9: |
i2c_write_byte(0x10); // Update Channel A |
break; |
case 10: |
case 10: |
i2c_write_byte(AnalogOffsetNick); // Value |
break; |
case 11: |
case 11: |
i2c_write_byte(0x80); // Value |
break; |
case 12: |
i2c_stop(); |
case 12: |
i2c_stop(); |
I2CTimeout = 10; |
i2c_start(); |
i2c_start(); |
break; |
case 13: |
case 13: |
i2c_write_byte(0x98); // Address of the DAC |
break; |
case 14: |
case 14: |
i2c_write_byte(0x12); // Update Channel B |
break; |
case 15: |
case 15: |
i2c_write_byte(AnalogOffsetRoll); // Value |
break; |
case 16: |
case 16: |
i2c_write_byte(0x80); // Value |
break; |
case 17: |
i2c_stop(); |
case 17: |
i2c_stop(); |
I2CTimeout = 10; |
i2c_start(); |
i2c_start(); |
break; |
case 18: |
case 18: |
i2c_write_byte(0x98); // Address of the DAC |
break; |
case 19: |
case 19: |
i2c_write_byte(0x14); // Update Channel C |
break; |
case 20: |
case 20: |
i2c_write_byte(AnalogOffsetGier); // Value |
break; |
case 21: |
case 21: |
i2c_write_byte(0x80); // Value |
break; |
case 22: |
i2c_stop(); |
case 22: |
i2c_stop(); |
I2CTimeout = 10; |
twi_state = 0; |
break; |
|
case 0: |
i2c_write_byte(0x82); // servo board |
twi_state = 100; |
break; |
case 100: // servo 1 |
i2c_write_byte(servoValues[0]); |
break; |
case 101: // servo 2 |
i2c_write_byte(servoValues[1]); |
break; |
case 102: // servo 3 |
i2c_write_byte(servoValues[2]); |
break; |
case 103: // servo 4 |
i2c_write_byte(servoValues[3]); |
break; |
case 104: // servo 5 |
i2c_write_byte(servoValues[4]); |
break; |
case 105: // servo 6 |
i2c_write_byte(servoValues[5]); |
break; |
case 106: |
i2c_write_byte(calc_crc(&servoValues, 6)); |
break; |
case 107: |
i2c_stop(); |
I2CTimeout = 10; |
twi_state = 0; |
break; |
default: twi_state = 0; |
break; |
break; |
} |
TWCR |= 0x80; |
} |