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