29 #define XDA_STATIC_LIB
31 #include <xscontroller/xscallback.h>
32 #include <xscontroller/xscontrol_def.h>
33 #include <xscontroller/xsdevice_def.h>
34 #include <xscontroller/xsscanner.h>
35 #include <xstypes/xsdatapacket.h>
36 #include <xstypes/xsoutputconfigurationarray.h>
37 #include <xstypes/xsportinfo.h>
38 #include <xstypes/xsstatusflag.h>
41 Journaller* gJournal = 0;
45 class MyXSensCallback :
public XsCallback
55 void onLiveDataAvailable(XsDevice*,
const XsDataPacket* ptrpacket)
override
60 if (!ptrpacket)
return;
61 const XsDataPacket& packet = *ptrpacket;
68 if (packet.containsOrientation())
70 XsEuler euler = packet.orientationEuler();
72 obs->dataIsPresent[
IMU_YAW] =
true;
78 XsQuaternion quat = packet.orientationQuaternion();
89 if (packet.containsCalibratedAcceleration())
91 XsVector acc_data = packet.calibratedAcceleration();
92 obs->rawMeasurements[
IMU_X_ACC] = acc_data[0];
94 obs->rawMeasurements[
IMU_Y_ACC] = acc_data[1];
96 obs->rawMeasurements[
IMU_Z_ACC] = acc_data[2];
100 if (packet.containsCalibratedGyroscopeData())
102 XsVector gyr_data = packet.calibratedGyroscopeData();
111 if (packet.containsCalibratedMagneticField())
113 XsVector mag_data = packet.calibratedMagneticField();
114 obs->rawMeasurements[
IMU_MAG_X] = mag_data[0];
116 obs->rawMeasurements[
IMU_MAG_Y] = mag_data[1];
118 obs->rawMeasurements[
IMU_MAG_Z] = mag_data[2];
122 if (packet.containsVelocity())
124 XsVector vel_data = packet.velocity();
125 obs->rawMeasurements[
IMU_X_VEL] = vel_data[0];
127 obs->rawMeasurements[
IMU_Y_VEL] = vel_data[1];
129 obs->rawMeasurements[
IMU_Z_VEL] = vel_data[2];
133 if (packet.containsTemperature())
139 if (packet.containsAltitude())
146 #if 0 // I can't find a generic conversion between sample time and seconds!
147 if (packet.containsSampleTime64())
149 dev->getDataPacketByIndex()
150 const uint64_t nowUI = packet.sampleTime64();
151 std::cout <<
"nowUI: " << nowUI <<
"\n";
154 if (m_timeStartUI == 0)
156 m_timeStartUI = nowUI;
160 AtUI = nowUI - m_timeStartUI;
162 obs->timestamp = m_timeStartTT + std::chrono::milliseconds(AtUI);
165 if (packet.containsUtcTime())
167 XsTimeInfo utc = packet.utcTime();
173 parts.
year = utc.m_year;
174 parts.
month = utc.m_month;
175 parts.
day = utc.m_day;
176 parts.
hour = utc.m_hour;
177 parts.
minute = utc.m_minute;
178 parts.
second = utc.m_second + (utc.m_nano * 1000000000.0);
191 if (packet.containsLatitudeLongitude())
193 XsVector lla_data = packet.latitudeLongitude();
201 if (packet.containsStatus() && packet.status() & XSF_GpsValid)
206 if (packet.containsUtcTime())
208 auto utc = packet.utcTime();
211 rGPS.
UTCTime.
sec = utc.m_second + (utc.m_nano * 1000000.0);
216 ((obs->timestamp.time_since_epoch().count() /
217 (60 * 60 * ((uint64_t)1000000 / 100))) %
220 ((obs->timestamp.time_since_epoch().count() /
221 (60 * ((uint64_t)1000000 / 100))) %
224 obs->timestamp.time_since_epoch().count() /
229 if (packet.containsVelocity())
231 XsVector vel_data = packet.velocity();
234 sqrt(vel_data[0] * vel_data[0] + vel_data[1] * vel_data[1]);
242 obsGPS->setMsg(rGPSs);
243 obsGPS->timestamp = obs->timestamp;
244 obsGPS->originalReceivedTimestamp = obs->timestamp;
245 obsGPS->has_satellite_timestamp =
false;
258 XsControl* m_xscontrol =
nullptr;
259 XsDevice* m_device =
nullptr;
261 std::shared_ptr<MyXSensCallback> myCallback =
262 std::make_shared<MyXSensCallback>();
273 #if defined(_MSC_VER)
274 #pragma comment(lib, "SetupAPI.lib")
275 #pragma comment(lib, "WinUsb.lib")
278 #endif // MRPT_HAS_xSENS
291 m_impl->m_xscontrol = XsControl::construct();
292 m_impl->myCallback->me =
this;
296 "MRPT has been compiled with 'BUILD_XSENS'=OFF, so this class "
305 m_impl->m_xscontrol->destruct();
312 if (
m_impl->m_device !=
nullptr)
314 m_impl->m_device->stopRecording();
315 m_impl->m_device->closeLogFile();
316 m_impl->m_device->removeCallbackHandler(
m_impl->myCallback.get());
331 std::this_thread::sleep_for(200ms);
339 "MRPT has been compiled with 'BUILD_XSENS'=OFF, so this class "
360 cout <<
"[CIMUXSens_MT4] Scanning for USB devices...\n";
362 XsPortInfoArray portInfoArray = XsScanner::scanPorts();
364 for (
const auto& portInfo : portInfoArray)
366 if (portInfo.deviceId().isMti() || portInfo.deviceId().isMtig())
375 "CIMUXSens_MT4: No 'portname' was specified and no "
376 "compatible XSens device was found in the system (%u "
377 "devices connected)",
378 portInfoArray.size());
381 cout <<
"[CIMUXSens_MT4] Found " << portInfoArray.size()
382 <<
" devices. Opening the first one.\n";
386 cout <<
"[CIMUXSens_MT4] Using user-supplied portname '"
397 if (mtPort.deviceId().toString().c_str() !=
m_deviceId)
399 "Device with ID: %s not found",
m_deviceId.c_str());
403 "[CIMUXSens_MT4] Found a device with ID: %s @ port: %s, baudrate: "
405 mtPort.deviceId().toString().toStdString().c_str(),
406 mtPort.portName().toStdString().c_str(), mtPort.baudrate());
408 if (!
m_impl->m_xscontrol->openPort(
409 mtPort.portName().toStdString(), mtPort.baudrate()))
412 m_impl->m_device =
m_impl->m_xscontrol->device(mtPort.deviceId());
416 "[CIMUXSens_MT4] Device: %s, with ID: %s opened.",
417 m_impl->m_device->productCode().toStdString().c_str(),
418 m_impl->m_device->deviceId().toString().c_str());
420 m_impl->m_device->addCallbackHandler(
m_impl->myCallback.get());
424 cout <<
"[CIMUXSens_MT4] Putting device into configuration "
427 if (!
m_impl->m_device->gotoConfig())
431 if (!
m_impl->m_device->readEmtsAndDeviceConfiguration())
437 XsOutputConfigurationArray configArray;
438 configArray.push_back(
440 configArray.push_back(
441 XsOutputConfiguration(XDI_SampleTimeFine,
m_sampleFreq));
442 configArray.push_back(
443 XsOutputConfiguration(XDI_SampleTimeCoarse,
m_sampleFreq));
444 configArray.push_back(
446 configArray.push_back(
448 configArray.push_back(
450 configArray.push_back(
452 configArray.push_back(
453 XsOutputConfiguration(XDI_MagneticField,
m_sampleFreq));
454 configArray.push_back(
457 configArray.push_back(
459 configArray.push_back(
461 configArray.push_back(
463 configArray.push_back(
464 XsOutputConfiguration(XDI_AltitudeEllipsoid,
m_sampleFreq));
466 if (!
m_impl->m_device->setOutputConfiguration(configArray))
467 throw std::runtime_error(
468 "Could not configure MTi device. Aborting.");
473 cout <<
"[CIMUXSens_MT4] Putting device into measurement mode..."
476 if (!
m_impl->m_device->gotoMeasurement())
483 "Failed to create a log file! (%s)",
487 "[CIMUXSens_MT4] Created a log file: %s",
490 if (!
m_impl->m_device->startRecording())
496 catch (std::exception&)
499 std::cerr <<
"[CIMUXSens_MT4] Error Could not initialize the device"
506 "MRPT has been compiled with 'BUILD_XSENS'=OFF, so this class "