void Packet::interpolate(const Packet& pa, const Packet& pb, const double f)
{
pa.getDataSize();
pb.getDataSize();
*this = pb;
if (m_itemCount != pa.m_itemCount)
return;
getDataSize();
CmtShortVector va, vb, vc;
CmtVector fa, fb, fc;
CmtQuat qf;
CmtEuler ea, eb, ec;
CmtMatrix mf;
Quaternion qa, qb, qc;
Matrix3x3 m;
double g = 1.0-f;
double da, db, dc;
uint16_t sa, sb, sc;
// walk over all items and interpolate all data items
for (uint16_t i = 0; i < m_itemCount; ++i)
{
//// raw data
if (pa.m_infoList[i].m_rawAcc != CMT_DATA_ITEM_NOT_AVAILABLE && pb.m_infoList[i].m_rawAcc != CMT_DATA_ITEM_NOT_AVAILABLE)
{
va = pa.getRawAcc(i); vb = pb.getRawAcc(i);
vc.m_data[0] = (uint16_t) (g*(double)va.m_data[0] + f*(double)vb.m_data[0]);
vc.m_data[1] = (uint16_t) (g*(double)va.m_data[1] + f*(double)vb.m_data[1]);
vc.m_data[2] = (uint16_t) (g*(double)va.m_data[2] + f*(double)vb.m_data[2]);
updateRawAcc(vc,i);
}
if (pa.m_infoList[i].m_rawGyr != CMT_DATA_ITEM_NOT_AVAILABLE && pb.m_infoList[i].m_rawGyr != CMT_DATA_ITEM_NOT_AVAILABLE)
{
va = pa.getRawGyr(i); vb = pb.getRawGyr(i);
vc.m_data[0] = (uint16_t) (g*(double)va.m_data[0] + f*(double)vb.m_data[0]);
vc.m_data[1] = (uint16_t) (g*(double)va.m_data[1] + f*(double)vb.m_data[1]);
vc.m_data[2] = (uint16_t) (g*(double)va.m_data[2] + f*(double)vb.m_data[2]);
updateRawGyr(vc,i);
}
if (pa.m_infoList[i].m_rawMag != CMT_DATA_ITEM_NOT_AVAILABLE && pb.m_infoList[i].m_rawMag != CMT_DATA_ITEM_NOT_AVAILABLE)
{
va = pa.getRawMag(i); vb = pb.getRawMag(i);
vc.m_data[0] = (uint16_t) (g*(double)va.m_data[0] + f*(double)vb.m_data[0]);
vc.m_data[1] = (uint16_t) (g*(double)va.m_data[1] + f*(double)vb.m_data[1]);
vc.m_data[2] = (uint16_t) (g*(double)va.m_data[2] + f*(double)vb.m_data[2]);
updateRawMag(vc,i);
}
if (pa.m_infoList[i].m_rawTemp != CMT_DATA_ITEM_NOT_AVAILABLE && pb.m_infoList[i].m_rawTemp != CMT_DATA_ITEM_NOT_AVAILABLE)
{
da = (double) pa.getRawTemp(i); db = (double) pb.getRawTemp(i);
dc = g*da + f*db;
updateRawTemp((uint16_t) dc,i);
}
//// calibrated data
if (pa.m_infoList[i].m_calAcc != CMT_DATA_ITEM_NOT_AVAILABLE && pb.m_infoList[i].m_calAcc != CMT_DATA_ITEM_NOT_AVAILABLE)
{
fa = pa.getCalAcc(i); fb = pb.getCalAcc(i);
fc.m_data[0] = (g*(double)fa.m_data[0] + f*(double)fb.m_data[0]);
fc.m_data[1] = (g*(double)fa.m_data[1] + f*(double)fb.m_data[1]);
fc.m_data[2] = (g*(double)fa.m_data[2] + f*(double)fb.m_data[2]);
updateCalAcc(fc,i);
}
if (pa.m_infoList[i].m_calGyr != CMT_DATA_ITEM_NOT_AVAILABLE && pb.m_infoList[i].m_calGyr != CMT_DATA_ITEM_NOT_AVAILABLE)
{
fa = pa.getCalGyr(i); fb = pb.getCalGyr(i);
fc.m_data[0] = (g*(double)fa.m_data[0] + f*(double)fb.m_data[0]);
fc.m_data[1] = (g*(double)fa.m_data[1] + f*(double)fb.m_data[1]);
fc.m_data[2] = (g*(double)fa.m_data[2] + f*(double)fb.m_data[2]);
updateCalGyr(fc,i);
}
if (pa.m_infoList[i].m_calMag != CMT_DATA_ITEM_NOT_AVAILABLE && pb.m_infoList[i].m_calMag != CMT_DATA_ITEM_NOT_AVAILABLE)
{
fa = pa.getCalMag(i); fb = pb.getCalMag(i);
fc.m_data[0] = (g*(double)fa.m_data[0] + f*(double)fb.m_data[0]);
fc.m_data[1] = (g*(double)fa.m_data[1] + f*(double)fb.m_data[1]);
fc.m_data[2] = (g*(double)fa.m_data[2] + f*(double)fb.m_data[2]);
updateCalMag(fc,i);
}
//// orientations
if (pa.m_infoList[i].m_oriQuat != CMT_DATA_ITEM_NOT_AVAILABLE && pb.m_infoList[i].m_oriQuat != CMT_DATA_ITEM_NOT_AVAILABLE)
{
qf = pa.getOriQuat(i);
qa.isArray(qf.m_data);
qf = pb.getOriQuat(i);
qb.isArray(qf.m_data);
qc.isSlerp(qa,qb,f);
qc.setArray(qf.m_data);
updateOriQuat(qf,i);
}
if (pa.m_infoList[i].m_oriEul != CMT_DATA_ITEM_NOT_AVAILABLE && pb.m_infoList[i].m_oriEul != CMT_DATA_ITEM_NOT_AVAILABLE)
{
ea = pa.getOriEuler(i); eb = pb.getOriEuler(i);
ec.m_roll = (g*(double)ea.m_roll + f*(double)eb.m_roll);
ec.m_pitch = (g*(double)ea.m_pitch + f*(double)eb.m_pitch);
ec.m_yaw = (g*(double)ea.m_yaw + f*(double)eb.m_yaw);
updateOriEuler(ec,i);
}
if (pa.m_infoList[i].m_oriMat != CMT_DATA_ITEM_NOT_AVAILABLE && pb.m_infoList[i].m_oriMat != CMT_DATA_ITEM_NOT_AVAILABLE)
{
mf = pa.getOriMatrix(i);
m.isArray(&mf.m_data[0][0]);
qa.isRmat(m);
mf = pb.getOriMatrix(i);
m.isArray(&mf.m_data[0][0]);
qb.isRmat(m);
qc.isSlerp(qa,qb,f);
m.isQuat(qc);
m.setArray(&mf.m_data[0][0]);
updateOriMatrix(mf,i);
}
//// other
if (pa.m_infoList[i].m_sc != CMT_DATA_ITEM_NOT_AVAILABLE && pb.m_infoList[i].m_sc != CMT_DATA_ITEM_NOT_AVAILABLE)
{
sa = pa.getSampleCounter(i); sb = pb.getSampleCounter(i);
sc = sb - sa;
dc = f*(double) sc;
sc = sa + (uint16_t) doubleToLong(dc);
updateSampleCounter(sc,i);
}
if (pa.m_infoList[i].m_acc_g != CMT_DATA_ITEM_NOT_AVAILABLE && pb.m_infoList[i].m_acc_g != CMT_DATA_ITEM_NOT_AVAILABLE)
{
fa = pa.getAccG(i); fb = pb.getAccG(i);
fc.m_data[0] = (g*(double)fa.m_data[0] + f*(double)fb.m_data[0]);
fc.m_data[1] = (g*(double)fa.m_data[1] + f*(double)fb.m_data[1]);
fc.m_data[2] = (g*(double)fa.m_data[2] + f*(double)fb.m_data[2]);
updateAccG(fc,i);
}
}
// some remaining stuff
// TOA remains equal to pb TOA
m_rtc = (TimeStamp) (g*(double) pa.m_rtc + f*(double) pb.m_rtc);
}
