Map: Add PFD, first person view and path smoothing. Only send changes via CZML.

2026-06-02 06:04:39 -04:00 · 2025-06-09 10:44:17 +01:00
parent 29f7d534e5
commit ff3b3f4ef5
21 changed files with 4249 additions and 1019 deletions
@@ -34,6 +34,11 @@ void MapItem::update(SWGSDRangel::SWGMapItem *mapItem)
    } else {
        m_label = "";
    }
+    if (mapItem->getLabelDateTime()) {
+        m_labelDateTime = QDateTime::fromString(*mapItem->getLabelDateTime(), Qt::ISODateWithMs);
+    } else {
+        m_labelDateTime = QDateTime();
+    }
    m_latitude = mapItem->getLatitude();
    m_longitude = mapItem->getLongitude();
    m_altitude = mapItem->getAltitude();
@@ -47,6 +52,8 @@ QGeoCoordinate MapItem::getCoordinates()
    return coords;
 }

+WhittakerEilers ObjectMapItem::m_filter;
+
 void ObjectMapItem::update(SWGSDRangel::SWGMapItem *mapItem)
 {
    MapItem::update(mapItem);
@@ -55,6 +62,11 @@ void ObjectMapItem::update(SWGSDRangel::SWGMapItem *mapItem)
    } else {
        m_positionDateTime = QDateTime();
    }
+    if (mapItem->getAltitudeDateTime()) {
+        m_altitudeDateTime = QDateTime::fromString(*mapItem->getAltitudeDateTime(), Qt::ISODateWithMs);
+    } else {
+        m_altitudeDateTime = QDateTime();
+    }
    m_useHeadingPitchRoll = mapItem->getOrientation() == 1;
    m_heading = mapItem->getHeading();
    m_pitch = mapItem->getPitch();
@@ -79,6 +91,14 @@ void ObjectMapItem::update(SWGSDRangel::SWGMapItem *mapItem)
    }
    m_labelAltitudeOffset = mapItem->getLabelAltitudeOffset();
    m_modelAltitudeOffset = mapItem->getModelAltitudeOffset();
+    // FIXME: See nodeTransformations comment in czml.cpp
+    // We can't use nodeTransformations, so adjust altitude instead
+    if (m_modelAltitudeOffset != 0)
+    {
+        m_labelAltitudeOffset -= m_modelAltitudeOffset;
+        m_altitude += m_modelAltitudeOffset;
+        m_modelAltitudeOffset = 0;
+    }
    m_altitudeReference = mapItem->getAltitudeReference();
    m_fixedPosition = mapItem->getFixedPosition();
    QList<SWGSDRangel::SWGMapAnimation *> *animations = mapItem->getAnimations();
@@ -91,7 +111,7 @@ void ObjectMapItem::update(SWGSDRangel::SWGMapItem *mapItem)
    findFrequencies();
    if (!m_fixedPosition)
    {
-        updateTrack(mapItem->getTrack());
+        updateTrack(mapItem->getTrack(), m_itemSettings);
        updatePredictedTrack(mapItem->getPredictedTrack());
    }
    if (mapItem->getAvailableFrom()) {
@@ -104,6 +124,47 @@ void ObjectMapItem::update(SWGSDRangel::SWGMapItem *mapItem)
    } else {
        m_availableUntil = QDateTime();
    }
+    if (mapItem->getAircraftState()) {
+        if (!m_aircraftState) {
+            m_aircraftState = new MapAircraftState();
+        }
+        SWGSDRangel::SWGMapAircraftState *as = mapItem->getAircraftState();
+        if (as->getCallsign()) {
+            m_aircraftState->m_callsign = *as->getCallsign();
+        }
+        if (as->getAircraftType()) {
+            m_aircraftState->m_aircraftType = *as->getAircraftType();
+        }
+        m_aircraftState->m_onSurface = as->getOnSurface();
+        m_aircraftState->m_indicatedAirspeed = as->getAirspeed();
+        if (as->getAirspeedDateTime()) {
+            m_aircraftState->m_indicatedAirspeedDateTime = *as->getAirspeedDateTime();
+        } else {
+            m_aircraftState->m_indicatedAirspeedDateTime = QString();
+        }
+        m_aircraftState->m_trueAirspeed = as->getTrueAirspeed();
+        m_aircraftState->m_groundspeed = as->getGroundspeed();
+        m_aircraftState->m_mach = as->getMach();
+        m_aircraftState->m_altitude = as->getAltitude();
+        if (as->getAltitudeDateTime()) {
+            m_aircraftState->m_altitudeDateTime = *as->getAltitudeDateTime();
+        } else {
+            m_aircraftState->m_altitudeDateTime = QString();
+        }
+        m_aircraftState->m_qnh = as->getQnh();
+        m_aircraftState->m_verticalSpeed = as->getVerticalSpeed();
+        m_aircraftState->m_heading = as->getHeading();
+        m_aircraftState->m_track = as->getTrack();
+        m_aircraftState->m_selectedAltitude = as->getSelectedAltitude();
+        m_aircraftState->m_selectedHeading = as->getSelectedHeading();
+        m_aircraftState->m_autopilot = as->getAutopilot();
+        m_aircraftState->m_verticalMode = (MapAircraftState::VerticalMode) as->getVerticalMode();
+        m_aircraftState->m_lateralMode = (MapAircraftState::LateralMode) as->getLateralMode();
+        m_aircraftState->m_tcasMode = (MapAircraftState::TCASMode) as->getTcasMode();
+        m_aircraftState->m_windSpeed = as->getWindSpeed();
+        m_aircraftState->m_windDirection = as->getWindDirection();
+        m_aircraftState->m_staticAirTemperature = as->getStaticAirTemperature();
+    }
 }

 void ImageMapItem::update(SWGSDRangel::SWGMapItem *mapItem)
