From 242a10c9d6b7bf2c9e582059b9aeac9a221f8c8f Mon Sep 17 00:00:00 2001 From: Mat Sutcliffe Date: Mon, 11 Oct 2021 18:52:17 +0100 Subject: [PATCH] Issue #94 Add new FSD packet type VisualPilotDataUpdate --- src/blackcore/fsd/fsdclient.cpp | 73 +++++++++++++++++ src/blackcore/fsd/fsdclient.h | 6 ++ src/blackcore/fsd/messagebase.h | 1 + src/blackcore/fsd/visualpilotdataupdate.cpp | 79 ++++++++++++++++++ src/blackcore/fsd/visualpilotdataupdate.h | 81 +++++++++++++++++++ .../fsd/testfsdmessages/testfsdmessages.cpp | 41 ++++++++++ 6 files changed, 281 insertions(+) create mode 100644 src/blackcore/fsd/visualpilotdataupdate.cpp create mode 100644 src/blackcore/fsd/visualpilotdataupdate.h diff --git a/src/blackcore/fsd/fsdclient.cpp b/src/blackcore/fsd/fsdclient.cpp index 0f05246da..52db5be10 100644 --- a/src/blackcore/fsd/fsdclient.cpp +++ b/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(); } diff --git a/src/blackcore/fsd/fsdclient.h b/src/blackcore/fsd/fsdclient.h index 0468fa07f..3522d59d0 100644 --- a/src/blackcore/fsd/fsdclient.h +++ b/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 diff --git a/src/blackcore/fsd/messagebase.h b/src/blackcore/fsd/messagebase.h index 97cfb6381..63d4e65c9 100644 --- a/src/blackcore/fsd/messagebase.h +++ b/src/blackcore/fsd/messagebase.h @@ -39,6 +39,7 @@ enum class MessageType FsdIdentification, KillRequest, PilotDataUpdate, + VisualPilotDataUpdate, Ping, Pong, ServerError, diff --git a/src/blackcore/fsd/visualpilotdataupdate.cpp b/src/blackcore/fsd/visualpilotdataupdate.cpp new file mode 100644 index 000000000..1b3ac8731 --- /dev/null +++ b/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(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()); + } +} diff --git a/src/blackcore/fsd/visualpilotdataupdate.h b/src/blackcore/fsd/visualpilotdataupdate.h new file mode 100644 index 000000000..5e127b08a --- /dev/null +++ b/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 diff --git a/tests/blackcore/fsd/testfsdmessages/testfsdmessages.cpp b/tests/blackcore/fsd/testfsdmessages/testfsdmessages.cpp index 7a0764ee8..356c0e309 100644 --- a/tests/blackcore/fsd/testfsdmessages/testfsdmessages.cpp +++ b/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");