int main(void)
{
Cmt3 serial;
Packet reply(1,0); /* 1 item, not xbus */
(void) signal(SIGINT, ctrlchandler);
atexit(exitFunc);
printf( "This example will connect to the MT, configure all sensors for euler output at 100Hz and\n"
"read data messages until Control-C is pressed\n");
char portname[32];
printf("Enter the port name you wish to connect to (default: /dev/ttyUSB0): ");
scanf("%s", portname);
if (strlen(portname) == 0)
sprintf(portname, "/dev/ttyUSB0");
if (serial.openPort(portname, B115200) != XRV_OK)
EXIT_ERROR("open");
printf("MT now in config mode\n");
CmtDeviceMode mode(CMT_OUTPUTMODE_ORIENT, CMT_OUTPUTSETTINGS_TIMESTAMP_SAMPLECNT | CMT_OUTPUTSETTINGS_ORIENTMODE_EULER, 100);
if (serial.setDeviceMode(mode, false, CMT_DID_BROADCAST))
EXIT_ERROR("set device mode");
printf("Device modes set\n");
if (serial.gotoMeasurement())
EXIT_ERROR("goto measurement");
printf("Now in measurement mode\n");
long msgCount = 0;
while (!quit)
{
if (serial.waitForDataMessage(&reply) != XRV_OK)
{
printf("\n");
EXIT_ERROR("read data message");
}
msgCount++;
printf("%3ld SC: %hu TOA: %llu TS: %u Roll %+6.1f Pitch %+6.1f Yaw %+6.1f \r",
msgCount,
reply.getSampleCounter(), // sample counter
reply.m_rtc, // Time Of Arrival / Real-Time Clock
getTimeOfDay(), // timestamp
(double) reply.getOriEuler().m_roll, // roll
(double) reply.getOriEuler().m_pitch, // pitch
(double) reply.getOriEuler().m_yaw // yaw
);
}
printf("\n\nDone reading\n");
return 0;
}
bool cmtScanPort(CmtPortInfo& portInfo, uint32_t baud, uint32_t singleScanTimeout, uint32_t scanTries)
{
uint32_t baudrate;
Cmt3 port;
port.setGotoConfigTries(scanTries?(uint16_t)scanTries:1);
port.setTimeoutConfig(singleScanTimeout);
XsensResultValue res;
if (baud == 0)
baudrate = CMT_BAUD_RATE_115K2;
else
baudrate = baud;
while(!abortScan)
{
// try to connect at current baudrate
#ifdef _WIN32
if ((res = port.openPort(portInfo.m_portNr,baudrate)) == XRV_OK)
#else
if ((res = port.openPort(portInfo.m_portName,baudrate)) == XRV_OK)
#endif
{
SCANLOG("SP: L3 port-check returns OK\n");
portInfo.m_baudrate = baudrate;
portInfo.m_deviceId = port.getMasterId();
return true; // this also closes the port
}
// failed, determine if we need to scan other baudrates or not
if (res != XRV_TIMEOUT && res != XRV_TIMEOUTNODATA && res != XRV_CONFIGCHECKFAIL)
{
SCANLOG("SP: L3 port-check returned ERROR, aborting\n");
return false;
}
SCANLOG("SP: L3 port-check returned TIMEOUT, check next baudrate or abort\n");
// not detected, try next baudrate
if (baud != 0)
return false;
switch(baudrate)
{
default:
case CMT_BAUD_RATE_115K2:
baudrate = CMT_BAUD_RATE_460K8; break;
case CMT_BAUD_RATE_460K8:
baudrate = CMT_BAUD_RATE_921K6; break;
case CMT_BAUD_RATE_921K6:
baudrate = CMT_BAUD_RATE_230K4; break;
case CMT_BAUD_RATE_230K4:
baudrate = CMT_BAUD_RATE_57K6; break;
case CMT_BAUD_RATE_57K6:
baudrate = CMT_BAUD_RATE_38K4; break;
case CMT_BAUD_RATE_38K4:
baudrate = CMT_BAUD_RATE_19K2; break;
case CMT_BAUD_RATE_19K2:
baudrate = CMT_BAUD_RATE_9600; break;
case CMT_BAUD_RATE_9600:
return false; // could not detect Xsens sensor, return false
}
}
return false;
}
int main(int argc, char* argv[])
{
Cmt3 serial;
Packet reply(1,0); /* 1 item, not xbus */
(void) signal(SIGINT, ctrlchandler);
atexit(exitFunc);
char portname[32];
if(argc > 1)
strcpy(portname, argv[1]);
if (strlen(portname) == 0)
sprintf(portname, "/dev/ttyUSB0");
if (serial.openPort(portname, B115200) != XRV_OK)
EXIT_ERROR("open");
printf("MT now in config mode\n");
//set the measurement timeout to 100ms (default is 16ms)
int timeOut = 100;
if (serial.setTimeoutMeasurement(timeOut) != XRV_OK)
EXIT_ERROR("set timeout");
printf("Measurement timeout set to %d ms\n", timeOut);
CmtDeviceMode mode(CMT_OUTPUTMODE_ORIENT | CMT_OUTPUTMODE_CALIB, CMT_OUTPUTSETTINGS_TIMESTAMP_SAMPLECNT | CMT_OUTPUTSETTINGS_COORDINATES_NED | CMT_OUTPUTSETTINGS_ORIENTMODE_MATRIX | CMT_OUTPUTSETTINGS_CALIBMODE_ACCGYR, 100);
if (serial.setDeviceMode(mode, false, CMT_DID_BROADCAST))
EXIT_ERROR("set device mode");
printf("Device modes set\n");
if (serial.gotoMeasurement())
EXIT_ERROR("goto measurement");
printf("Now in measurement mode\n");
FILE *fp;
if(argc > 2) { fp = fopen(argv[2],"w"); }
else { fp = fopen("./data.imu","w"); }
if (fp == NULL) {
perror("Failed to open file!");
return EXIT_FAILURE;
}
long msgCount = 0;
struct timeval lasttime, newtime;
gettimeofday(&lasttime,NULL);
gettimeofday(&newtime,NULL);
double xpos = 0;
double xvel = 0;
CmtMatrix matrix_data;
while (!quit)
{
if (serial.waitForDataMessage(&reply) != XRV_OK)
{
printf("\n");
EXIT_ERROR("read data message");
}
msgCount++;
matrix_data = reply.getOriMatrix();
// long double cos_roll = cosl((long double)reply.getOriEuler().m_roll*2*M_PI/360);
// long double cos_pitch = cosl((long double)reply.getOriEuler().m_pitch*2*M_PI/360);
// long double sin_roll = sinl((long double)reply.getOriEuler().m_roll*2*M_PI/360);
// long double sin_pitch = sinl((long double)reply.getOriEuler().m_pitch*2*M_PI/360);
long double xacc = (long double) reply.getCalData().m_acc.m_data[0];
long double yacc = (long double) reply.getCalData().m_acc.m_data[1];
long double zacc = (long double) reply.getCalData().m_acc.m_data[2];
long double xacc_comp = (long double) matrix_data.m_data[0][0]*xacc + matrix_data.m_data[1][0]*yacc + matrix_data.m_data[2][0]*zacc;
long double yacc_comp = (long double) matrix_data.m_data[0][1]*xacc + matrix_data.m_data[1][1]*yacc + matrix_data.m_data[2][1]*zacc;
long double zacc_comp = (long double) matrix_data.m_data[0][2]*xacc + matrix_data.m_data[1][2]*yacc + matrix_data.m_data[2][2]*zacc;
long double yaw;
if(matrix_data.m_data[0][1] < 0 && matrix_data.m_data[1][1] < 0) { yaw = 360*asinl(matrix_data.m_data[1][0])/2/M_PI; }
else if(matrix_data.m_data[1][1] > 0) { yaw = 180 - 360*asinl(matrix_data.m_data[1][0])/2/M_PI; }
else { yaw = 360 + 360*asinl(matrix_data.m_data[1][0])/2/M_PI; }
// long double xacc_comp = (long double) cos_pitch*xacc + sin_pitch*zacc;
// long double yacc_comp = (long double) sin_pitch*sin_roll*xacc + cos_roll*yacc - sin_roll*cos_pitch*zacc;
// long double zacc_comp = (long double) -cos_roll*sin_pitch*xacc + sin_roll*yacc + cos_roll*cos_pitch*zacc;
// fprintf(fp,"%3ld,%hu,%llu,%u,%f,%f,%f,%f,%f,%f\n",
printf("%3ld \t %hu \t %lu \t %u \t %f %f Yaw %Lf \t x %Lf \t y%Lf \t z %Lf \t x' %Lf \t y' %Lf \t z' %Lf %f \t %f\n",
msgCount,
reply.getSampleCounter(), // sample counter
reply.m_rtc, // Time Of Arrival / Real-Time Clock
getTimeOfDay(), // timestamp
matrix_data.m_data[1][1],matrix_data.m_data[0][1],
yaw , // yaw ps
xacc ,
yacc ,
zacc ,
xacc_comp ,
yacc_comp ,
zacc_comp,
xvel, xpos
);
xvel += xacc*10/1000;
xpos += xvel*10/1000;
//gettimeofday(&newtime,NULL);
//printf("Time %f\n",(double)(newtime.tv_usec - lasttime.tv_usec)/1000);
//lasttime = newtime;
}
fclose(fp);
printf("\n\nDone reading\n");
return 0;
}
