//////////////////////////////////////////////////////////////////////////
// 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();
}
//////////////////////////////////////////////////////////////////////////
// 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");
}
