//---------------------------------------------------------------------- // 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 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."; } }