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; }
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; }