CIMUXSens.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file     |
   | See: http://www.mrpt.org/Authors - All rights reserved.                |
   | Released under BSD License. See details in http://www.mrpt.org/License |
   +------------------------------------------------------------------------+ */
 
#include "hwdrivers-precomp.h"  // Precompiled headers
 
#include <mrpt/hwdrivers/CIMUXSens.h>
#include <mrpt/obs/CObservationIMU.h>
 
#include <thread>
IMPLEMENTS_GENERIC_SENSOR(CIMUXSens, mrpt::hwdrivers)
 
using namespace mrpt::utils;
using namespace mrpt::obs;
using namespace mrpt::hwdrivers;
using namespace std::literals;
 
#if MRPT_HAS_xSENS_MT3
#include "xSens_MT3/cmt1.h"
#include "xSens_MT3/cmt2.h"
#include "xSens_MT3/cmt3.h"
#include "xSens_MT3/cmtdef.h"
#include "xSens_MT3/cmtmessage.h"
#include "xSens_MT3/cmtpacket.h"
#include "xSens_MT3/cmtscan.h"
#include "xSens_MT3/xsens_fifoqueue.h"
#include "xSens_MT3/xsens_janitors.h"
#include "xSens_MT3/xsens_list.h"
#include "xSens_MT3/xsens_std.h"
#include "xSens_MT3/xsens_time.h"
#endif
 
// Adaptors for the "void*" memory blocks:
#define cmt3 (*static_cast<xsens::Cmt3*>(m_cmt3_ptr))
#define deviceId (*static_cast<CmtDeviceId*>(m_deviceId_ptr))
 
// Include libraries in linking:
#if MRPT_HAS_xSENS_MT3
#ifdef MRPT_OS_WINDOWS
// WINDOWS:
#if defined(_MSC_VER) || defined(__BORLANDC__)
#pragma comment(lib, "SetupAPI.lib")
#endif
#endif  // MRPT_OS_WINDOWS
#endif  // MRPT_HAS_xSENS_MT3
 
/*-------------------------------------------------------------
                                        CIMUXSens
-------------------------------------------------------------*/
CIMUXSens::CIMUXSens()
        : m_COMbauds(0),
          m_com_port(),
          m_timeStartUI(0),
          m_timeStartTT(0),
          m_sensorPose(),
          m_cmt3_ptr(nullptr),
          m_deviceId_ptr(nullptr),
          m_toutCounter(0)
{
        m_sensorLabel = "XSensMTi";
#if MRPT_HAS_xSENS_MT3
        m_cmt3_ptr = new xsens::Cmt3[1];
        m_deviceId_ptr = new CmtDeviceId[1];
 
#else
        THROW_EXCEPTION(
                "MRPT has been compiled with 'BUILD_XSENS_MT3'=OFF, so this class "
                "cannot be used.");
#endif
}
 
/*-------------------------------------------------------------
                                        ~CIMUXSens
-------------------------------------------------------------*/
CIMUXSens::~CIMUXSens()
{
#if MRPT_HAS_xSENS_MT3
        cmt3.closePort();
 
        delete[] & cmt3;
        m_cmt3_ptr = nullptr;
        delete[] & deviceId;
        m_deviceId_ptr = nullptr;
#endif
}
 
