Debian Codegraph @

libs/hwdrivers/src/CIMUXSens_MT4.cpp

raw
1 /* +------------------------------------------------------------------------+
2 | Mobile Robot Programming Toolkit (MRPT) |
3 | https://www.mrpt.org/ |
4 | |
5 | Copyright (c) 2005-2026, Individual contributors, see AUTHORS file |
6 | See: https://www.mrpt.org/Authors - All rights reserved. |
7 | Released under BSD License. See: https://www.mrpt.org/License |
8 +------------------------------------------------------------------------+ */
9
10 #include "hwdrivers-precomp.h" // Precompiled headers
11 //
12 #include <mrpt/hwdrivers/CIMUXSens_MT4.h>
13 #include <mrpt/obs/CObservationGPS.h>
14 #include <mrpt/obs/CObservationIMU.h>
15
16 #include <chrono>
17 #include <iostream>
18 #include <thread>
19
20 #if !MRPT_HAS_xSENS
21 namespace mrpt::hwdrivers
22 {
23 struct CIMUXSens_MT4::Impl
24 {
25 };
26 } // namespace mrpt::hwdrivers
27 #else
28
29 // Tell MSVC that we are not using MTSDK as DLL:
30 #define XDA_STATIC_LIB
31
32 #include <xscontroller/xscallback.h>
33 #include <xscontroller/xscontrol_def.h>
34 #include <xscontroller/xsdevice_def.h>
35 #include <xscontroller/xsscanner.h>
36 #include <xstypes/xsdatapacket.h>
37 #include <xstypes/xsoutputconfigurationarray.h>
38 #include <xstypes/xsportinfo.h>
39 #include <xstypes/xsstatusflag.h>
40
41 // MTSDK expects this global symbol as "extern":
42 Journaller* gJournal = 0;
43
44 namespace mrpt::hwdrivers
45 {
46 class MyXSensCallback : public XsCallback
47 {
48 public:
49 CIMUXSens_MT4* me = nullptr;
50
51 private:
52 // uint64_t m_timeStartUI = 0;
53 mrpt::system::TTimeStamp m_timeStartTT;
54
55 protected:
56 void onLiveDataAvailable(XsDevice*, const XsDataPacket* ptrpacket) override
57 {
58 using namespace mrpt::obs;
59
60 if (!me) return;
61 if (!ptrpacket) return;
62 const XsDataPacket& packet = *ptrpacket;
63
64 // Data properly collected: extract data fields
65 // -------------------------------------------------
66 me->m_state = mrpt::hwdrivers::CGenericSensor::ssWorking;
67 CObservationIMU::Ptr obs = std::make_shared<CObservationIMU>();
68
69 if (packet.containsOrientation())
70 {
71 XsEuler euler = packet.orientationEuler();
72 obs->rawMeasurements[IMU_YAW] = DEG2RAD(euler.yaw());
73 obs->dataIsPresent[IMU_YAW] = true;
74 obs->rawMeasurements[IMU_PITCH] = DEG2RAD(euler.pitch());
75 obs->dataIsPresent[IMU_PITCH] = true;
76 obs->rawMeasurements[IMU_ROLL] = DEG2RAD(euler.roll());
77 obs->dataIsPresent[IMU_ROLL] = true;
78
79 XsQuaternion quat = packet.orientationQuaternion();
80 obs->rawMeasurements[IMU_ORI_QUAT_X] = quat.x();
81 obs->dataIsPresent[IMU_ORI_QUAT_X] = true;
82 obs->rawMeasurements[IMU_ORI_QUAT_Y] = quat.y();
83 obs->dataIsPresent[IMU_ORI_QUAT_Y] = true;
84 obs->rawMeasurements[IMU_ORI_QUAT_Z] = quat.z();
85 obs->dataIsPresent[IMU_ORI_QUAT_Z] = true;
86 obs->rawMeasurements[IMU_ORI_QUAT_W] = quat.w();
87 obs->dataIsPresent[IMU_ORI_QUAT_W] = true;
88 }
89
90 if (packet.containsCalibratedAcceleration())
91 {
92 XsVector acc_data = packet.calibratedAcceleration();
93 obs->rawMeasurements[IMU_X_ACC] = acc_data[0];
94 obs->dataIsPresent[IMU_X_ACC] = true;
95 obs->rawMeasurements[IMU_Y_ACC] = acc_data[1];
96 obs->dataIsPresent[IMU_Y_ACC] = true;
97 obs->rawMeasurements[IMU_Z_ACC] = acc_data[2];
98 obs->dataIsPresent[IMU_Z_ACC] = true;
99 }
100
101 if (packet.containsCalibratedGyroscopeData())
102 {
103 XsVector gyr_data = packet.calibratedGyroscopeData();
104 obs->rawMeasurements[IMU_YAW_VEL] = gyr_data[2];
105 obs->dataIsPresent[IMU_YAW_VEL] = true;
106 obs->rawMeasurements[IMU_PITCH_VEL] = gyr_data[1];
107 obs->dataIsPresent[IMU_PITCH_VEL] = true;
108 obs->rawMeasurements[IMU_ROLL_VEL] = gyr_data[0];
109 obs->dataIsPresent[IMU_ROLL_VEL] = true;
110 }
111
112 if (packet.containsCalibratedMagneticField())
113 {
114 XsVector mag_data = packet.calibratedMagneticField();
115 obs->rawMeasurements[IMU_MAG_X] = mag_data[0];
116 obs->dataIsPresent[IMU_MAG_X] = true;
117 obs->rawMeasurements[IMU_MAG_Y] = mag_data[1];
118 obs->dataIsPresent[IMU_MAG_Y] = true;
119 obs->rawMeasurements[IMU_MAG_Z] = mag_data[2];
120 obs->dataIsPresent[IMU_MAG_Z] = true;
121 }
122
123 if (packet.containsVelocity())
124 {
125 XsVector vel_data = packet.velocity();
126 obs->rawMeasurements[IMU_X_VEL] = vel_data[0];
127 obs->dataIsPresent[IMU_X_VEL] = true;
128 obs->rawMeasurements[IMU_Y_VEL] = vel_data[1];
129 obs->dataIsPresent[IMU_Y_VEL] = true;
130 obs->rawMeasurements[IMU_Z_VEL] = vel_data[2];
131 obs->dataIsPresent[IMU_Z_VEL] = true;
132 }
133
134 if (packet.containsTemperature())
135 {
136 obs->rawMeasurements[IMU_TEMPERATURE] = packet.temperature();
137 obs->dataIsPresent[IMU_TEMPERATURE] = true;
138 }
139
140 if (packet.containsAltitude())
141 {
142 obs->rawMeasurements[IMU_ALTITUDE] = packet.altitude();
143 obs->dataIsPresent[IMU_ALTITUDE] = true;
144 }
145
146 // TimeStamp
147 #if 0 // I can't find a generic conversion between sample time and seconds!
148 if (packet.containsSampleTime64())
149 {
150 dev->getDataPacketByIndex()
151 const uint64_t nowUI = packet.sampleTime64();
152 std::cout << "nowUI: " << nowUI << "\n";
153
154 uint64_t AtUI = 0;
155 if (m_timeStartUI == 0)
156 {
157 m_timeStartUI = nowUI;
158 m_timeStartTT = mrpt::Clock::now();
159 }
160 else
161 AtUI = nowUI - m_timeStartUI; // ms
162
163 obs->timestamp = m_timeStartTT + std::chrono::milliseconds(AtUI);
164 }
165 else
166 if (packet.containsUtcTime())
167 {
168 XsTimeInfo utc = packet.utcTime();
169
170 mrpt::system::TTimeParts parts;
171
172 parts.day_of_week = 0;
173 parts.daylight_saving = 0;
174 parts.year = utc.m_year;
175 parts.month = utc.m_month;
176 parts.day = utc.m_day;
177 parts.hour = utc.m_hour;
178 parts.minute = utc.m_minute;
179 parts.second = utc.m_second + (utc.m_nano * 1000000000.0);
180
181 obs->timestamp = mrpt::system::buildTimestampFromParts(parts);
182 }
183 else
184 #endif
185 obs->timestamp = mrpt::Clock::now();
186
187 obs->sensorPose = me->m_sensorPose;
188 obs->sensorLabel = me->m_sensorLabel;
189
190 me->appendObservation(obs);
191
192 if (packet.containsLatitudeLongitude())
193 {
194 XsVector lla_data = packet.latitudeLongitude();
195
196 CObservationGPS::Ptr obsGPS = CObservationGPS::Create();
197 gnss::Message_NMEA_RMC rGPSs;
198 gnss::Message_NMEA_RMC::content_t& rGPS = rGPSs.fields;
199 rGPS.latitude_degrees = lla_data[0];
200 rGPS.longitude_degrees = lla_data[1];
201
202 if (packet.containsStatus() && packet.status() & XSF_GpsValid)
203 rGPS.validity_char = 'A';
204 else
205 rGPS.validity_char = 'V';
206
207 if (packet.containsUtcTime())
208 {
209 auto utc = packet.utcTime();
210 rGPS.UTCTime.hour = utc.m_hour;
211 rGPS.UTCTime.minute = utc.m_minute;
212 rGPS.UTCTime.sec = utc.m_second + (utc.m_nano * 1000000.0);
213 }
214 else
215 {
216 rGPS.UTCTime.hour =
217 ((obs->timestamp.time_since_epoch().count() / (60 * 60 * ((uint64_t)1000000 / 100))) %
218 24);
219 rGPS.UTCTime.minute =
220 ((obs->timestamp.time_since_epoch().count() / (60 * ((uint64_t)1000000 / 100))) % 60);
221 rGPS.UTCTime.sec = fmod(obs->timestamp.time_since_epoch().count() / (1000000.0 / 100), 60);
222 }
223
224 if (packet.containsVelocity())
225 {
226 XsVector vel_data = packet.velocity();
227
228 rGPS.speed_knots = sqrt(vel_data[0] * vel_data[0] + vel_data[1] * vel_data[1]);
229 rGPS.direction_degrees = 0; // Could be worked out from
230 // velocity and magnatic field
231 // perhaps.
232 }
233 else
234 rGPS.speed_knots = rGPS.direction_degrees = 0;
235
236 obsGPS->setMsg(rGPSs);
237 obsGPS->timestamp = obs->timestamp;
238 obsGPS->originalReceivedTimestamp = obs->timestamp;
239 obsGPS->has_satellite_timestamp = false;
240 obsGPS->sensorPose = me->m_sensorPose;
241 obsGPS->sensorLabel = me->m_sensorLabel;
242
243 me->appendObservation(obsGPS);
244 }
245 }
246 };
247
248 struct CIMUXSens_MT4::Impl
249 {
250 Impl() = default;
251
252 XsControl* m_xscontrol = nullptr;
253 XsDevice* m_device = nullptr;
254 XsPortInfo m_port;
255 std::shared_ptr<MyXSensCallback> myCallback = std::make_shared<MyXSensCallback>();
256 };
257
258 } // namespace mrpt::hwdrivers
259
260 #endif
261
262 // Include libraries in linking:
263 #if MRPT_HAS_xSENS
264 #ifdef _WIN32
265 // WINDOWS:
266 #if defined(_MSC_VER)
267 #pragma comment(lib, "SetupAPI.lib")
268 #pragma comment(lib, "WinUsb.lib")
269 #endif
270 #endif // _WIN32
271 #endif // MRPT_HAS_xSENS
272
273 IMPLEMENTS_GENERIC_SENSOR(CIMUXSens_MT4, mrpt::hwdrivers)
274
275 using namespace mrpt::obs;
276 using namespace mrpt::hwdrivers;
277 using namespace std;
278
279 CIMUXSens_MT4::CIMUXSens_MT4() : m_impl(mrpt::make_impl<CIMUXSens_MT4::Impl>())
280 {
281 m_sensorLabel = "XSensMTi_MT4";
282
283 #if MRPT_HAS_xSENS
284 m_impl->m_xscontrol = XsControl::construct();
285 m_impl->myCallback->me = this;
286
287 #else
288 THROW_EXCEPTION(
289 "MRPT has been compiled with 'MRPT_WITH_XSENS'=OFF, so this class "
290 "cannot be used.");
291 #endif
292 }
293
294 CIMUXSens_MT4::~CIMUXSens_MT4()
295 {
296 #if MRPT_HAS_xSENS
297 close();
298 m_impl->m_xscontrol->destruct();
299 #endif
300 }
301
302 void CIMUXSens_MT4::close()
303 {
304 #if MRPT_HAS_xSENS
305 if (m_impl->m_device != nullptr)
306 {
307 m_impl->m_device->stopRecording();
308 m_impl->m_device->closeLogFile();
309 m_impl->m_device->removeCallbackHandler(m_impl->myCallback.get());
310 }
311 m_impl->m_xscontrol->closePort(m_impl->m_port);
312
313 #endif
314 }
315
316 /*-------------------------------------------------------------
317 doProcess
318 -------------------------------------------------------------*/
319 void CIMUXSens_MT4::doProcess()
320 {
321 #if MRPT_HAS_xSENS
322 if (m_state == ssError)
323 {
324 std::this_thread::sleep_for(200ms);
325 initialize();
326 }
327
328 // Main processing happens asynchronously via callbacks.
329
330 #else
331 THROW_EXCEPTION(
332 "MRPT has been compiled with 'MRPT_WITH_XSENS'=OFF, so this class "
333 "cannot be used.");
334 #endif
335 }
336
337 /*-------------------------------------------------------------
338 initialize
339 -------------------------------------------------------------*/
340 void CIMUXSens_MT4::initialize()
341 {
342 #if MRPT_HAS_xSENS
343 m_state = ssInitializing;
344
345 try
346 {
347 // Try to open a specified device, or scan the bus?
348 XsPortInfo mtPort;
349
350 if (m_portname.empty())
351 {
352 if (m_verbose) cout << "[CIMUXSens_MT4] Scanning for USB devices...\n";
353
354 XsPortInfoArray portInfoArray = XsScanner::scanPorts();
355
356 for (const auto& portInfo : portInfoArray)
357 {
358 if (portInfo.deviceId().isMti() || portInfo.deviceId().isMtig())
359 {
360 mtPort = portInfo;
361 break;
362 }
363 }
364
365 if (mtPort.empty())
366 THROW_EXCEPTION_FMT(
367 "CIMUXSens_MT4: No 'portname' was specified and no "
368 "compatible XSens device was found in the system (%u "
369 "devices connected)",
370 static_cast<unsigned int>(portInfoArray.size()));
371
372 if (m_verbose)
373 cout << "[CIMUXSens_MT4] Found " << portInfoArray.size()
374 << " devices. Opening the first one.\n";
375 }
376 else
377 {
378 cout << "[CIMUXSens_MT4] Using user-supplied portname '" << m_portname << "' at "
379 << m_port_bauds << " baudrate.\n";
380
381 mtPort = XsPortInfo(m_portname, XsBaud_numericToRate(m_port_bauds));
382 }
383
384 if (mtPort.empty()) THROW_EXCEPTION("No MTi device found");
385
386 // Use the first detected device or the specified one:
387 if (!m_deviceId.empty())
388 {
389 if (mtPort.deviceId().toString().c_str() != m_deviceId)
390 THROW_EXCEPTION_FMT("Device with ID: %s not found", m_deviceId.c_str());
391 }
392
393 std::cout << mrpt::format(
394 "[CIMUXSens_MT4] Found a device with ID: %s @ port: %s, baudrate: "
395 "%d",
396 mtPort.deviceId().toString().toStdString().c_str(), mtPort.portName().toStdString().c_str(),
397 mtPort.baudrate());
398
399 if (!m_impl->m_xscontrol->openPort(mtPort.portName().toStdString(), mtPort.baudrate()))
400 THROW_EXCEPTION("Could not open port");
401
402 m_impl->m_device = m_impl->m_xscontrol->device(mtPort.deviceId());
403 ASSERT_(m_impl->m_device != nullptr);
404
405 std::cout << mrpt::format(
406 "[CIMUXSens_MT4] Device: %s, with ID: %s opened.",
407 m_impl->m_device->productCode().toStdString().c_str(),
408 m_impl->m_device->deviceId().toString().c_str());
409
410 m_impl->m_device->addCallbackHandler(m_impl->myCallback.get());
411
412 // Put the device in configuration mode
413 if (m_verbose)
414 cout << "[CIMUXSens_MT4] Putting device into configuration "
415 "mode...\n";
416
417 if (!m_impl->m_device->gotoConfig()) THROW_EXCEPTION("Could not go to config");
418
419 // read EMTS and device config stored in .mtb file header.
420 if (!m_impl->m_device->readEmtsAndDeviceConfiguration())
421 THROW_EXCEPTION("Could not read device configuration");
422
423 // Set configuration:
424 // if (mtPort.deviceId().isMti())
425 {
426 XsOutputConfigurationArray configArray;
427 configArray.push_back(XsOutputConfiguration(XDI_SampleTime64, m_sampleFreq));
428 configArray.push_back(XsOutputConfiguration(XDI_SampleTimeFine, m_sampleFreq));
429 configArray.push_back(XsOutputConfiguration(XDI_SampleTimeCoarse, m_sampleFreq));
430 configArray.push_back(XsOutputConfiguration(XDI_Quaternion, m_sampleFreq));
431 configArray.push_back(XsOutputConfiguration(XDI_Temperature, m_sampleFreq));
432 configArray.push_back(XsOutputConfiguration(XDI_Acceleration, m_sampleFreq));
433 configArray.push_back(XsOutputConfiguration(XDI_RateOfTurn, m_sampleFreq));
434 configArray.push_back(XsOutputConfiguration(XDI_MagneticField, m_sampleFreq));
435 configArray.push_back(XsOutputConfiguration(XDI_VelocityXYZ, m_sampleFreq));
436
437 configArray.push_back(XsOutputConfiguration(XDI_StatusByte, m_sampleFreq));
438 configArray.push_back(XsOutputConfiguration(XDI_LatLon, m_sampleFreq));
439 configArray.push_back(XsOutputConfiguration(XDI_UtcTime, m_sampleFreq));
440 configArray.push_back(XsOutputConfiguration(XDI_AltitudeEllipsoid, m_sampleFreq));
441
442 if (!m_impl->m_device->setOutputConfiguration(configArray))
443 throw std::runtime_error("Could not configure MTi device. Aborting.");
444 }
445
446 // Put the device in measurement mode
447 if (m_verbose) cout << "[CIMUXSens_MT4] Putting device into measurement mode..." << std::endl;
448
449 if (!m_impl->m_device->gotoMeasurement())
450 THROW_EXCEPTION("Could not put device into measurement mode");
451
452 if (!m_xsensLogFile.empty())
453 {
454 if (m_impl->m_device->createLogFile(m_xsensLogFile) != XRV_OK)
455 THROW_EXCEPTION_FMT("Failed to create a log file! (%s)", m_xsensLogFile.c_str());
456 else
457 printf("[CIMUXSens_MT4] Created a log file: %s", m_xsensLogFile.c_str());
458
459 if (!m_impl->m_device->startRecording()) THROW_EXCEPTION("Could not start recording");
460 }
461
462 m_state = ssWorking;
463 }
464 catch (std::exception&)
465 {
466 m_state = ssError;
467 std::cerr << "[CIMUXSens_MT4] Error Could not initialize the device" << std::endl;
468 throw;
469 }
470
471 #else
472 THROW_EXCEPTION(
473 "MRPT has been compiled with 'MRPT_WITH_XSENS'=OFF, so this class "
474 "cannot be used.");
475 #endif
476 }
477
478 /*-------------------------------------------------------------
479 loadConfig_sensorSpecific
480 -------------------------------------------------------------*/
481 void CIMUXSens_MT4::loadConfig_sensorSpecific(
482 const mrpt::config::CConfigFileBase& c, const std::string& s)
483 {
484 m_sensorPose.setFromValues(
485 c.read_float(s, "pose_x", 0, false), c.read_float(s, "pose_y", 0, false),
486 c.read_float(s, "pose_z", 0, false), DEG2RAD(c.read_float(s, "pose_yaw", 0, false)),
487 DEG2RAD(c.read_float(s, "pose_pitch", 0, false)),
488 DEG2RAD(c.read_float(s, "pose_roll", 0, false)));
489
490 m_sampleFreq = c.read_int(s, "sampleFreq", m_sampleFreq, false);
491 m_port_bauds = c.read_int(s, "baudRate", m_port_bauds, false);
492 m_deviceId = c.read_string(s, "deviceId", m_deviceId, false);
493 m_xsensLogFile = c.read_string(s, "logFile", m_xsensLogFile, false);
494
495 #ifdef _WIN32
496 m_portname = c.read_string(s, "portname_WIN", m_portname, false);
497 #else
498 m_portname = c.read_string(s, "portname_LIN", m_portname, false);
499 #endif
500 }