Skip to content

Commit

Permalink
Update device to 2020.0.2
Browse files Browse the repository at this point in the history 
The API changed a lot from 4.8.2 to 2020.0.2 .
The recommended API switched to callback based one,
but for the time being we kept most of the original logic and
just got the device to compile.

The device has not been tested.
  • Loading branch information
@traversaro
traversaro committed Dec 21, 2020
1 parent 2410691 commit f03045c
Show file tree
Hide file tree
Showing 3 changed files with 79 additions and 43 deletions.
29 changes: 22 additions & 7 deletions extern/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,17 +6,32 @@
# in the MT Software Suite, version 2020.0.2, available from https://www.xsens.com/mt-software-suite/
# If you want to update the code to a new version of the MT Software Suite, run make in the mtsdk
# example directory and copy the xspublic files from the public directory in this directory.
# Furthermore, you should update the CMake scripts options by inspecting the xspublic's Makefiles

# Globbing should be avoided in general, but this is a library that we use directly as provided by Xsens
file(GLOB xscommon_SRCS ${CMAKE_CURRENT_SOURCE_DIR}/xspublic/xscommon/*.cpp ${CMAKE_CURRENT_SOURCE_DIR}/xspublic/xscommon/*.c)
add_library(xscommon STATIC ${xscommon_SRCS})
target_include_directories(xscommon PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/xspublic)
target_include_directories(xscommon PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/xspublic/xscommon)
target_compile_options(xscommon PRIVATE "SHELL:-include xscommon_config.h" "SHELL:-include xstypes/xsens_compat.h")


file(GLOB xstypes_SRCS ${CMAKE_CURRENT_SOURCE_DIR}/xspublic/xstypes/*.cpp ${CMAKE_CURRENT_SOURCE_DIR}/xspublic/xstypes/*.c)
# See EXCLUDE argument in xstypes' Makefile
list(FILTER xstypes_SRCS EXCLUDE REGEX "xstypesdynlib")
add_library(xstypes STATIC ${xstypes_SRCS})
target_include_directories(xstypes PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/xspublic)
target_include_directories(xstypes PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/xspublic/xstypes)


file(GLOB xscontroller_SRCS ${CMAKE_CURRENT_SOURCE_DIR}/xspublic/xscontroller/*.cpp ${CMAKE_CURRENT_SOURCE_DIR}/xspublic/xscontroller/*.c)
add_library(xscontroller STATIC ${xscontroller_SRCS})
target_include_directories(xscontroller PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/xspublic)
target_include_directories(xscontroller PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/xspublic/xscontroller)
target_compile_options(xscontroller PRIVATE -include xscontrollerconfig.h)
target_compile_definitions(xscontroller PRIVATE -DHAVE_JOURNALLER)

add_library(xsens_mt_software_suite STATIC ${xscommon_SRCS} ${xstypes_SRCS} ${xscontroller_SRCS})
target_include_directories(xsens_mt_software_suite PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}
${CMAKE_CURRENT_SOURCE_DIR}/xspublic
${CMAKE_CURRENT_SOURCE_DIR}/xspublic/xscommon
${CMAKE_CURRENT_SOURCE_DIR}/xspublic/xscontroller
${CMAKE_CURRENT_SOURCE_DIR}/xspublic/xstypes)
find_package (Threads)
target_link_libraries(xsens_mt_software_suite)

add_library(xsens_mt_software_suite INTERFACE)
target_link_libraries(xsens_mt_software_suite INTERFACE xscommon xstypes xscontroller)
63 changes: 29 additions & 34 deletions xsensmt/XsensMT.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,13 +17,14 @@
#include <string>
#include <functional>

#include <xcommunication/int_xsdatapacket.h>
#include <xcommunication/legacydatapacket.h>
#include <xsens/xsdatapacket.h>
#include <xsens/xsmessagearray.h>

#include "XsensMT.h"

#include <xstypes/xseuler.h>
#include <xstypes/xsvector.h>
#include <xstypes/xsoutputconfiguration.h>
#include <xstypes/xsoutputconfigurationarray.h>


using namespace yarp::os;
using namespace yarp::dev;
using namespace yarp::sig;
Expand Down Expand Up @@ -115,42 +116,40 @@ bool XsensMT::open(yarp::os::Searchable &config)
m_portInfo = XsPortInfo(comPortString, XsBaud::numericToRate(baudRate));

yInfo("xsensmt: Opening serial port %s with baud rate %d.", comPortString.c_str(), baudRate);
if (!m_xsensDevice.openPort(m_portInfo))
if (!m_xsensCommunicator.openPort(m_portInfo))
{
yError("xsensmt: Could not open serial port.");
return false;
}

m_xsensDevice = new MtiBaseDevice(&m_xsensCommunicator);

yInfo("xsensmt: Putting device into configuration mode.");
// Put the device into configuration mode before configuring the device
if (!m_xsensDevice.gotoConfig())
if (!m_xsensDevice->gotoConfig())
{
yError("xsensmt: Could not put device in configuration mode.");
return false;
}

// Request the device Id to check the device type
m_portInfo.setDeviceId(m_xsensDevice.getDeviceId());
m_portInfo.setDeviceId(m_xsensDevice->deviceId());

// Check if we have an MTi / MTx / MTmk4 device
if (!m_portInfo.deviceId().isMt9c() && !m_portInfo.deviceId().isLegacyMtig() && !m_portInfo.deviceId().isMtMk4() && !m_portInfo.deviceId().isFmt_X000())
// Check if we have an MTi
if (!m_portInfo.deviceId().isMti())
{
yError("xsensmt: No MTi / MTx / MTmk4 device found. Aborting.");
yError("xsensmt: No MTi device found. Aborting.");
return false;
}

yInfo() << "xsensmt: Found a device with id: " << m_portInfo.deviceId().toString().toStdString() << " @ port: " << m_portInfo.portName().toStdString() << ", baudrate: " << m_portInfo.baudrate() << " .";
yInfo() << "xsensmt: Device: " << m_xsensDevice.getProductCode().toStdString() << " opened.";
// TODO migrate to modern API
//yInfo() << "xsensmt: Device: " << m_xsensDevice.getProductCode().toStdString() << " opened.";

// Configure the device. Note the differences between MTix and MTmk4
yInfo("xsensmt: Configuring the device of type %s.", m_portInfo.deviceId().toString().c_str());
if (m_portInfo.deviceId().isMt9c() || m_portInfo.deviceId().isLegacyMtig())
if (m_portInfo.deviceId().isMti())
{
yError("xsensmt: Device of type %s is not supported by the driver, aborting.", m_portInfo.deviceId().toString().c_str());
return false;
}
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);
Expand All @@ -161,9 +160,9 @@ bool XsensMT::open(yarp::os::Searchable &config)
configArray.push_back(acc);
configArray.push_back(gyro);
configArray.push_back(mag);
if (!m_xsensDevice.setOutputConfiguration(configArray))
if (!m_xsensDevice->setOutputConfiguration(configArray))
{
yError("xsensmt: Could not configure MTmk4 device. Aborting.");
yError("xsensmt: Could not configure device. Aborting.");
return false;
}

Expand Down Expand Up @@ -202,7 +201,7 @@ bool XsensMT::open(yarp::os::Searchable &config)

// Put the device in measurement mode
yInfo() << "xsensmt: Putting device into measurement mode.";
if (!m_xsensDevice.gotoMeasurement())
if (!m_xsensDevice->gotoMeasurement())
{
yError("xsensmt: Could not put device into measurement mode. Aborting.");
return false;
Expand All @@ -226,6 +225,8 @@ bool XsensMT::close()
{
m_sensorThread.join();
}

return true;
}

yarp::os::Stamp XsensMT::getLastInputStamp()
Expand All @@ -246,21 +247,15 @@ void XsensMT::sensorReadLoop()

while (!m_isDeviceClosing)
{
m_xsensDevice.readDataToBuffer(data);
m_xsensDevice.processBufferedData(data, msgs);
m_xsensCommunicator.readDataToBuffer(data);
m_xsensCommunicator.processBufferedData(data, msgs);
for (std::deque<XsMessage>::iterator it = msgs.begin(); it != msgs.end(); ++it)
{
// Retrieve a packet
XsDataPacket packet;
if ((*it).getMessageId() == XMID_MtData) {
LegacyDataPacket lpacket(1, false);
lpacket.setMessage((*it));
lpacket.setXbusSystem(false);
lpacket.setDeviceId(m_portInfo.deviceId(), 0);
lpacket.setDataFormat(XOM_Orientation, XOS_OrientationMode_Quaternion,0);
yError("xsensmt: Legacy packet received, but the legacy packet is not currently supported by the driver. Ignoring message.");
continue;
//XsDataPacket_assignFromLegacyDataPacket(&packet, &lpacket, 0);
}
else if ((*it).getMessageId() == XMID_MtData2) {
packet.setMessage((*it));
Expand Down Expand Up @@ -326,10 +321,10 @@ void XsensMT::sensorReadLoop()

bool XsensMT::setFilterProfile(const uint16_t profile)
{
XsMessage snd(XMID_SetFilterProfile, 2), rcv;
XsMessage snd(XMID_SetFilterProfile, 2);
snd.setDataShort(profile, 0);
m_xsensDevice.writeMessage(snd);
return m_xsensDevice.waitForMessage(XMID_SetFilterProfileAck, rcv);
m_xsensCommunicator.writeMessage(snd);
return true;
}

bool XsensMT::setOptionFlags(const uint32_t setFlags, const uint32_t clearFlags)
Expand All @@ -339,8 +334,8 @@ bool XsensMT::setOptionFlags(const uint32_t setFlags, const uint32_t clearFlags)
snd.setDataLong(setFlags, 0);
// ClearFlags (4 bytes)
snd.setDataLong(clearFlags, 4);
m_xsensDevice.writeMessage(snd);
return m_xsensDevice.waitForMessage(XMID_SetOptionFlagsAck, rcv);
m_xsensCommunicator.writeMessage(snd);
return true;
}

yarp::dev::MAS_status XsensMT::genericGetStatus(size_t sens_index) const
Expand Down
30 changes: 28 additions & 2 deletions xsensmt/XsensMT.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,7 +8,11 @@
#ifndef XSENS_MT_YARP_DRIVER
#define XSENS_MT_YARP_DRIVER

#include "deviceclass.h"
#include <xstypes/xsdatapacket.h>
#include <xstypes/xsportinfo.h>
#include <xscontroller/serialportcommunicator.h>

#include <xscontroller/mtibasedevice.h>

#include <yarp/dev/DeviceDriver.h>
#include <yarp/dev/GenericSensorInterfaces.h>
Expand All @@ -31,6 +35,27 @@ namespace dev
}


// Expose protected methods
namespace yarp
{
namespace dev
{
class YARPSerialPortCommunicator: public SerialPortCommunicator {
public:
XsResultValue readDataToBuffer(XsByteArray& raw) override
{
return this->readDataToBuffer(raw);
}

XsResultValue processBufferedData(const XsByteArray& rawIn, std::deque<XsMessage>& messages) override
{
return this->processBufferedData(rawIn, messages);
}

};
}
}

/**
*
* \section xsensmt Description of input parameters
Expand Down Expand Up @@ -285,7 +310,8 @@ class yarp::dev::XsensMT : public yarp::dev::IGenericSensor,
bool m_isSensorMeasurementAvailable{false};

// Interface exposed by the Xsens MT Software suite
DeviceClass m_xsensDevice;
YARPSerialPortCommunicator m_xsensCommunicator;
MtiBaseDevice* m_xsensDevice;
XsPortInfo m_portInfo;

yarp::os::Stamp m_lastReadStamp;
Expand Down

0 comments on commit f03045c

Please sign in to comment.