void ofxXsens::setup()
{
bSensorConnected = false;
if (!setSerialKey())
{
ofLogError("ofxXsens") << "Invalid serial key." << std::endl;
return;
}
// Create XsControl object
ofLogNotice("ofxXsens") << "Creating XsControl object..." << std::endl;
control = XsControl::construct();
assert(control != 0);
}
//--------------------------------------------------------------------------------
int main(void)
{
// Declare used variables
XsDevice* foundDevice;
XsControl* control;
XsPortInfo* portInfo;
XsPortInfo* foundPort;
XsDeviceId* foundDeviceId;
// Make sure to call the proper xxx_INIT macros to properly initialize structs
XsPortInfoArray portInfoArray = XSPORTINFOARRAY_INITIALIZER;
XsCallbackPlainC callbackHandler = XSCALLBACK_INITIALIZER;
XsString deviceIdString = XSSTRING_INITIALIZER;
XsString portNameString = XSSTRING_INITIALIZER;
XsQuaternion quaternion = XSQUATERNION_INITIALIZER;
XsEuler euler = XSEULER_INITIALIZER;
XsOutputConfigurationArray mtmk4_configArray = XSOUTPUTCONFIGURATIONARRAY_INITIALIZER;
XsDeviceMode mtix_deviceMode = XSDEVICEMODE_INITIALIZER;
XsOutputConfiguration mtmk4_config = XSOUTPUTCONFIGURATION_INITIALIZER;
XsSize portInfoIdx;
int deviceFound;
int configSucceeded;
if (!setSerialKey())
{
printf("Invalid serial key\n");
printf("Press a key to continue.\n");
(void)_getch();
return 1;
}
// Initialize global packet queue
PacketQueue_init(&packetQueue);
// Create XsControl object
printf("Creating XsControl object...\n");
control = XsControl_construct();
assert(control != 0);
// Scan for connected devices
printf("Scanning for devices...\n");
(void)XsScanner_scanPorts(&portInfoArray, XBR_Invalid, 100, TRUE, TRUE);
// Find an MTi / MTx / MTmk4 device
foundPort = 0;
foundDeviceId = 0;
portInfo = portInfoArray.m_data;
deviceFound = FALSE;
for (portInfoIdx = 0; portInfoIdx < portInfoArray.m_size; ++portInfoIdx)
{
foundPort = &portInfo[portInfoIdx];
foundDeviceId = &foundPort->m_deviceId;
if (XsDeviceId_isMtix(foundDeviceId) || XsDeviceId_isMtMk4(foundDeviceId))
{
deviceFound = TRUE;
break;
}
}
if (deviceFound)
{
XsDeviceId_toString(foundDeviceId, &deviceIdString);
printf("Found a device with id: %s @ port: %s, baudrate: %d\n", deviceIdString.m_data, foundPort->m_portName, foundPort->m_baudrate);
// Open the port with the detected device
printf("Opening port...\n");
XsString_assignCharArray(&portNameString, foundPort->m_portName);
if (XsControl_openPort(control, &portNameString, foundPort->m_baudrate, 0, TRUE))
{
// Get the device object
foundDevice = XsControl_device(control, foundDeviceId);
assert(foundDevice != 0);
// Print information about detected MTi / MTx / MTmk4 device
printf("Device: %s openend.\n", XsDevice_productCode(foundDevice, &deviceIdString)->m_data);
// Attach callback handler to device
callbackHandler.m_onDataAvailable = onDataAvailable;
XsDevice_addCallbackHandler(foundDevice, &callbackHandler, TRUE);
// Put the device in configuration mode
printf("Putting device into configuration mode...\n");
configSucceeded = FALSE;
if (XsDevice_gotoConfig(foundDevice)) // Put the device into configuration mode before configuring the device
{
// Configure the device. Note the differences between MTix and MTmk4
printf("Configuring the device...\n");
if (XsDeviceId_isMtix(foundDeviceId))
{
memset(&mtix_deviceMode, 0, sizeof(XsDeviceMode));
mtix_deviceMode.m_outputMode = XOM_Orientation; // output orientation data
mtix_deviceMode.m_outputSettings = XOS_OrientationMode_Quaternion; // output orientation data as quaternion
XsDeviceMode_setUpdateRate(&mtix_deviceMode, 100);// make a device mode with update rate: 100 Hz
// set the device configuration
if (XsDevice_setDeviceMode(foundDevice, &mtix_deviceMode))
{
configSucceeded = TRUE;
}
}
else if (XsDeviceId_isMtMk4(foundDeviceId))
{
mtmk4_config.m_dataIdentifier = XDI_Quaternion;
mtmk4_config.m_frequency = XDI_MAX_FREQUENCY;
XsOutputConfigurationArray_construct(&mtmk4_configArray, 1, &mtmk4_config);
if (XsDevice_setOutputConfiguration(foundDevice, &mtmk4_configArray))
{
configSucceeded = TRUE;
}
}
else
{
printf("Unknown device while configuring. Aborting.");
}
}
else
{
printf("Could not put device into configuration mode. Aborting.\n");
}
if (configSucceeded)
{
// Put the device in measurement mode
printf("Putting device into measurement mode...\n");
if (XsDevice_gotoMeasurement(foundDevice))
{
printf("\nMain loop (press any key to quit)\n");
printf("-------------------------------------------------------------------------------\n");
while (!_kbhit())
{
// Retrieve a packet from queue
XsDataPacket packet = XSDATAPACKET_INITIALIZER;
if (PacketQueue_remove(&packetQueue, &packet))
{
// Get the quaternion data
XsDataPacket_orientationQuaternion(&packet, &quaternion, XDI_CoordSysEnu);
// Convert packet to euler
XsDataPacket_orientationEuler(&packet, &euler, XDI_CoordSysEnu);
printf("\rq0:%5.2f,q1:%5.2f,q2:%5.2f,q3:%5.2f,roll:%7.2f,pitch:%7.2f,yaw:%7.2f",
quaternion.m_w, quaternion.m_x, quaternion.m_y, quaternion.m_z,
euler.m_roll, euler.m_pitch, euler.m_yaw
);
fflush(stdout);
}
XsTime_msleep(0);
}
_getch();
printf("\n-------------------------------------------------------------------------------\n");
}
else
{
printf("Could not put device into measurement mode. Aborting.\n");
}
}
else
{
printf("Could not configure device. Aborting.\n");
}
// Close port
printf("Closing port...\n");
XsControl_closePort(control, &portNameString);
}
else
{
printf("Could not open port. Aborting.\n");
}
}
else
{
printf("No MTi / MTx device found. Aborting.\n");
}
// Free XsControl object
printf("Freeing XsControl object...\n");
XsControl_destruct(control);
// free global packet queue
PacketQueue_destruct(&packetQueue);
printf("Successful exit.\n");
printf("Press a key to continue.\n");
(void)_getch();
return 0;
}
//----------------------------------------------------------------------
// 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;
}
