diff --git a/src/plugins/simulator/fs9/blacksimpluginfreefunctions.cpp b/src/plugins/simulator/fs9/blacksimpluginfreefunctions.cpp index 96ef0a7a3..c6d677e0a 100644 --- a/src/plugins/simulator/fs9/blacksimpluginfreefunctions.cpp +++ b/src/plugins/simulator/fs9/blacksimpluginfreefunctions.cpp @@ -13,153 +13,10 @@ #include "blackmisc/math/mathutils.h" #include "blackmisc/logmessage.h" -using namespace BlackMisc::Aviation; -using namespace BlackMisc::Geo; -using namespace BlackMisc::PhysicalQuantities; - namespace BlackSimPlugin { namespace Fs9 { - CAircraftSituation aircraftSituationfromFS9(const MPPositionVelocity &positionVelocity) - { - CAircraftSituation situation; - - double dHigh = positionVelocity.lat_i; - double dLow = positionVelocity.lat_f; - - dLow = dLow / 65536.0; - if (dHigh > 0) - dHigh = dHigh + dLow; - else - dHigh = dHigh - dLow; - - CCoordinateGeodetic position; - position.setLatitude(CLatitude(dHigh * 90.0 / 10001750.0, CAngleUnit::deg())); - - dHigh = positionVelocity.lon_hi; - dLow = positionVelocity.lon_lo; - - dLow = dLow / 65536.0; - if (dHigh > 0) - dHigh = dHigh + dLow; - else - dHigh = dHigh - dLow; - - position.setLongitude(CLongitude(dHigh * 360.0 / ( 65536.0 * 65536.0), CAngleUnit::deg())); - - dHigh = positionVelocity.alt_i; - dLow = positionVelocity.alt_f; - - dLow = dLow / 65536.0; - - situation.setPosition(position); - situation.setAltitude(CAltitude(dHigh + dLow, CAltitude::MeanSeaLevel, CLengthUnit::m())); - double groundSpeed = positionVelocity.ground_velocity / 65536.0; - situation.setGroundspeed(CSpeed(groundSpeed, CSpeedUnit::m_s())); - - FS_PBH pbhstrct; - pbhstrct.pbh = positionVelocity.pbh; - double pitch = pbhstrct.pitch / CFs9Sdk::pitchMultiplier(); - if (pitch > 180.0) - pitch -= 360; - - double bank = pbhstrct.bank / CFs9Sdk::bankMultiplier(); - if (bank > 180.0) - bank -= 360; - - situation.setPitch(CAngle(pitch, CAngleUnit::deg())); - situation.setBank(CAngle(bank, CAngleUnit::deg())); - situation.setHeading(CHeading(pbhstrct.hdg / CFs9Sdk::headingMultiplier(), CHeading::Magnetic, CAngleUnit::deg())); - - return situation; - } - - MPPositionVelocity aircraftSituationToFS9(const CAircraftSituation &oldSituation, const CAircraftSituation &newSituation, double updateInterval) - { - MPPositionVelocity positionVelocity; - - // Latitude - integer and decimal places - double latitude = newSituation.getPosition().latitude().value(CAngleUnit::deg()) * 10001750.0 / 90.0; - positionVelocity.lat_i = static_cast(latitude); - positionVelocity.lat_f = qAbs((latitude - positionVelocity.lat_i) * 65536); - - // Longitude - integer and decimal places - double longitude = newSituation.getPosition().longitude().value(CAngleUnit::deg()) * ( 65536.0 * 65536.0) / 360.0; - positionVelocity.lon_hi = static_cast(longitude); - positionVelocity.lon_lo = qAbs((longitude - positionVelocity.lon_hi) * 65536); - - // Altitude - integer and decimal places - double altitude = newSituation.getAltitude().value(CLengthUnit::m()); - positionVelocity.alt_i = static_cast(altitude); - positionVelocity.alt_f = (altitude - positionVelocity.alt_i) * 65536; - - // Pitch, Bank and Heading - FS_PBH pbhstrct; - pbhstrct.hdg = newSituation.getHeading().value(CAngleUnit::deg()) * CFs9Sdk::headingMultiplier(); - pbhstrct.pitch = newSituation.getPitch().value(CAngleUnit::deg()) * CFs9Sdk::pitchMultiplier(); - pbhstrct.bank = newSituation.getBank().value(CAngleUnit::deg()) * CFs9Sdk::bankMultiplier(); - positionVelocity.pbh = pbhstrct.pbh; - - // Ground velocity - positionVelocity.ground_velocity = newSituation.getGroundSpeed().value(CSpeedUnit::m_s()); - - // Altitude velocity - CCoordinateGeodetic oldPosition = oldSituation.getPosition(); - CCoordinateGeodetic newPosition = newSituation.getPosition(); - CCoordinateGeodetic helperPosition; - - // We want the distance in Latitude direction. Longitude must be equal for old and new position. - helperPosition.setLatitude(newPosition.latitude()); - helperPosition.setLongitude(oldPosition.longitude()); - CLength distanceLatitudeObj = calculateGreatCircleDistance(oldPosition, helperPosition); - - - // Now we want the Longitude distance. Latitude must be equal for old and new position. - helperPosition.setLatitude(oldPosition.latitude()); - helperPosition.setLongitude(newSituation.longitude()); - CLength distanceLongitudeObj = calculateGreatCircleDistance(oldPosition, helperPosition); - - // Latitude and Longitude velocity - positionVelocity.lat_velocity = distanceLatitudeObj.value(CLengthUnit::ft()) * 65536.0 / updateInterval; - if (oldPosition.latitude().value() > newSituation.latitude().value()) positionVelocity.lat_velocity *= -1; - positionVelocity.lon_velocity = distanceLongitudeObj.value(CLengthUnit::ft()) * 65536.0 / updateInterval; - if (oldPosition.longitude().value() > newSituation.longitude().value()) positionVelocity.lon_velocity *= -1; - - // TODO: Altitude velocity - - return positionVelocity; - } - - MPPositionSlewMode aircraftSituationToFS9(const CAircraftSituation &situation) - { - MPPositionSlewMode positionSlewMode; - - // Latitude - integer and decimal places - double latitude = situation.getPosition().latitude().value(CAngleUnit::deg()) * 10001750.0 / 90.0; - positionSlewMode.lat_i = static_cast(latitude); - positionSlewMode.lat_f = qAbs((latitude - positionSlewMode.lat_i) * 65536); - - // Longitude - integer and decimal places - double longitude = situation.getPosition().longitude().value(CAngleUnit::deg()) * ( 65536.0 * 65536.0) / 360.0; - positionSlewMode.lon_hi = static_cast(longitude); - positionSlewMode.lon_lo = qAbs((longitude - positionSlewMode.lon_hi) * 65536); - - // Altitude - integer and decimal places - double altitude = situation.getAltitude().value(CLengthUnit::m()); - positionSlewMode.alt_i = static_cast(altitude); - positionSlewMode.alt_f = (altitude - positionSlewMode.alt_i) * 65536; - - // Pitch, Bank and Heading - FS_PBH pbhstrct; - pbhstrct.hdg = situation.getHeading().value(CAngleUnit::deg()) * CFs9Sdk::headingMultiplier(); - pbhstrct.pitch = situation.getPitch().value(CAngleUnit::deg()) * CFs9Sdk::pitchMultiplier(); - pbhstrct.bank = situation.getBank().value(CAngleUnit::deg()) * CFs9Sdk::bankMultiplier(); - positionSlewMode.pbh = pbhstrct.pbh; - - return positionSlewMode; - } - HRESULT logDirectPlayError(HRESULT error) { QString errorMessage; diff --git a/src/plugins/simulator/fs9/blacksimpluginfreefunctions.h b/src/plugins/simulator/fs9/blacksimpluginfreefunctions.h index 43f5fc3e1..d0d7ca265 100644 --- a/src/plugins/simulator/fs9/blacksimpluginfreefunctions.h +++ b/src/plugins/simulator/fs9/blacksimpluginfreefunctions.h @@ -12,8 +12,6 @@ #ifndef BLACKSIMPLUGIN_FREEFUNCTIONS_H #define BLACKSIMPLUGIN_FREEFUNCTIONS_H -#include "multiplayerpackets.h" -#include "blackmisc/aviation/aircraftsituation.h" namespace BlackSimPlugin { @@ -46,16 +44,6 @@ namespace BlackSimPlugin pT = nullptr; } - //! Convert FS9 struct to aircraft situation - BlackMisc::Aviation::CAircraftSituation aircraftSituationfromFS9(const MPPositionVelocity &positionVelocity); - - //! Convert an aircraft situation to a FS9 struct - MPPositionVelocity aircraftSituationToFS9(const BlackMisc::Aviation::CAircraftSituation &oldSituation, - const BlackMisc::Aviation::CAircraftSituation &newSituation, - double updateInterval); - - //! Convert an aircraft situation to a FS9 struct - MPPositionSlewMode aircraftSituationToFS9(const BlackMisc::Aviation::CAircraftSituation &situation); //! Print the direct play error HRESULT logDirectPlayError(HRESULT error); diff --git a/src/plugins/simulator/fs9/fs9client.cpp b/src/plugins/simulator/fs9/fs9client.cpp index 56fc8186d..c63ff00b7 100644 --- a/src/plugins/simulator/fs9/fs9client.cpp +++ b/src/plugins/simulator/fs9/fs9client.cpp @@ -23,11 +23,95 @@ using namespace BlackCore; using namespace BlackMisc::Aviation; using namespace BlackMisc::Simulation; using namespace BlackMisc::PhysicalQuantities; +using namespace BlackMisc::Geo; namespace BlackSimPlugin { namespace Fs9 { + MPPositionVelocity aircraftSituationToFS9(const CAircraftSituation &oldSituation, const CAircraftSituation &newSituation, double updateInterval) + { + MPPositionVelocity positionVelocity; + + // Latitude - integer and decimal places + double latitude = newSituation.getPosition().latitude().value(CAngleUnit::deg()) * 10001750.0 / 90.0; + positionVelocity.lat_i = static_cast(latitude); + positionVelocity.lat_f = qAbs((latitude - positionVelocity.lat_i) * 65536); + + // Longitude - integer and decimal places + double longitude = newSituation.getPosition().longitude().value(CAngleUnit::deg()) * ( 65536.0 * 65536.0) / 360.0; + positionVelocity.lon_hi = static_cast(longitude); + positionVelocity.lon_lo = qAbs((longitude - positionVelocity.lon_hi) * 65536); + + // Altitude - integer and decimal places + double altitude = newSituation.getAltitude().value(CLengthUnit::m()); + positionVelocity.alt_i = static_cast(altitude); + positionVelocity.alt_f = (altitude - positionVelocity.alt_i) * 65536; + + // Pitch, Bank and Heading + FS_PBH pbhstrct; + pbhstrct.hdg = newSituation.getHeading().value(CAngleUnit::deg()) * CFs9Sdk::headingMultiplier(); + pbhstrct.pitch = newSituation.getPitch().value(CAngleUnit::deg()) * CFs9Sdk::pitchMultiplier(); + pbhstrct.bank = newSituation.getBank().value(CAngleUnit::deg()) * CFs9Sdk::bankMultiplier(); + positionVelocity.pbh = pbhstrct.pbh; + + // Ground velocity + positionVelocity.ground_velocity = newSituation.getGroundSpeed().value(CSpeedUnit::m_s()); + + // Altitude velocity + CCoordinateGeodetic oldPosition = oldSituation.getPosition(); + CCoordinateGeodetic newPosition = newSituation.getPosition(); + CCoordinateGeodetic helperPosition; + + // We want the distance in Latitude direction. Longitude must be equal for old and new position. + helperPosition.setLatitude(newPosition.latitude()); + helperPosition.setLongitude(oldPosition.longitude()); + CLength distanceLatitudeObj = calculateGreatCircleDistance(oldPosition, helperPosition); + + + // Now we want the Longitude distance. Latitude must be equal for old and new position. + helperPosition.setLatitude(oldPosition.latitude()); + helperPosition.setLongitude(newSituation.longitude()); + CLength distanceLongitudeObj = calculateGreatCircleDistance(oldPosition, helperPosition); + + // Latitude and Longitude velocity + positionVelocity.lat_velocity = distanceLatitudeObj.value(CLengthUnit::ft()) * 65536.0 / updateInterval; + if (oldPosition.latitude().value() > newSituation.latitude().value()) positionVelocity.lat_velocity *= -1; + positionVelocity.lon_velocity = distanceLongitudeObj.value(CLengthUnit::ft()) * 65536.0 / updateInterval; + if (oldPosition.longitude().value() > newSituation.longitude().value()) positionVelocity.lon_velocity *= -1; + + return positionVelocity; + } + + MPPositionSlewMode aircraftSituationToFS9(const CAircraftSituation &situation) + { + MPPositionSlewMode positionSlewMode; + + // Latitude - integer and decimal places + double latitude = situation.getPosition().latitude().value(CAngleUnit::deg()) * 10001750.0 / 90.0; + positionSlewMode.lat_i = static_cast(latitude); + positionSlewMode.lat_f = qAbs((latitude - positionSlewMode.lat_i) * 65536); + + // Longitude - integer and decimal places + double longitude = situation.getPosition().longitude().value(CAngleUnit::deg()) * ( 65536.0 * 65536.0) / 360.0; + positionSlewMode.lon_hi = static_cast(longitude); + positionSlewMode.lon_lo = qAbs((longitude - positionSlewMode.lon_hi) * 65536); + + // Altitude - integer and decimal places + double altitude = situation.getAltitude().value(CLengthUnit::m()); + positionSlewMode.alt_i = static_cast(altitude); + positionSlewMode.alt_f = (altitude - positionSlewMode.alt_i) * 65536; + + // Pitch, Bank and Heading + FS_PBH pbhstrct; + pbhstrct.hdg = situation.getHeading().value(CAngleUnit::deg()) * CFs9Sdk::headingMultiplier(); + pbhstrct.pitch = situation.getPitch().value(CAngleUnit::deg()) * CFs9Sdk::pitchMultiplier(); + pbhstrct.bank = situation.getBank().value(CAngleUnit::deg()) * CFs9Sdk::bankMultiplier(); + positionSlewMode.pbh = pbhstrct.pbh; + + return positionSlewMode; + } + CFs9Client::CFs9Client(const CCallsign &callsign, const QString &modelName, BlackCore::IInterpolator *interpolator, const CTime &updateInterval, QObject *owner) : CDirectPlayPeer(owner, callsign), diff --git a/src/plugins/simulator/fs9/simulatorfs9.cpp b/src/plugins/simulator/fs9/simulatorfs9.cpp index 160ffc19a..1ee872f29 100644 --- a/src/plugins/simulator/fs9/simulatorfs9.cpp +++ b/src/plugins/simulator/fs9/simulatorfs9.cpp @@ -39,6 +39,60 @@ namespace BlackSimPlugin { namespace Fs9 { + CAircraftSituation aircraftSituationfromFS9(const MPPositionVelocity &positionVelocity) + { + CAircraftSituation situation; + + double dHigh = positionVelocity.lat_i; + double dLow = positionVelocity.lat_f; + + dLow = dLow / 65536.0; + if (dHigh > 0) + dHigh = dHigh + dLow; + else + dHigh = dHigh - dLow; + + CCoordinateGeodetic position; + position.setLatitude(CLatitude(dHigh * 90.0 / 10001750.0, CAngleUnit::deg())); + + dHigh = positionVelocity.lon_hi; + dLow = positionVelocity.lon_lo; + + dLow = dLow / 65536.0; + if (dHigh > 0) + dHigh = dHigh + dLow; + else + dHigh = dHigh - dLow; + + position.setLongitude(CLongitude(dHigh * 360.0 / ( 65536.0 * 65536.0), CAngleUnit::deg())); + + dHigh = positionVelocity.alt_i; + dLow = positionVelocity.alt_f; + + dLow = dLow / 65536.0; + + situation.setPosition(position); + situation.setAltitude(CAltitude(dHigh + dLow, CAltitude::MeanSeaLevel, CLengthUnit::m())); + double groundSpeed = positionVelocity.ground_velocity / 65536.0; + situation.setGroundspeed(CSpeed(groundSpeed, CSpeedUnit::m_s())); + + FS_PBH pbhstrct; + pbhstrct.pbh = positionVelocity.pbh; + double pitch = pbhstrct.pitch / CFs9Sdk::pitchMultiplier(); + if (pitch > 180.0) + pitch -= 360; + + double bank = pbhstrct.bank / CFs9Sdk::bankMultiplier(); + if (bank > 180.0) + bank -= 360; + + situation.setPitch(CAngle(pitch, CAngleUnit::deg())); + situation.setBank(CAngle(bank, CAngleUnit::deg())); + situation.setHeading(CHeading(pbhstrct.hdg / CFs9Sdk::headingMultiplier(), CHeading::Magnetic, CAngleUnit::deg())); + + return situation; + } + CSimulatorFs9::CSimulatorFs9( const CSimulatorPluginInfo &info, const QSharedPointer &fs9Host,