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;
}
Exemple #2
0
/*-------------------------------------------------------------
					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);
		}
}