bool XSensINS::Iterate() {
AppCastingMOOSApp::Iterate();
// Read Data
XsByteArray data;
XsMessageArray msgs;
m_device.readDataToBuffer(data);
m_device.processBufferedData(data, msgs);
for (XsMessageArray::iterator it = msgs.begin(); it != msgs.end(); ++it) {
// Retrieve a packet
XsDataPacket packet;
packet.setMessage((*it));
// Convert packet to Euler
if(packet.containsOrientation()){
m_euler = packet.orientationEuler();
Notify("IMU_PITCH", -m_euler.pitch());
Notify("IMU_ROLL", m_euler.roll());
Notify("IMU_YAW", -m_euler.yaw() + m_yaw_declination);
}
// Acceleration
if(packet.containsCalibratedAcceleration()){
m_acceleration = packet.calibratedAcceleration();
Notify("IMU_ACC_X", m_acceleration[0]);
Notify("IMU_ACC_Y", m_acceleration[1]);
Notify("IMU_ACC_Z", m_acceleration[2]);
}
//Gyro
if(packet.containsCalibratedGyroscopeData()){
m_gyro = packet.calibratedGyroscopeData();
Notify("IMU_GYR_X", m_gyro[0]*180.0/M_PI);
Notify("IMU_GYR_Y", m_gyro[1]*180.0/M_PI);
Notify("IMU_GYR_Z", m_gyro[2]*180.0/M_PI);
}
//Magneto
if(packet.containsCalibratedMagneticField()){
m_mag = packet.calibratedMagneticField();
Notify("IMU_MAG_X", m_mag[0]);
Notify("IMU_MAG_Y", m_mag[1]);
Notify("IMU_MAG_Z", m_mag[2]);
Notify("IMU_MAG_N", sqrt(pow(m_mag[0], 2) + pow(m_mag[1], 2) + pow(m_mag[2], 2)));
}
//LatLon
if(packet.containsLatitudeLongitude()){
m_latlon = packet.latitudeLongitude();
Notify("INS_LAT", m_latlon[0]);
Notify("INS_LONG", m_latlon[1]);
}
}
msgs.clear();
AppCastingMOOSApp::PostReport();
return true;
}
void IMU::UpdateData(IMU_DATA &data)
{
XsByteArray imuData;
XsMessageArray msgs;
while (msgs.empty())
{
Aris::Core::Sleep(1);
pDevice->readDataToBuffer(imuData);
pDevice->processBufferedData(imuData, msgs);
}
//std::cout << "msg num:" << msgs.size()<<std::endl;
for (XsMessageArray::iterator it = msgs.begin(); it != msgs.end(); ++it)
{
// Retrieve a packet
XsDataPacket packet;
if ((*it).getMessageId() == XMID_MtData2)
{
packet.setMessage((*it));
packet.setDeviceId(pDevice->mtPort.deviceId());
}
// Get the all data
auto eul = packet.orientationEuler();
auto sdi = packet.sdiData();
auto acc = packet.calibratedAcceleration();
data.yaw = eul.yaw()*PI / 180;
data.pitch = eul.pitch()*PI / 180;
data.roll = eul.roll()*PI / 180;
data.va = sdi.orientationIncrement().x() * 2 * 100;
data.vb = sdi.orientationIncrement().y() * 2 * 100;
data.vc = sdi.orientationIncrement().z() * 2 * 100;
std::copy_n(acc.data(), acc.size(), data.acc);
data.time = packet.timeOfArrival().nowMs();
data.pmLhs = *pDevice->pmImuGround2BodyGround;
data.pmRhs = *pDevice->pmBody2Imu;
}
msgs.clear();
}
/*-------------------------------------------------------------
doProcess
-------------------------------------------------------------*/
void CIMUXSens_MT4::doProcess()
{
#if MRPT_HAS_xSENS_MT4
if(m_state == ssError)
{
mrpt::system::sleep(200);
initialize();
}
if(m_state == ssError)
return;
XsByteArray data;
XsMessageArray msgs;
my_xsens_device.readDataToBuffer(data);
my_xsens_device.processBufferedData(data, msgs);
for (XsMessageArray::iterator it = msgs.begin(); it != msgs.end(); ++it)
{
// Retrieve a packet
XsDataPacket packet;
if ((*it).getMessageId() == XMID_MtData)
{
LegacyDataPacket lpacket(1, false);
lpacket.setMessage((*it));
lpacket.setXbusSystem(false, false);
lpacket.setDeviceId(my_xsens_devid, 0);
lpacket.setDataFormat(XOM_Orientation, XOS_OrientationMode_Euler | XOS_Timestamp_PacketCounter | XOS_CalibratedMode_All/*XOS_OrientationMode_Quaternion*/,0); //lint !e534
XsDataPacket_assignFromXsLegacyDataPacket(&packet, &lpacket, 0);
}
else if ((*it).getMessageId() == XMID_MtData2) {
packet.setMessage((*it));
packet.setDeviceId(my_xsens_devid);
}
// Data properly collected: extract data fields
// -------------------------------------------------
m_state = ssWorking;
CObservationIMUPtr obs = CObservationIMU::Create();
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 (packet.containsSampleTime64())
{
const uint64_t nowUI = packet.sampleTime64();
uint64_t AtUI = 0;
if( m_timeStartUI == 0 )
{
m_timeStartUI = nowUI;
m_timeStartTT = mrpt::system::now();
}
else
AtUI = nowUI - m_timeStartUI;
double AtDO = AtUI * 1000.0; // Difference in intervals of 100 nsecs
obs->timestamp = m_timeStartTT + AtDO;
}
else if (packet.containsUtcTime())
{
XsUtcTime 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 obs->timestamp = mrpt::system::now();
obs->sensorPose = m_sensorPose;
obs->sensorLabel = m_sensorLabel;
appendObservation(obs);
if (packet.containsLatitudeLongitude())
{
XsVector lla_data = packet.latitudeLongitude();
CObservationGPSPtr obsGPS = CObservationGPSPtr( new CObservationGPS() );
CObservationGPS::TGPSDatum_RMC& rGPS = obsGPS->RMC_datum;
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())
{
XsUtcTime 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 / (60 * 60 * ((uint64_t)1000000 / 100))) % 24);
rGPS.UTCTime.minute = ((obs->timestamp / (60 * ((uint64_t)1000000 / 100))) % 60);
rGPS.UTCTime.sec = fmod(obs->timestamp / (1000000.0 / 100), 60);
}
obsGPS->has_RMC_datum = true;
obsGPS->timestamp = obs->timestamp;
obsGPS->sensorPose = m_sensorPose;
obsGPS->sensorLabel = m_sensorLabel;
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;
appendObservation(obsGPS);
}
std::cout << std::flush;
}
msgs.clear();
#else
THROW_EXCEPTION("MRPT has been compiled with 'BUILD_XSENS_MT4'=OFF, so this class cannot be used.");
#endif
}
/**
* \brief Fills data using XsDataPacket object
* @param _packet Incoming packet from Xsens device.
*/
void SensorData::fillData(XsDataPacket * _packet){
XsDataPacket packet = *_packet;
XsMessage msg = packet.toMessage();
XsSize msg_size = msg.getDataSize();
ROS_INFO_STREAM_THROTTLE(THROTTLE_VALUE, "Data Size: " << msg_size);
// Get the orientation data
if (packet.containsOrientation()) {
XsQuaternion quaternion = packet.orientationQuaternion();
q1 = quaternion.m_x;
q2 = quaternion.m_y;
q3 = quaternion.m_z;
q0 = quaternion.m_w;
XsEuler euler = packet.orientationEuler();
eroll = euler.m_roll;
epitch = euler.m_pitch;
eyaw = euler.m_yaw;
ROS_INFO_STREAM_THROTTLE(THROTTLE_VALUE,"Orientation: Roll:" << std::setw(7) << std::fixed << std::setprecision(2) << euler.m_roll
<< ", Pitch:" << std::setw(7) << std::fixed << std::setprecision(2) << euler.m_pitch
<< ", Yaw:" << std::setw(7) << std::fixed << std::setprecision(2) << euler.m_yaw);
}
// Get the gyroscope data
if (packet.containsCalibratedGyroscopeData()) {
XsVector gyroscope = packet.calibratedGyroscopeData();
gyrX = gyroscope.at(0);
gyrY = gyroscope.at(1);
gyrZ = gyroscope.at(2);
ROS_INFO_STREAM_THROTTLE(THROTTLE_VALUE, "Angular Velocity: ( "
<< gyrX << ", " << gyrY << ", " << gyrZ << ")" );
}
// Get the acceleration data
if (packet.containsCalibratedAcceleration()) {
XsVector acceleration = packet.calibratedAcceleration();
accX = acceleration.at(0);
accY = acceleration.at(1);
accZ = acceleration.at(2);
ROS_INFO_STREAM_THROTTLE(THROTTLE_VALUE, "Linear Acceleration: ("
<< accX << "," << accX << "," << accZ << ")" );
}
//Get the altitude data
if(packet.containsAltitude()){
mAltitude = packet.altitude();
ROS_INFO_STREAM_THROTTLE(THROTTLE_VALUE, "containsAltitude: ");
}
//Get the Latitude and Longitude Data
if(packet.containsLatitudeLongitude()){
XsVector latlon = packet.latitudeLongitude();
mLatitude = latlon[0];
mLongitude = latlon[1];
ROS_INFO_STREAM_THROTTLE(THROTTLE_VALUE, "containsLatitudeLongitude");
}
// Get the magnetic field data
if (packet.containsCalibratedMagneticField()) {
XsVector magneticField = packet.calibratedMagneticField();
ROS_INFO_STREAM_THROTTLE(THROTTLE_VALUE, "Magnetic Field: ("<< magneticField[0] <<" , "
<< magneticField[1] << " , "<< magneticField[2] << ")");
magX = magneticField.at(0);
magY = magneticField.at(1);
magZ = magneticField.at(2);
}
// Get Temperature data
if (packet.containsTemperature()){
double temperature = packet.temperature();
ROS_INFO_STREAM_THROTTLE(THROTTLE_VALUE, "Temperature : " << temperature);
mTemperature = temperature;
}
// Get Pressure Data
if(packet.containsPressure() ){
XsPressure pressure = packet.pressure();
ROS_INFO_STREAM_THROTTLE(THROTTLE_VALUE, "Pressure : " << pressure.m_pressure);
mPressure= pressure.m_pressure;
}
// Get Velocity Data
if( packet.containsVelocity() ){
XsVector velocity = packet.velocity();
mVelocityX = velocity.at(0);
mVelocityY = velocity.at(1);
mVelocityZ = velocity.at(2);
ROS_INFO_STREAM(" Velocity [ x (east) : " << mVelocityX << ", y (north) : "
<< mVelocityY << ", z (up) " << mVelocityZ << " ]");
}
// Get GPS PVT Data
if( packet.containsGpsPvtData() ){
XsGpsPvtData gpsPvtData = packet.gpsPvtData();
ROS_INFO_STREAM(" Horizontal accuracy: " << gpsPvtData.m_hacc );
}
if(packet.containsRawAcceleration() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "Raw Acceleration") ;}
if(packet.containsRawGyroscopeData() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "Raw Gyroscope") ;}
if(packet.containsRawGyroscopeTemperatureData() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "RawGyrTemp");}
if(packet.containsRawMagneticField() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "RawMag");}
if(packet.containsRawTemperature() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "RawTemp");}
if(packet.containsRawData() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "RawData");}
if(packet.containsCalibratedData() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "Calib Data");}
if(packet.containsSdiData() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "SDI");}
//if(packet.containsStatusByte() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "StatusByte");}
if(packet.containsDetailedStatus() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "DetailedStatus");}
if(packet.containsPacketCounter8() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "PacketCounter8");}
if(packet.containsPacketCounter() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "PacketCounter");}
if(packet.containsSampleTimeFine() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "SampleTimeFine");}
if(packet.containsSampleTimeCoarse() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "SampleTimeCoarse");}
if(packet.containsSampleTime64() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "SampleTime64");}
if(packet.containsFreeAcceleration() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "FreeAcceleration");}
if(packet.containsPressureAge() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "PressureAge");}
if(packet.containsAnalogIn1Data() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "AN1IN");}
if(packet.containsAnalogIn2Data() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "AN2IN");}
if(packet.containsPositionLLA() ){
XsVector posLLA = packet.positionLLA();
mLatitude = posLLA[0];
mLongitude = posLLA[1];
mAltitude = posLLA[2];
ROS_INFO_STREAM_THROTTLE(THROTTLE_VALUE, "mLatitude = " << mLatitude);
ROS_INFO_STREAM_THROTTLE(THROTTLE_VALUE, "mLongitude = " << mLongitude);
ROS_INFO_STREAM_THROTTLE(THROTTLE_VALUE, "mAltitude = " << mAltitude);
//for(int i=0; i < posLLA.size(); i++){
//ROS_INFO_STREAM("posLLA[" << i << "] : " << posLLA[i] );
//}
}
if(packet.containsUtcTime() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "UTC-Time");}
if(packet.containsFrameRange() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "Frame Range");}
if(packet.containsRssi() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "RSSI");}
if(packet.containsRawGpsDop() ){
//ROS_INFO_THROTTLE(THROTTLE_VALUE, "DOP");}
XsRawGpsDop dop = packet.rawGpsDop();
m_gdop = ((float) dop.m_gdop)/100;
m_pdop = ((float) dop.m_pdop)/100;
m_tdop = ((float) dop.m_tdop)/100;
m_vdop = ((float) dop.m_vdop)/100;
m_hdop = ((float) dop.m_hdop)/100;
m_edop = ((float) dop.m_edop)/100;
m_ndop = ((float) dop.m_ndop)/100;
m_itow = ((float) dop.m_itow)/1000;
ROS_INFO_STREAM_THROTTLE(THROTTLE_VALUE,
"GDOP = " << m_gdop <<
",PDOP = " << m_pdop <<
",TDOP = " << m_tdop <<
",VDOP = " << m_vdop <<
",HDOP = " << m_hdop <<
",EDOP = " << m_edop <<
",NDOP = " << m_ndop <<
",ITOW = " << m_itow);
}
if(packet.containsRawGpsSol() ){
// ROS_INFO_THROTTLE(THROTTLE_VALUE, "SOL");}
XsRawGpsSol sol = packet.rawGpsSol();
mPositionAccuracy = sol.m_pacc;
mSpeedAccuracy = sol.m_sacc;
mSatelliteNumber = sol.m_numsv;
mGpsFixStatus = sol.m_gpsfix;
gpsVelocityX=sol.m_ecef_vx/100;
gpsVelocityY=sol.m_ecef_vy/100;
gpsVelocityZ=sol.m_ecef_vz/100;
ROS_INFO_STREAM_THROTTLE(THROTTLE_VALUE,
"Pos Acc = " << mPositionAccuracy <<
",Speed Acc = " << mSpeedAccuracy <<
",Sat Number = " << mSatelliteNumber <<
",GPS FIX = " << std::hex << mGpsFixStatus);
}
if(packet.containsRawGpsTimeUtc() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "GPS-UTC");}
if(packet.containsRawGpsSvInfo() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "SV INFO");}
// if(packet.containsTriggerIndication() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "TRIGGER");}
//Get Status byte
if( packet.containsStatus() ){
mStatus = packet.status();
ROS_INFO_THROTTLE(10, "Status: %.8X", mStatus);
}
}