libs/hwdrivers/src/CIMUXSens_MT4.cpp

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#include "hwdrivers-precomp.h"  // Precompiled headers
//
#include <mrpt/hwdrivers/CIMUXSens_MT4.h>
#include <mrpt/obs/CObservationGPS.h>
#include <mrpt/obs/CObservationIMU.h>

#include <chrono>
#include <iostream>
#include <thread>

#if !MRPT_HAS_xSENS
namespace mrpt::hwdrivers
{
struct CIMUXSens_MT4::Impl
{
};
}  // namespace mrpt::hwdrivers
#else

// Tell MSVC that we are not using MTSDK as DLL:
#define XDA_STATIC_LIB

#include <xscontroller/xscallback.h>
#include <xscontroller/xscontrol_def.h>
#include <xscontroller/xsdevice_def.h>
#include <xscontroller/xsscanner.h>
#include <xstypes/xsdatapacket.h>
#include <xstypes/xsoutputconfigurationarray.h>
#include <xstypes/xsportinfo.h>
#include <xstypes/xsstatusflag.h>

// MTSDK expects this global symbol as "extern":
Journaller* gJournal = 0;

namespace mrpt::hwdrivers
{
class MyXSensCallback : public XsCallback
{
 public:
  CIMUXSens_MT4* me = nullptr;

 private:
  //	uint64_t m_timeStartUI = 0;
  mrpt::system::TTimeStamp m_timeStartTT;

