WebSVN - FlightCtrl - Diff - Rev 1775 and 1821 - /branches/dongfang_FC

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

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/twimaster.c - Rev 1775 → 1821