/*-------------------------------------------------------------
initialize
-------------------------------------------------------------*/
void CIMUXSens_MT4::initialize()
{
#if MRPT_HAS_xSENS_MT4
m_state = ssInitializing;
try
{
// Try to open a specified device, or scan the bus?
XsPortInfoArray portInfoArray;
if (m_portname.empty())
{
if (m_verbose) cout << "[CIMUXSens_MT4] Scanning for USB devices...\n";
xsEnumerateUsbDevices(portInfoArray);
if (portInfoArray.empty())
THROW_EXCEPTION("CIMUXSens_MT4: No 'portname' was specified and no XSens device was found after scanning the system!")
if (m_verbose) cout << "[CIMUXSens_MT4] Found " << portInfoArray.size() <<" devices. Opening the first one.\n";
}
else
{
XsPortInfo portInfo(m_portname, XsBaud::numericToRate(m_port_bauds));
if (m_verbose) cout << "[CIMUXSens_MT4] Using user-supplied portname '"<<m_portname<<"' at "<<m_port_bauds<<" baudrate.\n";
portInfoArray.push_back(portInfo);
}
// Use the first detected device
XsPortInfo mtPort = portInfoArray.at(0);
// Open the port with the detected device
cout << "[CIMUXSens_MT4] Opening port " << mtPort.portName().toStdString() << std::endl;
if (!my_xsens_device.openPort(mtPort))
throw std::runtime_error("Could not open port. Aborting.");
// Put the device in configuration mode
if (m_verbose) cout << "[CIMUXSens_MT4] Putting device into configuration mode...\n";
if (!my_xsens_device.gotoConfig()) // Put the device into configuration mode before configuring the device
throw std::runtime_error("Could not put device into configuration mode. Aborting.");
// Request the device Id to check the device type
mtPort.setDeviceId(my_xsens_device.getDeviceId());
my_xsens_devid = mtPort.deviceId();
// Check if we have an MTi / MTx / MTmk4 device
if (!mtPort.deviceId().isMtix() && !mtPort.deviceId().isMtMk4())
{
throw std::runtime_error("No MTi / MTx / MTmk4 device found. Aborting.");
}
cout << "[CIMUXSens_MT4] Found a device with id: " << mtPort.deviceId().toString().toStdString() << " @ port: " << mtPort.portName().toStdString() << ", baudrate: " << mtPort.baudrate() << std::endl;
// Print information about detected MTi / MTx / MTmk4 device
if (m_verbose) cout << "[CIMUXSens_MT4] Device: " << my_xsens_device.getProductCode().toStdString() << " opened." << std::endl;
// Configure the device. Note the differences between MTix and MTmk4
if (m_verbose) cout << "[CIMUXSens_MT4] Configuring the device..." << std::endl;
if (mtPort.deviceId().isMtix())
{
XsOutputMode outputMode = XOM_Orientation; // output orientation data
XsOutputSettings outputSettings = XOS_OrientationMode_Euler | XOS_Timestamp_PacketCounter | XOS_CalibratedMode_All; // XOS_OrientationMode_Quaternion; // output orientation data as quaternion
// set the device configuration
if (!my_xsens_device.setDeviceMode(outputMode, outputSettings))
throw std::runtime_error("Could not configure MT device. Aborting.");
}
else if (mtPort.deviceId().isMtMk4())
{
XsOutputConfigurationArray configArray;
configArray.push_back( XsOutputConfiguration(XDI_SampleTime64,m_sampleFreq) );
configArray.push_back( XsOutputConfiguration(XDI_SampleTimeFine,m_sampleFreq) );
configArray.push_back( XsOutputConfiguration(XDI_SampleTimeCoarse,m_sampleFreq) );
configArray.push_back( XsOutputConfiguration(XDI_Quaternion,m_sampleFreq) );
configArray.push_back( XsOutputConfiguration(XDI_Temperature,m_sampleFreq) );
configArray.push_back( XsOutputConfiguration(XDI_Acceleration,m_sampleFreq) );
configArray.push_back( XsOutputConfiguration(XDI_RateOfTurn,m_sampleFreq) );
configArray.push_back( XsOutputConfiguration(XDI_MagneticField,m_sampleFreq) );
configArray.push_back( XsOutputConfiguration(XDI_VelocityXYZ,m_sampleFreq) );
configArray.push_back( XsOutputConfiguration(XDI_StatusByte, m_sampleFreq) );
configArray.push_back( XsOutputConfiguration(XDI_LatLon, m_sampleFreq) );
configArray.push_back( XsOutputConfiguration(XDI_UtcTime, m_sampleFreq) );
configArray.push_back( XsOutputConfiguration(XDI_AltitudeEllipsoid, m_sampleFreq) );
if (!my_xsens_device.setOutputConfiguration(configArray))
throw std::runtime_error("Could not configure MTmk4 device. Aborting.");
}
else
{
throw std::runtime_error("Unknown device while configuring. Aborting.");
}
// Put the device in measurement mode
if (m_verbose) cout << "[CIMUXSens_MT4] Putting device into measurement mode..." << std::endl;
if (!my_xsens_device.gotoMeasurement())
throw std::runtime_error("Could not put device into measurement mode. Aborting.");
m_state = ssWorking;
}
//----------------------------------------------------------------------
// Main
//----------------------------------------------------------------------
int main(int argc, char* argv[])
{
(void)argc;
(void)argv;
int desiredUpdateRate = 75; // Use 75 Hz update rate for MTWs
int desiredRadioChannel = 19; // Use radio channel 19 for wireless master.
WirelessMasterCallback wirelessMasterCallback; // Callback for wireless master
std::vector<MtwCallback*> mtwCallbacks; // Callbacks for mtw devices
std::cout << "Verifying serial key..." << std::endl;
if (!setSerialKey()) {
std::cout << "Invalid serial key." << std::endl;
return EXIT_FAILURE;
}
std::cout << "Constructing XsControl..." << std::endl;
XsControl* control = XsControl::construct();
if (control == 0) {
std::cout << "Failed to construct XsControl instance." << std::endl;
}
try
{
std::cout << "Scanning ports..." << std::endl;
XsPortInfoArray detectedDevices = XsScanner::scanPorts();
std::cout << "Finding wireless master..." << std::endl;
XsPortInfoArray::const_iterator wirelessMasterPort = detectedDevices.begin();
while (wirelessMasterPort != detectedDevices.end() && !wirelessMasterPort->deviceId().isWirelessMaster()) {
++wirelessMasterPort;
}
if (wirelessMasterPort == detectedDevices.end()) {
throw std::runtime_error("No wireless masters found");
}
std::cout << "Wireless master found @ " << *wirelessMasterPort << std::endl;
std::cout << "Opening port..." << std::endl;
if (!control->openPort(wirelessMasterPort->portName().toStdString(), wirelessMasterPort->baudrate())) {
std::ostringstream error;
error << "Failed to open port " << *wirelessMasterPort;
throw std::runtime_error(error.str());
}
std::cout << "Getting XsDevice instance for wireless master..." << std::endl;
XsDevicePtr wirelessMasterDevice = control->device(wirelessMasterPort->deviceId());
if (wirelessMasterDevice == 0) {
std::ostringstream error;
error << "Failed to construct XsDevice instance: " << *wirelessMasterPort;
throw std::runtime_error(error.str());
}
std::cout << "XsDevice instance created @ " << *wirelessMasterDevice << std::endl;
std::cout << "Setting config mode..." << std::endl;
if (!wirelessMasterDevice->gotoConfig()) {
std::ostringstream error;
error << "Failed to goto config mode: " << *wirelessMasterDevice;
throw std::runtime_error(error.str());
}
std::cout << "Attaching callback handler..." << std::endl;
wirelessMasterDevice->addCallbackHandler(&wirelessMasterCallback);
std::cout << "Setting update rate to " << desiredUpdateRate << " Hz..." << std::endl;
if (!wirelessMasterDevice->setUpdateRate(desiredUpdateRate)) {
std::ostringstream error;
error << "Failed to set update rate: " << *wirelessMasterDevice;
throw std::runtime_error(error.str());
}
std::cout << "Setting radio channel to " << desiredRadioChannel << " and enabling radio..." << std::endl;
if (!wirelessMasterDevice->enableRadio(desiredRadioChannel)) {
std::ostringstream error;
error << "Failed to set radio channel: " << *wirelessMasterDevice;
throw std::runtime_error(error.str());
}
std::cout << "Waiting for MTW to wirelessly connect...\n" << std::endl;
bool waitForConnections = true;
size_t connectedMTWCount = wirelessMasterCallback.getWirelessMTWs().size();
do {
XsTime::msleep(100);
while (true) {
size_t nextCount = wirelessMasterCallback.getWirelessMTWs().size();
if (nextCount != connectedMTWCount) {
std::cout << "Number of connected MTWs: " << nextCount << ". Press 'Y' to start measurement." << std::endl;
connectedMTWCount = nextCount;
}
else {
break;
}
}
if (_kbhit()) {
waitForConnections = (toupper((char)_getch()) != 'Y');
}
}
while (waitForConnections);
std::cout << "Starting measurement..." << std::endl;
if (!wirelessMasterDevice->gotoMeasurement()) {
std::ostringstream error;
error << "Failed to goto measurement mode: " << *wirelessMasterDevice;
throw std::runtime_error(error.str());
}
std::cout << "Getting XsDevice instances for all MTWs..." << std::endl;
XsDeviceIdArray allDeviceIds = control->deviceIds();
XsDeviceIdArray mtwDeviceIds;
for (XsDeviceIdArray::const_iterator i = allDeviceIds.begin(); i != allDeviceIds.end(); ++i) {
if (i->isMtw()) {
mtwDeviceIds.push_back(*i);
}
}
XsDevicePtrArray mtwDevices;
for (XsDeviceIdArray::const_iterator i = mtwDeviceIds.begin(); i != mtwDeviceIds.end(); ++i) {
XsDevicePtr mtwDevice = control->device(*i);
if (mtwDevice != 0) {
mtwDevices.push_back(mtwDevice);
}
else {
throw std::runtime_error("Failed to create an MTW XsDevice instance");
}
}
std::cout << "Attaching callback handlers to MTWs..." << std::endl;
mtwCallbacks.resize(mtwDevices.size());
for (int i = 0; i < (int)mtwDevices.size(); ++i) {
mtwCallbacks[i] = new MtwCallback(i, mtwDevices[i]);
mtwDevices[i]->addCallbackHandler(mtwCallbacks[i]);
}
std::cout << "\nMain loop. Press any key to quit\n" << std::endl;
std::cout << "Waiting for data available..." << std::endl;
std::vector<XsEuler> eulerData(mtwCallbacks.size()); // Room to store euler data for each mtw
unsigned int printCounter = 0;
while (!_kbhit()) {
XsTime::msleep(0);
bool newDataAvailable = false;
for (size_t i = 0; i < mtwCallbacks.size(); ++i) {
if (mtwCallbacks[i]->dataAvailable()) {
newDataAvailable = true;
XsDataPacket const * packet = mtwCallbacks[i]->getOldestPacket();
eulerData[i] = packet->orientationEuler();
mtwCallbacks[i]->deleteOldestPacket();
}
}
if (newDataAvailable) {
// Don't print too often for performance. Console output is very slow.
if (printCounter % 25 == 0) {
for (size_t i = 0; i < mtwCallbacks.size(); ++i) {
std::cout << "[" << i << "]: ID: " << mtwCallbacks[i]->device().deviceId().toString().toStdString()
<< ", Roll: " << std::setw(7) << std::fixed << std::setprecision(2) << eulerData[i].m_roll
<< ", Pitch: " << std::setw(7) << std::fixed << std::setprecision(2) << eulerData[i].m_pitch
<< ", Yaw: " << std::setw(7) << std::fixed << std::setprecision(2) << eulerData[i].m_yaw
<< "\n";
}
}
++printCounter;
}
}
(void)_getch();
std::cout << "Setting config mode..." << std::endl;
if (!wirelessMasterDevice->gotoConfig()) {
std::ostringstream error;
error << "Failed to goto config mode: " << *wirelessMasterDevice;
throw std::runtime_error(error.str());
}
std::cout << "Disabling radio... " << std::endl;
if (!wirelessMasterDevice->disableRadio()) {
std::ostringstream error;
error << "Failed to disable radio: " << *wirelessMasterDevice;
throw std::runtime_error(error.str());
}
}
catch (std::exception const & ex) {
std::cout << ex.what() << std::endl;
std::cout << "****ABORT****" << std::endl;
}
catch (...)
{
std::cout << "An unknown fatal error has occured. Aborting." << std::endl;
std::cout << "****ABORT****" << std::endl;
}
std::cout << "Closing XsControl..." << std::endl;
control->close();
std::cout << "Deleting mtw callbacks..." << std::endl;
for (std::vector<MtwCallback*>::iterator i = mtwCallbacks.begin(); i != mtwCallbacks.end(); ++i) {
delete (*i);
}
std::cout << "Successful exit." << std::endl;
std::cout << "Press [ENTER] to continue." << std::endl; std::cin.get();
return 0;
}
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.");
}
}
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.";
}
}