 protected:
  void onLiveDataAvailable(XsDevice*, const XsDataPacket* ptrpacket) override
  {
    using namespace mrpt::obs;

    if (!me) return;
    if (!ptrpacket) return;
    const XsDataPacket& packet = *ptrpacket;

    // Data properly collected: extract data fields
    // -------------------------------------------------
    me->m_state = mrpt::hwdrivers::CGenericSensor::ssWorking;
    CObservationIMU::Ptr obs = std::make_shared<CObservationIMU>();

    if (packet.containsOrientation())
    {
      XsEuler euler = packet.orientationEuler();
      obs->rawMeasurements[IMU_YAW] = DEG2RAD(euler.yaw());
      obs->dataIsPresent[IMU_YAW] = true;
      obs->rawMeasurements[IMU_PITCH] = DEG2RAD(euler.pitch());
      obs->dataIsPresent[IMU_PITCH] = true;
      obs->rawMeasurements[IMU_ROLL] = DEG2RAD(euler.roll());
      obs->dataIsPresent[IMU_ROLL] = true;

      XsQuaternion quat = packet.orientationQuaternion();
      obs->rawMeasurements[IMU_ORI_QUAT_X] = quat.x();
      obs->dataIsPresent[IMU_ORI_QUAT_X] = true;
      obs->rawMeasurements[IMU_ORI_QUAT_Y] = quat.y();
      obs->dataIsPresent[IMU_ORI_QUAT_Y] = true;
      obs->rawMeasurements[IMU_ORI_QUAT_Z] = quat.z();
      obs->dataIsPresent[IMU_ORI_QUAT_Z] = true;
      obs->rawMeasurements[IMU_ORI_QUAT_W] = quat.w();
      obs->dataIsPresent[IMU_ORI_QUAT_W] = true;
    }

    if (packet.containsCalibratedAcceleration())
    {
      XsVector acc_data = packet.calibratedAcceleration();
      obs->rawMeasurements[IMU_X_ACC] = acc_data[0];
      obs->dataIsPresent[IMU_X_ACC] = true;
      obs->rawMeasurements[IMU_Y_ACC] = acc_data[1];
      obs->dataIsPresent[IMU_Y_ACC] = true;
      obs->rawMeasurements[IMU_Z_ACC] = acc_data[2];
      obs->dataIsPresent[IMU_Z_ACC] = true;
    }

    if (packet.containsCalibratedGyroscopeData())
    {
      XsVector gyr_data = packet.calibratedGyroscopeData();
      obs->rawMeasurements[IMU_YAW_VEL] = gyr_data[2];
      obs->dataIsPresent[IMU_YAW_VEL] = true;
      obs->rawMeasurements[IMU_PITCH_VEL] = gyr_data[1];
      obs->dataIsPresent[IMU_PITCH_VEL] = true;
      obs->rawMeasurements[IMU_ROLL_VEL] = gyr_data[0];
      obs->dataIsPresent[IMU_ROLL_VEL] = true;
    }

    if (packet.containsCalibratedMagneticField())
    {
      XsVector mag_data = packet.calibratedMagneticField();
      obs->rawMeasurements[IMU_MAG_X] = mag_data[0];
      obs->dataIsPresent[IMU_MAG_X] = true;
      obs->rawMeasurements[IMU_MAG_Y] = mag_data[1];
      obs->dataIsPresent[IMU_MAG_Y] = true;
      obs->rawMeasurements[IMU_MAG_Z] = mag_data[2];
      obs->dataIsPresent[IMU_MAG_Z] = true;
    }

    if (packet.containsVelocity())
    {
      XsVector vel_data = packet.velocity();
      obs->rawMeasurements[IMU_X_VEL] = vel_data[0];
      obs->dataIsPresent[IMU_X_VEL] = true;
      obs->rawMeasurements[IMU_Y_VEL] = vel_data[1];
      obs->dataIsPresent[IMU_Y_VEL] = true;
      obs->rawMeasurements[IMU_Z_VEL] = vel_data[2];
      obs->dataIsPresent[IMU_Z_VEL] = true;
    }

    if (packet.containsTemperature())
    {
      obs->rawMeasurements[IMU_TEMPERATURE] = packet.temperature();
      obs->dataIsPresent[IMU_TEMPERATURE] = true;
    }

    if (packet.containsAltitude())
    {
      obs->rawMeasurements[IMU_ALTITUDE] = packet.altitude();
      obs->dataIsPresent[IMU_ALTITUDE] = true;
    }

    // TimeStamp
#if 0  // I can't find a generic conversion between sample time and seconds!
		if (packet.containsSampleTime64())
		{
		dev->getDataPacketByIndex()
			const uint64_t nowUI = packet.sampleTime64();
			std::cout << "nowUI: " << nowUI << "\n";

			uint64_t AtUI = 0;
			if (m_timeStartUI == 0)
			{
				m_timeStartUI = nowUI;
				m_timeStartTT = mrpt::Clock::now();
			}
			else
				AtUI = nowUI - m_timeStartUI;  // ms

			obs->timestamp = m_timeStartTT + std::chrono::milliseconds(AtUI);
		}
		else
		if (packet.containsUtcTime())
		{
			XsTimeInfo utc = packet.utcTime();

			mrpt::system::TTimeParts parts;

			parts.day_of_week = 0;
			parts.daylight_saving = 0;
			parts.year = utc.m_year;
			parts.month = utc.m_month;
			parts.day = utc.m_day;
			parts.hour = utc.m_hour;
			parts.minute = utc.m_minute;
			parts.second = utc.m_second + (utc.m_nano * 1000000000.0);

			obs->timestamp = mrpt::system::buildTimestampFromParts(parts);
		}
		else
#endif
    obs->timestamp = mrpt::Clock::now();

    obs->sensorPose = me->m_sensorPose;
    obs->sensorLabel = me->m_sensorLabel;

    me->appendObservation(obs);

    if (packet.containsLatitudeLongitude())
    {
      XsVector lla_data = packet.latitudeLongitude();

      CObservationGPS::Ptr obsGPS = CObservationGPS::Create();
      gnss::Message_NMEA_RMC rGPSs;
      gnss::Message_NMEA_RMC::content_t& rGPS = rGPSs.fields;
      rGPS.latitude_degrees = lla_data[0];
      rGPS.longitude_degrees = lla_data[1];

      if (packet.containsStatus() && packet.status() & XSF_GpsValid)
        rGPS.validity_char = 'A';
      else
        rGPS.validity_char = 'V';

      if (packet.containsUtcTime())
      {
        auto utc = packet.utcTime();
        rGPS.UTCTime.hour = utc.m_hour;
        rGPS.UTCTime.minute = utc.m_minute;
        rGPS.UTCTime.sec = utc.m_second + (utc.m_nano * 1000000.0);
      }
      else
      {
        rGPS.UTCTime.hour =
            ((obs->timestamp.time_since_epoch().count() / (60 * 60 * ((uint64_t)1000000 / 100))) %
             24);
        rGPS.UTCTime.minute =
            ((obs->timestamp.time_since_epoch().count() / (60 * ((uint64_t)1000000 / 100))) % 60);
        rGPS.UTCTime.sec = fmod(obs->timestamp.time_since_epoch().count() / (1000000.0 / 100), 60);
      }

      if (packet.containsVelocity())
      {
        XsVector vel_data = packet.velocity();

        rGPS.speed_knots = sqrt(vel_data[0] * vel_data[0] + vel_data[1] * vel_data[1]);
        rGPS.direction_degrees = 0;  // Could be worked out from
                                     // velocity and magnatic field
                                     // perhaps.
      }
      else
        rGPS.speed_knots = rGPS.direction_degrees = 0;

      obsGPS->setMsg(rGPSs);
      obsGPS->timestamp = obs->timestamp;
      obsGPS->originalReceivedTimestamp = obs->timestamp;
      obsGPS->has_satellite_timestamp = false;
      obsGPS->sensorPose = me->m_sensorPose;
      obsGPS->sensorLabel = me->m_sensorLabel;

      me->appendObservation(obsGPS);
    }
  }
};

struct CIMUXSens_MT4::Impl
{
  Impl() = default;