/*-------------------------------------------------------------
                                        doProcess
-------------------------------------------------------------*/
void CIMUXSens::doProcess()
{
#if MRPT_HAS_xSENS_MT3
 
        if (m_state == ssError)
        {
                std::this_thread::sleep_for(200ms);
                initialize();
        }
 
        if (m_state == ssError) return;
 
        XsensResultValue res;
        unsigned int cont = 0;
 
        do
        {
                CmtTimeStamp nowUI;  // ms
 
                xsens::Packet packet(1 /*NDevices*/, cmt3.isXm() /*Is Bus master*/);
 
                res = cmt3.waitForDataMessage(&packet);
 
                if (res == XRV_OK)
                {
                        // Data properly collected
                        nowUI = packet.getRtc();
                        m_state = ssWorking;
 
                        CObservationIMU::Ptr obs =
                                mrpt::make_aligned_shared<CObservationIMU>();
 
                        // ANGLE MEASUREMENTS:
                        if (packet.containsOriEuler())
                        {
                                CmtEuler euler_data = packet.getOriEuler();
 
                                obs->rawMeasurements[IMU_YAW] = DEG2RAD(euler_data.m_yaw);
                                obs->dataIsPresent[IMU_YAW] = true;
                                obs->rawMeasurements[IMU_PITCH] = DEG2RAD(euler_data.m_pitch);
                                obs->dataIsPresent[IMU_PITCH] = true;
                                obs->rawMeasurements[IMU_ROLL] = DEG2RAD(euler_data.m_roll);
                                obs->dataIsPresent[IMU_ROLL] = true;
                        }
 
                        // ACCELEROMETERS MEASUREMENTS:
                        if (packet.containsCalAcc())
                        {
                                CmtVector acc_data = packet.getCalAcc();  // getRawAcc();
 
                                obs->rawMeasurements[IMU_X_ACC] = acc_data.m_data[0];
                                obs->dataIsPresent[IMU_X_ACC] = true;
                                obs->rawMeasurements[IMU_Y_ACC] = acc_data.m_data[1];
                                obs->dataIsPresent[IMU_Y_ACC] = true;
                                obs->rawMeasurements[IMU_Z_ACC] = acc_data.m_data[2];
                                obs->dataIsPresent[IMU_Z_ACC] = true;
                        }
 
                        // GYROSCOPES MEASUREMENTS:
                        if (packet.containsCalGyr())
                        {
                                CmtVector gir_data = packet.getCalGyr();  // getRawGyr();
 
                                obs->rawMeasurements[IMU_YAW_VEL] = gir_data.m_data[2];
                                obs->dataIsPresent[IMU_YAW_VEL] = true;
                                obs->rawMeasurements[IMU_PITCH_VEL] = gir_data.m_data[1];
                                obs->dataIsPresent[IMU_PITCH_VEL] = true;
                                obs->rawMeasurements[IMU_ROLL_VEL] = gir_data.m_data[0];
                                obs->dataIsPresent[IMU_ROLL_VEL] = true;
                        }
 
                        // TimeStamp
                        uint64_t AtUI = 0;
                        if (m_timeStartUI == 0)
                        {
                                m_timeStartUI = nowUI;
                                m_timeStartTT = mrpt::system::now();
                        }
                        else
                                AtUI = nowUI - m_timeStartUI;
 
                        double AtDO =
                                AtUI * 10000.0;  // Difference in intervals of 100 nsecs
                        obs->timestamp = m_timeStartTT + AtDO;
                        obs->sensorPose = m_sensorPose;
                        obs->sensorLabel = m_sensorLabel;
 
                        appendObservation(obs);
                        m_toutCounter = 0;
 
                }  // end if XRV_OK
 
                if (res == XRV_TIMEOUT)
                {
                        if (++m_toutCounter > 3)
                        {
                                m_toutCounter = 0;
                                m_state = ssError;
                                if (cmt3.isPortOpen()) cmt3.closePort();
 
                                std::cerr << "[CIMUXSens::doProcess()] Error: No data "
                                                         "available [XRV_TIMEOUT]"
                                                  << std::endl;
                        }
                }  // end if XRV_TIMEOUT
 
                if (res == XRV_TIMEOUTNODATA)
                {
                        //                      m_state                 = ssError;
                        //                      m_timeStartUI   = 0;
                        //                      if( cmt3.isPortOpen() )
                        //                              cmt3.closePort();
                        //                      std::cerr << "[CIMUXSens::doProcess()] Error: No data
                        // available [XRV_TIMEOUTNODATA]" << std::endl;
                }  // end if XRV_TIMEOUTNODATA
        } while (res == XRV_OK && cont++ < 30);
 
#else
        THROW_EXCEPTION(
                "MRPT has been compiled with 'BUILD_XSENS_MT3'=OFF, so this class "
                "cannot be used.");
#endif
}
 
/*-------------------------------------------------------------
                                        lookForPort
-------------------------------------------------------------*/
bool CIMUXSens::searchPortAndConnect()
{
#if MRPT_HAS_xSENS_MT3
        uint32_t baudrate;
        if (cmt3.getBaudrate(baudrate) == XRV_OK) return true;
 
        XsensResultValue res;
        xsens::List<CmtPortInfo> portInfo;
        unsigned long portCount = 0;
        unsigned short mtCount = 0;
 
        if (m_com_port.empty())
        {  // Scan COM ports
                std::cout << "Scanning for connected Xsens devices..." << std::endl;
                xsens::cmtScanPorts(portInfo);
                portCount = portInfo.length();
                std::cout << "Done" << std::endl;
                if (portCount == 0)
                {
                        std::cout << "No xSens device found" << std::endl;
                        m_state = ssError;
                        return false;
 
                }  // end if (error)
        }  // end if
        else  // Port defined by user in .ini file
        {
                CmtPortInfo pInfo;
                pInfo.m_baudrate = m_COMbauds;
                strcpy(
                        pInfo.m_portName,
                        m_com_port.c_str());  // m_portNr               = (unsigned char)m_com_port;
                portInfo.append(pInfo);
                portCount++;
        }  // end else
 
        ASSERT_(portCount == 1);
        std::cout << "Using COM port "
                          << portInfo[0].m_portName /*(long)portInfo[0].m_portNr*/ << " at "
                          << portInfo[0].m_baudrate << " baud" << std::endl;
        std::cout << "Opening port..." << std::endl;
        // open the port which the device is connected to and connect at the
        // device's baudrate.
        res = cmt3.openPort(portInfo[0].m_portName, portInfo[0].m_baudrate);
        if (res != XRV_OK)
        {
                std::cerr << "COM Port could not be opened" << std::endl;
                m_state = ssError;
                return false;
        }
        std::cout << "done" << std::endl;
 
        // get the Mt sensor count.
        std::cout
                << "Retrieving MotionTracker count (excluding attached Xbus Master(s))"
                << std::endl;
        mtCount = cmt3.getMtCount();
        std::cout << "MotionTracker count: " << mtCount << std::endl;
 
        ASSERT_(mtCount == 1);
 
        // retrieve the device IDs
        std::cout << "Retrieving MotionTracker device ID" << std::endl;
        res = cmt3.getDeviceId(mtCount, deviceId);
        std::cout << "Device ID at busId 1: " << (long)deviceId
                          << std::endl;  // printf("Device ID at busId 1: %08x\n",(long)
        // deviceId);
        if (res != XRV_OK)
        {
                std::cerr << "Device ID could not be gathered" << std::endl;
                m_state = ssError;
                return false;
        }
 
        return true;
#else
        return false;
#endif
}  // end lookForPort
 
