GPS差值

GPS差值

GPS更新频率一般是1-25HZ，要想更快可以通过差值的方法实现

#include <QCoreApplication>
#include <QList>
#include <QDateTime>
#include <QDebug>
#include <qmath.h>
#include <windows.h>

struct GpsPoint {
    QDateTime t;
    double lat, lon;   // deg
    float  speed, cog; // m/s, deg
};

/* 线性插值模板 */
template <typename T>
T lerp(T a, T b, double t) { return a + (b - a) * t; }

/* 1 Hz → 5 Hz 插值 */
QList<GpsPoint> interpolate5Hz(const QList<GpsPoint> &src)
{
    QList<GpsPoint> out;
    if (src.size() < 2) return src;

    for (int i = 0; i < src.size() - 1; ++i) {
        const GpsPoint &p0 = src[i];
        const GpsPoint &p1 = src[i + 1];
        double dtSec = p0.t.msecsTo(p1.t) / 1000.0;
        if (qFuzzyIsNull(dtSec)) continue;

        for (int j = 0; j < 5; ++j) {   // 0.0, 0.2, 0.4, 0.6, 0.8 s
            double t = j * 0.2 / dtSec; // 0.0 ~ 1.0
            if (t > 1.0) break;

            GpsPoint p;
            p.t   = p0.t.addMSecs(j * 200);           // 每 200 ms
            p.lat = lerp(p0.lat, p1.lat, t);
            p.lon = lerp(p0.lon, p1.lon, t);
            p.speed = lerp(p0.speed, p1.speed, t);
            p.cog   = lerp(p0.cog, p1.cog, t);
            out.append(p);
        }
    }
    // 补最后一个点
    out.append(src.last());
    return out;
}

/* 测试：1 Hz 输入 → 5 Hz 输出 */
int main(int argc, char *argv[])
{
    QCoreApplication a(argc, argv);

    /* 模拟 1 Hz GPS 数据（3 个点） */
    QList<GpsPoint> src = {
        {QDateTime::fromString("2025-06-23T14:00:00", Qt::ISODate), 31.0, 121.0, 5.0f, 45.0f},
        {QDateTime::fromString("2025-06-23T14:00:01", Qt::ISODate), 31.001, 121.002, 6.0f, 50.0f},
        {QDateTime::fromString("2025-06-23T14:00:02", Qt::ISODate), 31.002, 121.004, 7.0f, 55.0f}
    };
    qDebug() << src[0].t.toString("hh:mm:ss.zzz") << src[0].lat << src[0].lon << src[0].speed << src[0].cog;
    qDebug() << src[1].t.toString("hh:mm:ss.zzz") << src[1].lat << src[1].lon << src[1].speed << src[1].cog;
    qDebug() << src[2].t.toString("hh:mm:ss.zzz") << src[2].lat << src[2].lon << src[2].speed << src[2].cog;

    int  ii = 0;
    //while(1)
    {
        qDebug("ii:%d\n", ii++);
        Sleep(1000);
        QList<GpsPoint> dst = interpolate5Hz(src);
        for (const GpsPoint &p : dst)
        {
            qDebug() << p.t.toString("hh:mm:ss.zzz") << p.lat << p.lon << p.speed << p.cog;
        }
    }

    return a.exec();
}