Issue #94 Add new FSD packet type VisualPilotDataUpdate

src/blackcore/fsd/fsdclient.cpp

@@ -30,6 +30,7 @@
 #include "blackcore/fsd/serializer.h"
 #include "blackcore/fsd/servererror.h"
 #include "blackcore/fsd/interimpilotdataupdate.h"
+#include "blackcore/fsd/visualpilotdataupdate.h"
 #include "blackcore/fsd/planeinforequest.h"
 #include "blackcore/fsd/planeinformation.h"
 #include "blackcore/fsd/planeinforequestfsinn.h"
@@ -114,6 +115,9 @@ namespace BlackCore::Fsd
         m_interimPositionUpdateTimer.setObjectName(this->objectName().append(":m_interimPositionUpdateTimer"));
         connect(&m_interimPositionUpdateTimer, &QTimer::timeout, this, &CFSDClient::sendInterimPilotDataUpdate);
 
+        m_visualPositionUpdateTimer.setObjectName(this->objectName().append(":m_visualPositionUpdateTimer"));
+        connect(&m_visualPositionUpdateTimer, &QTimer::timeout, this, &CFSDClient::sendVisualPilotDataUpdate);
+
         m_scheduledConfigUpdate.setObjectName(this->objectName().append(":m_scheduledConfigUpdate"));
         connect(&m_scheduledConfigUpdate, &QTimer::timeout, this, &CFSDClient::sendIncrementalAircraftConfig);
 
@@ -382,6 +386,44 @@ namespace BlackCore::Fsd
         }
     }
 
+    void CFSDClient::sendVisualPilotDataUpdate()
+    {
+        if (this->getConnectionStatus().isDisconnected() && ! m_unitTestMode) { return; }
+        if (m_loginMode == CLoginMode::Observer || !isVisualPositionSendingEnabledForServer()) { return; }
+        const CSimulatedAircraft myAircraft(getOwnAircraft());
+
+        static constexpr double minVelocity = 0.00005;
+        if (std::abs(myAircraft.getVelocity().getVelocityX(CSpeedUnit::m_s())) < minVelocity &&
+            std::abs(myAircraft.getVelocity().getVelocityY(CSpeedUnit::m_s())) < minVelocity &&
+            std::abs(myAircraft.getVelocity().getVelocityZ(CSpeedUnit::m_s())) < minVelocity &&
+            std::abs(myAircraft.getVelocity().getPitchVelocity(CAngleUnit::rad(), CTimeUnit::s())) < minVelocity &&
+            std::abs(myAircraft.getVelocity().getRollVelocity(CAngleUnit::rad(), CTimeUnit::s())) < minVelocity &&
+            std::abs(myAircraft.getVelocity().getHeadingVelocity(CAngleUnit::rad(), CTimeUnit::s())) < minVelocity)
+        {
+            if (m_stoppedSendingVisualPositions) { return; }
+            m_stoppedSendingVisualPositions = true;
+        }
+        else
+        {
+            m_stoppedSendingVisualPositions = false;
+        }
+
+        VisualPilotDataUpdate visualPilotDataUpdate(getOwnCallsignAsString(),
+                myAircraft.latitude().value(CAngleUnit::deg()),
+                myAircraft.longitude().value(CAngleUnit::deg()),
+                myAircraft.getAltitude().value(CLengthUnit::ft()),
+                myAircraft.getPitch().value(CAngleUnit::deg()),
+                myAircraft.getBank().value(CAngleUnit::deg()),
+                myAircraft.getHeading().normalizedTo360Degrees().value(CAngleUnit::deg()),
+                myAircraft.getVelocity().getVelocityX(CSpeedUnit::m_s()),
+                myAircraft.getVelocity().getVelocityY(CSpeedUnit::m_s()),
+                myAircraft.getVelocity().getVelocityZ(CSpeedUnit::m_s()),
+                myAircraft.getVelocity().getPitchVelocity(CAngleUnit::rad(), CTimeUnit::s()),
+                myAircraft.getVelocity().getRollVelocity(CAngleUnit::rad(), CTimeUnit::s()),
+                myAircraft.getVelocity().getHeadingVelocity(CAngleUnit::rad(), CTimeUnit::s()));
+        sendQueudedMessage(visualPilotDataUpdate);
+    }
+
     void CFSDClient::sendAtcDataUpdate(double latitude, double longitude)
     {
         const AtcDataUpdate atcDataUpdate(getOwnCallsignAsString(), 199998, CFacilityType::OBS, 300, AtcRating::Observer, latitude, longitude, 0);
@@ -975,6 +1017,7 @@ namespace BlackCore::Fsd
         m_messageTypeMapping["$DI"] = MessageType::FsdIdentification;
         m_messageTypeMapping["$!!"] = MessageType::KillRequest;
         m_messageTypeMapping["@"]   = MessageType::PilotDataUpdate;
+        m_messageTypeMapping["^"]   = MessageType::VisualPilotDataUpdate;
         m_messageTypeMapping["$PI"] = MessageType::Ping;
         m_messageTypeMapping["$PO"] = MessageType::Pong;
         m_messageTypeMapping["$ER"] = MessageType::ServerError;
@@ -1174,6 +1217,31 @@ namespace BlackCore::Fsd
         emit euroscopeSimDataUpdatedReceived(situation, parts, currentOffsetTime(data.sender()), data.m_model, data.m_livery);
     }
 
