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



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];
 

volatile uint8_t DACValues[4];

  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;

  }
 

      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;

  uint8_t i;

 

  printf("\n\rFound BL-Ctrl: ");
 
  for(i = 0; i < MAX_MOTORS; i++) {

    motor[i].SetPoint = 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);
  }

Rev 1612	Rev 1775
Line 64...	Line 64...
64	volatile uint8_t motor_write = 0;	64	volatile uint8_t motor_write = 0;
65	volatile uint8_t motor_read = 0;	65	volatile uint8_t motor_read = 0;
66	volatile uint16_t I2CTimeout = 100;	66	volatile uint16_t I2CTimeout = 100;
67	uint8_t missingMotor = 0;	67	uint8_t missingMotor = 0;
Line 68...	Line 68...
68		68
Line 69...	Line 69...
69	MotorData_t Motor[MAX_MOTORS];	69	MotorData_t motor[MAX_MOTORS];
70		70
Line 71...	Line 71...
71	volatile uint8_t DACValues[4];	71	volatile uint8_t DACValues[4];
Line 99...	Line 99...
99	twi_state = TWI_STATE_MOTOR_TX;	99	twi_state = TWI_STATE_MOTOR_TX;
100	motor_write = 0;	100	motor_write = 0;
101	motor_read = 0;	101	motor_read = 0;
Line 102...	Line 102...
102		102
103	for(i=0; i < MAX_MOTORS; i++) {	103	for(i=0; i < MAX_MOTORS; i++) {
104	Motor[i].SetPoint = 0;	104	motor[i].SetPoint = 0;
105	Motor[i].Present = 0;	105	motor[i].Present = 0;
106	Motor[i].Error = 0;	106	motor[i].Error = 0;
107	Motor[i].MaxPWM = 0;	107	motor[i].MaxPWM = 0;
Line 108...	Line 108...
108	}	108	}
109		109
Line 204...	Line 204...
204	I2C_WriteByte(0x53 + (motor_read * 2)); // select slave adress in rx mode	204	I2C_WriteByte(0x53 + (motor_read * 2)); // select slave adress in rx mode
205	}	205	}
206	else I2C_WriteByte(0x52 + (motor_write * 2)); // select slave adress in tx mode	206	else I2C_WriteByte(0x52 + (motor_write * 2)); // select slave adress in tx mode
207	break;	207	break;
208	case 1: // Send Data to Slave	208	case 1: // Send Data to Slave
209	I2C_WriteByte(Motor[motor_write].SetPoint); // transmit rotation rate setpoint	209	I2C_WriteByte(motor[motor_write].SetPoint); // transmit rotation rate setpoint
210	break;	210	break;
211	case 2: // repeat case 0+1 for all motors	211	case 2: // repeat case 0+1 for all motors
212	if(TWSR == TW_MT_DATA_NACK) { // Data transmitted, NACK received	212	if(TWSR == TW_MT_DATA_NACK) { // Data transmitted, NACK received
213	if(!missing_motor) missing_motor = motor_write + 1;	213	if(!missing_motor) missing_motor = motor_write + 1;
214	if(++Motor[motor_write].Error == 0) Motor[motor_write].Error = 255; // increment error counter and handle overflow	214	if(++motor[motor_write].Error == 0) motor[motor_write].Error = 255; // increment error counter and handle overflow
215	}	215	}
216	I2C_Stop(TWI_STATE_MOTOR_TX);	216	I2C_Stop(TWI_STATE_MOTOR_TX);
217	I2CTimeout = 10;	217	I2CTimeout = 10;
218	motor_write++; // next motor	218	motor_write++; // next motor
219	I2C_Start(TWI_STATE_MOTOR_TX); // Repeated start -> switch slave or switch Master Transmit -> Master Receive	219	I2C_Start(TWI_STATE_MOTOR_TX); // Repeated start -> switch slave or switch Master Transmit -> Master Receive
220	break;	220	break;
221	// Master Receive Data	221	// Master Receive Data
222	case 3:	222	case 3:
223	if(TWSR != TW_MR_SLA_ACK) { // SLA+R transmitted, if not ACK received	223	if(TWSR != TW_MR_SLA_ACK) { // SLA+R transmitted, if not ACK received
224	// no response from the addressed slave received	224	// no response from the addressed slave received
225	Motor[motor_read].Present = 0;	225	motor[motor_read].Present = 0;
226	motor_read++; // next motor	226	motor_read++; // next motor
227	if(motor_read >= MAX_MOTORS) motor_read = 0; // restart reading of first motor if we have reached the last one	227	if(motor_read >= MAX_MOTORS) motor_read = 0; // restart reading of first motor if we have reached the last one
228	I2C_Stop(TWI_STATE_MOTOR_TX);	228	I2C_Stop(TWI_STATE_MOTOR_TX);
229	} else {	229	} else {
230	Motor[motor_read].Present = ('1' - '-') + motor_read;	230	motor[motor_read].Present = ('1' - '-') + motor_read;
231	I2C_ReceiveByte(); //Transmit 1st byte	231	I2C_ReceiveByte(); //Transmit 1st byte
232	}	232	}
233	missingMotor = missing_motor;	233	missingMotor = missing_motor;
234	missing_motor = 0;	234	missing_motor = 0;
235	break;	235	break;
236	case 4: //Read 1st byte and transmit 2nd Byte	236	case 4: //Read 1st byte and transmit 2nd Byte
237	Motor[motor_read].Current = TWDR;	237	motor[motor_read].Current = TWDR;
238	I2C_ReceiveLastByte(); // nack	238	I2C_ReceiveLastByte(); // nack
239	break;	239	break;
240	case 5:	240	case 5:
241	//Read 2nd byte	241	//Read 2nd byte
242	Motor[motor_read].MaxPWM = TWDR;	242	motor[motor_read].MaxPWM = TWDR;
243	motor_read++; // next motor	243	motor_read++; // next motor
244	if(motor_read >= MAX_MOTORS) motor_read = 0; // restart reading of first motor if we have reached the last one	244	if(motor_read >= MAX_MOTORS) motor_read = 0; // restart reading of first motor if we have reached the last one
245	I2C_Stop(TWI_STATE_MOTOR_TX);	245	I2C_Stop(TWI_STATE_MOTOR_TX);
246	break;	246	break;
Line 287...	Line 287...
287	uint8_t i;	287	uint8_t i;
Line 288...	Line 288...
288		288
Line 289...	Line 289...
289	printf("\n\rFound BL-Ctrl: ");	289	printf("\n\rFound BL-Ctrl: ");
290		290
291	for(i = 0; i < MAX_MOTORS; i++) {	291	for(i = 0; i < MAX_MOTORS; i++) {
Line 292...	Line 292...
292	Motor[i].SetPoint = 0;	292	motor[i].SetPoint = 0;
293	}	293	}
Line 294...	Line 294...
294		294
Line 295...	Line 295...
295	I2C_Start(TWI_STATE_MOTOR_TX);	295	I2C_Start(TWI_STATE_MOTOR_TX);
296	_delay_ms(2);	296	_delay_ms(2);
297		297
298	motor_read = 0; // read the first I2C-Data	298	motor_read = 0; // read the first I2C-Data
299		299
Line 300...	Line 300...
300	for(i = 0; i < MAX_MOTORS; i++) {	300	for(i = 0; i < MAX_MOTORS; i++) {
301	I2C_Start(TWI_STATE_MOTOR_TX);	301	I2C_Start(TWI_STATE_MOTOR_TX);
302	_delay_ms(2);	302	_delay_ms(2);
303	if(Motor[i].Present) printf("%d ",i+1);	303	if(motor[i].Present) printf("%d ",i+1);
304	}	304	}

Subversion Repositories FlightCtrl

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