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