  XsControl* m_xscontrol = nullptr;
  XsDevice* m_device = nullptr;
  XsPortInfo m_port;
  std::shared_ptr<MyXSensCallback> myCallback = std::make_shared<MyXSensCallback>();
};

}  // namespace mrpt::hwdrivers

#endif

// Include libraries in linking:
#if MRPT_HAS_xSENS
#ifdef _WIN32
// WINDOWS:
#if defined(_MSC_VER)
#pragma comment(lib, "SetupAPI.lib")
#pragma comment(lib, "WinUsb.lib")
#endif
#endif  // _WIN32
#endif  // MRPT_HAS_xSENS

IMPLEMENTS_GENERIC_SENSOR(CIMUXSens_MT4, mrpt::hwdrivers)

using namespace mrpt::obs;
using namespace mrpt::hwdrivers;
using namespace std;

CIMUXSens_MT4::CIMUXSens_MT4() : m_impl(mrpt::make_impl<CIMUXSens_MT4::Impl>())
{
  m_sensorLabel = "XSensMTi_MT4";

#if MRPT_HAS_xSENS
  m_impl->m_xscontrol = XsControl::construct();
  m_impl->myCallback->me = this;

#else
  THROW_EXCEPTION(
      "MRPT has been compiled with 'MRPT_WITH_XSENS'=OFF, so this class "
      "cannot be used.");
#endif
}

CIMUXSens_MT4::~CIMUXSens_MT4()
{
#if MRPT_HAS_xSENS
  close();
  m_impl->m_xscontrol->destruct();
#endif
}

void CIMUXSens_MT4::close()
{
#if MRPT_HAS_xSENS
  if (m_impl->m_device != nullptr)
  {
    m_impl->m_device->stopRecording();
    m_impl->m_device->closeLogFile();
    m_impl->m_device->removeCallbackHandler(m_impl->myCallback.get());
  }
  m_impl->m_xscontrol->closePort(m_impl->m_port);

#endif
}

/*-------------------------------------------------------------
          doProcess
-------------------------------------------------------------*/
void CIMUXSens_MT4::doProcess()
{
#if MRPT_HAS_xSENS
  if (m_state == ssError)
  {
    std::this_thread::sleep_for(200ms);
    initialize();
  }

  // Main processing happens asynchronously via callbacks.

#else
  THROW_EXCEPTION(
      "MRPT has been compiled with 'MRPT_WITH_XSENS'=OFF, so this class "
      "cannot be used.");
#endif
}

