SmartServoClass::getPosition returns wrong value

[metanav]

The SmartServoClass::getPosition method always returns 5160.88 when readWordCmd returns -1. The positionToAngle method converts -1 to 65535. Consequently, this sets limited_runtime_sec to a large value of around 5000/angular_velocity seconds. This is a bug!

float SmartServoClass::getPosition(uint8_t const id)
{
  if (!isValidId(id))
    return -1.0f;

  mbed::ScopedLock<rtos::Mutex> lock(_mtx);
  return positionToAngle(readWordCmd(id, REG(SmartServoRegister::POSITION_H)));
}

int SmartServoClass::readWordCmd(uint8_t const id, uint8_t const address) {
  if (readBuffer(id,address,2) == 0) {
    return (_rxBuf[5]<<8)|_rxBuf[6];
  }
  return -1;
}

inline float  positionToAngle(uint16_t const position) {
   return (MAX_ANGLE*position)/MAX_POSITION; 
}