+    void CFSDClient::handleVisualPilotDataUpdate(const QStringList &tokens)
+    {
+        const VisualPilotDataUpdate dataUpdate = VisualPilotDataUpdate::fromTokens(tokens);
+        const CCallsign callsign(dataUpdate.sender(), CCallsign::Aircraft);
+
+        CAircraftSituation situation(
+            callsign,
+            CCoordinateGeodetic(dataUpdate.m_latitude, dataUpdate.m_longitude, dataUpdate.m_altitudeTrue),
+            CHeading(dataUpdate.m_heading, CHeading::True, CAngleUnit::deg()),
+            CAngle(dataUpdate.m_pitch, CAngleUnit::deg()),
+            CAngle(dataUpdate.m_bank, CAngleUnit::deg()));
+
+        // not used
+        //situation.setVelocity(CAircraftVelocity(
+        //    dataUpdate.m_xVelocity, dataUpdate.m_yVelocity, dataUpdate.m_zVelocity, CSpeedUnit::m_s(),
+        //    dataUpdate.m_pitchRadPerSec, dataUpdate.m_bankRadPerSec, dataUpdate.m_headingRadPerSec, CAngleUnit::rad(), CTimeUnit::s()));
+
+        // Ref T297, default offset time
+        situation.setCurrentUtcTime();
+        const qint64 offsetTimeMs = receivedPositionFixTsAndGetOffsetTime(situation.getCallsign(), situation.getMSecsSinceEpoch());
+        situation.setTimeOffsetMs(offsetTimeMs);
+
+        emit visualPilotDataUpdateReceived(situation);
+    }
+
     void CFSDClient::handlePing(const QStringList &tokens)
     {
         const Ping ping = Ping::fromTokens(tokens);
@@ -1726,6 +1794,7 @@ namespace BlackCore::Fsd
 
     void CFSDClient::clearState()
     {
+        m_stoppedSendingVisualPositions = false;
         m_textMessagesToConsolidate.clear();
         m_pendingAtisQueries.clear();
         m_lastPositionUpdate.clear();
@@ -2077,6 +2146,7 @@ namespace BlackCore::Fsd
             case MessageType::TextMessage:       handleTextMessage(tokens);       break;
             case MessageType::PilotClientCom:    handleCustomPilotPacket(tokens); break;
             case MessageType::RevBClientParts:   handleRevBClientPartsPacket(tokens); break;
+            case MessageType::VisualPilotDataUpdate:   handleVisualPilotDataUpdate(tokens); break;
             case MessageType::EuroscopeSimData:  handleEuroscopeSimData(tokens);  break;
 
             // normally we should not get here
@@ -2138,12 +2208,15 @@ namespace BlackCore::Fsd
         // interim positions
         if (this->isInterimPositionSendingEnabledForServer()) { m_interimPositionUpdateTimer.start(c_updateInterimPostionIntervalMsec); }
         else { m_interimPositionUpdateTimer.stop(); }
+        if (this->isVisualPositionSendingEnabledForServer()) { m_visualPositionUpdateTimer.start(c_updateVisualPositionIntervalMsec); }
+        else { m_visualPositionUpdateTimer.stop(); }
     }
 
     void CFSDClient::stopPositionTimers()
     {
         m_positionUpdateTimer.stop();
         m_interimPositionUpdateTimer.stop();
+        m_visualPositionUpdateTimer.stop();
         m_scheduledConfigUpdate.stop();
         m_fsdSendMessageTimer.stop();
     }

src/blackcore/fsd/fsdclient.h

@@ -223,6 +223,7 @@ namespace BlackCore::Fsd
         void planeInformationReceived(const QString &sender, const QString &aircraft, const QString &airline, const QString &livery);
         void customPilotPacketReceived(const QString &sender, const QStringList &data);
         void interimPilotDataUpdatedReceived(const BlackMisc::Aviation::CAircraftSituation &situation);
+        void visualPilotDataUpdateReceived(const BlackMisc::Aviation::CAircraftSituation &situation);
         void euroscopeSimDataUpdatedReceived(const BlackMisc::Aviation::CAircraftSituation &situation, const BlackMisc::Aviation::CAircraftParts &parts, qint64 currentOffsetTimeMs, const QString &model, const QString &livery);
         void rawFsdMessage(const BlackMisc::Network::CRawFsdMessage &rawFsdMessage);
         void planeInformationFsinnReceived(const BlackMisc::Aviation::CCallsign &callsign, const QString &airlineIcaoDesignator, const QString &aircraftDesignator, const QString &combinedAircraftType, const QString &modelString);
@@ -261,6 +262,7 @@ namespace BlackCore::Fsd
         void sendDeleteAtc();
         void sendPilotDataUpdate();
         void sendInterimPilotDataUpdate();
+        void sendVisualPilotDataUpdate();
         void sendAtcDataUpdate(double latitude, double longitude);
         void sendPing(const QString &receiver);
         //
@@ -363,6 +365,7 @@ namespace BlackCore::Fsd
         void handleDeletePilot(const QStringList &tokens);
         void handleTextMessage(const QStringList &tokens);
         void handlePilotDataUpdate(const QStringList &tokens);
+        void handleVisualPilotDataUpdate(const QStringList &tokens);
         void handleEuroscopeSimData(const QStringList &tokens);
         void handlePing(const QStringList &tokens);
         void handlePong(const QStringList &tokens);
@@ -507,6 +510,7 @@ namespace BlackCore::Fsd
         QTimer m_scheduledConfigUpdate      { this }; //!< config updates
         QTimer m_positionUpdateTimer        { this }; //!< sending positions
         QTimer m_interimPositionUpdateTimer { this }; //!< sending interim positions
+        QTimer m_visualPositionUpdateTimer  { this }; //!< sending visual positions
         QTimer m_fsdSendMessageTimer        { this }; //!< FSD message sending
 
         qint64 m_additionalOffsetTime = 0; //!< additional offset time
@@ -556,7 +560,9 @@ namespace BlackCore::Fsd
         static int constexpr c_processingIntervalMsec           = 100;  //!< interval for the processing timer
         static int constexpr c_updatePostionIntervalMsec        = 5000; //!< interval for the position update timer (send our position to network)
         static int constexpr c_updateInterimPostionIntervalMsec = 1000; //!< interval for iterim position updates (send our position as interim position)
+        static int constexpr c_updateVisualPositionIntervalMsec = 200;  //!< interval for the VATSIM visual position updates (send our position and 6DOF velocity)
         static int constexpr c_sendFsdMsgIntervalMsec           = 10;   //!< interval for FSD send messages
+        bool m_stoppedSendingVisualPositions = false; //!< for when velocity drops to zero
     };
 } // ns
 

