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