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 
00034 MyXda::MyXda()
00035         : m_xsControl(XsControl::construct())
00036         , m_connectCount(0)
00037         , m_previousDisplayMode(DM_None)
00038 {
00039         m_xsControl->addCallbackHandler(this);
00040 }
00041 
00044 MyXda::~MyXda()
00045 {
00046         m_xsControl->removeCallbackHandler(this);
00047         m_xsControl->destruct();
00048 }
00049 
00052 void MyXda::openPorts()
00053 {
00054         XsPortInfoArray portinfo = XsScanner::scanPorts(XBR_Invalid, 0, true);
00055         for(XsPortInfoArray::const_iterator i = portinfo.begin(); i != portinfo.end(); ++i)
00056         {
00057                 printf("Opening port %s @ %d baud, device Id = %s\n", i->portName().toStdString().c_str(), i->baudrate(), i->deviceId().toString().toStdString().c_str());
00058                 xsControl()->openPort(i->portName(), i->baudrate());
00059         }
00060 }
00061 
00064 void MyXda::clearMeasurementScreen()
00065 {
00066         clearScreen();
00067         printf("The system is in measurement mode (press 'q' to exit)\n");
00068         printf("Use keys '1' to '4' to switch between display modes\n");
00069 }
00070 
00074 void MyXda::setDisplayMode(MyXda::DisplayMode displayMode)
00075 {
00076         m_displayMode = displayMode;
00077 }
00078 
00081 void MyXda::printDisplayMode()
00082 {
00083         gotoXY(0, 3);
00084         printf("Display mode = ");
00085         switch(m_displayMode)
00086         {
00087                 case DM_OrientationEuler:
00088                         printf("Orientation (euler)");
00089                         break;
00090 
00091                 case DM_OrientationQuaternion:
00092                         printf("Orientation (quaternion)");
00093                         break;
00094 
00095                 case DM_Sdi:
00096                         printf("SDI");
00097                         break;
00098 
00099                 case DM_OrientationMatrix:
00100                         printf("Orientation (matrix)");
00101                         break;
00102 
00103                 default:
00104                         printf("unknown");
00105                         break;
00106         }
00107 }
00108 
00112 void MyXda::onDataAvailable(XsDevice*, const XsDataPacket* packet)
00113 {
00114         //Get the list of devices for which packet has data
00115         XsDeviceIdArray ids = xsControl()->deviceIds();
00116 
00117         if(m_previousDisplayMode != m_displayMode)
00118         {
00119                 clearMeasurementScreen();
00120                 printDisplayMode();
00121                 m_previousDisplayMode = m_displayMode;
00122         }
00123 
00124         int row = 0;
00125         for(XsDeviceIdArray::const_iterator i = ids.begin(); i != ids.end(); ++i, row++)
00126         {
00127                 if (packet->deviceId() == *i)
00128                 {
00129                         if (packet->containsSdiData())
00130                         {
00131                                 switch(m_displayMode)
00132                                 {
00133                                         case DM_OrientationEuler:
00134                                         {
00135                                                 gotoXY(0, 4 + m_connectedDevices[i->toInt()]);
00136                                                 printf("MTw %s : ", i->toString().toStdString().c_str());
00137 
00138                                                 //Getting Euler angles
00139                                                 XsEuler oriEuler = packet->orientationEuler();
00140                                                 // printf("roll: % 4.3f\t pitch: % 4.3f\t yaw: % 4.3f              ", oriEuler.m_roll, oriEuler.m_pitch, oriEuler.m_yaw);
00141                                         } break;
00142 
00143                                         case DM_OrientationQuaternion:
00144                                         {
00145                                                 gotoXY(0, 4 + m_connectedDevices[i->toInt()]);
00146                                                 printf("MTw %s : ", i->toString().toStdString().c_str());
00147 
00148                                                 //Getting quaternions
00149                                                 XsQuaternion oriQuat = packet->orientationQuaternion();
00150                                                 // printf("w: % 3.3f x: % 3.3f y: % 3.3f z: % 3.3f", oriQuat.w(), oriQuat.x(), oriQuat.y(), oriQuat.z());
00151                                         } break;
00152 
00153                                         case DM_Sdi:
00154                                         {
00155                                                 gotoXY(0, 4 + m_connectedDevices[i->toInt()] * 3);
00156                                                 printf("MTw %s : ", i->toString().toStdString().c_str());
00157 
00158                                                 //Getting SDI data
00159                                                 XsSdiData sdiData = packet->sdiData();
00160                                                 gotoXY(0, 5 + m_connectedDevices[i->toInt()] * 3);
00161                                                 printf("\tOrientation incr. :\t (% 3.3f, % 3.3f, % 3.3f, % 3.3f)\n\tVelocity incr. :\t (% 3.3f, % 3.3f, % 3.3f)",
00162                                                                 sdiData.orientationIncrement()[0], sdiData.orientationIncrement()[1], sdiData.orientationIncrement()[2], sdiData.orientationIncrement()[3],
00163                                                                 sdiData.velocityIncrement()[0], sdiData.velocityIncrement()[1], sdiData.velocityIncrement()[2]);
00164                                         } break;
00165 
00166                                         case DM_OrientationMatrix:
00167                                         {
00168                                                 gotoXY(0, 4 + m_connectedDevices[i->toInt()] * 4);
00169                                                 printf("MTw %s : ", i->toString().toStdString().c_str());
00170 
00171                                                 gotoXY(0, 5 + m_connectedDevices[i->toInt()] * 4);
00172                                                 //Getting orientation matrix
00173                                                 XsMatrix3x3 oriMatrix = packet->orientationMatrix();
00174                                                 printf("\t| % 3.3f   % 3.3f   % 3.3f |\n", oriMatrix.value(0, 0), oriMatrix.value(0, 1), oriMatrix.value(0, 2));
00175                                                 printf("\t| % 3.3f   % 3.3f   % 3.3f |\n", oriMatrix.value(1, 0), oriMatrix.value(1, 1), oriMatrix.value(1, 2));
00176                                                 printf("\t| % 3.3f   % 3.3f   % 3.3f |\n", oriMatrix.value(2, 0), oriMatrix.value(2, 1), oriMatrix.value(2, 2));
00177                                         } break;
00178 
00179                                         default:
00180                                         {
00181                                         } break;
00182                                 }
00183                         }
00184                         break;
00185                 }
00186         }
00187 }
00188 
00189 


mtig_driver
Author(s): Lucas Casanova Nogueira
autogenerated on Thu Jun 6 2019 18:25:27