@@ -225,7 +286,145 @@ void ObjectMapItem::findFrequencies()
    }
 }

-void ObjectMapItem::updateTrack(QList<SWGSDRangel::SWGMapCoordinate *> *track)
+void ObjectMapItem::extrapolatePosition(QGeoCoordinate *c, const QDateTime& dateTime)
+{
+    int p1;
+    int p2;
+
+    // Find last two non extrapolated position
+    for (p2 = m_takenTrackPositionExtrapolated.size() - 1 ; p2 >= 0; p2--)
+    {
+        if (!m_takenTrackPositionExtrapolated[p2]) {
+            break;
+        }
+    }
+    for (p1 = p2 - 1 ; p1 >= 0; p1--)
+    {
+        if (!m_takenTrackPositionExtrapolated[p1]) {
+            break;
+        }
+    }
+
+    if (p1 < 0) {
+        return;
+    }
+
+
+    qint64 t1 = m_takenTrackDateTimes[p1]->msecsTo(*m_takenTrackDateTimes[p2]);
+    qint64 t2 = m_takenTrackDateTimes[p2]->msecsTo(dateTime);
+
+    double latV = (m_takenTrackCoords[p2]->latitude() - m_takenTrackCoords[p1]->latitude()) / t1;
+    double lonV = (m_takenTrackCoords[p2]->longitude() - m_takenTrackCoords[p1]->longitude()) / t1;
+
+    double newLat = m_takenTrackCoords[p2]->latitude() + latV * t2;
+    double newLon = m_takenTrackCoords[p2]->longitude() + lonV * t2;
+
+    c->setLatitude(newLat);
+    c->setLongitude(newLon);
+}
+
+void ObjectMapItem::extrapolateAltitude(QGeoCoordinate *c, const QDateTime& dateTime)
+{
+    int p1;
+    int p2;
+
+    // Find last two non extrapolated position
+    for (p2 = m_takenTrackPositionExtrapolated.size() - 1 ; p2 >= 0; p2--)
+    {
+        if (!m_takenTrackPositionExtrapolated[p2]) {
+            break;
+        }
+    }
+    for (p1 = p2 - 1 ; p1 >= 0; p1--)
+    {
+        if (!m_takenTrackPositionExtrapolated[p1]) {
+            break;
+        }
+    }
+
+    if (p1 < 0) {
+        return;
+    }
+
+
+    qint64 t1 = m_takenTrackDateTimes[p1]->msecsTo(*m_takenTrackDateTimes[p2]);
+    qint64 t2 = m_takenTrackDateTimes[p2]->msecsTo(dateTime);
+
+    double vertV = (m_takenTrackCoords[p2]->altitude() - m_takenTrackCoords[p1]->altitude()) / t1;
+
+    double newAlt = m_takenTrackCoords[p2]->latitude() + vertV * t2;
+
+    c->setAltitude(newAlt);
+}
+
+void ObjectMapItem::interpolatePosition(int p2, const float p3Latitude, const float p3Longitude, const QDateTime &p3DateTime)
+{
+    // p1 last non extrapolated position
+    // p2 interpolated position
+    // p3 current non extrapolated position
+
+    // Find last non extrapolated position
+    int p1;
+
+    for (p1 = p2 - 1; p1 >= 0; p1--)
+    {
+        if (!m_takenTrackPositionExtrapolated[p1]) {
+            break;
+        }
+    }
+    if (p1 < 0) {
+        return;
+    }
+
+    qint64 t1 = m_takenTrackDateTimes[p1]->msecsTo(p3DateTime);
+    qint64 t2 = m_takenTrackDateTimes[p1]->msecsTo(*m_takenTrackDateTimes[p2]);
+
+    double latV = (p3Latitude - m_takenTrackCoords[p1]->latitude()) / t1;
+    double lonV = (p3Longitude - m_takenTrackCoords[p1]->longitude()) / t1;
+
+    double newLat = m_takenTrackCoords[p1]->latitude() + latV * t2;
+    double newLon = m_takenTrackCoords[p1]->longitude() + lonV * t2;
+
+    m_takenTrackCoords[p2]->setLatitude(newLat);
+    m_takenTrackCoords[p2]->setLongitude(newLon);
+
+    m_interpolatedCoords.append(m_takenTrackCoords[p2]);
+    m_interpolatedDateTimes.append(m_takenTrackDateTimes[p2]);
+}
+
+void ObjectMapItem::interpolateAltitude(int p2, const float p3Altitude, const QDateTime &p3DateTime)
+{
+    // p1 last non extrapolated position
+    // p2 interpolated position
+    // p3 current non extrapolated position
+
+    // Find last non extrapolated position
+    int p1;
+
+    for (p1 = p2 - 1; p1 >= 0; p1--)
+    {
+        if (!m_takenTrackPositionExtrapolated[p1]) {
+            break;
+        }
+    }
+    if (p1 < 0) {
+        return;
+    }
+
+    qint64 t1 = m_takenTrackDateTimes[p1]->msecsTo(p3DateTime);
+    qint64 t2 = m_takenTrackDateTimes[p1]->msecsTo(*m_takenTrackDateTimes[p2]);
+
+    double vertV = (p3Altitude - m_takenTrackCoords[p1]->altitude()) / t1;
+
+    double newAlt = m_takenTrackCoords[p1]->altitude() + vertV * t2;
+
+    m_takenTrackCoords[p2]->setAltitude(newAlt);
+
+    m_interpolatedCoords.append(m_takenTrackCoords[p2]);
+    m_interpolatedDateTimes.append(m_takenTrackDateTimes[p2]);
+}
+
+void ObjectMapItem::updateTrack(QList<SWGSDRangel::SWGMapCoordinate *> *track, MapSettings::MapItemSettings *itemSettings)
 {
    if (track != nullptr)
    {
@@ -233,6 +432,8 @@ void ObjectMapItem::updateTrack(QList<SWGSDRangel::SWGMapCoordinate *> *track)
        m_takenTrackCoords.clear();
        qDeleteAll(m_takenTrackDateTimes);
        m_takenTrackDateTimes.clear();
+        m_takenTrackPositionExtrapolated.clear();
+        m_takenTrackAltitudeExtrapolated.clear();
        m_takenTrack.clear();
        m_takenTrack1.clear();
        m_takenTrack2.clear();
@@ -243,6 +444,8 @@ void ObjectMapItem::updateTrack(QList<SWGSDRangel::SWGMapCoordinate *> *track)
            QDateTime *d = new QDateTime(QDateTime::fromString(*p->getDateTime(), Qt::ISODate));
            m_takenTrackCoords.push_back(c);
            m_takenTrackDateTimes.push_back(d);
+            m_takenTrackPositionExtrapolated.push_back(false);
+            m_takenTrackAltitudeExtrapolated.push_back(false);
            m_takenTrack.push_back(QVariant::fromValue(*c));
        }
    }
