void IMU::Initiate()
{
XsPortInfoArray portInfoArray;
xsEnumerateUsbDevices(portInfoArray);
if (!portInfoArray.size())
{
#ifdef PLATFORM_IS_WINDOWS
throw std::runtime_error("IMU: failed to find IMU sensor");
#endif
#ifdef PLATFORM_IS_LINUX
XsPortInfo portInfo(pDevice->port, XsBaud::numericToRate(pDevice->baudRate));
portInfoArray.push_back(portInfo);
#endif
}
pDevice->mtPort = portInfoArray.at(0);
// Open the port with the detected device
if (!pDevice->openPort(pDevice->mtPort))
throw std::runtime_error("IMU: could not open port.");
Aris::Core::Sleep(10);
// Put the device in configuration mode
if (!pDevice->gotoConfig()) // Put the device into configuration mode before configuring the device
{
throw std::runtime_error("IMU: could not put device into configuration mode");
}
// Request the device Id to check the device type
pDevice->mtPort.setDeviceId(pDevice->getDeviceId());
// Check if we have an MTi / MTx / MTmk4 device
if (!pDevice->mtPort.deviceId().isMtMk4())
{
throw std::runtime_error("IMU: No MTi / MTx / MTmk4 device found.");
}
// Check device type
if (pDevice->mtPort.deviceId().isMtMk4())
{
XsOutputConfiguration config0(XDI_Quaternion, pDevice->sampleRate);
XsOutputConfiguration config1(XDI_DeltaQ, pDevice->sampleRate);
XsOutputConfiguration config2(XDI_DeltaV, pDevice->sampleRate);
XsOutputConfiguration config3(XDI_Acceleration, pDevice->sampleRate);
XsOutputConfigurationArray configArray;
configArray.push_back(config0);
configArray.push_back(config1);
configArray.push_back(config2);
configArray.push_back(config3);
if (!pDevice->setOutputConfiguration(configArray))
{
throw std::runtime_error("IMU: Could not configure MTmk4 pDevice-> Aborting.");
}
}
else
{
throw std::runtime_error("IMU: Unknown device while configuring. Aborting.");
}
// Put the device in measurement mode
if (!pDevice->gotoMeasurement())
{
throw std::runtime_error("IMU: Could not put device into measurement mode. Aborting.");
}
}
int main() {
typedef boost::mt19937 rng_type;
typedef std::vector<unsigned int> vector_type;
rng_type rng(seed);
boost::variate_generator<rng_type&, boost::uniform_real<> >
random(rng, boost::uniform_real<>(0, 1));
std::cout << "[[random permutaion test 1]]\n";
std::cout << "generating " << trial1 << " random permutations of "
<< n << " integers [0..." << n - 1 << "]\n";
for (unsigned int i = 0; i < trial1; ++i) {
vector_type result(n);
for (unsigned int j = 0; j < n; ++j) result[j] = j;
looper::random_shuffle(result.begin(), result.end(), random);
for (unsigned int j = 0; j < n; ++j) {
std::cout << result[j] << '\t';
}
std::cout << std::endl;
}
std::cout << "\n[[random permutaion test 2 (with restrictions)]]\n";
std::cout << "generating " << trial2 << " partitioned random permutations\n";
for (unsigned int t = 0; t < trial2; t++) {
vector_type config0(n);
for (unsigned int i = 0; i < n; i++) {
config0[i] = 2 * random();
}
vector_type config1(config0);
looper::random_shuffle(config1.begin(), config1.end(), random);
std::cout << "conf0";
for (unsigned int i = 0; i < n; i++) std::cout << '\t' << config0[i];
std::cout << std::endl;
std::cout << "conf1";
for (unsigned int i = 0; i < n; i++) std::cout << '\t' << config1[i];
std::cout << std::endl;
vector_type result(n);
for (unsigned int j = 0; j < n; ++j) result[j] = j;
// generate partitioned permutation
looper::partitioned_random_shuffle(result.begin(), result.end(),
config0.begin(), config1.begin(),
random);
std::cout << "perm";
for (unsigned int i = 0; i < n; i++) std::cout << '\t' << result[i];
std::cout << std::endl;
std::cout << "check";
int chk = 0;
for (unsigned int i = 0; i < n; i++) {
std::cout << '\t' << (config0[i] ^ config1[result[i]]);
chk += (config0[i] ^ config1[result[i]]);
}
std::cout << std::endl;
if (chk > 0) { std::cout << "Error occurs! Stop.\n"; std::exit(-1); }
std::cout << std::endl;
}
}
