Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add parameter period to set the output frequency of XSens device #34

Merged
merged 5 commits into from
Sep 28, 2022
Merged

Add parameter period to set the output frequency of XSens device #34

Show file tree
Hide file tree
Changes from 4 commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
192e57a
add param period
HosameldinMohamed May 25, 2022
69cedec
change fine to asString
HosameldinMohamed May 25, 2022
9904541
test which data type is found
HosameldinMohamed May 26, 2022
5fa0899
period check isInt or isFloat
HosameldinMohamed May 26, 2022
28bdd1b
set period in seconds + add small description
HosameldinMohamed Sep 27, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
26 changes: 21 additions & 5 deletions xsensmt/XsensMT.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -108,13 +108,29 @@ bool XsensMT::open(yarp::os::Searchable &config)
yWarning() << "xsensmt - Parameter \"frame_name\" not set. Using the same value as \"sensor_name\" for this parameter.";
}

yDebug() << "xsensmt - test - xsensmt_period, check xsensmt_period: " << config.check("xsensmt_period");
yDebug() << "xsensmt - test - xsensmt_period, xsensmt_period is string: " << config.find("xsensmt_period").isString();
yDebug() << "xsensmt - test - xsensmt_period, xsensmt_period is int: " << config.find("xsensmt_period").isInt32();
yDebug() << "xsensmt - test - xsensmt_period, xsensmt_period is float: " << config.find("xsensmt_period").isFloat64();

if (config.check("xsensmt_period") && ( config.find("xsensmt_period").isInt32() || config.find("xsensmt_period").isFloat64()))
traversaro marked this conversation as resolved.
Show resolved Hide resolved
{
m_outputPeriod = config.find("xsensmt_period").asFloat64();
traversaro marked this conversation as resolved.
Show resolved Hide resolved
}
else
{
m_outputPeriod = 10; // 10ms
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The latest style for most YARP devices is to use seconds to represent periods, see for example https://www.yarp.it//v3.5/classControlBoard__nws__yarp.html#details . Even if some icub devices still use milliseconds, I would try to be consistent with the new devices and use seconds here.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Ok now the expected period is in seconds 28bdd1b

yWarning() << "xsensmt - Parameter \"xsensmt_period\" not set. Using the value " << m_outputPeriod << " ms for this parameter.";
}
m_outputFrequency = 1/m_outputPeriod * 1000;

std::string comPortString = config.check("serial", yarp::os::Value("/dev/ttyUSB0"), "File of the serial device.").asString().c_str();
int baudRate = config.check("baud", yarp::os::Value(115200), "Baud rate used by the serial communication.").asInt32();
m_timeoutInSecond = config.check("timeout", yarp::os::Value(0.1), "Timeout of the driver").asFloat64();

m_portInfo = XsPortInfo(comPortString, XsBaud::numericToRate(baudRate));

yInfo("xsensmt: Opening serial port %s with baud rate %d.", comPortString.c_str(), baudRate);
yInfo("xsensmt: Opening serial port %s with baud rate %d and output period %4.2f ms.", comPortString.c_str(), baudRate, m_outputPeriod);
if (!m_xsensDevice.openPort(m_portInfo))
{
yError("xsensmt: Could not open serial port.");
Expand Down Expand Up @@ -151,10 +167,10 @@ bool XsensMT::open(yarp::os::Searchable &config)
}
else if (m_portInfo.deviceId().isMtMk4() || m_portInfo.deviceId().isFmt_X000())
{
XsOutputConfiguration euler(XDI_EulerAngles, 100);
XsOutputConfiguration acc(XDI_Acceleration, 100);
XsOutputConfiguration gyro(XDI_RateOfTurn, 100);
XsOutputConfiguration mag(XDI_MagneticField, 100);
XsOutputConfiguration euler(XDI_EulerAngles, m_outputFrequency);
XsOutputConfiguration acc(XDI_Acceleration, m_outputFrequency);
XsOutputConfiguration gyro(XDI_RateOfTurn, m_outputFrequency);
XsOutputConfiguration mag(XDI_MagneticField, m_outputFrequency);

XsOutputConfigurationArray configArray;
configArray.push_back(euler);
Expand Down
2 changes: 2 additions & 0 deletions xsensmt/XsensMT.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -268,6 +268,8 @@ class yarp::dev::XsensMT : public yarp::dev::IGenericSensor,

std::string m_sensorName;
std::string m_frameName;
double m_outputPeriod;
double m_outputFrequency;

// Send a SetFilterProfile message to set the filter profile
bool setFilterProfile(const uint16_t profile);
Expand Down