66,7 → 66,7 |
volatile uint16_t I2CTimeout = 100; |
uint8_t missingMotor = 0; |
|
MotorData_t Motor[MAX_MOTORS]; |
MotorData_t motor[MAX_MOTORS]; |
|
volatile uint8_t DACValues[4]; |
uint8_t DACChannel = 0; |
101,10 → 101,10 |
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; |
motor[i].SetPoint = 0; |
motor[i].Present = 0; |
motor[i].Error = 0; |
motor[i].MaxPWM = 0; |
} |
|
SREG = sreg; |
206,12 → 206,12 |
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 |
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 |
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; |
222,12 → 222,12 |
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[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; |
motor[motor_read].Present = ('1' - '-') + motor_read; |
I2C_ReceiveByte(); //Transmit 1st byte |
} |
missingMotor = missing_motor; |
234,12 → 234,12 |
missing_motor = 0; |
break; |
case 4: //Read 1st byte and transmit 2nd Byte |
Motor[motor_read].Current = TWDR; |
motor[motor_read].Current = TWDR; |
I2C_ReceiveLastByte(); // nack |
break; |
case 5: |
//Read 2nd byte |
Motor[motor_read].MaxPWM = TWDR; |
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); |
289,7 → 289,7 |
printf("\n\rFound BL-Ctrl: "); |
|
for(i = 0; i < MAX_MOTORS; i++) { |
Motor[i].SetPoint = 0; |
motor[i].SetPoint = 0; |
} |
|
I2C_Start(TWI_STATE_MOTOR_TX); |
300,11 → 300,11 |
for(i = 0; i < MAX_MOTORS; i++) { |
I2C_Start(TWI_STATE_MOTOR_TX); |
_delay_ms(2); |
if(Motor[i].Present) printf("%d ",i+1); |
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; |
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; |
} |
} |