Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 1820 → Rev 1821

/branches/dongfang_FC_rewrite/twimaster.c
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;
}
}