myxda.cpp
Go to the documentation of this file.
00001 /* Copyright (c) Xsens Technologies B.V., 2006-2012. All rights reserved.
00002 
00003         This source code is provided under the MT SDK Software License Agreement
00004         and is intended for use only by Xsens Technologies BV and
00005         those that have explicit written permission to use it from
00006         Xsens Technologies BV.
00007 
00008         THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY
00009         KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE
00010         IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A
00011         PARTICULAR PURPOSE.
00012  */
00013 
00014 #include "myxda.h"
00015 #include <stdio.h>
00016 #include "console.h"
00017 #include <xsens/xscontrol.h>
00018 #include <xsens/xsscanner.h>
00019 #include <xsens/xseuler.h>
00020 #include <xsens/xsmatrix3x3.h>
00021 #include <xsens/xssdidata.h>
00022 #include <xsens/xsportinfoarray.h>
00023 #include <xsens/xsdeviceidarray.h>
00024 
00032 MyXda::MyXda()
00033         : m_xsControl(XsControl::construct())
00034         , m_connectCount(0)
00035         , m_previousDisplayMode(DM_None)
00036 {
00037         m_xsControl->addCallbackHandler(this);
00038 }
00039 
00042 MyXda::~MyXda()
00043 {
00044         m_xsControl->removeCallbackHandler(this);
00045         m_xsControl->destruct();
00046 }
00047 
00050 void MyXda::openPorts()
00051 {
00052         XsPortInfoArray portinfo = XsScanner::scanPorts(XBR_Invalid, 0, true);
00053         for(XsPortInfoArray::const_iterator i = portinfo.begin(); i != portinfo.end(); ++i)
00054         {
00055                 printf("Opening port %s @ %d baud, device Id = %s\n", i->portName().toStdString().c_str(), i->baudrate(), i->deviceId().toString().toStdString().c_str());
00056                 xsControl()->openPort(i->portName(), i->baudrate());
00057         }
00058 }
00059 
00062 void MyXda::clearMeasurementScreen()
00063 {
00064         clearScreen();
00065         printf("The system is in measurement mode (press 'q' to exit)\n");
00066         printf("Use keys '1' to '4' to switch between display modes\n");
00067 }
00068 
00072 void MyXda::setDisplayMode(MyXda::DisplayMode displayMode)
00073 {
00074         m_displayMode = displayMode;
00075 }
00076 
00079 void MyXda::printDisplayMode()
00080 {
00081         gotoXY(0, 3);
00082         printf("Display mode = ");
00083         switch(m_displayMode)
00084         {
00085                 case DM_OrientationEuler:
00086                         printf("Orientation (euler)");
00087                         break;
00088 
00089                 case DM_OrientationQuaternion:
00090                         printf("Orientation (quaternion)");
00091                         break;
00092 
00093                 case DM_Sdi:
00094                         printf("SDI");
00095                         break;
00096 
00097                 case DM_OrientationMatrix:
00098                         printf("Orientation (matrix)");
00099                         break;
00100 
00101                 default:
00102                         printf("unknown");
00103                         break;
00104         }
00105 }
00106 
00110 void MyXda::onDataAvailable(XsDevice*, const XsDataPacket* packet)
00111 {
00112         //Get the list of devices for which packet has data
00113         XsDeviceIdArray ids = xsControl()->deviceIds();
00114 
00115         if(m_previousDisplayMode != m_displayMode)
00116         {
00117                 clearMeasurementScreen();
00118                 printDisplayMode();
00119                 m_previousDisplayMode = m_displayMode;
00120         }
00121 
00122         int row = 0;
00123         for(XsDeviceIdArray::const_iterator i = ids.begin(); i != ids.end(); ++i, row++)
00124         {
00125                 if (packet->deviceId() == *i)
00126                 {
00127                         if (packet->containsSdiData())
00128                         {
00129                                 switch(m_displayMode)
00130                                 {
00131                                         case DM_OrientationEuler:
00132                                         {
00133                                                 gotoXY(0, 4 + m_connectedDevices[i->toInt()]);
00134                                                 printf("MTw %s : ", i->toString().toStdString().c_str());
00135 
00136                                                 //Getting Euler angles
00137                                                 XsEuler oriEuler = packet->orientationEuler();
00138                                                 printf("roll: % 4.3f\t pitch: % 4.3f\t yaw: % 4.3f              ", oriEuler.m_roll, oriEuler.m_pitch, oriEuler.m_yaw);
00139                                         } break;
00140 
00141                                         case DM_OrientationQuaternion:
00142                                         {
00143                                                 gotoXY(0, 4 + m_connectedDevices[i->toInt()]);
00144                                                 printf("MTw %s : ", i->toString().toStdString().c_str());
00145 
00146                                                 //Getting quaternions
00147                                                 XsQuaternion oriQuat = packet->orientationQuaternion();
00148                                                 printf("w: % 3.3f x: % 3.3f y: % 3.3f z: % 3.3f", oriQuat.m_w, oriQuat.m_x, oriQuat.m_y, oriQuat.m_z);
00149                                         } break;
00150 
00151                                         case DM_Sdi:
00152                                         {
00153                                                 gotoXY(0, 4 + m_connectedDevices[i->toInt()] * 3);
00154                                                 printf("MTw %s : ", i->toString().toStdString().c_str());
00155 
00156                                                 //Getting SDI data
00157                                                 XsSdiData sdiData = packet->sdiData();
00158                                                 gotoXY(0, 5 + m_connectedDevices[i->toInt()] * 3);
00159                                                 printf("\tOrientation incr. :\t (% 3.3f, % 3.3f, % 3.3f, % 3.3f)\n\tVelocity incr. :\t (% 3.3f, % 3.3f, % 3.3f)",
00160                                                                 sdiData.orientationIncrement()[0], sdiData.orientationIncrement()[1], sdiData.orientationIncrement()[2], sdiData.orientationIncrement()[3],
00161                                                                 sdiData.velocityIncrement()[0], sdiData.velocityIncrement()[1], sdiData.velocityIncrement()[2]);
00162                                         } break;
00163 
00164                                         case DM_OrientationMatrix:
00165                                         {
00166                                                 gotoXY(0, 4 + m_connectedDevices[i->toInt()] * 4);
00167                                                 printf("MTw %s : ", i->toString().toStdString().c_str());
00168 
00169                                                 gotoXY(0, 5 + m_connectedDevices[i->toInt()] * 4);
00170                                                 //Getting orientation matrix
00171                                                 XsMatrix3x3 oriMatrix = packet->orientationMatrix();
00172                                                 printf("\t| % 3.3f   % 3.3f   % 3.3f |\n", oriMatrix.value(0, 0), oriMatrix.value(0, 1), oriMatrix.value(0, 2));
00173                                                 printf("\t| % 3.3f   % 3.3f   % 3.3f |\n", oriMatrix.value(1, 0), oriMatrix.value(1, 1), oriMatrix.value(1, 2));
00174                                                 printf("\t| % 3.3f   % 3.3f   % 3.3f |\n", oriMatrix.value(2, 0), oriMatrix.value(2, 1), oriMatrix.value(2, 2));
00175                                         } break;
00176 
00177                                         default:
00178                                         {
00179                                         } break;
00180                                 }
00181                         }
00182                         break;
00183                 }
00184         }
00185 }
00186 


receive_xsens
Author(s): Ji Zhang, Silvio Maeta
autogenerated on Sat Jun 8 2019 19:32:50