/*-------------------------------------------------------------
                                        initialize
-------------------------------------------------------------*/
void CIMUXSens::initialize()
{
#if MRPT_HAS_xSENS_MT3
 
        XsensResultValue res;
 
        if (cmt3.isPortOpen()) return;
 
        m_state = ssInitializing;
 
        // Search for the COM PORT and connect
        if (!searchPortAndConnect())
        {
                m_state = ssError;
                std::cerr << "Error Could not initialize the device" << std::endl;
                return;
        }
 
        std::cout << "xSens IMU detected and connected" << std::endl;
        CmtOutputMode mode = CMT_OUTPUTMODE_ORIENT | CMT_OUTPUTMODE_CALIB;
        CmtOutputSettings settings = CMT_OUTPUTSETTINGS_ORIENTMODE_EULER |
                                                                 CMT_OUTPUTSETTINGS_TIMESTAMP_SAMPLECNT |
                                                                 CMT_OUTPUTSETTINGS_CALIBMODE_ACCGYR;
 
        // set the sensor to config state
        res = cmt3.gotoConfig();
        if (res != XRV_OK)
        {
                m_state = ssError;  // EXIT_ON_ERROR(res,"gotoConfig");
                std::cerr << "An error ocurred when setting the device to config mode"
                                  << std::endl;
                return;
        }
 
        unsigned short sampleFreq;
        sampleFreq = cmt3.getSampleFrequency();
 
        // set the device output mode for the device(s)
        std::cout << "Configuring mode selection" << std::endl;
        CmtDeviceMode deviceMode(mode, settings, sampleFreq);
        res = cmt3.setDeviceMode(deviceMode, true, deviceId);
        if (res != XRV_OK)
        {
                m_state = ssError;  // EXIT_ON_ERROR(res,"setDeviceMode");
                std::cerr << "An error ocurred when configuring the device"
                                  << std::endl;
                return;
        }
 
        // start receiving data
        res = cmt3.gotoMeasurement();
        if (res != XRV_OK)
        {
                m_state = ssError;  // EXIT_ON_ERROR(res,"gotoMeasurement");
                std::cerr
                        << "An error ocurred when setting the device to measurement mode"
                        << std::endl;
                return;
        }
 
        std::cout << "Getting initial TimeStamp" << std::endl;
        // Get initial TimeStamp
        xsens::Packet packet(1 /*NDevices*/, cmt3.isXm() /*Is Bus master*/);
        do
        {
                res = cmt3.waitForDataMessage(&packet);
                if (res == XRV_OK)
                {
                        m_timeStartUI = (uint64_t)packet.getRtc();
                        m_timeStartTT = mrpt::system::now();
                }  // end if
        } while (res != XRV_OK);
 
        std::cout << "Gathering data" << std::endl;
        m_state = ssWorking;
 
#else
        THROW_EXCEPTION(
                "MRPT has been compiled with 'BUILD_XSENS_MT3'=OFF, so this class "
                "cannot be used.");
#endif
}
 
/*-------------------------------------------------------------
                                        loadConfig_sensorSpecific
-------------------------------------------------------------*/
void CIMUXSens::loadConfig_sensorSpecific(
        const mrpt::utils::CConfigFileBase& configSource,
        const std::string& iniSection)
{
        m_sensorPose.setFromValues(
                configSource.read_float(iniSection, "pose_x", 0, false),
                configSource.read_float(iniSection, "pose_y", 0, false),
                configSource.read_float(iniSection, "pose_z", 0, false),
                DEG2RAD(configSource.read_float(iniSection, "pose_yaw", 0, false)),
                DEG2RAD(configSource.read_float(iniSection, "pose_pitch", 0, false)),
                DEG2RAD(configSource.read_float(iniSection, "pose_roll", 0, false)));
 
        m_COMbauds =
                configSource.read_int(iniSection, "baudRate", m_COMbauds, false);
 
#ifdef MRPT_OS_WINDOWS
        m_com_port =
                configSource.read_string(iniSection, "COM_port_WIN", m_com_port, false);
#else
        m_com_port =
                configSource.read_string(iniSection, "COM_port_LIN", m_com_port, false);
#endif
}