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