122,8 → 122,10 |
Motor[i].SetPoint = 0; |
Motor[i].SetPointLowerBits = 0; |
Motor[i].State = 0; |
Motor[i].ReadMode = BL_READMODE_STATUS; |
Motor[i].Current = 0; |
Motor[i].MaxPWM = 0; |
Motor[i].Temperature = 0; |
} |
|
SREG = sreg; |
155,10 → 157,6 |
static uint8_t *pBuff = 0; |
static uint8_t BuffLen = 0; |
|
#define BL_READ_STATUS 0 |
#define BL_READ_CONFIG 16 |
static uint8_t BLReadMode = BL_READ_STATUS; |
|
switch (twi_state++) |
{ |
// Master Transmit |
189,16 → 187,16 |
} |
break; |
case 2: // lower bits of setpoint (higher resolution) |
if ((motor_write == motor_read) && ((0x0001<<motor_read) & BLConfig_ReadMask)) |
if ((0x0001<<motor_write) & BLConfig_ReadMask) |
{ |
BLReadMode = BL_READ_CONFIG; // configuration request |
Motor[motor_write].ReadMode = BL_READMODE_CONFIG; // configuration request |
} |
else |
{ |
BLReadMode = BL_READ_STATUS; // normal status request |
Motor[motor_write].ReadMode = BL_READMODE_STATUS; // normal status request |
} |
// send read mode and the lower bits of setpoint |
I2C_WriteByte((BLReadMode<<3)|(Motor[motor_write].SetPointLowerBits & 0x07)); |
I2C_WriteByte((Motor[motor_write].ReadMode<<3)|(Motor[motor_write].SetPointLowerBits & 0x07)); |
// configuration tranmission request? |
if((0x0001<<motor_write) & BLConfig_WriteMask) |
{ // redirect tx pointer to configuration data |
251,14 → 249,14 |
if(Motor[motor_read].Version & MOTOR_STATE_NEW_PROTOCOL_MASK) |
{ |
// new BL found |
switch(BLReadMode) |
switch(Motor[motor_read].ReadMode) |
{ |
case BL_READ_CONFIG: |
case BL_READMODE_CONFIG: |
pBuff = (uint8_t*)&BLConfig; |
BuffLen = sizeof(BLConfig_t); |
break; |
|
case BL_READ_STATUS: |
case BL_READMODE_STATUS: |
pBuff = (uint8_t*)&(Motor[motor_read].Current); |
if(motor_read == motor_read_temperature) BuffLen = 3; // read Current, MaxPwm & Temp |
else BuffLen = 1;// read Current only |