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