/*-------------------------------------------------------------
          initialize
-------------------------------------------------------------*/
void CIMUXSens_MT4::initialize()
{
#if MRPT_HAS_xSENS
  m_state = ssInitializing;

  try
  {
    // Try to open a specified device, or scan the bus?
    XsPortInfo mtPort;

    if (m_portname.empty())
    {
      if (m_verbose) cout << "[CIMUXSens_MT4] Scanning for USB devices...\n";

      XsPortInfoArray portInfoArray = XsScanner::scanPorts();

      for (const auto& portInfo : portInfoArray)
      {
        if (portInfo.deviceId().isMti() || portInfo.deviceId().isMtig())
        {
          mtPort = portInfo;
          break;
        }
      }

      if (mtPort.empty())
        THROW_EXCEPTION_FMT(
            "CIMUXSens_MT4: No 'portname' was specified and no "
            "compatible XSens device was found in the system (%u "
            "devices connected)",
            static_cast<unsigned int>(portInfoArray.size()));

      if (m_verbose)
        cout << "[CIMUXSens_MT4] Found " << portInfoArray.size()
             << " devices. Opening the first one.\n";
    }
    else
    {
      cout << "[CIMUXSens_MT4] Using user-supplied portname '" << m_portname << "' at "
           << m_port_bauds << " baudrate.\n";

      mtPort = XsPortInfo(m_portname, XsBaud_numericToRate(m_port_bauds));
    }

    if (mtPort.empty()) THROW_EXCEPTION("No MTi device found");

    // Use the first detected device or the specified one:
    if (!m_deviceId.empty())
    {
      if (mtPort.deviceId().toString().c_str() != m_deviceId)
        THROW_EXCEPTION_FMT("Device with ID: %s not found", m_deviceId.c_str());
    }

    std::cout << mrpt::format(
        "[CIMUXSens_MT4] Found a device with ID: %s @ port: %s, baudrate: "
        "%d",
        mtPort.deviceId().toString().toStdString().c_str(), mtPort.portName().toStdString().c_str(),
        mtPort.baudrate());

    if (!m_impl->m_xscontrol->openPort(mtPort.portName().toStdString(), mtPort.baudrate()))
      THROW_EXCEPTION("Could not open port");

    m_impl->m_device = m_impl->m_xscontrol->device(mtPort.deviceId());
    ASSERT_(m_impl->m_device != nullptr);

    std::cout << mrpt::format(
        "[CIMUXSens_MT4] Device: %s, with ID: %s opened.",
        m_impl->m_device->productCode().toStdString().c_str(),
        m_impl->m_device->deviceId().toString().c_str());

    m_impl->m_device->addCallbackHandler(m_impl->myCallback.get());

    // Put the device in configuration mode
    if (m_verbose)
      cout << "[CIMUXSens_MT4] Putting device into configuration "
              "mode...\n";

    if (!m_impl->m_device->gotoConfig()) THROW_EXCEPTION("Could not go to config");

    // read EMTS and device config stored in .mtb file header.
    if (!m_impl->m_device->readEmtsAndDeviceConfiguration())
      THROW_EXCEPTION("Could not read device configuration");

    // Set configuration:
    // if (mtPort.deviceId().isMti())
    {
      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 (!m_impl->m_device->setOutputConfiguration(configArray))
        throw std::runtime_error("Could not configure MTi device. Aborting.");
    }

    // Put the device in measurement mode
    if (m_verbose) cout << "[CIMUXSens_MT4] Putting device into measurement mode..." << std::endl;

    if (!m_impl->m_device->gotoMeasurement())
      THROW_EXCEPTION("Could not put device into measurement mode");

    if (!m_xsensLogFile.empty())
    {
      if (m_impl->m_device->createLogFile(m_xsensLogFile) != XRV_OK)
        THROW_EXCEPTION_FMT("Failed to create a log file! (%s)", m_xsensLogFile.c_str());
      else
        printf("[CIMUXSens_MT4] Created a log file: %s", m_xsensLogFile.c_str());

      if (!m_impl->m_device->startRecording()) THROW_EXCEPTION("Could not start recording");
    }

    m_state = ssWorking;
  }
  catch (std::exception&)
  {
    m_state = ssError;
    std::cerr << "[CIMUXSens_MT4] Error Could not initialize the device" << std::endl;
    throw;
  }

#else
  THROW_EXCEPTION(
      "MRPT has been compiled with 'MRPT_WITH_XSENS'=OFF, so this class "
      "cannot be used.");
#endif
}

/*-------------------------------------------------------------
          loadConfig_sensorSpecific
-------------------------------------------------------------*/
void CIMUXSens_MT4::loadConfig_sensorSpecific(
    const mrpt::config::CConfigFileBase& c, const std::string& s)
{
  m_sensorPose.setFromValues(
      c.read_float(s, "pose_x", 0, false), c.read_float(s, "pose_y", 0, false),
      c.read_float(s, "pose_z", 0, false), DEG2RAD(c.read_float(s, "pose_yaw", 0, false)),
      DEG2RAD(c.read_float(s, "pose_pitch", 0, false)),
      DEG2RAD(c.read_float(s, "pose_roll", 0, false)));

  m_sampleFreq = c.read_int(s, "sampleFreq", m_sampleFreq, false);
  m_port_bauds = c.read_int(s, "baudRate", m_port_bauds, false);
  m_deviceId = c.read_string(s, "deviceId", m_deviceId, false);
  m_xsensLogFile = c.read_string(s, "logFile", m_xsensLogFile, false);

#ifdef _WIN32
  m_portname = c.read_string(s, "portname_WIN", m_portname, false);
#else
  m_portname = c.read_string(s, "portname_LIN", m_portname, false);
#endif
}