src/blackcore/fsd/messagebase.h

@@ -39,6 +39,7 @@ enum class MessageType
     FsdIdentification,
     KillRequest,
     PilotDataUpdate,
+    VisualPilotDataUpdate,
     Ping,
     Pong,
     ServerError,

src/blackcore/fsd/visualpilotdataupdate.cpp

@@ -0,0 +1,79 @@
+/* Copyright (C) 2019
+ * swift project community / contributors
+ *
+ * This file is part of swift project. It is subject to the license terms in the LICENSE file found in the top-level
+ * directory of this distribution. No part of swift project, including this file, may be copied, modified, propagated,
+ * or distributed except according to the terms contained in the LICENSE file.
+ */
+
+#include "visualpilotdataupdate.h"
+#include "pbh.h"
+#include "serializer.h"
+
+#include "blackmisc/logmessage.h"
+
+using namespace BlackMisc;
+using namespace BlackMisc::Aviation;
+
+namespace BlackCore::Fsd
+{
+    VisualPilotDataUpdate::VisualPilotDataUpdate() : MessageBase()
+    { }
+
+    VisualPilotDataUpdate::VisualPilotDataUpdate(const QString &sender, double latitude, double longitude, double altitudeTrue,
+                                                    double pitch, double bank, double heading, double xVelocity, double yVelocity, double zVelocity,
+                                                    double pitchRadPerSec, double bankRadPerSec, double headingRadPerSec)
+        : MessageBase(sender, {}),
+            m_latitude(latitude),
+            m_longitude(longitude),
+            m_altitudeTrue(altitudeTrue),
+            m_pitch(pitch),
+            m_bank(bank),
+            m_heading(heading),
+            m_xVelocity(xVelocity),
+            m_yVelocity(yVelocity),
+            m_zVelocity(zVelocity),
+            m_pitchRadPerSec(pitchRadPerSec),
+            m_bankRadPerSec(bankRadPerSec),
+            m_headingRadPerSec(headingRadPerSec)
+    { }
+
+    QStringList VisualPilotDataUpdate::toTokens() const
+    {
+        std::uint32_t pbh;
+        packPBH(m_pitch, m_bank, m_heading, false/*! \todo check if needed? */, pbh);
+
+        QStringList tokens;
+        tokens.push_back(m_sender);
+        tokens.push_back(QString::number(m_latitude,  'f', 7));
+        tokens.push_back(QString::number(m_longitude, 'f', 7));
+        tokens.push_back(QString::number(m_altitudeTrue,     'f', 2));
+        tokens.push_back(QString::number(pbh));
+        tokens.push_back(QString::number(m_xVelocity, 'f', 4));
+        tokens.push_back(QString::number(m_yVelocity, 'f', 4));
+        tokens.push_back(QString::number(m_zVelocity, 'f', 4));
+        tokens.push_back(QString::number(m_pitchRadPerSec,   'f', 4));
+        tokens.push_back(QString::number(m_headingRadPerSec, 'f', 4));
+        tokens.push_back(QString::number(m_bankRadPerSec,    'f', 4));
+        return tokens;
+    }
+
+    VisualPilotDataUpdate VisualPilotDataUpdate::fromTokens(const QStringList &tokens)
+    {
+        if (tokens.size() < 11)
+        {
+            CLogMessage(static_cast<VisualPilotDataUpdate *>(nullptr)).debug(u"Wrong number of arguments.");
+            return {};
+        }
+
+        double pitch = 0.0;
+        double bank  = 0.0;
+        double heading = 0.0;
+        bool unused = false; //! \todo check if needed?
+        unpackPBH(tokens[4].toUInt(), pitch, bank, heading, unused);
+
+        return VisualPilotDataUpdate(tokens[0], tokens[1].toDouble(), tokens[2].toDouble(), tokens[3].toDouble(),
+                pitch, bank, heading, tokens[5].toDouble(), tokens[6].toDouble(), tokens[7].toDouble(),
+                tokens[8].toDouble(), tokens[10].toDouble(), tokens[9].toDouble());
+    }
+}

