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
		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();
		}
Exemple #3
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
}