/*! \relates XsFile \brief Retrieves the full path for a filename \param[in] filename The filename to expand \param[out] fullPath The filename with a fully expanded path is returned in this parameter. This parameter must be allocated by the caller. \returns XRV_OK if the fullpath could be retrieved, XRV_NULLPTR if fullPath is NULL, XRV_ERROR otherwise */ XsResultValue XsFile_fullPath(const struct XsString* filename, struct XsString* fullPath) { XsResultValue result = XRV_OK; if (fullPath == NULL) { result = XRV_NULLPTR; } else { #ifdef _WIN32 wchar_t filenamew[XS_MAX_FILENAME_LENGTH]; wchar_t fullpath[XS_MAX_FILENAME_LENGTH]; (void)XsString_copyToWCharArray(filename, filenamew, XS_MAX_FILENAME_LENGTH); if (_wfullpath(fullpath, filenamew, XS_MAX_FILENAME_LENGTH) == NULL) result = XRV_ERROR; else XsString_assignWCharArray(fullPath, fullpath); #else // based on the assumption that this doesn't concern the serial port, handle // it the same way using realpath(). Apparently realpath() doesn't require a // maximum length. One would possibly want to write a wrapper for it. char fullpath[XS_MAX_FILENAME_LENGTH*2]; if (realpath(filename->m_data, fullpath) == NULL) result = XRV_ERROR; else XsString_assignCharArray(fullPath, fullpath); #endif } return result; }
/*! \brief Get an error string after a failure occurred \param[in,out] error the string to fill with the result error */ void XsLibraryLoader_getErrorString(XsString* error) { #ifdef __GNUC__ XsString_assignCharArray(error, dlerror()); #elif defined(_MSC_VER) LPTSTR errorText = NULL; (void)FormatMessageW(FORMAT_MESSAGE_FROM_SYSTEM |FORMAT_MESSAGE_ALLOCATE_BUFFER |FORMAT_MESSAGE_IGNORE_INSERTS, NULL, GetLastError(), MAKELANGID(LANG_NEUTRAL, SUBLANG_DEFAULT), (LPTSTR)&errorText, 0, NULL); XsString_assignWCharArray(error, errorText); LocalFree(errorText); #endif }
//-------------------------------------------------------------------------------- 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; }