src/blackcore/fsd/visualpilotdataupdate.h

@@ -0,0 +1,81 @@
+/* Copyright (C) 2019
+ * swift project community / contributors
+ *
+ * This file is part of swift project. It is subject to the license terms in the LICENSE file found in the top-level
+ * directory of this distribution. No part of swift project, including this file, may be copied, modified, propagated,
+ * or distributed except according to the terms contained in the LICENSE file.
+ */
+
+//! \file
+
+#ifndef BLACKCORE_FSD_VISUALPILOTDATAUPDATE_H
+#define BLACKCORE_FSD_VISUALPILOTDATAUPDATE_H
+
+#include "messagebase.h"
+#include "enums.h"
+
+namespace BlackCore::Fsd
+{
+    //! Pilot data update broadcasted to pilots in range every 0.2 seconds.
+    class BLACKCORE_EXPORT VisualPilotDataUpdate : public MessageBase
+    {
+    public:
+        //! Constructor
+        VisualPilotDataUpdate(const QString &sender, double latitude, double longitude, double altitudeTrue,
+                                double pitch, double bank, double heading, double xVelocity, double yVelocity, double zVelocity,
+                                double pitchRadPerSec, double bankRadPerSec, double headingRadPerSec);
+
+        //! Message converted to tokens
+        QStringList toTokens() const;
+
+        //! Construct from tokens
+        static VisualPilotDataUpdate fromTokens(const QStringList &tokens);
+
+        //! PDU identifier
+        static QString pdu() { return "^"; }
+
+        //! Properties
+        //! @{
+        double m_latitude      = 0.0;
+        double m_longitude     = 0.0;
+        double m_altitudeTrue  = 0.0;
+        double m_pitch         = 0.0;
+        double m_bank          = 0.0;
+        double m_heading       = 0.0;
+        double m_xVelocity     = 0.0;
+        double m_yVelocity     = 0.0;
+        double m_zVelocity     = 0.0;
+        double m_pitchRadPerSec     = 0.0;
+        double m_bankRadPerSec      = 0.0;
+        double m_headingRadPerSec   = 0.0;
+        //! @}
+
+    private:
+        VisualPilotDataUpdate();
+    };
+
+    //! Equal to operator
+    inline bool operator==(const VisualPilotDataUpdate &lhs, const VisualPilotDataUpdate &rhs)
+    {
+        return  qFuzzyCompare(lhs.m_latitude, rhs.m_latitude) &&
+                qFuzzyCompare(lhs.m_longitude, rhs.m_longitude) &&
+                qFuzzyCompare(lhs.m_altitudeTrue, rhs.m_altitudeTrue) &&
+                qFuzzyCompare(lhs.m_pitch, rhs.m_pitch) &&
+                qFuzzyCompare(lhs.m_bank, rhs.m_bank) &&
+                qFuzzyCompare(lhs.m_heading, rhs.m_heading) &&
+                qFuzzyCompare(lhs.m_xVelocity, rhs.m_xVelocity) &&
+                qFuzzyCompare(lhs.m_yVelocity, rhs.m_yVelocity) &&
+                qFuzzyCompare(lhs.m_zVelocity, rhs.m_zVelocity) &&
+                qFuzzyCompare(lhs.m_pitchRadPerSec, rhs.m_pitchRadPerSec) &&
+                qFuzzyCompare(lhs.m_bankRadPerSec, rhs.m_bankRadPerSec) &&
+                qFuzzyCompare(lhs.m_headingRadPerSec, rhs.m_headingRadPerSec);
+    }
+
+    //! Not equal to operator
+    inline bool operator!=(const VisualPilotDataUpdate &lhs, const VisualPilotDataUpdate &rhs)
+    {
+        return !(lhs == rhs);
+    }
+}
+
+#endif // guard

