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); }