/**
* @brief Prints the active settings.
* @details Reads the settings currently stored in the device
* and prints it in ROS debug stream.
*/
void mtiG::printSettings(){
XsOutputConfigurationArray deviceConfig = device->outputConfiguration();
//Print settings from the device
for(int i =0; i < deviceConfig.size() ; i++)
{
ROS_DEBUG("%.8x; Freq = %d", deviceConfig[i].m_dataIdentifier, deviceConfig[i].m_frequency);
}
//Print the settings stored in mSettings
ROS_DEBUG_STREAM(
"\nOrientation = " << mSettings.orientationData <<
"\nGPS = " << mSettings.gpsData <<
"\nTemperature = " << mSettings.temperatureData <<
"\nAcceleration = " << mSettings.accelerationData <<
"\nPressure = " << mSettings.pressureData <<
"\nMagnetic = " << mSettings.magneticData <<
"\nAltitude = " << mSettings.altitudeData <<
"\nGyroscope = " << mSettings.gyroscopeData <<
"\nVelocity = " << mSettings.velocityData <<
"\nOrientation Frequency:" << mSettings.orientationFreq <<
"\nGPS Frequency:" << mSettings.gpsFreq <<
"\nTemperature Frequency:" << mSettings.temperatureFreq <<
"\nAcceleration Frequency:" << mSettings.accelerationFreq <<
"\nPressure Frequency:" << mSettings.pressureFreq <<
"\nMagnetic Frequency:" << mSettings.magneticFreq <<
"\nAltitude Frequency:" << mSettings.altitudeFreq <<
"\nGyroscope Frequency:" << mSettings.gyroscopeFreq <<
"\nVelocity Frequency:" << mSettings.velocityFreq
);
}
/*! \brief Set the output configuration of a device
\param config An array XsOutputConfigurationArray) containing the one or multiple XsOutputConfigurations
\return True when successful
*/
bool setOutputConfiguration(XsOutputConfigurationArray& config)
{
XsMessage snd(XMID_SetOutputConfiguration, 4), rcv;
if (config.size() == 0)
{
snd.setDataShort((uint16_t)XDI_None, 0);
snd.setDataShort(0, 2);
}
else
{
for (XsSize i = 0; i < (XsSize) config.size(); ++i)
{
snd.setDataShort((uint16_t)config[i].m_dataIdentifier, i*4);
snd.setDataShort(config[i].m_frequency, i*4+2);
}
}
writeMessage(snd);
return waitForMessage(XMID_SetOutputConfigurationAck, rcv);
}
/*-------------------------------------------------------------
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;
}
bool XSensINS::OnStartUp() {
AppCastingMOOSApp::OnStartUp();
// XSens Configuration Array
XsOutputConfigurationArray configArray;
// MOOS parser
STRING_LIST sParams;
m_MissionReader.EnableVerbatimQuoting(false);
if (!m_MissionReader.GetConfiguration(GetAppName(), sParams))
reportConfigWarning("No config block found for " + GetAppName());
STRING_LIST::iterator p;
sParams.reverse();
for (p = sParams.begin() ; p != sParams.end() ; p++) {
string orig = *p;
string line = *p;
string param = toupper(biteStringX(line, '='));
string value = line;
bool handled = false;
if (param == "UART_BAUD_RATE") {
m_uart_baud_rate = atoi(value.c_str());
handled = true;
}
else if (param == "SERIAL_PORT") {
m_uart_port = value;
handled = true;
}
else if (param == "XDI_EULERANGLES"){
XsOutputConfiguration config(XDI_EulerAngles, atoi(value.c_str()));
configArray.push_back(config);
handled = true;
}
else if (param == "XDI_ACCELERATION"){
XsOutputConfiguration config(XDI_Acceleration, atoi(value.c_str()));
configArray.push_back(config);
handled = true;
}
else if (param == "XDI_RATEOFTURN"){
XsOutputConfiguration config(XDI_RateOfTurn, atoi(value.c_str()));
configArray.push_back(config);
handled = true;
}
else if (param == "XDI_MAGNETICFIELD"){
XsOutputConfiguration config(XDI_MagneticField, atoi(value.c_str()));
configArray.push_back(config);
handled = true;
}
else if (param == "XDI_LATLON"){
XsOutputConfiguration config(XDI_LatLon, atoi(value.c_str()));
configArray.push_back(config);
handled = true;
}
else if (param == "XDI_VELOCITYXYZ"){
XsOutputConfiguration config(XDI_VelocityXYZ, atoi(value.c_str()));
configArray.push_back(config);
handled = true;
}
if(!handled)
reportUnhandledConfigWarning(orig);
}
registerVariables();
//------ OPEN INS ---------------//
XsPortInfo mtPort(m_uart_port, XsBaud::numericToRate(m_uart_baud_rate));
if (!m_device.openPort(mtPort)) {
cout << "CANNOT OPEN THE PORT : " << m_uart_port << '\n';
reportRunWarning("Could not open the COM port" + m_uart_port);
}
//------ CONFIGURE INS ---------------//
// Put the device into configuration mode before configuring the device
if (!m_device.gotoConfig()) {
reportRunWarning("Could not begin the config mode");
}
// Save INS Config
if (!m_device.setOutputConfiguration(configArray)) {
reportRunWarning("Could not save config");
}
//------ START INS ---------------//
if (!m_device.gotoMeasurement()) {
reportRunWarning("Could not start the INS");
}
return true;
}
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.");
}
}
bool XSensINS::OnStartUp() {
AppCastingMOOSApp::OnStartUp();
STRING_LIST sParams;
m_MissionReader.EnableVerbatimQuoting(false);
if (!m_MissionReader.GetValue("XSENSINS_SERIAL_PORT",m_uart_port))
reportConfigWarning("No XSENSINS_SERIAL_PORT config found for " + GetAppName());
if (!m_MissionReader.GetConfiguration(GetAppName(), sParams))
reportConfigWarning("No config block found for " + GetAppName());
STRING_LIST::iterator p;
sParams.reverse();
for (p = sParams.begin() ; p != sParams.end() ; p++) {
string orig = *p;
string line = *p;
string param = toupper(biteStringX(line, '='));
string value = line;
bool handled = false;
if (param == "m_uart_baud_rate") {
m_uart_baud_rate = atoi(value.c_str());
handled = true;
}
else if (param == "YAW_DECLINATION") {
m_yaw_declination = atoi(value.c_str());
handled = true;
}
if(!handled)
reportUnhandledConfigWarning(orig);
}
registerVariables();
//------ OPEN INS ---------------//
XsPortInfo mtPort("/dev/xsens", XsBaud::numericToRate(115200));
if (!m_device.openPort(mtPort)) {
cout << "CANNOT OPEN THE PORT" << '\n';
reportRunWarning("Could not open the COM port" + m_uart_port);
}
//------ CONFIGURE INS ---------------//
// Put the device into configuration mode before configuring the device
if (!m_device.gotoConfig()) {
reportRunWarning("Could not begin the config mode");
}
XsOutputConfiguration euler(XDI_EulerAngles, 25);
XsOutputConfiguration acceleration(XDI_Acceleration, 25);
XsOutputConfiguration rateOfTurn(XDI_RateOfTurn, 25);
XsOutputConfiguration magnetic(XDI_MagneticField, 25);
XsOutputConfiguration latlon(XDI_LatLon, 25);
XsOutputConfigurationArray configArray;
configArray.push_back(euler);
configArray.push_back(acceleration);
configArray.push_back(rateOfTurn);
configArray.push_back(magnetic);
configArray.push_back(latlon);
// Save INS Config
if (!m_device.setOutputConfiguration(configArray)) {
reportRunWarning("Could not save config");
}
//------ START INS ---------------//
if (!m_device.gotoMeasurement()) {
reportRunWarning("Could not start the INS");
}
return true;
}
/**
* @brief Creates an XsOutputConfigurationArray and pushes it to the sensor.
* @details
* - Configures the sensor with desired modules
* - Refer to xsdataidentifier.h
*/
void mtiG::configure(){
XsOutputConfigurationArray configArray;
if(mSettings.orientationData){ //Quaternion - containsOrientation
XsOutputConfiguration quat(XDI_Quaternion, mSettings.orientationFreq);// para pedir quaternion
configArray.push_back(quat);
}
if(mSettings.gpsData){
//LATITUDE E LONGITUDE -containsLatitudeLongitude
XsOutputConfiguration gps(XDI_LatLon, mSettings.gpsFreq);// para pedir gps, //XDI_Quaternion 06/04
configArray.push_back(gps);
XsOutputConfiguration gps_age(XDI_GpsAge, mSettings.gpsFreq);// para pedir gps, //XDI_Quaternion 06/04
configArray.push_back(gps_age);
XsOutputConfiguration gps_sol(XDI_GpsSol, mSettings.gpsFreq);// para pedir gps, //XDI_Quaternion 06/04
configArray.push_back(gps_sol);
XsOutputConfiguration gps_dop(XDI_GpsDop, mSettings.gpsFreq);// para pedir gps, //XDI_Quaternion 06/04
configArray.push_back(gps_dop);
}
if(mSettings.temperatureData){
//TEMPERATURA - containsTemperature
XsOutputConfiguration temp(XDI_Temperature, mSettings.temperatureFreq);
configArray.push_back(temp);
}
if(mSettings.accelerationData){
//ACCELERATION - containsCalibratedAcceleration
XsOutputConfiguration accel(XDI_Acceleration, mSettings.accelerationFreq);
configArray.push_back(accel);
}
if(mSettings.pressureData){
//PRESSURE - containsPressure
XsOutputConfiguration baro(XDI_BaroPressure, mSettings.pressureFreq);
configArray.push_back(baro);
}
if(mSettings.magneticData){
//MAGNETIC FIELD - containsCalibratedMagneticField
XsOutputConfiguration magnet(XDI_MagneticField, mSettings.magneticFreq);
configArray.push_back(magnet);
}
if(mSettings.altitudeData){
//ALTITUDE - containsAltitude
XsOutputConfiguration alt(XDI_AltitudeEllipsoid, mSettings.altitudeFreq);
configArray.push_back(alt);
}
if(mSettings.gyroscopeData){
//GYRO - containsCalibratedGyroscopeData
XsOutputConfiguration gyro(XDI_RateOfTurn, mSettings.gyroscopeFreq);
configArray.push_back(gyro);
}
if(mSettings.velocityData){
//VELOCIDADE XYZ
XsOutputConfiguration vel_xyz(XDI_VelocityXYZ, mSettings.velocityFreq);
configArray.push_back(vel_xyz);
}
// Puts configArray into the device, overwriting the current configuration
if (!device->setOutputConfiguration(configArray))
{
throw std::runtime_error("Could not configure MTmk4 device. Aborting.");
}
}
/**
* @brief Pulls the active configuration from the device.
* @details Fills mSettings from XsOutputConfigurationArray, the current
* active settings in the Xsens.
*/
void mtiG::readSettings(){
//Gets outputConfiguration from the device
XsOutputConfigurationArray deviceConfig = device->outputConfiguration();
//default initialization
mSettings.orientationData = false;
mSettings.gpsData = false;
mSettings.temperatureData = false;
mSettings.accelerationData = false;
mSettings.pressureData = false;
mSettings.magneticData = false;
mSettings.altitudeData = false;
mSettings.gyroscopeData = false;
mSettings.velocityData = false;
for(int i =0; i < deviceConfig.size() ; i++)
{
//ROS_DEBUG("%.8x", deviceConfig[i].m_dataIdentifier);
switch(deviceConfig[i].m_dataIdentifier){
// ORIENTATION
case (XDI_Quaternion):
mSettings.orientationData = true;
break;
// GPS
case (XDI_LatLon):
case (XDI_GpsAge):
case (XDI_GpsDop):
case (XDI_GpsSol):
ROS_DEBUG("GPS ENABLED IN CURRENT CONFIGURATION");
mSettings.gpsData=true;
break;
// TEMPERATURE
case (XDI_Temperature):
mSettings.temperatureData=true;
break;
// ACCELERATION
case (XDI_Acceleration):
mSettings.accelerationData=true;
break;
// PRESSURE
case (XDI_BaroPressure):
mSettings.pressureData=true;
break;
// MAGNETIC
case (XDI_MagneticField):
mSettings.magneticData=true;
break;
// ALTITUDE
case (XDI_AltitudeEllipsoid):
mSettings.altitudeData=true;
break;
// GYROSCOPE
case (XDI_RateOfTurn):
mSettings.gyroscopeData=true;
break;
// VELOCITY
case (XDI_VelocityXYZ):
mSettings.velocityData=true;
break;
}
}
}
void ofxXsens::connect()
{
try {
// Scan for connected devices
ofLogNotice("ofxXsens") << "Scanning for devices...";
XsPortInfoArray portInfoArray = XsScanner::scanPorts();
// Find an MTi / MTx / MTmk4 device
mtPort = portInfoArray.begin();
while (mtPort != portInfoArray.end() && !mtPort->deviceId().isMtix() && !mtPort->deviceId().isMtMk4()) {++mtPort;}
if (mtPort == portInfoArray.end())
{
throw std::runtime_error("No MTi / MTx / MTmk4 device found. Aborting.");
}
ofLogNotice("ofXsens") << "Found a device with id: " << mtPort->deviceId().toString().toStdString() << " @ port: " << mtPort->portName().toStdString() << ", baudrate: " << mtPort->baudrate();
// Open the port with the detected device
ofLogNotice("ofxXsens") << "Opening port..." << std::endl;
if (!control->openPort(mtPort->portName().toStdString(), mtPort->baudrate()))
{
throw std::runtime_error("Could not open port. Aborting.");
}
try
{
// Get the device object
XsDevice* device = control->device(mtPort->deviceId());
assert(device != 0);
// Print information about detected MTi / MTx / MTmk4 device
ofLogNotice("ofxXsens") << "Device: " << device->productCode().toStdString() << " opened." << std::endl;
//Attach callback handler to device
device->addCallbackHandler(&callback);
// Put the device in configuration mode
ofLogNotice("ofxXsens") << "Putting device into configuration mode..." << std::endl;
if (!device->gotoConfig()) // Put the device into configuration mode before configuring the device
{
throw std::runtime_error("Could not put device into configuration mode. Aborting.");
}
// Configure the device. Note the differences between MTix and MTmk4
ofLogNotice("ofxXsens") << "Configuring the device..." << std::endl;
if (device->deviceId().isMtix())
{
ofLogNotice("ofxXsens") << "isMtix..." << std::endl;
XsOutputMode outputMode = XOM_Orientation; // output orientation data
XsOutputSettings outputSettings = XOS_OrientationMode_Quaternion; // output orientation data as quaternion
XsDeviceMode deviceMode(100); // make a device mode with update rate: 100 Hz
deviceMode.setModeFlag(outputMode);
deviceMode.setSettingsFlag(outputSettings);
// set the device configuration
if (!device->setDeviceMode(deviceMode))
{
throw std::runtime_error("Could not configure MTmki device. Aborting.");
}
bSensorConnected = true;
}
else if (device->deviceId().isMtMk4())
{
ofLogNotice("ofxXsens") << "isMtMk4..." << std::endl;
XsOutputConfiguration quat(XDI_Quaternion, 0);
//XsOutputConfiguration gpssol(XDI_GpsSol, 0);
XsOutputConfiguration latlong(XDI_LatLon, 0);
XsOutputConfigurationArray configArray;
configArray.push_back(quat);
//configArray.push_back(gpssol);
configArray.push_back(latlong);
if (!device->setOutputConfiguration(configArray))
{
throw std::runtime_error("Could not configure MTmk4 device. Aborting.");
}
bSensorConnected = true;
}
else
{
throw std::runtime_error("Unknown device while configuring. Aborting.");
}
// Put the device in measurement mode
std::cout << "Putting device into measurement mode..." << std::endl;
if (!device->gotoMeasurement())
{
throw std::runtime_error("Could not put device into measurement mode. Aborting.");
}
}
catch (std::runtime_error const & error)
{
std::cout << error.what() << std::endl;
}
catch (...)
{
ofLogError("ofxXsens") << "An unknown fatal error has occured. Aborting." << std::endl;
}
}
catch (runtime_error const & error)
{
ofLogError("ofxXsens") << error.what();
}
catch (...)
{
ofLogError("ofxXsens") << "An unknown fatal error has occured.";
}
}