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
}