/*! \brief Read all messages from the buffered read data after adding new data supplied in \a rawIn
\details This function will read all present messages in the read buffer. In order for this function
to work, you need to call readDataToBuffer() first.
\param rawIn The buffered data in which to search for messages
\param messages The messages found in the data
\return The messages that were read.
*/
XsResultValue processBufferedData(XsByteArray& rawIn, XsMessageArray& messages)
{
ProtocolHandler protocol;
if (rawIn.size())
m_dataBuffer.append(rawIn);
int popped = 0;
messages.clear();
for(;;)
{
XsByteArray raw(m_dataBuffer.data()+popped, m_dataBuffer.size()-popped);
XsMessage message;
MessageLocation location = protocol.findMessage(message, raw);
if (location.isValid())
{
// message is valid, remove data from cache
popped += location.m_size + location.m_startPos;
messages.push_back(message);
}
else
{
if (popped)
m_dataBuffer.pop_front(popped);
if (messages.empty())
return XRV_TIMEOUTNODATA;
return XRV_OK;
}
}
}
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();
}
/*! \brief Wait for the requested XsXbusMessageId
\param xmid The message id to wait for
\param rcv The received message
\return Whether the requested message was found
*/
bool waitForMessage(XsXbusMessageId xmid, XsMessage& rcv)
{
XsByteArray data;
XsMessageArray msgs;
bool foundAck = false;
do
{
readDataToBuffer(data);
processBufferedData(data, msgs);
for (XsMessageArray::iterator it = msgs.begin(); it != msgs.end(); ++it)
if ((*it).getMessageId() == xmid)
{
foundAck = true;
rcv = *it;
}
} while (!foundAck);
return foundAck;
}
/*-------------------------------------------------------------
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
}
