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 }