//! Do any setup necessary for a connection here
bool cXsensUsbPort::configure()
{
// Perform hardware scan
int mtCount = scanHardware();
std::cout << mtCount << " Xsens Devices Found" << std::endl;
// Check for successful detection
if (mtCount == 0)
{
std::cout << "No XSens devices found" << std::endl;
mImu.closePort();
return false;
}
// Set device to user input settings
mResult = mImu.gotoConfig();
if (mResult != XRV_OK)
{
std::cout << "Error: XSens device could not open config." << std::endl;
return false;
}
mSampleFrequency = mImu.getSampleFrequency();
// set the device output mode for the device(s)
CmtDeviceMode deviceMode(mOutputMode, mSettings, mSampleFrequency);
mResult = mImu.setDeviceMode(deviceMode,true,mDeviceId);
if (mResult != XRV_OK)
{
std::cout << "Error: XSens device could not set device mode." << std::endl;
return false;
}
// start receiving data
mResult = mImu.gotoMeasurement();
if (mResult != XRV_OK)
{
std::cout << "Error: XSens device could not return to measurement." << std::endl;
return false;
}
return true;
}
void ofxXsens::connect()
{
try {
// Scan for connected devices
ofLogNotice("ofxXsens") << "Scanning for devices...";
XsPortInfoArray portInfoArray = XsScanner::scanPorts();
// Find an MTi / MTx / MTmk4 device
mtPort = portInfoArray.begin();
while (mtPort != portInfoArray.end() && !mtPort->deviceId().isMtix() && !mtPort->deviceId().isMtMk4()) {++mtPort;}
if (mtPort == portInfoArray.end())
{
throw std::runtime_error("No MTi / MTx / MTmk4 device found. Aborting.");
}
ofLogNotice("ofXsens") << "Found a device with id: " << mtPort->deviceId().toString().toStdString() << " @ port: " << mtPort->portName().toStdString() << ", baudrate: " << mtPort->baudrate();
// Open the port with the detected device
ofLogNotice("ofxXsens") << "Opening port..." << std::endl;
if (!control->openPort(mtPort->portName().toStdString(), mtPort->baudrate()))
{
throw std::runtime_error("Could not open port. Aborting.");
}
try
{
// Get the device object
XsDevice* device = control->device(mtPort->deviceId());
assert(device != 0);
// Print information about detected MTi / MTx / MTmk4 device
ofLogNotice("ofxXsens") << "Device: " << device->productCode().toStdString() << " opened." << std::endl;
//Attach callback handler to device
device->addCallbackHandler(&callback);
// Put the device in configuration mode
ofLogNotice("ofxXsens") << "Putting device into configuration mode..." << std::endl;
if (!device->gotoConfig()) // Put the device into configuration mode before configuring the device
{
throw std::runtime_error("Could not put device into configuration mode. Aborting.");
}
// Configure the device. Note the differences between MTix and MTmk4
ofLogNotice("ofxXsens") << "Configuring the device..." << std::endl;
if (device->deviceId().isMtix())
{
ofLogNotice("ofxXsens") << "isMtix..." << std::endl;
XsOutputMode outputMode = XOM_Orientation; // output orientation data
XsOutputSettings outputSettings = XOS_OrientationMode_Quaternion; // output orientation data as quaternion
XsDeviceMode deviceMode(100); // make a device mode with update rate: 100 Hz
deviceMode.setModeFlag(outputMode);
deviceMode.setSettingsFlag(outputSettings);
// set the device configuration
if (!device->setDeviceMode(deviceMode))
{
throw std::runtime_error("Could not configure MTmki device. Aborting.");
}
bSensorConnected = true;
}
else if (device->deviceId().isMtMk4())
{
ofLogNotice("ofxXsens") << "isMtMk4..." << std::endl;
XsOutputConfiguration quat(XDI_Quaternion, 0);
//XsOutputConfiguration gpssol(XDI_GpsSol, 0);
XsOutputConfiguration latlong(XDI_LatLon, 0);
XsOutputConfigurationArray configArray;
configArray.push_back(quat);
//configArray.push_back(gpssol);
configArray.push_back(latlong);
if (!device->setOutputConfiguration(configArray))
{
throw std::runtime_error("Could not configure MTmk4 device. Aborting.");
}
bSensorConnected = true;
}
else
{
throw std::runtime_error("Unknown device while configuring. Aborting.");
}
// Put the device in measurement mode
std::cout << "Putting device into measurement mode..." << std::endl;
if (!device->gotoMeasurement())
{
throw std::runtime_error("Could not put device into measurement mode. Aborting.");
}
}
catch (std::runtime_error const & error)
{
std::cout << error.what() << std::endl;
}
catch (...)
{
ofLogError("ofxXsens") << "An unknown fatal error has occured. Aborting." << std::endl;
}
}
catch (runtime_error const & error)
{
ofLogError("ofxXsens") << error.what();
}
catch (...)
{
ofLogError("ofxXsens") << "An unknown fatal error has occured.";
}
}
