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 ofxXsens::update() { if (callback.packetAvailable()) { // Retrieve a packet XsDataPacket packet = callback.getNextPacket(); //Get the quaternion data XsQuaternion quaternion = packet.orientationQuaternion(); ofLogVerbose("ofxXsens") << "\r" << "q0:" << std::setw(5) << std::fixed << std::setprecision(2) << quaternion.m_w << ",q1:" << std::setw(5) << std::fixed << std::setprecision(2) << quaternion.m_x << ",q2:" << std::setw(5) << std::fixed << std::setprecision(2) << quaternion.m_y << ",q3:" << std::setw(5) << std::fixed << std::setprecision(2) << quaternion.m_z ; // Convert packet to euler euler = packet.orientationEuler(); ofLogVerbose("ofxXsens") << ",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 ; // GPS if(packet.containsLatitudeLongitude()) { lat_long = packet.latitudeLongitude(); ofLogVerbose("ofxXsens") << "\n" << "Latitude:" << std::setprecision(9) << lat_long[0] << " Longitude:" << lat_long[1]; } else if(packet.containsPositionLLA()) { ofLogVerbose("ofxXsens") << "\n" << "Packet contains position LLA data" ; } else if(packet.containsRawGpsSol()){ ofLogVerbose("ofxXsens") << "\n" << "Packet contains RAW GPS data" ; } } }
/*------------------------------------------------------------- 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); } }