/*-------------------------------------------------------------
initialize
-------------------------------------------------------------*/
void CIMUXSens_MT4::initialize()
{
#if MRPT_HAS_xSENS_MT4
m_state = ssInitializing;
try
{
// Try to open a specified device, or scan the bus?
XsPortInfoArray portInfoArray;
if (m_portname.empty())
{
if (m_verbose) cout << "[CIMUXSens_MT4] Scanning for USB devices...\n";
xsEnumerateUsbDevices(portInfoArray);
if (portInfoArray.empty())
THROW_EXCEPTION("CIMUXSens_MT4: No 'portname' was specified and no XSens device was found after scanning the system!")
if (m_verbose) cout << "[CIMUXSens_MT4] Found " << portInfoArray.size() <<" devices. Opening the first one.\n";
}
else
{
XsPortInfo portInfo(m_portname, XsBaud::numericToRate(m_port_bauds));
if (m_verbose) cout << "[CIMUXSens_MT4] Using user-supplied portname '"<<m_portname<<"' at "<<m_port_bauds<<" baudrate.\n";
portInfoArray.push_back(portInfo);
}
// Use the first detected device
XsPortInfo mtPort = portInfoArray.at(0);
// Open the port with the detected device
cout << "[CIMUXSens_MT4] Opening port " << mtPort.portName().toStdString() << std::endl;
if (!my_xsens_device.openPort(mtPort))
throw std::runtime_error("Could not open port. Aborting.");
// Put the device in configuration mode
if (m_verbose) cout << "[CIMUXSens_MT4] Putting device into configuration mode...\n";
if (!my_xsens_device.gotoConfig()) // Put the device into configuration mode before configuring the device
throw std::runtime_error("Could not put device into configuration mode. Aborting.");
// Request the device Id to check the device type
mtPort.setDeviceId(my_xsens_device.getDeviceId());
my_xsens_devid = mtPort.deviceId();
// Check if we have an MTi / MTx / MTmk4 device
if (!mtPort.deviceId().isMtix() && !mtPort.deviceId().isMtMk4())
{
throw std::runtime_error("No MTi / MTx / MTmk4 device found. Aborting.");
}
cout << "[CIMUXSens_MT4] Found a device with id: " << mtPort.deviceId().toString().toStdString() << " @ port: " << mtPort.portName().toStdString() << ", baudrate: " << mtPort.baudrate() << std::endl;
// Print information about detected MTi / MTx / MTmk4 device
if (m_verbose) cout << "[CIMUXSens_MT4] Device: " << my_xsens_device.getProductCode().toStdString() << " opened." << std::endl;
// Configure the device. Note the differences between MTix and MTmk4
if (m_verbose) cout << "[CIMUXSens_MT4] Configuring the device..." << std::endl;
if (mtPort.deviceId().isMtix())
{
XsOutputMode outputMode = XOM_Orientation; // output orientation data
XsOutputSettings outputSettings = XOS_OrientationMode_Euler | XOS_Timestamp_PacketCounter | XOS_CalibratedMode_All; // XOS_OrientationMode_Quaternion; // output orientation data as quaternion
// set the device configuration
if (!my_xsens_device.setDeviceMode(outputMode, outputSettings))
throw std::runtime_error("Could not configure MT device. Aborting.");
}
else if (mtPort.deviceId().isMtMk4())
{
XsOutputConfigurationArray configArray;
configArray.push_back( XsOutputConfiguration(XDI_SampleTime64,m_sampleFreq) );
configArray.push_back( XsOutputConfiguration(XDI_SampleTimeFine,m_sampleFreq) );
configArray.push_back( XsOutputConfiguration(XDI_SampleTimeCoarse,m_sampleFreq) );
configArray.push_back( XsOutputConfiguration(XDI_Quaternion,m_sampleFreq) );
configArray.push_back( XsOutputConfiguration(XDI_Temperature,m_sampleFreq) );
configArray.push_back( XsOutputConfiguration(XDI_Acceleration,m_sampleFreq) );
configArray.push_back( XsOutputConfiguration(XDI_RateOfTurn,m_sampleFreq) );
configArray.push_back( XsOutputConfiguration(XDI_MagneticField,m_sampleFreq) );
configArray.push_back( XsOutputConfiguration(XDI_VelocityXYZ,m_sampleFreq) );
configArray.push_back( XsOutputConfiguration(XDI_StatusByte, m_sampleFreq) );
configArray.push_back( XsOutputConfiguration(XDI_LatLon, m_sampleFreq) );
configArray.push_back( XsOutputConfiguration(XDI_UtcTime, m_sampleFreq) );
configArray.push_back( XsOutputConfiguration(XDI_AltitudeEllipsoid, m_sampleFreq) );
if (!my_xsens_device.setOutputConfiguration(configArray))
throw std::runtime_error("Could not configure MTmk4 device. Aborting.");
}
else
{
throw std::runtime_error("Unknown device while configuring. Aborting.");
}
// Put the device in measurement mode
if (m_verbose) cout << "[CIMUXSens_MT4] Putting device into measurement mode..." << std::endl;
if (!my_xsens_device.gotoMeasurement())
throw std::runtime_error("Could not put device into measurement mode. Aborting.");
m_state = ssWorking;
}
void IMU::Initiate()
{
XsPortInfoArray portInfoArray;
xsEnumerateUsbDevices(portInfoArray);
if (!portInfoArray.size())
{
#ifdef PLATFORM_IS_WINDOWS
throw std::runtime_error("IMU: failed to find IMU sensor");
#endif
#ifdef PLATFORM_IS_LINUX
XsPortInfo portInfo(pDevice->port, XsBaud::numericToRate(pDevice->baudRate));
portInfoArray.push_back(portInfo);
#endif
}
pDevice->mtPort = portInfoArray.at(0);
// Open the port with the detected device
if (!pDevice->openPort(pDevice->mtPort))
throw std::runtime_error("IMU: could not open port.");
Aris::Core::Sleep(10);
// Put the device in configuration mode
if (!pDevice->gotoConfig()) // Put the device into configuration mode before configuring the device
{
throw std::runtime_error("IMU: could not put device into configuration mode");
}
// Request the device Id to check the device type
pDevice->mtPort.setDeviceId(pDevice->getDeviceId());
// Check if we have an MTi / MTx / MTmk4 device
if (!pDevice->mtPort.deviceId().isMtMk4())
{
throw std::runtime_error("IMU: No MTi / MTx / MTmk4 device found.");
}
// Check device type
if (pDevice->mtPort.deviceId().isMtMk4())
{
XsOutputConfiguration config0(XDI_Quaternion, pDevice->sampleRate);
XsOutputConfiguration config1(XDI_DeltaQ, pDevice->sampleRate);
XsOutputConfiguration config2(XDI_DeltaV, pDevice->sampleRate);
XsOutputConfiguration config3(XDI_Acceleration, pDevice->sampleRate);
XsOutputConfigurationArray configArray;
configArray.push_back(config0);
configArray.push_back(config1);
configArray.push_back(config2);
configArray.push_back(config3);
if (!pDevice->setOutputConfiguration(configArray))
{
throw std::runtime_error("IMU: Could not configure MTmk4 pDevice-> Aborting.");
}
}
else
{
throw std::runtime_error("IMU: Unknown device while configuring. Aborting.");
}
// Put the device in measurement mode
if (!pDevice->gotoMeasurement())
{
throw std::runtime_error("IMU: Could not put device into measurement mode. Aborting.");
}
}
