57,6 → 57,7 |
#include "twimaster.h" |
#include "fc.h" |
#include "analog.h" |
#include "uart.h" |
|
volatile uint8_t twi_state = TWI_STATE_MOTOR_TX; |
volatile uint8_t dac_channel = 0; |
349,7 → 350,7 |
case 22: |
I2C_Stop(TWI_STATE_MOTOR_TX); |
I2CTimeout = 10; |
// repeat case 7...10 until all DAC Channels are updated |
// repeat case 18...22 until all DAC Channels are updated |
if(dac_channel < 2) |
{ |
dac_channel ++; // jump to next channel |
356,8 → 357,9 |
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 |
BLFlags |= BLFLAG_TX_COMPLETE; |
} |
break; |
|
377,17 → 379,17 |
{ |
uint8_t i; |
|
if(MotorenEin) return(0); // not when motors are running! |
if(motor > MAX_MOTORS) return (0); // motor does not exist! |
if(MotorenEin || PC_MotortestActive) return(BLCONFIG_ERR_MOTOR_RUNNING); // not when motors are running! |
if(motor > MAX_MOTORS) return (BLCONFIG_ERR_MOTOR_NOT_EXIST); // motor does not exist! |
if(motor) |
{ |
if(!(Motor[motor-1].State & MOTOR_STATE_PRESENT_MASK)) return(0); // motor does not exist! |
if(!(Motor[motor-1].Version & MOTOR_STATE_NEW_PROTOCOL_MASK)) return(0); // not a new BL! |
if(!(Motor[motor-1].State & MOTOR_STATE_PRESENT_MASK)) return(BLCONFIG_ERR_MOTOR_NOT_EXIST); // motor does not exist! |
if(!(Motor[motor-1].Version & MOTOR_STATE_NEW_PROTOCOL_MASK)) return(BLCONFIG_ERR_HW_NOT_COMPATIBLE); // not a new BL! |
} |
// check BL configuration to send |
if(BLConfig.Revision != BLCONFIG_REVISION) return (0); // bad revison |
if(BLConfig.Revision != BLCONFIG_REVISION) return (BLCONFIG_ERR_SW_NOT_COMPATIBLE); // bad revison |
i = RAM_Checksum((uint8_t*)&BLConfig, sizeof(BLConfig_t) - 1); |
if(i != BLConfig.crc) return(0); // bad checksum |
if(i != BLConfig.crc) return(BLCONFIG_ERR_CHECKSUM); // bad checksum |
|
while(!(BLFlags & BLFLAG_TX_COMPLETE)); //wait for complete transfer |
|
400,7 → 402,6 |
{ |
BLConfig_WriteMask = 0x0001<<(motor-1); |
} |
|
for(i = 0; i < MAX_MOTORS; i++) |
{ |
if((0x0001<<i) & BLConfig_WriteMask) |
409,6 → 410,7 |
Motor[i].SetPointLowerBits = 0; |
} |
} |
|
motor_write = 0; |
// needs at least MAX_MOTORS loops of 2 ms (12*2ms = 24ms) |
do |
416,7 → 418,7 |
I2C_Start(TWI_STATE_MOTOR_TX); // start an i2c transmission |
while(!(BLFlags & BLFLAG_TX_COMPLETE)); //wait for complete transfer |
}while(BLConfig_WriteMask); // repeat until the BL config has been sent |
return(1); |
return(BLCONFIG_SUCCESS); |
} |
|
uint8_t I2C_ReadBLConfig(uint8_t motor) |
423,14 → 425,16 |
{ |
uint8_t i; |
|
if(MotorenEin) return(0); // not when motors are running! |
if((motor == 0) || (motor > MAX_MOTORS)) return (0); // motor does not exist! |
if(!(Motor[motor-1].State & MOTOR_STATE_PRESENT_MASK)) return(0); // motor does not exist! |
if(!(Motor[motor-1].Version & MOTOR_STATE_NEW_PROTOCOL_MASK)) return(0); // not a new BL! |
if(MotorenEin || PC_MotortestActive) return(BLCONFIG_ERR_MOTOR_RUNNING); // not when motors are running! |
if((motor == 0) || (motor > MAX_MOTORS)) return (BLCONFIG_ERR_MOTOR_NOT_EXIST); // motor does not exist! |
if(!(Motor[motor-1].State & MOTOR_STATE_PRESENT_MASK)) return(BLCONFIG_ERR_MOTOR_NOT_EXIST); // motor does not exist! |
if(!(Motor[motor-1].Version & MOTOR_STATE_NEW_PROTOCOL_MASK)) return(BLCONFIG_ERR_HW_NOT_COMPATIBLE); // not a new BL! |
|
while(!(BLFlags & BLFLAG_TX_COMPLETE)); //wait for complete transfer |
|
// prepare the bitmask |
BLConfig_ReadMask = 0x0001<<(motor-1); |
|
for(i = 0; i < MAX_MOTORS; i++) |
{ |
if((0x0001<<i) & BLConfig_ReadMask) |
439,6 → 443,7 |
Motor[i].SetPointLowerBits = 0; |
} |
} |
|
motor_read = 0; |
BLConfig.Revision = 0; // bad revision |
BLConfig.crc = 0; // bad checksum |
449,8 → 454,9 |
while(!(BLFlags & BLFLAG_TX_COMPLETE)); //wait for complete transfer |
}while(BLConfig_ReadMask); // repeat until the BL config has been received from all motors |
// validate result |
if(BLConfig.Revision != BLCONFIG_REVISION) return (0); // bad revison |
if(BLConfig.Revision != BLCONFIG_REVISION) return (BLCONFIG_ERR_SW_NOT_COMPATIBLE); // bad revison |
i = RAM_Checksum((uint8_t*)&BLConfig, sizeof(BLConfig_t) - 1); |
if(i != BLConfig.crc) return(0); // bad checksum |
return(1); |
if(i != BLConfig.crc) return(BLCONFIG_ERR_CHECKSUM); // bad checksum |
return(BLCONFIG_SUCCESS); |
} |
|