Go to the documentation of this file.
25 #if MRPT_HAS_xSENS_MT4
61 DeviceClass() : m_streamInterface(nullptr) {}
64 if (m_streamInterface)
66 delete m_streamInterface;
67 m_streamInterface =
nullptr;
82 if (m_streamInterface->open(portInfo) !=
XRV_OK)
return false;
90 if (m_streamInterface) m_streamInterface->close();
105 const int maxSz = 8192;
107 if (raw.size())
return XRV_OK;
126 if (rawIn.size()) m_dataBuffer.append(rawIn);
134 m_dataBuffer.data() + popped, m_dataBuffer.size() - popped);
142 messages.push_back(message);
146 if (popped) m_dataBuffer.pop_front(popped);
164 bool foundAck =
false;
167 readDataToBuffer(
data);
168 processBufferedData(
data, msgs);
171 if ((*it).getMessageId() == xmid)
189 return (m_streamInterface->writeData(raw) ==
XRV_OK);
206 bool gotoMeasurement()
224 const char* pc = (
const char*)rcv.getDataBuffer(0);
225 std::string result(pc ? pc :
"", rcv.getDataSize());
226 std::string::size_type thingy = result.find(
" ");
229 result.begin() + thingy, result.end());
247 return rcv.getDataLong();
264 sndOM.setDataShort((
uint16_t)outputMode);
270 snd.setDataLong((
uint32_t)outputSettings);
285 if (config.size() == 0)
288 snd.setDataShort(0, 2);
294 snd.setDataShort((
uint16_t)config[i].m_dataIdentifier, i * 4);
295 snd.setDataShort(config[i].m_frequency, i * 4 + 2);
310 #define my_xsens_device (*static_cast<DeviceClass*>(m_dev_ptr))
311 #define my_xsens_devid (*static_cast<XsDeviceId*>(m_devid_ptr))
314 #if MRPT_HAS_xSENS_MT4
317 #if defined(_MSC_VER)
318 #pragma comment(lib, "SetupAPI.lib")
319 #pragma comment(lib, "WinUsb.lib")
322 #endif // MRPT_HAS_xSENS_MT4
327 CIMUXSens_MT4::CIMUXSens_MT4()
339 #if MRPT_HAS_xSENS_MT4
344 "MRPT has been compiled with 'BUILD_XSENS_MT4'=OFF, so this class "
354 #if MRPT_HAS_xSENS_MT4
356 delete static_cast<DeviceClass*
>(
m_dev_ptr);
369 #if MRPT_HAS_xSENS_MT4
372 std::this_thread::sleep_for(200ms);
403 packet.setMessage((*it));
412 if (packet.containsOrientation())
414 XsEuler euler = packet.orientationEuler();
416 obs->dataIsPresent[
IMU_YAW] =
true;
420 obs->dataIsPresent[
IMU_ROLL] =
true;
433 if (packet.containsCalibratedAcceleration())
435 XsVector acc_data = packet.calibratedAcceleration();
436 obs->rawMeasurements[
IMU_X_ACC] = acc_data[0];
438 obs->rawMeasurements[
IMU_Y_ACC] = acc_data[1];
440 obs->rawMeasurements[
IMU_Z_ACC] = acc_data[2];
444 if (packet.containsCalibratedGyroscopeData())
446 XsVector gyr_data = packet.calibratedGyroscopeData();
455 if (packet.containsCalibratedMagneticField())
457 XsVector mag_data = packet.calibratedMagneticField();
458 obs->rawMeasurements[
IMU_MAG_X] = mag_data[0];
460 obs->rawMeasurements[
IMU_MAG_Y] = mag_data[1];
462 obs->rawMeasurements[
IMU_MAG_Z] = mag_data[2];
466 if (packet.containsVelocity())
468 XsVector vel_data = packet.velocity();
469 obs->rawMeasurements[
IMU_X_VEL] = vel_data[0];
471 obs->rawMeasurements[
IMU_Y_VEL] = vel_data[1];
473 obs->rawMeasurements[
IMU_Z_VEL] = vel_data[2];
477 if (packet.containsTemperature())
483 if (packet.containsAltitude())
490 if (packet.containsSampleTime64())
492 const uint64_t nowUI = packet.sampleTime64();
507 else if (packet.containsUtcTime())
532 if (packet.containsLatitudeLongitude())
534 XsVector lla_data = packet.latitudeLongitude();
543 if (packet.containsStatus() && packet.status() &
XSF_GpsValid)
548 if (packet.containsUtcTime())
558 ((obs->timestamp / (60 * 60 * ((
uint64_t)1000000 / 100))) %
561 ((obs->timestamp / (60 * ((
uint64_t)1000000 / 100))) % 60);
562 rGPS.
UTCTime.
sec = fmod(obs->timestamp / (1000000.0 / 100), 60);
565 if (packet.containsVelocity())
567 XsVector vel_data = packet.velocity();
570 sqrt(vel_data[0] * vel_data[0] + vel_data[1] * vel_data[1]);
578 obsGPS->setMsg(rGPSs);
579 obsGPS->timestamp = obs->timestamp;
580 obsGPS->originalReceivedTimestamp = obs->timestamp;
581 obsGPS->has_satellite_timestamp =
false;
591 "MRPT has been compiled with 'BUILD_XSENS_MT4'=OFF, so this class "
601 #if MRPT_HAS_xSENS_MT4
612 cout <<
"[CIMUXSens_MT4] Scanning for USB devices...\n";
615 if (portInfoArray.empty())
617 "CIMUXSens_MT4: No 'portname' was specified and no XSens "
618 "device was found after scanning the system!")
621 cout <<
"[CIMUXSens_MT4] Found " << portInfoArray.size()
622 <<
" devices. Opening the first one.\n";
629 cout <<
"[CIMUXSens_MT4] Using user-supplied portname '"
631 portInfoArray.push_back(portInfo);
638 cout <<
"[CIMUXSens_MT4] Opening port "
639 << mtPort.portName().toStdString() << std::endl;
642 throw std::runtime_error(
"Could not open port. Aborting.");
646 cout <<
"[CIMUXSens_MT4] Putting device into configuration "
651 throw std::runtime_error(
652 "Could not put device into configuration mode. Aborting.");
660 if (!mtPort.deviceId().isMtix() && !mtPort.deviceId().isMtMk4())
662 throw std::runtime_error(
663 "No MTi / MTx / MTmk4 device found. Aborting.");
665 cout <<
"[CIMUXSens_MT4] Found a device with id: "
666 << mtPort.deviceId().toString().toStdString()
667 <<
" @ port: " << mtPort.portName().toStdString()
668 <<
", baudrate: " << mtPort.baudrate() << std::endl;
672 cout <<
"[CIMUXSens_MT4] Device: "
678 cout <<
"[CIMUXSens_MT4] Configuring the device..." << std::endl;
679 if (mtPort.deviceId().isMtix())
691 throw std::runtime_error(
692 "Could not configure MT device. Aborting.");
694 else if (mtPort.deviceId().isMtMk4())
697 configArray.push_back(
699 configArray.push_back(
701 configArray.push_back(
703 configArray.push_back(
705 configArray.push_back(
707 configArray.push_back(
709 configArray.push_back(
711 configArray.push_back(
713 configArray.push_back(
716 configArray.push_back(
718 configArray.push_back(
720 configArray.push_back(
722 configArray.push_back(
726 throw std::runtime_error(
727 "Could not configure MTmk4 device. Aborting.");
731 throw std::runtime_error(
732 "Unknown device while configuring. Aborting.");
737 cout <<
"[CIMUXSens_MT4] Putting device into measurement mode..."
740 throw std::runtime_error(
741 "Could not put device into measurement mode. Aborting.");
745 catch (std::exception&)
748 std::cerr <<
"Error Could not initialize the device" << std::endl;
754 "MRPT has been compiled with 'BUILD_XSENS_MT4'=OFF, so this class "
767 configSource.
read_float(iniSection,
"pose_x", 0,
false),
768 configSource.
read_float(iniSection,
"pose_y", 0,
false),
769 configSource.
read_float(iniSection,
"pose_z", 0,
false),
@ IMU_MAG_Y
y magnetic field value (local/vehicle frame) (gauss)
int8_t validity_char
This will be: 'A'=OK or 'V'=void.
struct XsOutputConfiguration XsOutputConfiguration
@ IMU_ALTITUDE
altitude from an altimeter (meters)
struct XsByteArray XsByteArray
unsigned __int16 uint16_t
double speed_knots
Measured speed (in knots)
Message protocol handling class.
struct XsMessageArray XsMessageArray
@ IMU_Z_VEL
z-axis velocity (global/navigation frame) (m/sec)
bool setDataFormat(const XsDataFormat &format, int32_t index=0)
Sets the data format of the device with the given index to format.
void setDeviceId(XsDeviceId deviceId, int32_t index)
Sets the device ID of the device with the given index to deviceid.
@ XRV_OK
Operation was performed successfully.
@ IMU_PITCH
orientation pitch absolute value (global/navigation frame) (rad)
void setFromValues(const double x0, const double y0, const double z0, const double yaw=0, const double pitch=0, const double roll=0)
Set the pose from a 3D position (meters) and yaw/pitch/roll angles (radians) - This method recomputes...
@ XMID_SetOutputSettingsAck
mrpt::system::TTimeStamp buildTimestampFromParts(const mrpt::system::TTimeParts &p)
Builds a timestamp from the parts (Parts are in UTC)
struct XsOutputConfigurationArray XsOutputConfigurationArray
@ IMU_MAG_X
x magnetic field value (local/vehicle frame) (gauss)
@ XOS_Timestamp_PacketCounter
double second
Minute (0-59)
@ IMU_ROLL_VEL
roll angular velocity (local/vehicle frame) (rad/sec)
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
XsOutputMode
Bit values for legacy output mode.
mrpt::poses::CPose3D m_sensorPose
@ IMU_ROLL
orientation roll absolute value (global/navigation frame) (rad)
Contains classes for various device interfaces.
void setMessage(const XsMessage &message)
Set the source message to msg.
#define THROW_EXCEPTION(msg)
This namespace contains representation of robot actions and observations.
@ XOS_OrientationMode_Euler
std::string read_string(const std::string §ion, const std::string &name, const std::string &defaultValue, bool failIfNotFound=false) const
The parts of a date/time (it's like the standard 'tm' but with fractions of seconds).
mrpt::system::TTimeStamp m_timeStartTT
XsResultValue
Xsens result values.
@ XMID_SetOutputConfigurationAck
A structure for storing UTC Time values.
@ IMU_ORI_QUAT_W
Orientation Quaternion W (global/navigation frame)
int read_int(const std::string §ion, const std::string &name, int defaultValue, bool failIfNotFound=false) const
virtual MessageLocation findMessage(XsMessage &rcv, const XsByteArray &raw) const
Find the first message in the raw byte stream.
bool xsEnumerateUsbDevices(XsPortInfoList &ports)
Enumerate Xsens USB devices.
virtual ~CIMUXSens_MT4()
Destructor.
void setXbusSystem(bool xbus, bool convert=false)
Sets the xbus flag.
double direction_degrees
Measured speed direction (in degrees)
double latitude_degrees
The measured latitude, in degrees (North:+ , South:-)
Contains data received from a device or read from a file.
void loadConfig_sensorSpecific(const mrpt::config::CConfigFileBase &configSource, const std::string &iniSection)
See the class documentation at the top for expected parameters.
Stores the location of a message in a buffer using a start position and a size.
GLsizei GLsizei GLenum GLenum const GLvoid * data
@ IMU_PITCH_VEL
pitch angular velocity (local/vehicle frame) (rad/sec)
@ IMU_Y_VEL
y-axis velocity (global/navigation frame) (m/sec)
An IoInterface for dealing specifically with Xsens USB devices.
static int composeMessage(XsByteArray &raw, const XsMessage &msg)
Compose a message for transmission.
double longitude_degrees
The measured longitude, in degrees (East:+ , West:-)
struct XsPortInfoArray XsPortInfoArray
int m_size
The size of the message, when less than 0 it indicates the expected message size.
This class allows loading and storing values and vectors of different types from a configuration text...
@ IMU_Z_ACC
z-axis acceleration (local/vehicle frame) (m/sec2)
content_t fields
Message content, accesible by individual fields.
@ IMU_MAG_Z
z magnetic field value (local/vehicle frame) (gauss)
std::shared_ptr< CObservationIMU > Ptr
@ IMU_ORI_QUAT_Y
Orientation Quaternion Y (global/navigation frame)
@ IMU_TEMPERATURE
temperature (degrees Celsius)
uint8_t m_second
The second (if time is valid)
uint8_t m_minute
The minute (if time is valid)
void initialize()
Turns on the xSens device and configure it for getting orientation data.
@ XMID_SetOutputConfiguration
unsigned __int64 uint64_t
uint8_t m_month
The month (if date is valid)
std::shared_ptr< CObservationGPS > Ptr
void appendObservation(const mrpt::serialization::CSerializable::Ptr &obj)
Like appendObservations() but for just one observation.
size_t XsSize
XsSize must be unsigned number!
@ IMU_X_ACC
x-axis acceleration (local/vehicle frame) (m/sec2)
int daylight_saving
Day of week (1:Sunday, 7:Saturday)
XsXbusMessageId
Xsens Xbus Message Identifiers.
struct XsDeviceId XsDeviceId
std::string m_sensorLabel
See CGenericSensor.
float read_float(const std::string §ion, const std::string &name, float defaultValue, bool failIfNotFound=false) const
uint8_t m_hour
The hour (if time is valid)
@ IMU_ORI_QUAT_Z
Orientation Quaternion Z (global/navigation frame)
std::string m_portname
The USB or COM port name (if blank -> autodetect)
int m_port_bauds
Baudrate, only for COM ports.
Contains an MTData XsMessage and supports functions for extracting contained data.
XsOutputSettings
Bit values for output settings.
#define IMPLEMENTS_GENERIC_SENSOR(class_name, NameSpace)
This must be inserted in all CGenericSensor classes implementation files:
A class for interfacing XSens 4th generation Inertial Measuring Units (IMUs): MTi 10-series,...
@ XMID_GotoMeasurementAck
uint32_t m_nano
Nanosecond part of the time.
This class stores messages from GNSS or GNSS+IMU devices, from consumer-grade inexpensive GPS receive...
uint8_t day_of_week
Seconds (0.0000-59.9999)
Structure for storing a single message.
@ IMU_Y_ACC
y-axis acceleration (local/vehicle frame) (m/sec2)
void doProcess()
This method will be invoked at a minimum rate of "process_rate" (Hz)
uint8_t m_day
The day of the month (if date is valid)
The low-level serial communication class.
uint8_t minute
Hour (0-23)
@ IMU_YAW
orientation yaw absolute value (global/navigation frame) (rad)
GLsizei const GLchar ** string
UTC_time UTCTime
The GPS sensor measured timestamp (in UTC time)
@ IMU_YAW_VEL
yaw angular velocity (local/vehicle frame) (rad/sec)
void XsDataPacket_assignFromXsLegacyDataPacket(struct XsDataPacket *thisPtr, struct LegacyDataPacket const *pack, int index)
uint16_t m_year
The year (if date is valid)
@ IMU_ORI_QUAT_X
Orientation Quaternion X (global/navigation frame)
unsigned __int32 uint32_t
@ IMU_X_VEL
x-axis velocity (global/navigation frame) (m/sec)
double DEG2RAD(const double x)
Degrees to radians.
Page generated by Doxygen 1.8.17 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at miƩ 12 jul 2023 10:03:34 CEST | |