////////////////////////////////////////////////////////////////////////// // doMTSettings // // Set user settings in MTi/MTx // Assumes initialized global MTComm class void AHRS::doMtSettings(xsens::Cmt3 &cmt3, CmtOutputMode &mode, CmtOutputSettings &settings, CmtDeviceId deviceIds[]) { XsensResultValue res; unsigned long mtCount = cmt3.getMtCount(); // set sensor to config sate res = cmt3.gotoConfig(); EXIT_ON_ERROR(res,"gotoConfig"); unsigned short sampleFreq; sampleFreq = cmt3.getSampleFrequency(); // set the device output mode for the device(s) for (unsigned int i = 0; i < mtCount; i++) { CmtDeviceMode deviceMode(mode, settings, sampleFreq); if ((deviceIds[i] & 0xFFF00000) != 0x00500000) { // not an MTi-G, remove all GPS related stuff deviceMode.m_outputMode &= 0xFF0F; } res = cmt3.setDeviceMode(deviceMode, true, deviceIds[i]); EXIT_ON_ERROR(res,"setDeviceMode"); } // start receiving data res = cmt3.gotoMeasurement(); EXIT_ON_ERROR(res,"gotoMeasurement"); }
////////////////////////////////////////////////////////////////////////// // doMTSettings // // Set user settings in MTi/MTx // Assumes initialized global MTComm class void XSensDriver::doMtSettings( xsens::Cmt3 &cmt3, CmtOutputMode &mode, CmtOutputSettings &settings, CmtDeviceId deviceIds[] ) { XsensResultValue res; unsigned long mtCount = cmt3.getMtCount(); // set sensor to config sate res = cmt3.gotoConfig(); unsigned short sampleFreq; sampleFreq = 100;//cmt3.getSampleFrequency(); std::cout << "sampling at " << sampleFreq << std::endl; // set the device output mode for the device(s) std::cout << "Configuring your mode selection" << std::endl; for ( unsigned int i = 0; i < mtCount; i++ ) { CmtDeviceMode deviceMode( mode, settings, sampleFreq ); if ( ( deviceIds[i] & 0xFFF00000 ) != 0x00500000 ) { // not an MTi-G, remove all GPS related stuff deviceMode.m_outputMode &= 0xFF0F; } res = cmt3.setDeviceMode( deviceMode, true, deviceIds[i] ); } // start receiving data res = cmt3.gotoMeasurement(); }
////////////////////////////////////////////////////////////////////////// // doHardwareScan // // Checks available COM ports and scans for MotionTrackers int AHRS::doHardwareScan(xsens::Cmt3 &cmt3, CmtDeviceId deviceIds[]) { XsensResultValue res; List<CmtPortInfo> portInfo; unsigned long portCount = 0; int mtCount; xsens::cmtScanPorts(portInfo); portCount = portInfo.length(); if (portCount == 0) { printf("No MotionTrackers found \n\n"); return 0; } for(int p = 0; p < (int)portCount; p++){ res = cmt3.openPort(portInfo[p].m_portName, portInfo[p].m_baudrate); EXIT_ON_ERROR(res,"cmtOpenPort"); } //get the Mt sensor count. mtCount = cmt3.getMtCount(); // retrieve the device IDs for(int j = 0; j < mtCount; j++){ res = cmt3.getDeviceId((unsigned char)(j+1), deviceIds[j]); EXIT_ON_ERROR(res,"getDeviceId"); } return mtCount; }
void exitFunc(void) { // Close any open COM ports cmt3.closePort(); // get rid of keystrokes before we post our message while (_kbhit()) { _getch(); } // wait for a keypress if (!g_userQuit) { printf("Press a key to exit\n"); _getch(); } if(g_outputFile != NULL) { fclose(g_outputFile); } }
int main(int argc, char **argv) { unsigned long mtCount; /* initialize carmen */ //carmen_randomize(&argc, &argv); carmen_ipc_initialize(argc, argv); carmen_param_check_version(argv[0]); mode = 6; settings = 2; const char* pName = NULL; read_parameters(argc, argv); pName = dev; packet = new xsens::Packet((unsigned short) mtCount, cmt3.isXm()); /* Initializing Xsens */ initializeXsens(cmt3, mode, settings, mtCount, (char*)pName); /* Setup exit handler */ signal(SIGINT, shutdown_xsens); /* register localize related messages */ // register_ipc_messages(); carmen_xsens_define_messages(); while(1){ getDataFromXsens(cmt3, packet, mode, settings, g_data); publish_xsens_global(g_data, /*mode,*/ settings); sleep(0.1); } /* Loop forever */ carmen_ipc_dispatch(); return 0; }
////////////////////////////////////////////////////////////////////////// // doHardwareScan // // Checks available COM ports and scans for MotionTrackers int XSensDriver::doHardwareScan( xsens::Cmt3 &cmt3, CmtDeviceId deviceIds[] ) { XsensResultValue res; List<CmtPortInfo> portInfo; unsigned long portCount = 0; int mtCount; std::cout << "Scanning for connected Xsens devices..." << std::endl; // xsens::cmtScanPorts( portInfo ); CmtPortInfo target_port = { 0, 0, 0, "" }; sprintf( target_port.m_portName, "%s", port_.c_str() ); if( cmtScanPort( target_port, target_port.m_baudrate ) ) portInfo.append( target_port ); portCount = portInfo.length(); std::cout << "done" << std::endl; if ( portCount == 0 ) { std::cout << "No MotionTrackers found" << std::endl; return 0; } for ( int i = 0; i < (int) portCount; i++ ) { std::cout << "Using COM port at [ " << portInfo[i].m_portName << " ] "; std::cout << "baud [ "; switch ( portInfo[i].m_baudrate ) { case B9600: std::cout << "9k6"; break; case B19200: std::cout << "19k2"; break; case B38400: std::cout << "38k4"; break; case B57600: std::cout << "57k6"; break; case B115200: std::cout << "115k2"; break; case B230400: std::cout << "230k4"; break; case B460800: std::cout << "460k8"; break; case B921600: std::cout << "921k6"; break; default: std::cout << portInfo[i].m_baudrate; } std::cout << " ]" << std::endl; } std::cout << "Opening ports..."; //open the port which the device is connected to and connect at the device's baudrate. for ( int p = 0; p < (int) portCount; p++ ) { res = cmt3.openPort( portInfo[p].m_portName, portInfo[p].m_baudrate ); // EXIT_ON_ERROR(res,"cmtOpenPort"); } std::cout << "done" << std::endl; //get the Mt sensor count. std::cout << "Retrieving MotionTracker count (excluding attached Xbus Master(s))" << std::endl; mtCount = cmt3.getMtCount(); std::cout << "MotionTracker count: " << mtCount << std::endl; // retrieve the device IDs std::cout << "Retrieving MotionTrackers device ID(s)" << std::endl; for ( int j = 0; j < mtCount; j++ ) { res = cmt3.getDeviceId( (unsigned char) ( j + 1 ), deviceIds[j] ); // EXIT_ON_ERROR(res,"getDeviceId"); std::cout << "Device ID at busId " << j + 1 << "," << (long) deviceIds[j] << std::endl; } return mtCount; }
int main(int argc, char * argv[]) { int l_retMain = 0; XsensResultValue l_res; int l_nDevices = 0; Packet* l_packet = NULL; bool l_reportRaw = false; bool l_outBinary = false; CmtVector l_acc, l_mag, l_rot; CmtShortVector l_accR, l_magR, l_rotR; CmtEuler l_eul; CmtQuat l_quat; CmtRawData l_raw; double l_timeStamp; uint32_t* l_devIDs; int l_packetCnt; // Set exit function atexit(exitFunc); if(argc < 2) { usage(); l_retMain = -1; goto end; } init(argc, argv, &g_dataOptions, &g_processOptions); report_tasks(&g_dataOptions, &g_processOptions); l_outBinary = g_processOptions.get_binaryMode(); l_res = cmt3.openLogFile(g_processOptions.get_infile().c_str()); if(l_res != XRV_OK) { printf("Error opening file '%s'!\n",g_processOptions.get_infile().c_str()); l_retMain = -1; goto end; } if(g_processOptions.get_outfile().length() > 0) { g_outputFile = fopen(g_processOptions.get_outfile().c_str(),"w"); } else { if(l_outBinary) { g_outputFile = fopen("output.bin","w"); } else { g_outputFile = fopen("output.ascii","w"); } } if(g_outputFile == NULL) { printf("Error opening output file '%s'!\n",g_processOptions.get_outfile().c_str()); l_retMain = -1; goto end; } // see how many devices were present.. l_nDevices = cmt3.getMtCount(); // initialize the packet structure l_packet = new Packet((unsigned short)l_nDevices,cmt3.isXm()); // make room for the dev ids l_devIDs = (uint32_t*)calloc(l_nDevices,sizeof(uint32_t)); for(int d = 0; d < l_nDevices; d++) { cmt3.getMtDeviceId(d,l_devIDs[d]); } // check if there is raw data... cmt3.readDataPacket(l_packet); if(g_dataOptions.use_raw && !l_packet->containsRawData()) { warning("[WARNING]: Data does not contain raw data!\n"); g_dataOptions.use_raw = false; } cmt3.resetLogFileReadPos(); write_header(g_outputFile,l_nDevices,l_devIDs,g_dataOptions.get_options(), g_processOptions.get_options()); l_reportRaw = g_dataOptions.use_raw; while(l_res == XRV_OK) { l_res = cmt3.readDataPacket(l_packet); l_packetCnt++; for(int dev = 0; dev < l_nDevices; dev++) { if(l_reportRaw) { l_raw = l_packet->getRawData(); } // report the time stamp if(g_dataOptions.use_tim) { l_timeStamp = (double)l_packet->getRtc(dev); write_1vec(g_outputFile,&l_timeStamp,&g_processOptions); } if(g_dataOptions.use_acc) { l_acc = l_packet->getCalAcc(dev); write_3vec(g_outputFile,l_acc.m_data,&g_processOptions); if(l_reportRaw) { l_accR = l_raw.m_acc; write_3vec(g_outputFile,l_accR.m_data,&g_processOptions); } } if(g_dataOptions.use_gyr) { l_rot = l_packet->getCalGyr(dev); write_3vec(g_outputFile,l_rot.m_data,&g_processOptions); if(l_reportRaw) { l_rotR = l_raw.m_gyr; write_3vec(g_outputFile,l_rotR.m_data,&g_processOptions); } } if(g_dataOptions.use_mag) { l_mag = l_packet->getCalMag(dev); write_3vec(g_outputFile,l_mag.m_data,&g_processOptions); if(l_reportRaw) { l_magR = l_raw.m_mag; write_3vec(g_outputFile,l_mag.m_data,&g_processOptions); } } if(g_dataOptions.use_eul) { l_eul = l_packet->getOriEuler(dev); write_3vec(g_outputFile,l_eul.m_roll, l_eul.m_pitch, l_eul.m_yaw,&g_processOptions); } if(g_dataOptions.use_qua) { l_quat = l_packet->getOriQuat(dev); write_4vec(g_outputFile,l_quat.m_data,&g_processOptions); } } if(l_outBinary == false) { fprintf(g_outputFile,"\n"); } } free(l_devIDs); fclose(g_outputFile); g_outputFile = NULL; end: return l_retMain; }