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