@@ -253,28 +456,188 @@ void ObjectMapItem::updateTrack(QList<SWGSDRangel::SWGMapCoordinate *> *track)
        {
            QGeoCoordinate *c = new QGeoCoordinate(m_latitude, m_longitude, m_altitude);
            m_takenTrackCoords.push_back(c);
-            if (m_positionDateTime.isValid()) {
+            if (m_altitudeDateTime.isValid()) {
+                m_takenTrackDateTimes.push_back(new QDateTime(m_altitudeDateTime));
+            } else if (m_positionDateTime.isValid()) {
                m_takenTrackDateTimes.push_back(new QDateTime(m_positionDateTime));
            } else {
                m_takenTrackDateTimes.push_back(new QDateTime(QDateTime::currentDateTime()));
            }
+            m_takenTrackPositionExtrapolated.push_back(false);
+            m_takenTrackAltitudeExtrapolated.push_back(false);
            m_takenTrack.push_back(QVariant::fromValue(*c));
        }
        else
        {
-            QGeoCoordinate *prev = m_takenTrackCoords.last();
+            // For Whittaker-Eilers filtering, we need to make sure we don't have 2 data with the same time
+            // so we just update the last item if the prev time is the same
+            // To reduce size of list for stationary items, we only store two items with same position
+            // We store two, rather than one, so that we have the times this position was arrived at and left
+
+            const bool interpolate = false;
+
+            QGeoCoordinate *prev1 = m_takenTrackCoords.last();
+            bool samePos1 = (prev1->latitude() == m_latitude) && (prev1->longitude() == m_longitude) && (prev1->altitude() == m_altitude);
+            QGeoCoordinate *prev2 = m_takenTrackCoords.size() > 1 ? m_takenTrackCoords[m_takenTrackCoords.size() - 2] : nullptr;
+            bool samePos2 = prev2 && samePos1 ? (prev2->latitude() == m_latitude) && (prev2->longitude() == m_longitude) && (prev2->altitude() == m_altitude) : false;
+
            QDateTime *prevDateTime = m_takenTrackDateTimes.last();
-            if ((prev->latitude() != m_latitude) || (prev->longitude() != m_longitude)
-                || (prev->altitude() != m_altitude) || (*prevDateTime != m_positionDateTime))
+
+            QGeoCoordinate c(m_latitude, m_longitude, m_altitude);
+            int prevSize = m_takenTrackPositionExtrapolated.size();
+
+            if (m_altitudeDateTime.isValid() && m_positionDateTime.isValid() && (m_altitudeDateTime > m_positionDateTime))
            {
-                QGeoCoordinate *c = new QGeoCoordinate(m_latitude, m_longitude, m_altitude);
-                m_takenTrackCoords.push_back(c);
-                if (m_positionDateTime.isValid()) {
-                    m_takenTrackDateTimes.push_back(new QDateTime(m_positionDateTime));
-                } else {
-                    m_takenTrackDateTimes.push_back(new QDateTime(QDateTime::currentDateTime()));
+                if (interpolate)
+                {
+                    for (int i = m_takenTrackAltitudeExtrapolated.size() - 1; (i >= 0) && m_takenTrackAltitudeExtrapolated[i]; i--)
+                    {
+                        interpolateAltitude(i, m_altitude, m_altitudeDateTime);
+                        m_takenTrackAltitudeExtrapolated[i] = false;
+                    }
+                }
+
+                if (samePos2)
+                {
+                    *m_takenTrackDateTimes.last() = m_altitudeDateTime;
+                }
+                else
+                {
+                    extrapolatePosition(&c, m_altitudeDateTime);
+                    if (m_altitudeDateTime == *prevDateTime)
+                    {
+                        m_takenTrackPositionExtrapolated[m_takenTrackPositionExtrapolated.size() - 1] = true;
+                        *m_takenTrackCoords.last() = c;
+                    }
+                    else
+                    {
+                        m_takenTrackDateTimes.push_back(new QDateTime(m_altitudeDateTime));
+                        m_takenTrackPositionExtrapolated.push_back(true);
+                        m_takenTrackAltitudeExtrapolated.push_back(false);
+                        m_takenTrackCoords.push_back(new QGeoCoordinate(c));
+                    }
+                }
+            }
+            else if (m_positionDateTime.isValid())
+            {
+                if (interpolate)
+                {
+                    for (int i = m_takenTrackPositionExtrapolated.size() - 1; (i >= 0) && m_takenTrackPositionExtrapolated[i]; i--)
+                    {
+                        interpolatePosition(i, m_latitude, m_longitude, m_positionDateTime);
+                        m_takenTrackPositionExtrapolated[i] = false;
+                    }
+                }
+
+                if (m_positionDateTime > *m_takenTrackDateTimes.last())
+                {
+                    if (samePos2)
+                    {
+                        *m_takenTrackDateTimes.last() = m_positionDateTime;
+                    }
+                    else
+                    {
+                        bool extrapolateAlt = m_altitudeDateTime.isValid() && (m_positionDateTime > m_altitudeDateTime);
+                        if (extrapolateAlt) {
+                            extrapolateAltitude(&c, m_positionDateTime);
+                        }
+                        if (m_positionDateTime == *prevDateTime)
+                        {
+                            m_takenTrackAltitudeExtrapolated[m_takenTrackPositionExtrapolated.size() - 1] = extrapolateAlt;
+                            *m_takenTrackCoords.last() = c;
+                        }
+                        else
+                        {
+                            m_takenTrackDateTimes.push_back(new QDateTime(m_positionDateTime));
+                            m_takenTrackPositionExtrapolated.push_back(false);
+                            m_takenTrackAltitudeExtrapolated.push_back(extrapolateAlt);
+                            m_takenTrackCoords.push_back(new QGeoCoordinate(c));
+                        }
+                    }
+                }
+                else
+                {
+                    //qDebug() << "m_positionDateTime matches last datetime" << samePos1 << samePos2;
+                }
+            }
+            else
+            {
+                m_takenTrackDateTimes.push_back(new QDateTime(QDateTime::currentDateTime()));
+                m_takenTrackPositionExtrapolated.push_back(false);
+                m_takenTrackAltitudeExtrapolated.push_back(false);
+                //m_takenTrackCoords.push_back(c);
+                m_takenTrackCoords.push_back(new QGeoCoordinate(c));
+            }
+            //m_takenTrack.push_back(QVariant::fromValue(*c));
+            m_takenTrack.push_back(QVariant::fromValue(c));
+
+            if (m_takenTrackDateTimes.size() >= 2) {
+                if (*m_takenTrackDateTimes[m_takenTrackDateTimes.size() - 1] < *m_takenTrackDateTimes[m_takenTrackDateTimes.size() - 2]) {
+                    qDebug() << "Out of order";
+                }
+            }
+
+            if ((m_takenTrackPositionExtrapolated.size() > 0) && (prevSize != m_takenTrackPositionExtrapolated.size()))
+            {
+                const int filterLen = itemSettings->m_smoothingWindow;
+                if ((filterLen > 0) && (m_takenTrackCoords.size() >= filterLen) && (m_takenTrackCoords.size() % (filterLen/2)) == 0)
+                {
+                    // Filter last filterLen coords
+                    QVector<double> x(filterLen);
+                    QVector<double> y1(filterLen);
+                    QVector<double> y2(filterLen);
+                    QVector<double> y3(filterLen);
+                    QVector<double> w1(filterLen);
+                    QVector<double> w3(filterLen);
+
+                    //qDebug() << "Filter from" << (m_takenTrackCoords.size() - (filterLen - 0)) << "to" << (m_takenTrackCoords.size() - (filterLen - (filterLen - 1)));
+                    for (int i = 0; i < filterLen; i++)
+                    {
+                        int idx = m_takenTrackCoords.size() - (filterLen - i);
+                        x[i] = (m_takenTrackDateTimes[idx]->toMSecsSinceEpoch() - m_takenTrackDateTimes[0]->toMSecsSinceEpoch()) / 1000.0;
+                        y1[i] = m_takenTrackCoords[idx]->latitude();
+                        y2[i] = m_takenTrackCoords[idx]->longitude();
+                        y3[i] = m_takenTrackCoords[idx]->altitude();
+                        if (i < (filterLen / 4))
+                        {
+                            w1[i] = 10.0; // Try to avoid discontinuities between windows
+                            w3[i] = 10.0;
+                        }
+                        else if (i == filterLen - 1)
+                        {
+                            w1[i] = 1.0;
+                            w3[i] = 1.0;
+                        }
+                        else
+                        {
+                            w1[i] = m_takenTrackPositionExtrapolated[idx] ? 0.0 : 1.0;
+                            w3[i] = m_takenTrackAltitudeExtrapolated[idx] ? 0.0 : 1.0;
+                        }
+                    }
+
+                    const double lambda = itemSettings->m_smoothingLambda;
+                    m_filter.filter(x.data(), y1.data(), w1.data(), filterLen, lambda);
+                    m_filter.filter(x.data(), y2.data(), w1.data(), filterLen, lambda);
+                    m_filter.filter(x.data(), y3.data(), w3.data(), filterLen, lambda);
+
+                    for (int i = 0; i < filterLen; i++)
+                    {
+                        int idx = m_takenTrackCoords.size() - (filterLen - i);
+                        m_takenTrackCoords[idx]->setLatitude(y1[i]);
+                        m_takenTrackCoords[idx]->setLongitude(y2[i]);
+                        m_takenTrackCoords[idx]->setAltitude(y3[i]);
+                        m_takenTrackPositionExtrapolated[idx] = false;
+                        m_takenTrackAltitudeExtrapolated[idx] = false;
+
+                        m_interpolatedCoords.append(m_takenTrackCoords[idx]);
+                        m_interpolatedDateTimes.append(m_takenTrackDateTimes[idx]);
+                    }
+
+                    // Update current position
+                    m_latitude = m_takenTrackCoords[filterLen-1]->latitude();
+                    m_longitude = m_takenTrackCoords[filterLen-1]->longitude();
+                    m_altitude = m_takenTrackCoords[filterLen-1]->altitude();
                }
-                m_takenTrack.push_back(QVariant::fromValue(*c));
            }
        }
    }