59,12 → 59,12 |
#include "configuration.h" |
#include "printf_P.h" |
|
volatile uint8_t twi_state = TWI_STATE_MOTOR_TX; |
volatile uint8_t dac_channel = 0; |
volatile uint8_t motor_write = 0; |
volatile uint8_t motor_read = 0; |
volatile uint16_t I2CTimeout = 100; |
uint8_t missingMotor = 0; |
volatile uint8_t twi_state = TWI_STATE_MOTOR_TX; |
volatile uint8_t dac_channel = 0; |
volatile uint8_t motor_write = 0; |
volatile uint8_t motor_read = 0; |
volatile uint16_t I2CTimeout = 100; |
uint8_t missingMotor = 0; |
|
MotorData_t motor[MAX_MOTORS]; |
|
78,36 → 78,36 |
* Initialize I2C (TWI) |
**************************************************/ |
void I2C_init(void) { |
uint8_t i; |
uint8_t sreg = SREG; |
cli(); |
|
// SDA is INPUT |
DDRC &= ~(1<<DDC1); |
// SCL is output |
DDRC |= (1<<DDC0); |
// pull up SDA |
PORTC |= (1<<PORTC0)|(1<<PORTC1); |
|
// TWI Status Register |
// prescaler 1 (TWPS1 = 0, TWPS0 = 0) |
TWSR &= ~((1<<TWPS1)|(1<<TWPS0)); |
|
// set TWI Bit Rate Register |
TWBR = ((SYSCLK/SCL_CLOCK)-16)/2; |
|
twi_state = TWI_STATE_MOTOR_TX; |
motor_write = 0; |
motor_read = 0; |
|
for(i=0; i < MAX_MOTORS; i++) { |
motor[i].SetPoint = 0; |
motor[i].Present = 0; |
motor[i].Error = 0; |
motor[i].MaxPWM = 0; |
} |
|
SREG = sreg; |
uint8_t i; |
uint8_t sreg = SREG; |
cli(); |
|
// SDA is INPUT |
DDRC &= ~(1 << DDC1); |
// SCL is output |
DDRC |= (1 << DDC0); |
// pull up SDA |
PORTC |= (1 << PORTC0) | (1 << PORTC1); |
|
// TWI Status Register |
// prescaler 1 (TWPS1 = 0, TWPS0 = 0) |
TWSR &= ~((1 << TWPS1) | (1 << TWPS0)); |
|
// set TWI Bit Rate Register |
TWBR = ((SYSCLK / SCL_CLOCK) - 16) / 2; |
|
twi_state = TWI_STATE_MOTOR_TX; |
motor_write = 0; |
motor_read = 0; |
|
for (i = 0; i < MAX_MOTORS; i++) { |
motor[i].SetPoint = 0; |
motor[i].Present = 0; |
motor[i].Error = 0; |
motor[i].MaxPWM = 0; |
} |
|
SREG = sreg; |
} |
|
/**************************************** |
114,16 → 114,16 |
* Start I2C |
****************************************/ |
void I2C_Start(uint8_t start_state) { |
twi_state = start_state; |
// TWI Control Register |
// clear TWI interrupt flag (TWINT=1) |
// disable TWI Acknowledge Bit (TWEA = 0) |
// enable TWI START Condition Bit (TWSTA = 1), MASTER |
// disable TWI STOP Condition Bit (TWSTO = 0) |
// disable TWI Write Collision Flag (TWWC = 0) |
// enable i2c (TWEN = 1) |
// enable TWI Interrupt (TWIE = 1) |
TWCR = (1<<TWINT) | (1<<TWSTA) | (1<<TWEN) | (1<<TWIE); |
twi_state = start_state; |
// TWI Control Register |
// clear TWI interrupt flag (TWINT=1) |
// disable TWI Acknowledge Bit (TWEA = 0) |
// enable TWI START Condition Bit (TWSTA = 1), MASTER |
// disable TWI STOP Condition Bit (TWSTO = 0) |
// disable TWI Write Collision Flag (TWWC = 0) |
// enable i2c (TWEN = 1) |
// enable TWI Interrupt (TWIE = 1) |
TWCR = (1 << TWINT) | (1 << TWSTA) | (1 << TWEN) | (1 << TWIE); |
} |
|
/**************************************** |
130,16 → 130,16 |
* Stop I2C |
****************************************/ |
void I2C_Stop(uint8_t start_state) { |
twi_state = start_state; |
// TWI Control Register |
// clear TWI interrupt flag (TWINT=1) |
// disable TWI Acknowledge Bit (TWEA = 0) |
// diable TWI START Condition Bit (TWSTA = 1), no MASTER |
// enable TWI STOP Condition Bit (TWSTO = 1) |
// disable TWI Write Collision Flag (TWWC = 0) |
// enable i2c (TWEN = 1) |
// disable TWI Interrupt (TWIE = 0) |
TWCR = (1<<TWINT) | (1<<TWSTO) | (1<<TWEN); |
twi_state = start_state; |
// TWI Control Register |
// clear TWI interrupt flag (TWINT=1) |
// disable TWI Acknowledge Bit (TWEA = 0) |
// diable TWI START Condition Bit (TWSTA = 1), no MASTER |
// enable TWI STOP Condition Bit (TWSTO = 1) |
// disable TWI Write Collision Flag (TWWC = 0) |
// enable i2c (TWEN = 1) |
// disable TWI Interrupt (TWIE = 0) |
TWCR = (1 << TWINT) | (1 << TWSTO) | (1 << TWEN); |
} |
|
/**************************************** |
146,12 → 146,12 |
* Write to I2C |
****************************************/ |
void I2C_WriteByte(int8_t byte) { |
// move byte to send into TWI Data Register |
TWDR = byte; |
// clear interrupt flag (TWINT = 1) |
// enable i2c bus (TWEN = 1) |
// enable interrupt (TWIE = 1) |
TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE); |
// move byte to send into TWI Data Register |
TWDR = byte; |
// clear interrupt flag (TWINT = 1) |
// enable i2c bus (TWEN = 1) |
// enable interrupt (TWIE = 1) |
TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWIE); |
} |
|
/**************************************** |
158,14 → 158,14 |
* Receive byte and send ACK |
****************************************/ |
void I2C_ReceiveByte(void) { |
TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE) | (1<<TWEA); |
TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWIE) | (1 << TWEA); |
} |
|
/**************************************** |
* I2C receive last byte and send no ACK |
****************************************/ |
void I2C_ReceiveLastByte(void){ |
TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE); |
void I2C_ReceiveLastByte(void) { |
TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWIE); |
} |
|
/**************************************** |
172,139 → 172,148 |
* Reset I2C |
****************************************/ |
void I2C_Reset(void) { |
// stop i2c bus |
I2C_Stop(TWI_STATE_MOTOR_TX); |
twi_state = 0; |
motor_write = TWDR; |
motor_write = 0; |
motor_read = 0; |
TWCR = (1<<TWINT); // reset to original state incl. interrupt flag reset |
TWAMR = 0; |
TWAR = 0; |
TWDR = 0; |
TWSR = 0; |
TWBR = 0; |
I2C_init(); |
I2C_Start(TWI_STATE_MOTOR_TX); |
// stop i2c bus |
I2C_Stop(TWI_STATE_MOTOR_TX); |
twi_state = 0; |
motor_write = TWDR; |
motor_write = 0; |
motor_read = 0; |
TWCR = (1 << TWINT); // reset to original state incl. interrupt flag reset |
TWAMR = 0; |
TWAR = 0; |
TWDR = 0; |
TWSR = 0; |
TWBR = 0; |
I2C_init(); |
I2C_Start(TWI_STATE_MOTOR_TX); |
} |
|
/**************************************** |
* I2C ISR |
****************************************/ |
ISR (TWI_vect) { |
static uint8_t missing_motor = 0; |
switch (twi_state++) { // First i2c_start from SendMotorData() |
// Master Transmit |
case 0: // TWI_STATE_MOTOR_TX |
// skip motor if not used in mixer |
while((Mixer.Motor[motor_write][MIX_THROTTLE] <= 0) && (motor_write < MAX_MOTORS)) motor_write++; |
if(motor_write >= MAX_MOTORS) { // writing finished, read now |
motor_write = 0; |
twi_state = TWI_STATE_MOTOR_RX; |
I2C_WriteByte(0x53 + (motor_read * 2)); // select slave adress in rx mode |
} |
else I2C_WriteByte(0x52 + (motor_write * 2)); // select slave adress in tx mode |
break; |
case 1: // Send Data to Slave |
I2C_WriteByte(motor[motor_write].SetPoint); // transmit rotation rate setpoint |
break; |
case 2: // repeat case 0+1 for all motors |
if(TWSR == TW_MT_DATA_NACK) { // Data transmitted, NACK received |
if(!missing_motor) missing_motor = motor_write + 1; |
if(++motor[motor_write].Error == 0) motor[motor_write].Error = 255; // increment error counter and handle overflow |
} |
I2C_Stop(TWI_STATE_MOTOR_TX); |
I2CTimeout = 10; |
motor_write++; // next motor |
I2C_Start(TWI_STATE_MOTOR_TX); // Repeated start -> switch slave or switch Master Transmit -> Master Receive |
break; |
// Master Receive Data |
case 3: |
if(TWSR != TW_MR_SLA_ACK) { // SLA+R transmitted, if not ACK received |
// no response from the addressed slave received |
motor[motor_read].Present = 0; |
motor_read++; // next motor |
if(motor_read >= MAX_MOTORS) motor_read = 0; // restart reading of first motor if we have reached the last one |
I2C_Stop(TWI_STATE_MOTOR_TX); |
} else { |
motor[motor_read].Present = ('1' - '-') + motor_read; |
I2C_ReceiveByte(); //Transmit 1st byte |
} |
missingMotor = missing_motor; |
missing_motor = 0; |
break; |
case 4: //Read 1st byte and transmit 2nd Byte |
motor[motor_read].Current = TWDR; |
I2C_ReceiveLastByte(); // nack |
break; |
case 5: |
//Read 2nd byte |
motor[motor_read].MaxPWM = TWDR; |
motor_read++; // next motor |
if(motor_read >= MAX_MOTORS) motor_read = 0; // restart reading of first motor if we have reached the last one |
I2C_Stop(TWI_STATE_MOTOR_TX); |
break; |
|
// Writing ADC values. |
case 7: |
I2C_WriteByte(0x98); // Address the DAC |
break; |
|
case 8: |
I2C_WriteByte(0x10 + (DACChannel << 1)); // Select DAC Channel (0x10 = A, 0x12 = B, 0x14 = C) |
break; |
|
case 9: |
I2C_WriteByte(DACValues[DACChannel]); |
break; |
|
case 10: |
I2C_WriteByte(0x80); // 2nd byte for all channels is 0x80 |
break; |
|
case 11: |
I2C_Stop(TWI_STATE_MOTOR_TX); |
I2CTimeout = 10; |
// repeat case 7...10 until all DAC Channels are updated |
if(DACChannel < 2) { |
DACChannel ++; // jump to next channel |
I2C_Start(TWI_STATE_GYRO_OFFSET_TX); // start transmission for next channel |
} else { |
DACChannel = 0; // reset dac channel counter |
} |
break; |
|
default: |
I2C_Stop(TWI_STATE_MOTOR_TX); |
I2CTimeout = 10; |
motor_write = 0; |
motor_read = 0; |
} |
ISR (TWI_vect) |
{ |
static uint8_t missing_motor = 0; |
switch (twi_state++) { // First i2c_start from SendMotorData() |
// Master Transmit |
case 0: // TWI_STATE_MOTOR_TX |
// skip motor if not used in mixer |
while ((Mixer.Motor[motor_write][MIX_THROTTLE] <= 0) && (motor_write |
< MAX_MOTORS)) |
motor_write++; |
if (motor_write >= MAX_MOTORS) { // writing finished, read now |
motor_write = 0; |
twi_state = TWI_STATE_MOTOR_RX; |
I2C_WriteByte(0x53 + (motor_read * 2)); // select slave adress in rx mode |
} else |
I2C_WriteByte(0x52 + (motor_write * 2)); // select slave adress in tx mode |
break; |
case 1: // Send Data to Slave |
I2C_WriteByte(motor[motor_write].SetPoint); // transmit rotation rate setpoint |
break; |
case 2: // repeat case 0+1 for all motors |
if (TWSR == TW_MT_DATA_NACK) { // Data transmitted, NACK received |
if (!missing_motor) |
missing_motor = motor_write + 1; |
if (++motor[motor_write].Error == 0) |
motor[motor_write].Error = 255; // increment error counter and handle overflow |
} |
I2C_Stop(TWI_STATE_MOTOR_TX); |
I2CTimeout = 10; |
motor_write++; // next motor |
I2C_Start(TWI_STATE_MOTOR_TX); // Repeated start -> switch slave or switch Master Transmit -> Master Receive |
break; |
// Master Receive Data |
case 3: |
if (TWSR != TW_MR_SLA_ACK) { // SLA+R transmitted, if not ACK received |
// no response from the addressed slave received |
motor[motor_read].Present = 0; |
motor_read++; // next motor |
if (motor_read >= MAX_MOTORS) |
motor_read = 0; // restart reading of first motor if we have reached the last one |
I2C_Stop(TWI_STATE_MOTOR_TX); |
} else { |
motor[motor_read].Present = ('1' - '-') + motor_read; |
I2C_ReceiveByte(); //Transmit 1st byte |
} |
missingMotor = missing_motor; |
missing_motor = 0; |
break; |
case 4: //Read 1st byte and transmit 2nd Byte |
motor[motor_read].Current = TWDR; |
I2C_ReceiveLastByte(); // nack |
break; |
case 5: |
//Read 2nd byte |
motor[motor_read].MaxPWM = TWDR; |
motor_read++; // next motor |
if (motor_read >= MAX_MOTORS) |
motor_read = 0; // restart reading of first motor if we have reached the last one |
I2C_Stop(TWI_STATE_MOTOR_TX); |
break; |
|
// Writing ADC values. |
case 7: |
I2C_WriteByte(0x98); // Address the DAC |
break; |
|
case 8: |
I2C_WriteByte(0x10 + (DACChannel << 1)); // Select DAC Channel (0x10 = A, 0x12 = B, 0x14 = C) |
break; |
|
case 9: |
I2C_WriteByte(DACValues[DACChannel]); |
break; |
|
case 10: |
I2C_WriteByte(0x80); // 2nd byte for all channels is 0x80 |
break; |
|
case 11: |
I2C_Stop(TWI_STATE_MOTOR_TX); |
I2CTimeout = 10; |
// repeat case 7...10 until all DAC Channels are updated |
if (DACChannel < 2) { |
DACChannel++; // jump to next channel |
I2C_Start(TWI_STATE_GYRO_OFFSET_TX); // start transmission for next channel |
} else { |
DACChannel = 0; // reset dac channel counter |
} |
break; |
|
default: |
I2C_Stop(TWI_STATE_MOTOR_TX); |
I2CTimeout = 10; |
motor_write = 0; |
motor_read = 0; |
} |
} |
|
extern void twi_diagnostics(void) { |
// Check connected BL-Ctrls |
uint8_t i; |
|
printf("\n\rFound BL-Ctrl: "); |
|
for(i = 0; i < MAX_MOTORS; i++) { |
motor[i].SetPoint = 0; |
} |
// Check connected BL-Ctrls |
uint8_t i; |
|
I2C_Start(TWI_STATE_MOTOR_TX); |
_delay_ms(2); |
|
motor_read = 0; // read the first I2C-Data |
printf("\n\rFound BL-Ctrl: "); |
|
for(i = 0; i < MAX_MOTORS; i++) { |
I2C_Start(TWI_STATE_MOTOR_TX); |
_delay_ms(2); |
if(motor[i].Present) printf("%d ",i+1); |
} |
for (i = 0; i < MAX_MOTORS; i++) { |
motor[i].SetPoint = 0; |
} |
|
for(i = 0; i < MAX_MOTORS; i++) { |
if(!motor[i].Present && Mixer.Motor[i][MIX_THROTTLE] > 0) printf("\n\r\n\r!! MISSING BL-CTRL: %d !!",i + 1); |
motor[i].Error = 0; |
} |
I2C_Start(TWI_STATE_MOTOR_TX); |
_delay_ms(2); |
|
motor_read = 0; // read the first I2C-Data |
|
for (i = 0; i < MAX_MOTORS; i++) { |
I2C_Start(TWI_STATE_MOTOR_TX); |
_delay_ms(2); |
if (motor[i].Present) |
printf("%d ",i+1); |
} |
|
for (i = 0; i < MAX_MOTORS; i++) { |
if (!motor[i].Present && Mixer.Motor[i][MIX_THROTTLE] > 0) |
printf("\n\r\n\r!! MISSING BL-CTRL: %d !!",i + 1); |
motor[i].Error = 0; |
} |
} |