71,17 → 71,9 |
|
MotorData_t Motor[MAX_MOTORS]; |
|
BLConfig_t BLConfig = |
{ |
MASK_SET_PWM_SCALING|MASK_SET_CURRENT_LIMIT|MASK_SET_TEMP_LIMIT|MASK_SET_CURRENT_SCALING|MASK_SET_BITCONFIG, |
255, // MaxPWM |
30, // Current Limit in A |
99, // Temperature Limit in °C |
64, // Current Scaling |
0, // BitConfig |
}; |
BLConfig_t BLConfig[MAX_MOTORS]; |
|
uint8_t *pTxBuff = (uint8_t*)&BLConfig; |
uint8_t *pTxBuff = (uint8_t*)&BLConfig[0]; |
|
#define BL_CONF_SYNC_BYTE '#' |
|
123,6 → 115,12 |
Motor[i].State = 0; |
Motor[i].Current = 0; |
Motor[i].MaxPWM = 0; |
BLConfig[i].SetMask = MASK_SET_PWM_SCALING|MASK_SET_CURRENT_LIMIT|MASK_SET_TEMP_LIMIT|MASK_SET_CURRENT_SCALING|MASK_SET_BITCONFIG; |
BLConfig[i].PwmScaling = 255; // MaxPWM |
BLConfig[i].CurrentLimit = 30; // Current Limit in A |
BLConfig[i].TempLimit = 99; // Temperature Limit in °C |
BLConfig[i].CurrentScaling = 64; // Current Scaling |
BLConfig[i].BitConfig = 0; // BitConfig |
} |
|
SREG = sreg; |
235,149 → 233,118 |
{ |
// Master Transmit |
case 0: // TWI_STATE_MOTOR_TX |
// skip motor if not used in mixer |
while((Mixer.Motor[motor_write][MIX_GAS] <= 0) && (motor_write < MAX_MOTORS)) motor_write++; |
if(motor_write >= MAX_MOTORS) // writing finished, read now |
{ |
motor_write = 0; // reset motor write counter for next cycle |
twi_state = TWI_STATE_MOTOR_RX; |
I2C_WriteByte(0x53 + (motor_read * 2) ); // select slave address in rx mode |
} |
else I2C_WriteByte(0x52 + (motor_write * 2) ); // select slave address in tx mode |
break; |
// skip motor if not used in mixer |
while((Mixer.Motor[motor_write][MIX_GAS] <= 0) && (motor_write < MAX_MOTORS)) motor_write++; |
if(motor_write >= MAX_MOTORS) // writing finished, read now |
{ |
motor_write = 0; // reset motor write counter for next cycle |
twi_state = TWI_STATE_MOTOR_RX; |
I2C_WriteByte(0x53 + (motor_read * 2) ); // select slave address in rx mode |
} |
else I2C_WriteByte(0x52 + (motor_write * 2) ); // select slave address in tx mode |
break; |
case 1: // Send Data to Slave |
I2C_WriteByte(Motor[motor_write].SetPoint); // transmit setpoint |
// if old version has been detected |
if(!(Motor[motor_write].Version & MOTOR_STATE_NEW_PROTOCOL_MASK)) |
{ |
twi_state = 4; //jump over sending more data |
} |
else if(!(Motor[motor_write].SetPointLowerBits || (BLFlags & BLFLAG_SEND_CONFIG)) ) |
{ // or LowerBits are zero and no BlConfig should be sent (saves round trip time) |
twi_state = 4; //jump over sending more data |
} |
break; |
I2C_WriteByte(Motor[motor_write].SetPoint); // transmit setpoint |
// if old version has been detected |
if(!(Motor[motor_write].Version & MOTOR_STATE_NEW_PROTOCOL_MASK)) |
{ |
twi_state = 4; //jump over sending more data |
} |
else if(!(Motor[motor_write].SetPointLowerBits || (BLFlags & BLFLAG_SEND_CONFIG)) ) |
{ // or LowerBits are zero and no BlConfig should be sent (saves round trip time) |
twi_state = 4; //jump over sending more data |
} |
break; |
case 2: // lower bits of setpoint (higher resolution) |
I2C_WriteByte((Motor[motor_write].SetPointLowerBits << 1) & 0x07); // send the lower bits of setpoint |
// transmit config only on demand and the motors are not running and only for one motor per round trip |
if( (BLFlags & BLFLAG_SEND_CONFIG) && !(FCFlags & FCFLAG_MOTOR_RUN) && (motor_write == motor_read)) |
{ // prepare sending of configuration |
byte_counter = 0; // reset send byte counter |
crc = 0xAA; // init checksum |
} |
else |
{ // jump to state for end of transmission for that motor |
twi_state = 4; |
} |
break; |
I2C_WriteByte((Motor[motor_write].SetPointLowerBits << 1) & 0x07); // send the lower bits of setpoint |
// transmit config only on demand and the motors are not running and only for one motor per round trip |
if( (BLFlags & BLFLAG_SEND_CONFIG) && !(FCFlags & FCFLAG_MOTOR_RUN) && (motor_write == motor_read)) |
{ // prepare sending of configuration |
byte_counter = 0; // reset send byte counter |
crc = 0xAA; // init checksum |
} |
else |
{ // jump to state for end of transmission for that motor |
twi_state = 4; |
} |
break; |
case 3: // send configuration |
if(!byte_counter) // first byte? |
{ |
I2C_WriteByte(BL_CONF_SYNC_BYTE); |
crc += BL_CONF_SYNC_BYTE; // update crc |
} |
else if(byte_counter >= sizeof(BLConfig_t)+1) // last byte? |
{ // send crc byte at the end |
I2C_WriteByte(crc); |
} |
else // transmit configuration to BLs |
{ |
I2C_WriteByte(pTxBuff[byte_counter-1]); // submit next byte |
crc += pTxBuff[byte_counter-1]; // update crc |
twi_state = 3; // stay in this state |
} |
byte_counter++; // next byte |
break; |
if(!byte_counter) // first byte? |
{ |
I2C_WriteByte(BL_CONF_SYNC_BYTE); |
crc += BL_CONF_SYNC_BYTE; // update crc |
pTxBuff = (uint8_t*)&BLConfig[motor_write]; // select configuration for motor |
} |
else if(byte_counter >= sizeof(BLConfig_t)+1) // last byte? |
{ // send crc byte at the end |
I2C_WriteByte(crc); |
} |
else // transmit configuration to BLs |
{ |
I2C_WriteByte(pTxBuff[byte_counter-1]); // submit next byte |
crc += pTxBuff[byte_counter-1]; // update crc |
twi_state = 3; // stay in this state |
} |
byte_counter++; // next byte |
break; |
case 4: // repeat case 0-4 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].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[motor_write].State++; // 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; |
if(TWSR == TW_MT_DATA_NACK) // Data transmitted, NACK received |
{ |
if(!missing_motor) missing_motor = motor_write + 1; |
if((Motor[motor_write].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[motor_write].State++; // 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 5: // TWI_STATE_MOTOR_RX |
if(TWSR != TW_MR_SLA_ACK) // SLA+R transmitted, if not ACK received |
{ // no response from the addressed slave received |
Motor[motor_read].State &= ~MOTOR_STATE_PRESENT_MASK; // clear present bit |
if(++motor_read >= MAX_MOTORS) |
{ // all motors read |
motor_read = 0; // restart from beginning |
BLFlags &= ~BLFLAG_SEND_CONFIG; |
if(++motor_read_temperature >= MAX_MOTORS) |
{ |
motor_read_temperature = 0; |
BLFlags &= ~BLFLAG_READ_VERSION; |
} |
} |
I2C_Stop(TWI_STATE_MOTOR_TX); |
BLFlags |= BLFLAG_TX_COMPLETE; |
} |
else |
{ // motor successfully addressed |
Motor[motor_read].State |= MOTOR_STATE_PRESENT_MASK; // set present bit |
// read cylclic more data |
if(motor_read == motor_read_temperature) |
if(TWSR != TW_MR_SLA_ACK) // SLA+R transmitted, if not ACK received |
{ // no response from the addressed slave received |
Motor[motor_read].State &= ~MOTOR_STATE_PRESENT_MASK; // clear present bit |
if(++motor_read >= MAX_MOTORS) |
{ // all motors read |
motor_read = 0; // restart from beginning |
BLFlags &= ~BLFLAG_SEND_CONFIG; |
if(++motor_read_temperature >= MAX_MOTORS) |
{ |
read_more = 1; |
I2C_ReceiveByte(); // read next byte |
motor_read_temperature = 0; |
BLFlags &= ~BLFLAG_READ_VERSION; |
} |
else |
{ |
read_more = 0; |
I2C_ReceiveLastByte(); //read last byte |
} |
} |
MissingMotor = missing_motor; |
missing_motor = 0; |
break; |
case 6: // 1st byte recieved |
Motor[motor_read].Current = TWDR; // get Current |
if(read_more) |
I2C_Stop(TWI_STATE_MOTOR_TX); |
BLFlags |= BLFLAG_TX_COMPLETE; |
} |
else |
{ // motor successfully addressed |
Motor[motor_read].State |= MOTOR_STATE_PRESENT_MASK; // set present bit |
// read cylclic more data |
if(motor_read == motor_read_temperature) |
{ |
I2C_ReceiveByte(); // read 2nd byte |
read_more = 1; |
I2C_ReceiveByte(); // read next byte |
} |
else |
{ |
if(++motor_read >= MAX_MOTORS) |
{ |
motor_read = 0; // restart from beginning |
BLFlags &= ~BLFLAG_SEND_CONFIG; |
if(++motor_read_temperature >= MAX_MOTORS) |
{ |
motor_read_temperature = 0; |
BLFlags &= ~BLFLAG_READ_VERSION; |
} |
} |
I2C_Stop(TWI_STATE_MOTOR_TX); |
BLFlags |= BLFLAG_TX_COMPLETE; |
read_more = 0; |
I2C_ReceiveLastByte(); //read last byte |
} |
break; |
case 7: // 2nd byte recieved |
Motor[motor_read].MaxPWM = TWDR; // get MaxPWM |
if(BLFlags & BLFLAG_READ_VERSION) |
{ |
if(TWDR == 250) |
{ // a new BL found |
if(!(FCFlags & FCFLAG_MOTOR_RUN)) Motor[motor_read].Version |= MOTOR_STATE_NEW_PROTOCOL_MASK; |
} |
else |
{ // its not a new BL |
Motor[motor_read].Version = 0; |
} |
} |
I2C_ReceiveLastByte(); // nack |
break; |
|
case 8: // 3rd byte received |
Motor[motor_read].Temperature = TWDR; // get Temperature |
} |
MissingMotor = missing_motor; |
missing_motor = 0; |
break; |
case 6: // 1st byte recieved |
Motor[motor_read].Current = TWDR; // get Current |
if(read_more) |
{ |
I2C_ReceiveByte(); // read 2nd byte |
} |
else |
{ |
if(++motor_read >= MAX_MOTORS) |
{ |
motor_read = 0; // restart reading of first motor |
motor_read = 0; // restart from beginning |
BLFlags &= ~BLFLAG_SEND_CONFIG; |
if(++motor_read_temperature >= MAX_MOTORS) |
{ |
385,59 → 352,92 |
BLFlags &= ~BLFLAG_READ_VERSION; |
} |
} |
I2C_Stop(TWI_STATE_MOTOR_TX); |
BLFlags |= BLFLAG_TX_COMPLETE; |
break; |
I2C_Stop(TWI_STATE_MOTOR_TX); |
BLFlags |= BLFLAG_TX_COMPLETE; |
} |
break; |
case 7: // 2nd byte recieved |
Motor[motor_read].MaxPWM = TWDR; // get MaxPWM |
if(BLFlags & BLFLAG_READ_VERSION) |
{ |
if(TWDR == 250) |
{ // a new BL found |
if(!(FCFlags & FCFLAG_MOTOR_RUN)) Motor[motor_read].Version |= MOTOR_STATE_NEW_PROTOCOL_MASK; |
} |
else |
{ // its not a new BL |
Motor[motor_read].Version = 0; |
} |
} |
I2C_ReceiveLastByte(); // nack |
break; |
|
case 8: // 3rd byte received |
Motor[motor_read].Temperature = TWDR; // get Temperature |
if(++motor_read >= MAX_MOTORS) |
{ |
motor_read = 0; // restart reading of first motor |
BLFlags &= ~BLFLAG_SEND_CONFIG; |
if(++motor_read_temperature >= MAX_MOTORS) |
{ |
motor_read_temperature = 0; |
BLFlags &= ~BLFLAG_READ_VERSION; |
} |
} |
I2C_Stop(TWI_STATE_MOTOR_TX); |
BLFlags |= BLFLAG_TX_COMPLETE; |
break; |
|
// writing Gyro-Offsets |
case 18: |
I2C_WriteByte(0x98); // Address the DAC |
break; |
I2C_WriteByte(0x98); // Address the DAC |
break; |
|
case 19: |
I2C_WriteByte(0x10 + (dac_channel * 2)); // Select DAC Channel (0x10 = A, 0x12 = B, 0x14 = C) |
break; |
I2C_WriteByte(0x10 + (dac_channel * 2)); // Select DAC Channel (0x10 = A, 0x12 = B, 0x14 = C) |
break; |
|
case 20: |
switch(dac_channel) |
{ |
case 0: |
I2C_WriteByte(DacOffsetGyroNick); // 1st byte for Channel A |
break; |
case 1: |
I2C_WriteByte(DacOffsetGyroRoll); // 1st byte for Channel B |
break; |
case 2: |
I2C_WriteByte(DacOffsetGyroYaw ); // 1st byte for Channel C |
break; |
} |
break; |
switch(dac_channel) |
{ |
case 0: |
I2C_WriteByte(DacOffsetGyroNick); // 1st byte for Channel A |
break; |
case 1: |
I2C_WriteByte(DacOffsetGyroRoll); // 1st byte for Channel B |
break; |
case 2: |
I2C_WriteByte(DacOffsetGyroYaw ); // 1st byte for Channel C |
break; |
} |
break; |
|
case 21: |
I2C_WriteByte(0x80); // 2nd byte for all channels is 0x80 |
break; |
I2C_WriteByte(0x80); // 2nd byte for all channels is 0x80 |
break; |
|
case 22: |
I2C_Stop(TWI_STATE_MOTOR_TX); |
I2CTimeout = 10; |
// repeat case 7...10 until all DAC Channels are updated |
if(dac_channel < 2) |
{ |
dac_channel ++; // jump to next channel |
I2C_Start(TWI_STATE_GYRO_OFFSET_TX); // start transmission for next channel |
} |
else |
{ // data to last motor send |
dac_channel = 0; // reset dac channel counter |
} |
break; |
I2C_Stop(TWI_STATE_MOTOR_TX); |
I2CTimeout = 10; |
// repeat case 7...10 until all DAC Channels are updated |
if(dac_channel < 2) |
{ |
dac_channel ++; // jump to next channel |
I2C_Start(TWI_STATE_GYRO_OFFSET_TX); // start transmission for next channel |
} |
else |
{ // data to last motor send |
dac_channel = 0; // reset dac channel counter |
} |
break; |
|
default: |
I2C_Stop(TWI_STATE_MOTOR_TX); |
BLFlags &= ~BLFLAG_TX_COMPLETE; |
I2CTimeout = 10; |
motor_write = 0; |
motor_read = 0; |
I2C_Stop(TWI_STATE_MOTOR_TX); |
BLFlags &= ~BLFLAG_TX_COMPLETE; |
I2CTimeout = 10; |
motor_write = 0; |
motor_read = 0; |
break; |
} |
} |
|
444,9 → 444,9 |
|
void I2C_SendBLConfig(void) |
{ |
uint8_t i; |
unsigned char i; |
while(!(BLFlags & BLFLAG_TX_COMPLETE)); //wait for complete transfer |
BLFlags |= BLFLAG_SEND_CONFIG; // enable sending of BL config |
while(!(BLFlags & BLFLAG_TX_COMPLETE)); //wait for complete transfer |
for(i = 0; i < MAX_MOTORS; i++) |
{ |
Motor[i].SetPoint = 0; |