tests/blackcore/fsd/testfsdmessages/testfsdmessages.cpp

@@ -37,6 +37,7 @@
 #include "blackcore/fsd/serializer.h"
 #include "blackcore/fsd/servererror.h"
 #include "blackcore/fsd/interimpilotdataupdate.h"
+#include "blackcore/fsd/visualpilotdataupdate.h"
 #include "blackcore/fsd/planeinforequest.h"
 #include "blackcore/fsd/planeinformation.h"
 #include "blackcore/fsd/planeinforequestfsinn.h"
@@ -84,6 +85,7 @@ namespace BlackMiscTest
         void testKillRequest();
         void testPBH();
         void testPilotDataUpdate();
+        void testVisualPilotDataUpdate();
         void testPing();
         void testPlaneInfoRequest();
         void testPlaneInformation();
@@ -498,6 +500,45 @@ namespace BlackMiscTest
         QCOMPARE(messageFromTokens.m_onGround, true);
     }
 
+    void CTestFsdMessages::testVisualPilotDataUpdate()
+    {
+        const VisualPilotDataUpdate message("ABCD", 43.1257891, -72.1584142, 12000.12, -2, 3, 280, -1.0001, 2.0001, 3.0001, -0.0349, 0.0524, 0.0175);
+
+        QCOMPARE(QString("ABCD"), message.sender());
+        QCOMPARE(43.1257891, message.m_latitude);
+        QCOMPARE(-72.1584142, message.m_longitude);
+        QCOMPARE(12000.12, message.m_altitudeTrue);
+        QCOMPARE(-2, message.m_pitch);
+        QCOMPARE(3, message.m_bank);
+        QCOMPARE(280, message.m_heading);
+        QCOMPARE(-1.0001, message.m_xVelocity);
+        QCOMPARE(2.0001, message.m_yVelocity);
+        QCOMPARE(3.0001, message.m_zVelocity);
+        QCOMPARE(-0.0349, message.m_pitchRadPerSec);
+        QCOMPARE(0.0524, message.m_bankRadPerSec);
+        QCOMPARE(0.0175, message.m_headingRadPerSec);
+
+        QString stringRef("ABCD:43.1257891:-72.1584142:12000.12:25132144:-1.0001:2.0001:3.0001:-0.0349:0.0175:0.0524");
+        QString str = message.toTokens().join(":");
+        QCOMPARE(str, stringRef);
+
+        QStringList tokens = QString("ABCD:43.1257891:-72.1584142:12000.12:25132144:-1.0001:2.0001:3.0001:-0.0349:0.0175:0.0524").split(':');
+        auto messageFromTokens = VisualPilotDataUpdate::fromTokens(tokens);
+        QCOMPARE(QString("ABCD"), messageFromTokens.sender());
+        QCOMPARE(43.1257891, messageFromTokens.m_latitude);
+        QCOMPARE(-72.1584142, messageFromTokens.m_longitude);
+        QCOMPARE(12000.12, messageFromTokens.m_altitudeTrue);
+        QVERIFY(message.m_pitch - messageFromTokens.m_pitch < 1.0);
+        QVERIFY(message.m_bank - messageFromTokens.m_bank < 1.0);
+        QVERIFY(message.m_heading - messageFromTokens.m_heading < 1.0);
+        QCOMPARE(-1.0001, messageFromTokens.m_xVelocity);
+        QCOMPARE(2.0001, messageFromTokens.m_yVelocity);
+        QCOMPARE(3.0001, messageFromTokens.m_zVelocity);
+        QCOMPARE(-0.0349, messageFromTokens.m_pitchRadPerSec);
+        QCOMPARE(0.0524, messageFromTokens.m_bankRadPerSec);
+        QCOMPARE(0.0175, messageFromTokens.m_headingRadPerSec);
+    }
+
     void CTestFsdMessages::testPing()
     {
         const Ping message("ABCD", "SERVER", "85275222");