From f6e4e89679776d85567f14dce1ca5da756a0cdd4 Mon Sep 17 00:00:00 2001 From: Stuart Bowman Date: Fri, 26 Jun 2026 14:43:44 -0400 Subject: [PATCH 1/8] chore: update to latest internal codebase before splitting apart --- unittest/src/Public/aircraft_intent_tests.cpp | 541 ------------------ unittest/src/Public/public.cmake | 2 +- unittest/src/Public/threedof_glider_tests.cpp | 274 +++++++++ unittest/src/Public/windstack_tests.cpp | 1 - unittest/src/utils/public/PublicUtils.cpp | 39 -- 5 files changed, 275 insertions(+), 582 deletions(-) delete mode 100644 unittest/src/Public/aircraft_intent_tests.cpp create mode 100644 unittest/src/Public/threedof_glider_tests.cpp diff --git a/unittest/src/Public/aircraft_intent_tests.cpp b/unittest/src/Public/aircraft_intent_tests.cpp deleted file mode 100644 index 857e6b0..0000000 --- a/unittest/src/Public/aircraft_intent_tests.cpp +++ /dev/null @@ -1,541 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include - -#include -#include -#include -#include -#include -#include -#include - -#include "public/AircraftIntent.h" -#include "public/AircraftIntentLoader.h" -#include "public/CoreUtils.h" -#include "public/SingleTangentPlaneSequence.h" -#include "public/TangentPlaneSequence.h" -#include "utils/public/PublicUtils.h" - -using namespace aaesim::test::utils; - -namespace aaesim { -namespace test { -namespace open_source { - -class AircraftIntentTester : public AircraftIntent { - public: - explicit AircraftIntentTester(const AircraftIntent aircraft_intent) {} - std::list Wrapper_AddConnectingTfLeg(const std::list &first_waypoint_vector, - const std::list &second_waypoint_vector) { - return AddConnectingLeg(first_waypoint_vector, second_waypoint_vector); - } - static AircraftIntentTester MakeEmptyAircraftIntent() { - const AircraftIntent aircraft_intent; - return AircraftIntentTester(aircraft_intent); - } -}; - -AircraftIntent LoadAircraftIntent(DecodedStream *stream) { - aaesim::loaders::AircraftIntentLoader aircraft_intent_loader; - aircraft_intent_loader.load(stream); - return aircraft_intent_loader.BuildAircraftIntent(); -} - -TEST(AircraftIntent, Copy) { - const AircraftIntent expected_intent = PublicUtils::PrepareAircraftIntent("./resources/enroute_waypoints_test1.txt"); - - // This is the tested method - AircraftIntent copy_of_intent; - copy_of_intent.Copy(expected_intent); - // ------------------------- - - ASSERT_TRUE(copy_of_intent == expected_intent); -} - -TEST(AircraftIntent, CopyConstructor) { - const AircraftIntent expected_intent = PublicUtils::PrepareAircraftIntent("./resources/enroute_waypoints_test1.txt"); - - // This is the tested method - const AircraftIntent copy_of_intent(expected_intent); - // ------------------------- - - ASSERT_TRUE(copy_of_intent == expected_intent); -} - -TEST(AircraftIntent, AddConnectingTfLeg_test_0) { - std::list first_vector_set, second_vector_set; - second_vector_set.push_back(Waypoint("1", Units::ONE_RADIAN_ANGLE, Units::ONE_RADIAN_ANGLE)); - AircraftIntentTester aircraft_intent_tester = AircraftIntentTester::MakeEmptyAircraftIntent(); - const std::list modified_waypoints_vector = - aircraft_intent_tester.Wrapper_AddConnectingTfLeg(first_vector_set, second_vector_set); - - EXPECT_EQ(modified_waypoints_vector.size(), 1); - EXPECT_EQ(second_vector_set.back().GetName(), modified_waypoints_vector.front().GetName()); -} - -TEST(AircraftIntent, AddConnectingTfLeg_test_1) { - std::list first_vector_set, second_vector_set; - first_vector_set.push_back(Waypoint("1", Units::ONE_RADIAN_ANGLE, Units::ONE_RADIAN_ANGLE)); - - AircraftIntentTester aircraft_intent_tester = AircraftIntentTester::MakeEmptyAircraftIntent(); - const std::list modified_waypoints_vector = - aircraft_intent_tester.Wrapper_AddConnectingTfLeg(first_vector_set, second_vector_set); - - EXPECT_EQ(modified_waypoints_vector.size(), 1); - EXPECT_LT(first_vector_set.back().GetName().compare(modified_waypoints_vector.front().GetName()), 0); -} - -TEST(AircraftIntent, AddConnectingTfLeg_test_2) { - std::list first_vector_set, second_vector_set; - first_vector_set.push_back(Waypoint("1", Units::ONE_RADIAN_ANGLE, Units::ONE_RADIAN_ANGLE)); - second_vector_set.push_back(Waypoint("2", Units::ONE_RADIAN_ANGLE, Units::ONE_RADIAN_ANGLE)); - - AircraftIntentTester aircraft_intent_tester = AircraftIntentTester::MakeEmptyAircraftIntent(); - const std::list modified_waypoints_vector = - aircraft_intent_tester.Wrapper_AddConnectingTfLeg(first_vector_set, second_vector_set); - - EXPECT_EQ(modified_waypoints_vector.size(), 2); - EXPECT_LT(first_vector_set.back().GetName().compare(modified_waypoints_vector.front().GetName()), 0); - EXPECT_EQ(second_vector_set.back().GetName().compare(modified_waypoints_vector.back().GetName()), 0); -} - -TEST(AircraftIntent, LoadEndToEndWaypoints) { - std::string test_file_to_load = "./resources/aircraftintent_full_route_kden_kphx.txt"; - FILE *fp; - fp = fopen(test_file_to_load.c_str(), "r"); - if (fp == NULL) { - std::cout << "Test resource file " << test_file_to_load << " not found." << std::endl; - FAIL(); - } - - DecodedStream stream; - bool r = stream.open_file(test_file_to_load); - if (!r) { - std::cout << "Test resource file " << test_file_to_load << " could not be loaded." << std::endl; - FAIL(); - } - stream.set_echo(false); - - SingleTangentPlaneSequence::ClearStaticMembers(); - CoreUtils::UpdateMaximumAllowableSingleLegLength(Units::infinity()); - - // This is the tested method --------------- - AircraftIntent aircraft_intent = LoadAircraftIntent(&stream); - // ----------------------------------------- - - ASSERT_TRUE(aircraft_intent.GetNumberOfWaypoints() == 26); - ASSERT_TRUE(aircraft_intent.GetWaypoint(0).GetName().compare("17R") == 0); - ASSERT_TRUE(aircraft_intent.GetRouteData().m_waypoint_phase_of_flight.front() == - AircraftIntent::WaypointPhaseOfFlight::ASCENT); - ASSERT_TRUE(aircraft_intent.GetWaypoint(aircraft_intent.GetNumberOfWaypoints() - 1).GetName().compare("UXCUN") == 0); - ASSERT_TRUE(aircraft_intent.GetRouteData().m_waypoint_phase_of_flight.back() == - AircraftIntent::WaypointPhaseOfFlight::DESCENT); - EXPECT_TRUE(aircraft_intent.ContainsWaypointName("SIGHT")); - EXPECT_FALSE(aircraft_intent.ContainsWaypointName("NOTFOUND")); - - CoreUtils::ResetMaximumAllowableSingleLegLength(); -} - -TEST(AircraftIntent, LoadOldWaypointDefinitionCleanly) { - std::string test_file_to_load = "./resources/aircraft_intent_tight_turn.txt"; - FILE *fp; - fp = fopen(test_file_to_load.c_str(), "r"); - if (fp == NULL) { - std::cout << "Test resource file " << test_file_to_load << " not found." << std::endl; - FAIL(); - } - - DecodedStream stream; - bool r = stream.open_file(test_file_to_load); - if (!r) { - std::cout << "Test resource file " << test_file_to_load << " could not be loaded." << std::endl; - FAIL(); - } - stream.set_echo(false); - - SingleTangentPlaneSequence::ClearStaticMembers(); - CoreUtils::UpdateMaximumAllowableSingleLegLength(Units::infinity()); - - // This is the tested method --------------- - AircraftIntent aircraft_intent = LoadAircraftIntent(&stream); - // ----------------------------------------- - - ASSERT_TRUE(aircraft_intent.GetNumberOfWaypoints() == 15); - ASSERT_TRUE(aircraft_intent.GetWaypoint(0).GetName().compare("TACUS") == 0); - ASSERT_TRUE(aircraft_intent.GetRouteData().m_waypoint_phase_of_flight.front() == - AircraftIntent::WaypointPhaseOfFlight::DESCENT); - ASSERT_TRUE(aircraft_intent.GetWaypoint(aircraft_intent.GetNumberOfWaypoints() - 1).GetName().compare("RELIN") == 0); - ASSERT_TRUE(aircraft_intent.GetRouteData().m_waypoint_phase_of_flight.back() == - AircraftIntent::WaypointPhaseOfFlight::DESCENT); - - CoreUtils::ResetMaximumAllowableSingleLegLength(); -} - -TEST(AircraftIntent, wgs84_to_xy) { - // open the test data file and get a stream - std::string testData = "./resources/EAGUL5_GALLUP.txt"; - FILE *fp; - fp = fopen(testData.c_str(), "r"); - if (fp == NULL) { - printf("Test resource file %s not found.", testData.c_str()); - } - - DecodedStream stream; - bool r = stream.open_file(testData); - if (!r) { - stream.report_error("cant open file\n"); - exit(-60); - } - stream.set_echo(false); - - SingleTangentPlaneSequence::ClearStaticMembers(); - CoreUtils::UpdateMaximumAllowableSingleLegLength(Units::infinity()); - AircraftIntent aiTest = LoadAircraftIntent(&stream); // read the test data - aiTest.UpdateXYZFromLatLonWgs84(); - - /* - * Note: the hardcoded expect data below comes from a MATLAB implementation. - */ - - // Asserts on xWp - const double xWpExpected[13] = {277562.820999161, - 201232.863838201, - 186285.934613104, - 134053.369540901, - 100297.996878049, - 73830.6751753431, - 44152.7658582631, - 39745.2432526958, - 25354.9291255275, - 18785.3834675821, - 11622.3437133676, - 6287.15962065872, - 0}; - for (int var = 0; var < aiTest.GetNumberOfWaypoints(); ++var) { - EXPECT_NEAR(xWpExpected[var], aiTest.GetRouteData().m_x[var].value(), TOLERANCE_METERS_TIGHT); - } - - // Asserts on yWp - const double yWpExpected[13] = {233339.721977913, - 155112.436782425, - 140854.236841671, - 117022.543148477, - 101469.15976212, - 76726.3797563801, - 48809.8190509554, - 42659.8427600031, - 22574.5169130158, - 13184.5343448488, - 3044.06282124402, - -10.9324304166729, - 0}; - for (int var = 0; var < aiTest.GetNumberOfWaypoints(); ++var) { - EXPECT_NEAR(yWpExpected[var], aiTest.GetRouteData().m_y[var].value(), TOLERANCE_METERS_TIGHT); - } - - CoreUtils::ResetMaximumAllowableSingleLegLength(); -} - -TEST(AircraftIntent, xyz_to_wgs84) { - std::string testData = "./resources/EAGUL5_GALLUP.txt"; - FILE *fp; - fp = fopen(testData.c_str(), "r"); - if (fp == NULL) { - printf("Test resource file %s not found.", testData.c_str()); - FAIL(); - } - - DecodedStream stream; - bool r = stream.open_file(testData); - if (!r) { - stream.report_error("cant open file\n"); - FAIL(); - } - stream.set_echo(false); - - SingleTangentPlaneSequence::ClearStaticMembers(); - AircraftIntent aiTest = LoadAircraftIntent(&stream); - aiTest.UpdateXYZFromLatLonWgs84(); - - // Convert back to lat/lon - Units::Angle latRad[128], lonRad[128]; - for (int i = 0; i < aiTest.GetNumberOfWaypoints(); i++) { - aiTest.GetLatLonFromXYZ(Units::MetersLength(aiTest.GetRouteData().m_x[i]), - Units::MetersLength(aiTest.GetRouteData().m_y[i]), - Units::MetersLength(aiTest.GetRouteData().m_z[i]), latRad[i], lonRad[i]); - } - - auto wp_list = aiTest.GetWaypointList(); - const auto tangent_plane_sequence = std::make_shared(wp_list); - const auto waypoints = tangent_plane_sequence->GetWaypointsFromInitialization(); - - for (int var = 0; var < aiTest.GetNumberOfWaypoints(); ++var) { - EXPECT_NEAR(Units::RadiansAngle(waypoints[var].GetLatitude()).value(), Units::RadiansAngle(latRad[var]).value(), - TOLERANCE_RADIANS); - } - - for (int var = 0; var < aiTest.GetNumberOfWaypoints(); ++var) { - EXPECT_NEAR(Units::RadiansAngle(waypoints[var].GetLongitude()).value(), Units::RadiansAngle(lonRad[var]).value(), - TOLERANCE_RADIANS); - } - - // go back the other way - for (int i = 0; i < aiTest.GetNumberOfWaypoints(); ++i) { - EarthModel::GeodeticPosition geo; - geo.latitude = Units::RadiansAngle(latRad[i]); - geo.longitude = Units::RadiansAngle(lonRad[i]); - geo.altitude = Units::MetersLength(0); - - EarthModel::LocalPositionEnu enu; - tangent_plane_sequence->ConvertGeodeticToLocal(geo, enu); - - EXPECT_NEAR(aiTest.GetRouteData().m_x[i].value(), Units::MetersLength(enu.x).value(), TOLERANCE_METERS); - EXPECT_NEAR(aiTest.GetRouteData().m_y[i].value(), Units::MetersLength(enu.y).value(), TOLERANCE_METERS); - } -} - -TEST(AircraftIntent, load_waypoints_variations) { - std::vector> test_files = { - std::make_pair("./resources/aircraft_intent_load_test1.txt", 27), - std::make_pair("./resources/aircraft_intent_load_test2.txt", 12), - std::make_pair("./resources/aircraft_intent_load_test3.txt", 2), - std::make_pair("./resources/aircraft_intent_load_test4.txt", 13), - std::make_pair("./resources/aircraft_intent_load_test5.txt", 16), - std::make_pair("./resources/aircraft_intent_load_test6.txt", 15), - std::make_pair("./resources/aircraft_intent_load_test7.txt", 26), - }; - - for (auto test_details : test_files) { - auto test_file = test_details.first; - auto expected_waypoint_count = test_details.second; - FILE *fp; - fp = fopen(test_file.c_str(), "r"); - if (fp == NULL) { - std::cout << "Test resource file " << test_file << " not found." << std::endl; - FAIL(); - } - - DecodedStream stream; - bool r = stream.open_file(test_file); - if (!r) { - std::cout << "Test resource file " << test_file << " could not be loaded." << std::endl; - FAIL(); - } - stream.set_echo(false); - - SingleTangentPlaneSequence::ClearStaticMembers(); - CoreUtils::UpdateMaximumAllowableSingleLegLength(Units::infinity()); - - // This is the tested method --------------- - AircraftIntent aircraft_intent = LoadAircraftIntent(&stream); - EXPECT_EQ(expected_waypoint_count, aircraft_intent.GetNumberOfWaypoints()); - } -} - -TEST(AircraftIntent, load_no_waypoints_throws) { - std::string test_file = "./resources/aircraft_intent_load_test8.txt"; - FILE *fp; - fp = fopen(test_file.c_str(), "r"); - if (fp == NULL) { - std::cout << "Test resource file " << test_file << " not found." << std::endl; - FAIL(); - } - - DecodedStream stream; - bool r = stream.open_file(test_file); - if (!r) { - std::cout << "Test resource file " << test_file << " could not be loaded." << std::endl; - FAIL(); - } - stream.set_echo(false); - - SingleTangentPlaneSequence::ClearStaticMembers(); - CoreUtils::UpdateMaximumAllowableSingleLegLength(Units::infinity()); - - aaesim::loaders::AircraftIntentLoader aircraft_intent_loader; - EXPECT_ANY_THROW(aircraft_intent_loader.load(&stream)); -} - -TEST(AircraftIntent, test_consistency_long_route) { - SingleTangentPlaneSequence::ClearStaticMembers(); - CoreUtils::ResetMaximumAllowableSingleLegLength(); - - std::string test_file = "./resources/long_distance_route.txt"; - FILE *fp; - fp = fopen(test_file.c_str(), "r"); - if (fp == NULL) { - std::cout << "Test resource file " << test_file << " not found." << std::endl; - FAIL(); - } - - DecodedStream stream; - bool r = stream.open_file(test_file); - if (!r) { - std::cout << "Test resource file " << test_file << " could not be loaded." << std::endl; - FAIL(); - } - stream.set_echo(false); - - AircraftIntent aircraft_intent = LoadAircraftIntent(&stream); - auto wplist = aircraft_intent.GetWaypointList(); - auto position_converter = std::make_shared(wplist); - for (Waypoint wp : aircraft_intent.GetWaypointList()) { - EarthModel::LocalPositionEnu enu_position; - EarthModel::GeodeticPosition geodetic_position; - position_converter->ConvertGeodeticToLocal(EarthModel::GeodeticPosition::CreateFromWaypoint(wp), enu_position); - position_converter->ConvertLocalToGeodetic(enu_position, geodetic_position); - EXPECT_NEAR(Units::RadiansAngle(geodetic_position.latitude).value(), - Units::RadiansAngle(wp.GetLatitude()).value(), 1e-5); - EXPECT_NEAR(Units::RadiansAngle(geodetic_position.longitude).value(), - Units::RadiansAngle(wp.GetLongitude()).value(), 1e-5); - } -} - -TEST(AircraftIntent, CopyAndTrimAfterNamedWaypoint1) { - std::string test_file_to_load = "./resources/aircraftintent_full_route_kden_kphx.txt"; - const std::string waypoint_name_to_trim("VNNOM"); - AircraftIntent test_aircraft_intent = aaesim::test::utils::PublicUtils::LoadAircraftIntent(test_file_to_load); - const uint before_trim_count = test_aircraft_intent.GetNumberOfWaypoints(); - const std::string final_waypoint_before_trim(test_aircraft_intent.GetWaypointList().back().GetName()); - AircraftIntent trimmed_result = - AircraftIntent::CopyAndTrimAfterNamedWaypoint(test_aircraft_intent, waypoint_name_to_trim); - EXPECT_GT(before_trim_count, trimmed_result.GetNumberOfWaypoints()); - EXPECT_TRUE(trimmed_result.ContainsWaypointName(waypoint_name_to_trim)); - EXPECT_FALSE(trimmed_result.ContainsWaypointName(final_waypoint_before_trim)); - EXPECT_TRUE(trimmed_result.GetRouteData().m_name.back().compare(waypoint_name_to_trim) == 0); - EXPECT_EQ(test_aircraft_intent.GetPlannedCruiseAltitude(), trimmed_result.GetPlannedCruiseAltitude()); - EXPECT_EQ(test_aircraft_intent.GetPlannedCruiseMach(), trimmed_result.GetPlannedCruiseMach()); -} - -TEST(AircraftIntent, CopyAndTrimAfterNamedWaypoint2) { - std::string test_file_to_load = "./resources/aircraftintent_full_route_kden_kphx.txt"; - const std::string waypoint_name_to_trim("SOLAR"); - AircraftIntent test_aircraft_intent = aaesim::test::utils::PublicUtils::LoadAircraftIntent(test_file_to_load); - const uint before_trim_count = test_aircraft_intent.GetNumberOfWaypoints(); - const std::string final_waypoint_before_trim(test_aircraft_intent.GetWaypointList().back().GetName()); - AircraftIntent trimmed_result = - AircraftIntent::CopyAndTrimAfterNamedWaypoint(test_aircraft_intent, waypoint_name_to_trim); - EXPECT_GT(before_trim_count, trimmed_result.GetNumberOfWaypoints()); - EXPECT_TRUE(trimmed_result.ContainsWaypointName(waypoint_name_to_trim)); - EXPECT_FALSE(trimmed_result.ContainsWaypointName(final_waypoint_before_trim)); - EXPECT_TRUE(trimmed_result.GetRouteData().m_name.back().compare(waypoint_name_to_trim) == 0); - EXPECT_EQ(test_aircraft_intent.GetPlannedCruiseAltitude(), trimmed_result.GetPlannedCruiseAltitude()); - EXPECT_EQ(test_aircraft_intent.GetPlannedCruiseMach(), trimmed_result.GetPlannedCruiseMach()); -} - -TEST(AircraftIntent, CopyAndTrimNoChange) { - std::string test_file_to_load = "./resources/aircraftintent_full_route_kden_kphx.txt"; - const std::string waypoint_name_to_trim("UXCUN"); - AircraftIntent test_aircraft_intent = aaesim::test::utils::PublicUtils::LoadAircraftIntent(test_file_to_load); - AircraftIntent trimmed_result = - AircraftIntent::CopyAndTrimAfterNamedWaypoint(test_aircraft_intent, waypoint_name_to_trim); - EXPECT_TRUE(test_aircraft_intent == trimmed_result); -} - -TEST(AircraftIntent, findCommonWaypoint_NoCommonRoute) { - const AircraftIntent ai1 = PublicUtils::LoadAircraftIntent("./resources/EAGUL5_GALLUP.txt"); - const AircraftIntent ai2 = PublicUtils::LoadAircraftIntent("./resources/TACUStoRELINIntent.txt"); - const std::pair expected(-1, -1); - const std::pair actual = ai1.FindCommonWaypoint(ai2); - EXPECT_EQ(expected, actual); -} - -TEST(AircraftIntent, findCommonWaypoint_CommonRoute) { - const AircraftIntent ai1 = PublicUtils::LoadAircraftIntent("./resources/EAGUL5_GALLUP.txt"); - const std::pair expected(0, 0); - const std::pair actual = ai1.FindCommonWaypoint(ai1); // check against self -- all waypoints in common - EXPECT_EQ(expected, actual); -} - -TEST(AircraftIntent, findCommonWaypoint_PartiallyCommonRoute) { - CoreUtils::UpdateMaximumAllowableSingleLegLength(Units::infinity()); - const AircraftIntent ai1 = PublicUtils::LoadAircraftIntent("./resources/EAGUL5_GALLUP.txt"); - const AircraftIntent ai2 = PublicUtils::LoadAircraftIntent("./resources/AchievePointCalcs_Intent.txt"); - const std::pair expected(2, 0); - const std::pair actual = ai1.FindCommonWaypoint(ai2); - EXPECT_EQ(expected, actual); - CoreUtils::ResetMaximumAllowableSingleLegLength(); -} -TEST(AircraftIntent, insertNewPointAtBeginning) { - /* - * Load an aircraft intent definition from resources. - * Insert a new location and test to make sure the new location - * is at the correct index. - */ - // open the test data file and get a stream - std::string testData = "./resources/EAGUL5_GALLUP.txt"; - AircraftIntent aircraftIntent = PublicUtils::PrepareAircraftIntent(testData); - - const auto idx = 0; // insert as first point - const std::string testName("testpt"); - const Units::MetersLength x(10.0), y(10000.0); - aircraftIntent.InsertPairAtIndex(testName, x, y, idx); - - // Assert - const double tol = Units::MetersLength(1e-1).value(); - EXPECT_EQ(testName, aircraftIntent.GetWaypointName(idx)); - EXPECT_NEAR(x.value(), aircraftIntent.GetWaypointX(idx).value(), tol); - EXPECT_NEAR(y.value(), aircraftIntent.GetWaypointY(idx).value(), tol); -} - -TEST(AircraftIntent, findWaypointIx) { - CoreUtils::UpdateMaximumAllowableSingleLegLength(Units::infinity()); - - AircraftIntent intent = PublicUtils::PrepareAircraftIntent("./resources/findIndexIntent.txt"); - - if (!intent.IsLoaded()) { - FAIL(); - } - - EXPECT_EQ(9, intent.GetWaypointIndexByName("JAGAL")); // waypoint in list - EXPECT_EQ(-1, intent.GetWaypointIndexByName("SATURN")); // waypoint not in list - EXPECT_EQ(-1, intent.GetWaypointIndexByName("")); // empty waypoint - - CoreUtils::ResetMaximumAllowableSingleLegLength(); -} - -TEST(AircraftIntent, insertNewPointAtEnd) { - /* - * Load an aircraft intent definition from resources. - * Insert a new location and test to make sure the new location - * is at the correct index. - */ - // open the test data file and get a stream - std::string testData = "./resources/EAGUL5_GALLUP.txt"; - AircraftIntent aircraftIntent = PublicUtils::PrepareAircraftIntent(testData); - - const auto idx = - aircraftIntent.GetNumberOfWaypoints(); // insert as last point, so one index beyond end (zero-based) - const std::string testName("testpt"); - const Units::MetersLength x(10.0), y(10000.0); - aircraftIntent.InsertPairAtIndex(testName, x, y, idx); - - // Assert - const auto testidx = aircraftIntent.GetNumberOfWaypoints() - 1; - const double tol = Units::MetersLength(1e-1).value(); - EXPECT_EQ(testName, aircraftIntent.GetWaypointName(testidx)); - EXPECT_NEAR(x.value(), aircraftIntent.GetWaypointX(testidx).value(), tol); - EXPECT_NEAR(y.value(), aircraftIntent.GetWaypointY(testidx).value(), tol); -} - -} // namespace open_source -} // namespace test -} // namespace aaesim diff --git a/unittest/src/Public/public.cmake b/unittest/src/Public/public.cmake index 6448a44..882db55 100644 --- a/unittest/src/Public/public.cmake +++ b/unittest/src/Public/public.cmake @@ -5,7 +5,6 @@ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fPIC") set(PUBLIC_LIBRARY_TEST_SOURCE ${UNITTEST_DIR}/src/Public/geolib_tests.cpp - ${UNITTEST_DIR}/src/Public/aircraft_intent_tests.cpp ${UNITTEST_DIR}/src/Public/windstack_tests.cpp ${UNITTEST_DIR}/src/Public/utility_tests.cpp ${UNITTEST_DIR}/src/Public/public_tests.cpp @@ -13,6 +12,7 @@ set(PUBLIC_LIBRARY_TEST_SOURCE ${UNITTEST_DIR}/src/Public/tangent_plane_tests.cpp ${UNITTEST_DIR}/src/Public/wind_blending_tests.cpp ${UNITTEST_DIR}/src/Public/earth_model_tests.cpp + ${UNITTEST_DIR}/src/Public/threedof_glider_tests.cpp ) add_executable(public_test diff --git a/unittest/src/Public/threedof_glider_tests.cpp b/unittest/src/Public/threedof_glider_tests.cpp new file mode 100644 index 0000000..14ff496 --- /dev/null +++ b/unittest/src/Public/threedof_glider_tests.cpp @@ -0,0 +1,274 @@ +// **************************************************************************** +// NOTICE +// +// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 +// and is subject to Federal Aviation Administration Acquisition Management System +// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). +// +// The contents of this document reflect the views of the author and The MITRE +// Corporation and do not necessarily reflect the views of the Federal Aviation +// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA +// nor the DOT makes any warranty or guarantee, expressed or implied, concerning +// the content or accuracy of these views. +// +// For further information, please contact The MITRE Corporation, Contracts Management +// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. +// +// (c) 2026 The MITRE Corporation. All Rights Reserved. +// **************************************************************************** + +#include + +#include +#include +#include + +#include "public/AircraftControl.h" +#include "public/FixedMassAircraftPerformance.h" +#include "public/NullPositionEstimator.h" +#include "public/SimulationTime.h" +#include "public/ThreeDOFDynamics.h" +#include "public/USStandardAtmosphere1976.h" +#include "public/WeatherTruth.h" +#include "public/WindZero.h" +#include "public/ZeroWindTrueWeatherOperator.h" + +using namespace aaesim::open_source; + +namespace aaesim { +namespace test { +namespace { + +class SimulationTimeStepGuard { + public: + explicit SimulationTimeStepGuard(Units::Time time_step) + : original_time_step_(SimulationTime::GetSimulationTimeStep()) { + SimulationTime::SetSimulationTimeStep(time_step); + } + ~SimulationTimeStepGuard() { SimulationTime::SetSimulationTimeStep(original_time_step_); } + + private: + Units::SecondsTime original_time_step_; +}; + +class IdealGliderPerformance final : public FixedMassAircraftPerformance { + public: + void GetDragCoefficients(const Units::Speed &calibrated_airspeed, const Units::Length &altitude_msl, + const bada_utils::FlapConfiguration ¤t_flap_configuration, double &cd0, double &cd2, + double &gear, bada_utils::FlapConfiguration &flap_configuration) const override { + SetZeroDrag(cd0, cd2, gear); + flap_configuration = current_flap_configuration; + } + + void GetDragCoefficientsAndIncrementFlapConfiguration(const Units::Speed &calibrated_airspeed, + const Units::Length &altitude_msl, double &cd0, double &cd2, + double &gear, + bada_utils::FlapConfiguration &updated_flap_setting) override { + SetZeroDrag(cd0, cd2, gear); + updated_flap_setting = bada_utils::FlapConfiguration::CRUISE; + } + + void GetCurrentDragCoefficients(double &cd0, double &cd2, double &gear) const override { + SetZeroDrag(cd0, cd2, gear); + } + + void GetConfigurationForIncreasedDrag(const Units::Speed &calibrated_airspeed, const Units::Length &altitude_msl, + bada_utils::FlapConfiguration &updated_flap_setting) override { + updated_flap_setting = bada_utils::FlapConfiguration::CRUISE; + } + + Units::NewtonsForce GetMaxThrust(const Units::Length &altitude_msl, bada_utils::FlapConfiguration flap_configuration, + bada_utils::EngineThrustMode engine_thrust_mode, + Units::AbsCelsiusTemperature temperature_offset) const override { + return Units::NewtonsForce(0); + } + + void GetCoefficientsForFlapConfiguration(bada_utils::FlapConfiguration flap_configuration, double &cd0, double &cd2, + double &gear) const override { + SetZeroDrag(cd0, cd2, gear); + } + + bada_utils::FlapConfiguration GetFlapConfigurationForState( + const Units::Speed &calibrated_airspeed, const Units::Length &altitude_msl, + const bada_utils::FlapConfiguration ¤t_flap_configuration) const override { + return current_flap_configuration; + } + + Units::Mass GetAircraftMass() const override { return Units::KilogramsMass(50000.0); } + + double GetAircraftMassPercentile() const override { return 0.5; } + + bada_utils::FlapSpeeds GetFlapSpeeds() const override { return bada_utils::FlapSpeeds{}; } + + bada_utils::FlapConfiguration GetCurrentFlapConfiguration() const override { + return bada_utils::FlapConfiguration::CRUISE; + } + + void UpdateMassFraction(BoundedValue mass_fraction) override {} + + bada_utils::AircraftType GetAircraftTypeInformation() const override { return bada_utils::AircraftType{}; } + + bada_utils::Mass GetAircraftMassInformation() const override { return bada_utils::Mass{}; } + + bada_utils::FlightEnvelope GetFlightEnvelopeInformation() const override { return bada_utils::FlightEnvelope{}; } + + bada_utils::Aerodynamics GetAerodynamicsInformation() const override { + bada_utils::Aerodynamics aerodynamics{}; + aerodynamics.S = Units::MetersArea(100.0); + aerodynamics.cruise.V_stall = Units::MetersPerSecondSpeed(1.0); + return aerodynamics; + } + + bada_utils::EngineThrust GetEngineThrustInformation() const override { return bada_utils::EngineThrust{}; } + + bada_utils::FuelFlow GetFuelFlowInformation() const override { return bada_utils::FuelFlow{}; } + + bada_utils::GroundMovement GetGroundMovementInformation() const override { return bada_utils::GroundMovement{}; } + + bada_utils::Procedure GetProcedureInformation(unsigned int index) const override { return bada_utils::Procedure{}; } + + bada_utils::AircraftPerformance GetAircraftPerformanceInformation() const override { + return bada_utils::AircraftPerformance{}; + } + + std::string GetAircraftTypeIdentifier() const override { return "IDEAL_GLIDER"; } + + private: + static void SetZeroDrag(double &cd0, double &cd2, double &gear) { + cd0 = 0.0; + cd2 = 0.0; + gear = 0.0; + } +}; + +std::shared_ptr MakeZeroWindStandardAtmosphereOperator() { + auto atmosphere = std::make_shared(); + auto wind = std::make_shared(atmosphere); + auto weather_truth = std::make_shared(wind, atmosphere, true); + return std::make_shared(weather_truth); +} + +std::shared_ptr MakeNullAircraftControl( + const std::shared_ptr &aircraft_performance) { + auto aircraft_control = AircraftControl::Builder() + .WithCruiseDescentLateralController(std::make_shared()) + .WithCruiseDescentVerticalController(std::make_shared()) + .Build(); + aircraft_control->Initialize(aircraft_performance); + return aircraft_control; +} + +Guidance MakeCruiseDescentGuidance(Units::Speed true_airspeed, Units::Length altitude_msl, + Units::SignedAngle track_enu) { + Guidance guidance; + guidance.m_active_guidance_phase = GuidanceFlightPhase::CRUISE_DESCENT; + guidance.m_ground_speed = true_airspeed; + guidance.m_ias_command = true_airspeed; + guidance.m_reference_altitude = altitude_msl; + guidance.m_enu_track_angle = track_enu; + guidance.m_reference_bank_angle = Units::zero(); + guidance.m_vertical_speed = Units::zero(); + guidance.m_cross_track_error = Units::zero(); + guidance.m_use_cross_track = false; + return guidance; +} + +ThreeDOFDynamics MakeInitializedGlider(const std::shared_ptr &aircraft_performance, + Units::Length initial_altitude_msl, Units::Speed initial_true_airspeed, + Units::SignedAngle initial_track_enu, + const EarthModel::LocalPositionEnu &initial_position_enu) { + ThreeDOFDynamics dynamics; + dynamics.Initialize(SimulationTime::Of(Units::ZERO_TIME), aircraft_performance, + EarthModel::GeodeticPosition::Of(Units::ZERO_ANGLE, Units::ZERO_ANGLE), initial_position_enu, + initial_altitude_msl, initial_true_airspeed, initial_track_enu, 0.5, + std::make_shared(), MakeZeroWindStandardAtmosphereOperator()); + return dynamics; +} + +AircraftState RunUpdates(ThreeDOFDynamics &dynamics, const Guidance &guidance, + const std::shared_ptr &aircraft_control, int update_count) { + AircraftState state; + for (int update_index = 1; update_index <= update_count; ++update_index) { + state = dynamics.Update(42, SimulationTime::Of(Units::SecondsTime(update_index)), guidance, aircraft_control); + } + return state; +} + +} // namespace + +TEST(ThreeDofGliderKinematics, level_eastbound_motion_matches_constant_velocity_solution) { + const SimulationTimeStepGuard time_step_guard(Units::SecondsTime(1.0)); + const auto aircraft_performance = std::make_shared(); + const auto aircraft_control = MakeNullAircraftControl(aircraft_performance); + + const Units::MetersLength initial_x(1200.0); + const Units::MetersLength initial_y(-300.0); + const Units::MetersLength initial_altitude_msl(3000.0); + const Units::MetersPerSecondSpeed initial_true_airspeed(210.0); + const Units::SignedDegreesAngle initial_track_enu(0.0); + const int update_count = 5; + + auto dynamics = + MakeInitializedGlider(aircraft_performance, initial_altitude_msl, initial_true_airspeed, initial_track_enu, + EarthModel::LocalPositionEnu::Of(initial_x, initial_y, Units::zero())); + + const auto state = + RunUpdates(dynamics, MakeCruiseDescentGuidance(initial_true_airspeed, initial_altitude_msl, initial_track_enu), + aircraft_control, update_count); + + const Units::SecondsTime elapsed_time(update_count * SimulationTime::GetSimulationTimeStep().value()); + const Units::MetersLength expected_x = initial_x + initial_true_airspeed * elapsed_time; + + EXPECT_NEAR(expected_x.value(), Units::MetersLength(state.GetPositionEnuX()).value(), 1e-9); + EXPECT_NEAR(initial_y.value(), Units::MetersLength(state.GetPositionEnuY()).value(), 1e-9); + EXPECT_NEAR(initial_altitude_msl.value(), Units::MetersLength(state.GetAltitudeMsl()).value(), 1e-9); + EXPECT_NEAR(initial_true_airspeed.value(), Units::MetersPerSecondSpeed(state.GetTrueAirspeed()).value(), 1e-9); + EXPECT_NEAR(initial_true_airspeed.value(), Units::MetersPerSecondSpeed(state.GetGroundSpeed()).value(), 1e-9); + EXPECT_NEAR(0.0, Units::MetersPerSecondSpeed(state.GetVerticalSpeed()).value(), 1e-12); + EXPECT_NEAR( + 0.0, + Units::MetersSecondAcceleration(dynamics.GetEquationsOfMotionStateDerivative().true_airspeed_deriv).value(), + 1e-12); + EXPECT_NEAR(0.0, + Units::RadiansPerSecondAngularSpeed(dynamics.GetEquationsOfMotionStateDerivative().gamma_deriv).value(), + 1e-12); + EXPECT_NEAR( + 0.0, Units::RadiansPerSecondAngularSpeed(dynamics.GetEquationsOfMotionStateDerivative().heading_deriv).value(), + 1e-12); +} + +TEST(ThreeDofGliderKinematics, level_motion_resolves_three_four_five_heading_components) { + const SimulationTimeStepGuard time_step_guard(Units::SecondsTime(1.0)); + const auto aircraft_performance = std::make_shared(); + const auto aircraft_control = MakeNullAircraftControl(aircraft_performance); + + const Units::MetersLength initial_x(-75.0); + const Units::MetersLength initial_y(40.0); + const Units::MetersLength initial_altitude_msl(1800.0); + const Units::MetersPerSecondSpeed initial_true_airspeed(250.0); + const Units::SignedRadiansAngle initial_track_enu(std::atan2(4.0, 3.0)); + const int update_count = 4; + + auto dynamics = + MakeInitializedGlider(aircraft_performance, initial_altitude_msl, initial_true_airspeed, initial_track_enu, + EarthModel::LocalPositionEnu::Of(initial_x, initial_y, Units::zero())); + + const auto state = + RunUpdates(dynamics, MakeCruiseDescentGuidance(initial_true_airspeed, initial_altitude_msl, initial_track_enu), + aircraft_control, update_count); + + const Units::SecondsTime elapsed_time(update_count * SimulationTime::GetSimulationTimeStep().value()); + const Units::MetersLength expected_x = initial_x + initial_true_airspeed * elapsed_time * 3.0 / 5.0; + const Units::MetersLength expected_y = initial_y + initial_true_airspeed * elapsed_time * 4.0 / 5.0; + + EXPECT_NEAR(expected_x.value(), Units::MetersLength(state.GetPositionEnuX()).value(), 1e-9); + EXPECT_NEAR(expected_y.value(), Units::MetersLength(state.GetPositionEnuY()).value(), 1e-9); + EXPECT_NEAR(initial_altitude_msl.value(), Units::MetersLength(state.GetAltitudeMsl()).value(), 1e-9); + EXPECT_NEAR(initial_true_airspeed.value(), Units::MetersPerSecondSpeed(state.GetTrueAirspeed()).value(), 1e-9); + EXPECT_NEAR(initial_true_airspeed.value(), Units::MetersPerSecondSpeed(state.GetGroundSpeed()).value(), 1e-9); + EXPECT_NEAR(Units::RadiansAngle(initial_track_enu).value(), + Units::RadiansAngle(state.GetHeadingCcwFromEastRadians()).value(), 1e-12); +} + +} // namespace test +} // namespace aaesim diff --git a/unittest/src/Public/windstack_tests.cpp b/unittest/src/Public/windstack_tests.cpp index 73cda63..6ca87d4 100644 --- a/unittest/src/Public/windstack_tests.cpp +++ b/unittest/src/Public/windstack_tests.cpp @@ -18,7 +18,6 @@ // **************************************************************************** #include -#include #include #include diff --git a/unittest/src/utils/public/PublicUtils.cpp b/unittest/src/utils/public/PublicUtils.cpp index bc41748..c214ac1 100644 --- a/unittest/src/utils/public/PublicUtils.cpp +++ b/unittest/src/utils/public/PublicUtils.cpp @@ -19,14 +19,8 @@ #include "utils/public/PublicUtils.h" -#include -#include -#include #include -#include "public/AircraftIntentLoader.h" -#include "public/SingleTangentPlaneSequence.h" - using namespace std; using namespace aaesim::open_source; @@ -80,36 +74,3 @@ std::vector aaesim::test::utils::PublicUtils::CreateStraightHori return horizontal_traj; } - -AircraftIntent aaesim::test::utils::PublicUtils::LoadAircraftIntent(std::string parmsfile) { - DecodedStream intentstream; - aaesim::loaders::AircraftIntentLoader intent_loader; - FILE *fp = fopen(parmsfile.c_str(), "r"); - if (fp == nullptr) { - std::cout << "Intent file " << parmsfile.c_str() << " not found" << std::endl; - } else { - bool r = intentstream.open_file(parmsfile); - - if (!r) { - std::cout << "Can't open intent parameters file " << parmsfile.c_str() << std::endl; - } else { - SingleTangentPlaneSequence::ClearStaticMembers(); // make sure singleton is clear - - intentstream.set_echo(false); // default set to false, must turn it on in input file - - intent_loader.load(&intentstream); - } - } - - fclose(fp); - return intent_loader.BuildAircraftIntent(); -} - -AircraftIntent aaesim::test::utils::PublicUtils::PrepareAircraftIntent(std::string parmsfile) { - AircraftIntent intent = LoadAircraftIntent(parmsfile); - if (intent.IsLoaded()) { - intent.UpdateXYZFromLatLonWgs84(); - } - - return intent; -} From 28c8ebcf18a15129e916585a8226a678021be599 Mon Sep 17 00:00:00 2001 From: Stuart Bowman Date: Fri, 26 Jun 2026 15:57:12 -0400 Subject: [PATCH 2/8] chore: moved code to separate repo: aircraft_simulation_core --- AircraftDynamicsTestFramework/CMakeLists.txt | 7 +- AircraftDynamicsTestFramework/framework.cmake | 3 - CMakeLists.txt | 12 +- Public/ADSBSVReport.cpp | 157 -- Public/AchieveObserver.cpp | 89 - Public/Aircraft.cpp | 28 - Public/AircraftCalculations.cpp | 482 ----- Public/AircraftControl.cpp | 70 - Public/AircraftIntent.cpp | 532 ------ Public/AircraftIntentLoader.cpp | 77 - Public/AircraftSpeed.cpp | 47 - Public/AircraftState.cpp | 284 --- Public/AlongPathDistanceCalculator.cpp | 168 -- Public/ArcOnEllipsoid.cpp | 186 -- Public/Atmosphere.cpp | 41 - Public/BlendWindsVerticallyByAltitude.cpp | 141 -- Public/CMakeLists.txt | 119 -- Public/CalcWindGradControl.cpp | 71 - Public/ClimbPhaseVerticalController.cpp | 80 - Public/ClosestPointMetric.cpp | 85 - Public/ConfigurationFileReader.cpp | 72 - Public/ControlCommands.cpp | 20 - Public/CoreUtils.cpp | 164 -- Public/CrossTrackObserver.cpp | 38 - Public/CustomMath.cpp | 247 --- Public/DMatrix.cpp | 251 --- Public/DVector.cpp | 133 -- Public/DataReader.cpp | 128 -- Public/DefaultLateralController.cpp | 92 - Public/DirectionOfFlightCourseCalculator.cpp | 183 -- Public/DynamicsObserver.cpp | 43 - Public/EarthModel.cpp | 42 - Public/EllipsoidalEarthModel.cpp | 81 - Public/EnvReader.cpp | 70 - Public/Environment.cpp | 37 - Public/EuclideanThreeDofDynamics.cpp | 58 - Public/EuclideanTrajectoryPredictor.cpp | 1123 ----------- Public/EuclideanWaypointMonitor.cpp | 77 - Public/FlightEnvelopeSpeedLimiter.cpp | 92 - Public/FmsWaypointSequenceFile.cpp | 84 - Public/ForeWindReader.cpp | 63 - Public/FullWindTrueWeatherOperator.cpp | 40 - Public/GeolibUtils.cpp | 366 ---- Public/Guidance.cpp | 44 - Public/HfpReader.cpp | 150 -- Public/HfpReader2020.cpp | 168 -- Public/HfpReaderPre2020.cpp | 152 -- Public/HorizontalPath.cpp | 34 - Public/HorizontalPathTracker.cpp | 250 --- Public/HorizontalTurnPath.cpp | 43 - Public/IMCommandObserver.cpp | 55 - Public/InternalObserver.cpp | 1072 ----------- Public/InvalidIndexException.cpp | 32 - Public/KinematicDescent4DPredictor.cpp | 1512 --------------- Public/KinematicTrajectoryPredictor.cpp | 77 - Public/LatitudeLongitudePoint.cpp | 77 - Public/LegacyPositionEstimator.cpp | 44 - Public/LineOnEllipsoid.cpp | 169 -- Public/LocalTangentPlane.cpp | 143 -- Public/MaintainMetric.cpp | 156 -- Public/MergePointMetric.cpp | 175 -- Public/NMObserver.cpp | 82 - Public/NMObserverEntry.cpp | 38 - Public/NullSpeedLimiter.cpp | 40 - Public/NullWindEvaluator.cpp | 45 - Public/OutputHandler.cpp | 23 - Public/PassThroughAssap.cpp | 38 - Public/PilotDelay.cpp | 244 --- Public/PositionCalculator.cpp | 147 -- Public/PrecalcConstraint.cpp | 61 - Public/PrecalcWaypoint.cpp | 33 - Public/PredictionFileBase.cpp | 56 - Public/RandomGenerator.cpp | 114 -- Public/RefReader.cpp | 58 - Public/RunFile.cpp | 26 - Public/Scenario.cpp | 60 - Public/ScenarioUtils.cpp | 27 - Public/SingleTangentPlaneSequence.cpp | 42 - Public/SpeedOnPitchControl.cpp | 153 -- Public/SpeedOnThrustControl.cpp | 113 -- Public/StandardAtmosphere.cpp | 103 - Public/StatisticalPilotDelay.cpp | 187 -- Public/StereographicProjection.cpp | 288 --- Public/TangentPlaneSequence.cpp | 144 -- Public/ThreeDOFDynamics.cpp | 422 ---- Public/TvReader.cpp | 122 -- Public/USStandardAtmosphere1976.cpp | 186 -- Public/VectorDifferenceWindEvaluator.cpp | 63 - Public/VerticalPath.cpp | 124 -- Public/VerticalPathObserver.cpp | 135 -- Public/VerticalPredictor.cpp | 347 ---- Public/Waypoint.cpp | 72 - Public/WaypointLoader.cpp | 121 -- Public/WaypointLoader.h | 45 - Public/WeatherEstimate.cpp | 172 -- Public/WeatherPrediction.cpp | 48 - Public/WeatherTruth.cpp | 40 - Public/Wgs84PrecalcWaypoint.cpp | 39 - Public/Wind.cpp | 54 - Public/WindStack.cpp | 278 --- Public/WindZero.cpp | 63 - Public/ZeroWindTrueWeatherOperator.cpp | 34 - include/public/ADSBReceiver.h | 44 - include/public/ADSBSVReport.h | 135 -- include/public/ADSBTransmitter.h | 50 - include/public/ASSAP.h | 45 - include/public/AbstractAscentController.h | 65 - include/public/AbstractDescentController.h | 66 - include/public/AbstractTrueWeatherOperator.h | 44 - include/public/AchieveObserver.h | 55 - include/public/Aircraft.h | 39 - include/public/AircraftCalculations.h | 149 -- include/public/AircraftControl.h | 136 -- include/public/AircraftControllerFactory.h | 98 - include/public/AircraftIntent.h | 328 ---- include/public/AircraftIntentLoader.h | 58 - include/public/AircraftSpeed.h | 60 - include/public/AircraftState.h | 209 -- include/public/AlongPathDistanceCalculator.h | 80 - include/public/ArcOnEllipsoid.h | 103 - include/public/AscentController.h | 34 - include/public/Atmosphere.h | 147 -- include/public/BadaUtils.h | 261 --- .../public/BlendWindsVerticallyByAltitude.h | 40 - include/public/CalcWindGradControl.h | 67 - include/public/ClimbPhaseVerticalController.h | 63 - include/public/ClosestPointMetric.h | 57 - include/public/ConfigurationFileReader.h | 38 - include/public/ControlCommands.h | 58 - include/public/CoreUtils.h | 152 -- include/public/CrossTrackObserver.h | 40 - include/public/CustomMath.h | 47 - include/public/DMatrix.h | 83 - include/public/DVector.h | 60 - include/public/DataReader.h | 73 - include/public/DefaultLateralController.h | 56 - .../DirectionOfFlightCourseCalculator.h | 72 - include/public/DynamicsObserver.h | 37 - include/public/DynamicsState.h | 57 - include/public/EarthModel.h | 150 -- include/public/EllipsoidalEarthModel.h | 55 - include/public/EllipsoidalPositionEstimator.h | 40 - include/public/EnvReader.h | 51 - include/public/Environment.h | 42 - include/public/EquationsOfMotionState.h | 40 - include/public/EquationsOfMotionStateDeriv.h | 36 - include/public/EuclideanThreeDofDynamics.h | 48 - include/public/EuclideanTightTurnResolver.h | 28 - include/public/EuclideanTrajectoryPredictor.h | 199 -- include/public/EuclideanWaypointMonitor.h | 56 - include/public/FixedMassAircraftPerformance.h | 122 -- include/public/FlightDeckApplication.h | 70 - include/public/FlightDeckApplicationLibrary.h | 60 - include/public/FlightDeckApplicationLoader.h | 39 - include/public/FlightDeckDataWriter.h | 34 - include/public/FlightEnvelopeSpeedLimiter.h | 52 - include/public/FmsWaypointSequenceFile.h | 50 - include/public/ForeWindReader.h | 47 - include/public/FullWindTrueWeatherOperator.h | 43 - include/public/GeolibUtils.h | 307 --- include/public/Guidance.h | 103 - include/public/GuidanceCalculator.h | 61 - include/public/HfpReader.h | 76 - include/public/HfpReader2020.h | 101 - include/public/HfpReaderPre2020.h | 78 - include/public/HorizontalPath.h | 64 - include/public/HorizontalPathTracker.h | 149 -- include/public/HorizontalTurnPath.h | 64 - include/public/IMCommandObserver.h | 47 - include/public/InternalObserver.h | 237 --- include/public/InvalidIndexException.h | 32 - include/public/KinematicDescent4DPredictor.h | 161 -- include/public/KinematicTrajectoryPredictor.h | 122 -- include/public/KiteTightTurnResolver.h | 111 -- include/public/LateralController.h | 49 - include/public/LatitudeLongitudePoint.h | 87 - include/public/LawOfSinesResolver.h | 57 - include/public/LegacyPositionEstimator.h | 43 - include/public/LineOnEllipsoid.h | 86 - include/public/LocalTangentPlane.h | 99 - include/public/Log4cplusSetup.h | 50 - include/public/MaintainMetric.h | 87 - include/public/MergePointMetric.h | 86 - include/public/NMObserver.h | 60 - include/public/NMObserverEntry.h | 40 - include/public/NullADSBReceiver.h | 61 - include/public/NullAdsbTransmitter.h | 39 - include/public/NullAtmosphere.h | 92 - include/public/NullFlightDeckApplication.h | 43 - include/public/NullPilotDelay.h | 39 - include/public/NullPositionEstimator.h | 37 - include/public/NullSpeedLimiter.h | 44 - include/public/NullWindEvaluator.h | 49 - include/public/OutputHandler.h | 75 - include/public/PassThroughAssap.h | 45 - include/public/PilotDelay.h | 34 - include/public/PositionCalculator.h | 58 - include/public/PrecalcConstraint.h | 88 - include/public/PrecalcWaypoint.h | 52 - include/public/PredictedWindEvaluator.h | 41 - include/public/PredictionFileBase.h | 105 - include/public/RandomGenerator.h | 101 - include/public/RefReader.h | 49 - include/public/RunFile.h | 32 - include/public/Scenario.h | 40 - include/public/ScenarioEntity.h | 34 - include/public/ScenarioEventNotifier.h | 33 - include/public/ScenarioUtils.h | 78 - include/public/ShapeOnEllipsoid.h | 133 -- include/public/SimulationTime.h | 86 - include/public/SingleTangentPlaneSequence.h | 35 - include/public/SpeedBrakeController.h | 68 - include/public/SpeedCommandLimiter.h | 48 - include/public/SpeedOnPitchControl.h | 47 - include/public/SpeedOnThrustControl.h | 45 - include/public/StandardAtmosphere.h | 73 - include/public/StatisticalPilotDelay.h | 116 -- include/public/StereographicProjection.h | 58 - include/public/TakeOffVerticalController.h | 46 - include/public/TangentPlaneSequence.h | 107 -- include/public/ThreeDOFDynamics.h | 128 -- include/public/Token.h | 66 - include/public/TrueWeatherOperator.h | 46 - include/public/TurnAnticipation.h | 38 - include/public/TvReader.h | 81 - include/public/USStandardAtmosphere1976.h | 74 - .../public/VectorDifferenceWindEvaluator.h | 49 - include/public/VerticalController.h | 110 -- include/public/VerticalPath.h | 78 - include/public/VerticalPathObserver.h | 80 - include/public/VerticalPathUtils.h | 232 --- include/public/VerticalPredictor.h | 158 -- include/public/WGS84EarthModelConstants.h | 29 - include/public/Waypoint.h | 180 -- include/public/WaypointPassingMonitor.h | 35 - include/public/WeatherEstimate.h | 119 -- include/public/WeatherPrediction.h | 72 - include/public/WeatherTruth.h | 37 - include/public/Wgs84PrecalcWaypoint.h | 51 - include/public/Wind.h | 63 - include/public/WindBlendingAlgorithm.h | 30 - include/public/WindStack.h | 84 - include/public/WindZero.h | 54 - include/public/ZeroWindTrueWeatherOperator.h | 39 - include/public/minicsv.h | 1008 ---------- include/public/version.h | 72 - include/utility/BoundedValue.h | 133 -- include/utility/CsvParser.h | 114 -- include/utility/CustomUnits.h | 164 -- include/utility/FilePath.h | 79 - include/utility/Logging.h | 27 - include/utility/ProcessingTimeStats.h | 94 - include/utility/UtilityConstants.h | 39 - include/utility/UtilityTemplates.h | 25 - include/utility/constants.h | 71 - include/utility/dev-notes.md | 3 - include/utility/micros.h | 48 - unittest/src/Public/earth_model_tests.cpp | 127 -- unittest/src/Public/geolib_tests.cpp | 1690 ----------------- unittest/src/Public/public.cmake | 40 - .../src/Public/public_atmosphere_tests.cpp | 115 -- unittest/src/Public/public_tests.cpp | 1292 ------------- unittest/src/Public/tangent_plane_tests.cpp | 131 -- unittest/src/Public/threedof_glider_tests.cpp | 274 --- unittest/src/Public/utility_tests.cpp | 53 - unittest/src/Public/wind_blending_tests.cpp | 134 -- unittest/src/Public/windstack_tests.cpp | 215 --- .../src/utils/public/OldCustomMathUtils.cpp | 409 ---- .../src/utils/public/OldCustomMathUtils.h | 85 - unittest/src/utils/public/PublicUtils.cpp | 76 - unittest/src/utils/public/PublicUtils.h | 45 - unittest/unittest.cmake | 9 +- 272 files changed, 7 insertions(+), 32577 deletions(-) delete mode 100644 Public/ADSBSVReport.cpp delete mode 100644 Public/AchieveObserver.cpp delete mode 100644 Public/Aircraft.cpp delete mode 100644 Public/AircraftCalculations.cpp delete mode 100644 Public/AircraftControl.cpp delete mode 100644 Public/AircraftIntent.cpp delete mode 100644 Public/AircraftIntentLoader.cpp delete mode 100644 Public/AircraftSpeed.cpp delete mode 100644 Public/AircraftState.cpp delete mode 100644 Public/AlongPathDistanceCalculator.cpp delete mode 100644 Public/ArcOnEllipsoid.cpp delete mode 100644 Public/Atmosphere.cpp delete mode 100644 Public/BlendWindsVerticallyByAltitude.cpp delete mode 100644 Public/CMakeLists.txt delete mode 100644 Public/CalcWindGradControl.cpp delete mode 100644 Public/ClimbPhaseVerticalController.cpp delete mode 100644 Public/ClosestPointMetric.cpp delete mode 100644 Public/ConfigurationFileReader.cpp delete mode 100644 Public/ControlCommands.cpp delete mode 100644 Public/CoreUtils.cpp delete mode 100644 Public/CrossTrackObserver.cpp delete mode 100644 Public/CustomMath.cpp delete mode 100644 Public/DMatrix.cpp delete mode 100644 Public/DVector.cpp delete mode 100644 Public/DataReader.cpp delete mode 100644 Public/DefaultLateralController.cpp delete mode 100644 Public/DirectionOfFlightCourseCalculator.cpp delete mode 100644 Public/DynamicsObserver.cpp delete mode 100644 Public/EarthModel.cpp delete mode 100644 Public/EllipsoidalEarthModel.cpp delete mode 100644 Public/EnvReader.cpp delete mode 100644 Public/Environment.cpp delete mode 100644 Public/EuclideanThreeDofDynamics.cpp delete mode 100644 Public/EuclideanTrajectoryPredictor.cpp delete mode 100644 Public/EuclideanWaypointMonitor.cpp delete mode 100644 Public/FlightEnvelopeSpeedLimiter.cpp delete mode 100644 Public/FmsWaypointSequenceFile.cpp delete mode 100644 Public/ForeWindReader.cpp delete mode 100644 Public/FullWindTrueWeatherOperator.cpp delete mode 100644 Public/GeolibUtils.cpp delete mode 100644 Public/Guidance.cpp delete mode 100644 Public/HfpReader.cpp delete mode 100644 Public/HfpReader2020.cpp delete mode 100644 Public/HfpReaderPre2020.cpp delete mode 100644 Public/HorizontalPath.cpp delete mode 100644 Public/HorizontalPathTracker.cpp delete mode 100644 Public/HorizontalTurnPath.cpp delete mode 100644 Public/IMCommandObserver.cpp delete mode 100644 Public/InternalObserver.cpp delete mode 100644 Public/InvalidIndexException.cpp delete mode 100644 Public/KinematicDescent4DPredictor.cpp delete mode 100644 Public/KinematicTrajectoryPredictor.cpp delete mode 100644 Public/LatitudeLongitudePoint.cpp delete mode 100644 Public/LegacyPositionEstimator.cpp delete mode 100644 Public/LineOnEllipsoid.cpp delete mode 100644 Public/LocalTangentPlane.cpp delete mode 100644 Public/MaintainMetric.cpp delete mode 100644 Public/MergePointMetric.cpp delete mode 100644 Public/NMObserver.cpp delete mode 100644 Public/NMObserverEntry.cpp delete mode 100644 Public/NullSpeedLimiter.cpp delete mode 100644 Public/NullWindEvaluator.cpp delete mode 100644 Public/OutputHandler.cpp delete mode 100644 Public/PassThroughAssap.cpp delete mode 100644 Public/PilotDelay.cpp delete mode 100644 Public/PositionCalculator.cpp delete mode 100644 Public/PrecalcConstraint.cpp delete mode 100644 Public/PrecalcWaypoint.cpp delete mode 100644 Public/PredictionFileBase.cpp delete mode 100644 Public/RandomGenerator.cpp delete mode 100644 Public/RefReader.cpp delete mode 100644 Public/RunFile.cpp delete mode 100644 Public/Scenario.cpp delete mode 100644 Public/ScenarioUtils.cpp delete mode 100644 Public/SingleTangentPlaneSequence.cpp delete mode 100644 Public/SpeedOnPitchControl.cpp delete mode 100644 Public/SpeedOnThrustControl.cpp delete mode 100644 Public/StandardAtmosphere.cpp delete mode 100644 Public/StatisticalPilotDelay.cpp delete mode 100644 Public/StereographicProjection.cpp delete mode 100644 Public/TangentPlaneSequence.cpp delete mode 100644 Public/ThreeDOFDynamics.cpp delete mode 100644 Public/TvReader.cpp delete mode 100644 Public/USStandardAtmosphere1976.cpp delete mode 100644 Public/VectorDifferenceWindEvaluator.cpp delete mode 100644 Public/VerticalPath.cpp delete mode 100644 Public/VerticalPathObserver.cpp delete mode 100644 Public/VerticalPredictor.cpp delete mode 100644 Public/Waypoint.cpp delete mode 100644 Public/WaypointLoader.cpp delete mode 100644 Public/WaypointLoader.h delete mode 100644 Public/WeatherEstimate.cpp delete mode 100644 Public/WeatherPrediction.cpp delete mode 100644 Public/WeatherTruth.cpp delete mode 100644 Public/Wgs84PrecalcWaypoint.cpp delete mode 100644 Public/Wind.cpp delete mode 100644 Public/WindStack.cpp delete mode 100644 Public/WindZero.cpp delete mode 100644 Public/ZeroWindTrueWeatherOperator.cpp delete mode 100644 include/public/ADSBReceiver.h delete mode 100644 include/public/ADSBSVReport.h delete mode 100644 include/public/ADSBTransmitter.h delete mode 100644 include/public/ASSAP.h delete mode 100644 include/public/AbstractAscentController.h delete mode 100644 include/public/AbstractDescentController.h delete mode 100644 include/public/AbstractTrueWeatherOperator.h delete mode 100644 include/public/AchieveObserver.h delete mode 100644 include/public/Aircraft.h delete mode 100644 include/public/AircraftCalculations.h delete mode 100644 include/public/AircraftControl.h delete mode 100644 include/public/AircraftControllerFactory.h delete mode 100644 include/public/AircraftIntent.h delete mode 100644 include/public/AircraftIntentLoader.h delete mode 100644 include/public/AircraftSpeed.h delete mode 100644 include/public/AircraftState.h delete mode 100644 include/public/AlongPathDistanceCalculator.h delete mode 100644 include/public/ArcOnEllipsoid.h delete mode 100644 include/public/AscentController.h delete mode 100644 include/public/Atmosphere.h delete mode 100644 include/public/BadaUtils.h delete mode 100644 include/public/BlendWindsVerticallyByAltitude.h delete mode 100644 include/public/CalcWindGradControl.h delete mode 100644 include/public/ClimbPhaseVerticalController.h delete mode 100644 include/public/ClosestPointMetric.h delete mode 100644 include/public/ConfigurationFileReader.h delete mode 100644 include/public/ControlCommands.h delete mode 100644 include/public/CoreUtils.h delete mode 100644 include/public/CrossTrackObserver.h delete mode 100644 include/public/CustomMath.h delete mode 100644 include/public/DMatrix.h delete mode 100644 include/public/DVector.h delete mode 100644 include/public/DataReader.h delete mode 100644 include/public/DefaultLateralController.h delete mode 100644 include/public/DirectionOfFlightCourseCalculator.h delete mode 100644 include/public/DynamicsObserver.h delete mode 100644 include/public/DynamicsState.h delete mode 100644 include/public/EarthModel.h delete mode 100644 include/public/EllipsoidalEarthModel.h delete mode 100644 include/public/EllipsoidalPositionEstimator.h delete mode 100644 include/public/EnvReader.h delete mode 100644 include/public/Environment.h delete mode 100644 include/public/EquationsOfMotionState.h delete mode 100644 include/public/EquationsOfMotionStateDeriv.h delete mode 100644 include/public/EuclideanThreeDofDynamics.h delete mode 100644 include/public/EuclideanTightTurnResolver.h delete mode 100644 include/public/EuclideanTrajectoryPredictor.h delete mode 100644 include/public/EuclideanWaypointMonitor.h delete mode 100644 include/public/FixedMassAircraftPerformance.h delete mode 100644 include/public/FlightDeckApplication.h delete mode 100644 include/public/FlightDeckApplicationLibrary.h delete mode 100644 include/public/FlightDeckApplicationLoader.h delete mode 100644 include/public/FlightDeckDataWriter.h delete mode 100644 include/public/FlightEnvelopeSpeedLimiter.h delete mode 100644 include/public/FmsWaypointSequenceFile.h delete mode 100644 include/public/ForeWindReader.h delete mode 100644 include/public/FullWindTrueWeatherOperator.h delete mode 100644 include/public/GeolibUtils.h delete mode 100644 include/public/Guidance.h delete mode 100644 include/public/GuidanceCalculator.h delete mode 100644 include/public/HfpReader.h delete mode 100644 include/public/HfpReader2020.h delete mode 100644 include/public/HfpReaderPre2020.h delete mode 100644 include/public/HorizontalPath.h delete mode 100644 include/public/HorizontalPathTracker.h delete mode 100644 include/public/HorizontalTurnPath.h delete mode 100644 include/public/IMCommandObserver.h delete mode 100644 include/public/InternalObserver.h delete mode 100644 include/public/InvalidIndexException.h delete mode 100644 include/public/KinematicDescent4DPredictor.h delete mode 100644 include/public/KinematicTrajectoryPredictor.h delete mode 100644 include/public/KiteTightTurnResolver.h delete mode 100644 include/public/LateralController.h delete mode 100644 include/public/LatitudeLongitudePoint.h delete mode 100644 include/public/LawOfSinesResolver.h delete mode 100644 include/public/LegacyPositionEstimator.h delete mode 100644 include/public/LineOnEllipsoid.h delete mode 100644 include/public/LocalTangentPlane.h delete mode 100644 include/public/Log4cplusSetup.h delete mode 100644 include/public/MaintainMetric.h delete mode 100644 include/public/MergePointMetric.h delete mode 100644 include/public/NMObserver.h delete mode 100644 include/public/NMObserverEntry.h delete mode 100644 include/public/NullADSBReceiver.h delete mode 100644 include/public/NullAdsbTransmitter.h delete mode 100644 include/public/NullAtmosphere.h delete mode 100644 include/public/NullFlightDeckApplication.h delete mode 100644 include/public/NullPilotDelay.h delete mode 100644 include/public/NullPositionEstimator.h delete mode 100644 include/public/NullSpeedLimiter.h delete mode 100644 include/public/NullWindEvaluator.h delete mode 100644 include/public/OutputHandler.h delete mode 100644 include/public/PassThroughAssap.h delete mode 100644 include/public/PilotDelay.h delete mode 100644 include/public/PositionCalculator.h delete mode 100644 include/public/PrecalcConstraint.h delete mode 100644 include/public/PrecalcWaypoint.h delete mode 100644 include/public/PredictedWindEvaluator.h delete mode 100644 include/public/PredictionFileBase.h delete mode 100644 include/public/RandomGenerator.h delete mode 100644 include/public/RefReader.h delete mode 100644 include/public/RunFile.h delete mode 100644 include/public/Scenario.h delete mode 100644 include/public/ScenarioEntity.h delete mode 100644 include/public/ScenarioEventNotifier.h delete mode 100644 include/public/ScenarioUtils.h delete mode 100644 include/public/ShapeOnEllipsoid.h delete mode 100644 include/public/SimulationTime.h delete mode 100644 include/public/SingleTangentPlaneSequence.h delete mode 100644 include/public/SpeedBrakeController.h delete mode 100644 include/public/SpeedCommandLimiter.h delete mode 100644 include/public/SpeedOnPitchControl.h delete mode 100644 include/public/SpeedOnThrustControl.h delete mode 100644 include/public/StandardAtmosphere.h delete mode 100644 include/public/StatisticalPilotDelay.h delete mode 100644 include/public/StereographicProjection.h delete mode 100644 include/public/TakeOffVerticalController.h delete mode 100644 include/public/TangentPlaneSequence.h delete mode 100644 include/public/ThreeDOFDynamics.h delete mode 100644 include/public/Token.h delete mode 100644 include/public/TrueWeatherOperator.h delete mode 100644 include/public/TurnAnticipation.h delete mode 100644 include/public/TvReader.h delete mode 100644 include/public/USStandardAtmosphere1976.h delete mode 100644 include/public/VectorDifferenceWindEvaluator.h delete mode 100644 include/public/VerticalController.h delete mode 100644 include/public/VerticalPath.h delete mode 100644 include/public/VerticalPathObserver.h delete mode 100644 include/public/VerticalPathUtils.h delete mode 100644 include/public/VerticalPredictor.h delete mode 100644 include/public/WGS84EarthModelConstants.h delete mode 100644 include/public/Waypoint.h delete mode 100644 include/public/WaypointPassingMonitor.h delete mode 100644 include/public/WeatherEstimate.h delete mode 100644 include/public/WeatherPrediction.h delete mode 100644 include/public/WeatherTruth.h delete mode 100644 include/public/Wgs84PrecalcWaypoint.h delete mode 100644 include/public/Wind.h delete mode 100644 include/public/WindBlendingAlgorithm.h delete mode 100644 include/public/WindStack.h delete mode 100644 include/public/WindZero.h delete mode 100644 include/public/ZeroWindTrueWeatherOperator.h delete mode 100755 include/public/minicsv.h delete mode 100644 include/public/version.h delete mode 100644 include/utility/BoundedValue.h delete mode 100644 include/utility/CsvParser.h delete mode 100644 include/utility/CustomUnits.h delete mode 100644 include/utility/FilePath.h delete mode 100644 include/utility/Logging.h delete mode 100644 include/utility/ProcessingTimeStats.h delete mode 100644 include/utility/UtilityConstants.h delete mode 100644 include/utility/UtilityTemplates.h delete mode 100644 include/utility/constants.h delete mode 100644 include/utility/dev-notes.md delete mode 100644 include/utility/micros.h delete mode 100644 unittest/src/Public/earth_model_tests.cpp delete mode 100644 unittest/src/Public/geolib_tests.cpp delete mode 100644 unittest/src/Public/public.cmake delete mode 100644 unittest/src/Public/public_atmosphere_tests.cpp delete mode 100644 unittest/src/Public/public_tests.cpp delete mode 100644 unittest/src/Public/tangent_plane_tests.cpp delete mode 100644 unittest/src/Public/threedof_glider_tests.cpp delete mode 100644 unittest/src/Public/utility_tests.cpp delete mode 100644 unittest/src/Public/wind_blending_tests.cpp delete mode 100644 unittest/src/Public/windstack_tests.cpp delete mode 100644 unittest/src/utils/public/OldCustomMathUtils.cpp delete mode 100644 unittest/src/utils/public/OldCustomMathUtils.h delete mode 100644 unittest/src/utils/public/PublicUtils.cpp delete mode 100644 unittest/src/utils/public/PublicUtils.h diff --git a/AircraftDynamicsTestFramework/CMakeLists.txt b/AircraftDynamicsTestFramework/CMakeLists.txt index d844172..075f77d 100644 --- a/AircraftDynamicsTestFramework/CMakeLists.txt +++ b/AircraftDynamicsTestFramework/CMakeLists.txt @@ -46,15 +46,12 @@ include(sample_algorithm_library.cmake OPTIONAL) add_library(framework STATIC ${SOURCE_FILES} ${DATA_READER_FILES} ${DATA_LOADER_FILES} ${DATA_WRITER_FILES}) target_include_directories(framework PUBLIC - ${aaesim_INCLUDE_DIRS} - ${geolib_idealab_INCLUDE_DIRS}) + ${aaesim_INCLUDE_DIRS}) target_link_libraries(framework PUBLIC - mitre::fsloader ${BADA_LIBRARY} ${SAMPLE_ALGORITHM_LIBRARY} - log4cplus::log4cplus - pub) + mitre::oss::simcore) if (DEFINED BADA_LIBRARY) # Add a compile definition to the build target_compile_definitions(framework PUBLIC "MITRE_BADA3_LIBRARY") diff --git a/AircraftDynamicsTestFramework/framework.cmake b/AircraftDynamicsTestFramework/framework.cmake index d93fda2..52b60b2 100644 --- a/AircraftDynamicsTestFramework/framework.cmake +++ b/AircraftDynamicsTestFramework/framework.cmake @@ -22,11 +22,8 @@ if(NOT ${BUILD_LIBARIES_ONLY}) add_executable(FMACM ${FMACM_MAIN_SRC}) target_link_libraries(FMACM framework) target_include_directories(FMACM PUBLIC - $ - $ $ $ - $ ) set_target_properties(FMACM PROPERTIES RUNTIME_OUTPUT_DIRECTORY ${CMAKE_SOURCE_DIR}/bin diff --git a/CMakeLists.txt b/CMakeLists.txt index 4b99bb2..f6198b0 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -55,20 +55,16 @@ set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -Wall -Wno-unused-function - set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -Wall -Wno-unused-function -Wno-sign-compare -O3 -g3") CPMAddPackage( - NAME fsloader - GIT_REPOSITORY https://github.com/mitre/fsloader.git - GIT_TAG 1.1.0 + NAME aircraft_simulation_core + GITHUB_REPOSITORY mitre/aircraft_simulation_core + GIT_TAG feat/add-code # mitre/aircraft_simulation_core#1 OPTIONS - "FSLOADER_BUILD_EXAMPLE OFF" - "BUILD_SHARED_LIBS FALSE" + "SIMCORE_BUILD_TESTING OFF" ) -set (PUBLIC_DIR ${CMAKE_CURRENT_SOURCE_DIR}/Public) set (FRAMEWORK_DIR ${CMAKE_CURRENT_SOURCE_DIR}/AircraftDynamicsTestFramework) set (aaesim_INCLUDE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include ) set (UNITTEST_DIR ${CMAKE_CURRENT_SOURCE_DIR}/unittest) -add_subdirectory(${PUBLIC_DIR}) - include(${UNITTEST_DIR}/unittest.cmake OPTIONAL) include(${FRAMEWORK_DIR}/framework.cmake OPTIONAL) diff --git a/Public/ADSBSVReport.cpp b/Public/ADSBSVReport.cpp deleted file mode 100644 index f2c2045..0000000 --- a/Public/ADSBSVReport.cpp +++ /dev/null @@ -1,157 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include "public/ADSBSVReport.h" - -#include "public/CustomMath.h" - -namespace aaesim::open_source { -const ADSBSVReport ADSBSVReport::EMPTY_REPORT{}; - -bool ADSBSVReport::operator==(const ADSBSVReport &in) { - if (m_id != in.m_id || m_time != in.m_time || m_has_position != in.m_has_position || - m_has_velocity != in.m_has_velocity) { - return false; - } - - if (m_has_position) { - if (m_enu_x != in.m_enu_x || m_enu_y != in.m_enu_y || m_altitude_msl != in.m_altitude_msl || - m_nacp != in.m_nacp || m_nicp != in.m_nicp || - m_horizontal_position_quantum != in.m_horizontal_position_quantum || - m_vertical_position_quantum != in.m_vertical_position_quantum) { - return false; - } - } - - if (this->m_has_velocity) { - if (m_enu_xd != in.m_enu_xd || m_enu_yd != in.m_enu_yd || m_altitude_rate != in.m_altitude_rate || - m_nacv != in.m_nacv || m_nicv != in.m_nicv || - m_horizontal_velocity_quantum != in.m_horizontal_velocity_quantum || - m_vertical_velocity_quantum != in.m_vertical_velocity_quantum) { - return false; - } - } - - return true; -} - -ADSBSVReport::ADSBSVReport(const Builder &builder) { - m_id = builder.GetUniqueId(); - m_time = builder.GetTimestamp(); - m_nacp = builder.GetNACp(); - m_nacv = builder.GetNACv(); - m_nicp = builder.GetNICp(); - m_nicv = builder.GetNICv(); - m_horizontal_position_quantum = builder.GetHorizontalPositionQuantum(); - m_vertical_position_quantum = builder.GetVerticalPositionQuantum(); - m_horizontal_velocity_quantum = builder.GetHorizontalVelocityQuantum(); - m_vertical_velocity_quantum = builder.GetVerticalVelocityQuantum(); - if (builder.HasPosition()) { - m_has_position = true; - m_enu_x = quantize(builder.GetPositionEnuX(), m_horizontal_position_quantum); - m_enu_y = quantize(builder.GetPositionEnuY(), m_horizontal_position_quantum); - m_latitude = builder.GetLatitude(); - m_longitude = builder.GetLongitude(); - m_altitude_msl = quantize(builder.GetAltitudeMsl(), m_vertical_position_quantum); - } - if (builder.HasVelocity()) { - m_has_velocity = true; - m_enu_xd = quantize(builder.GetGroundSpeedEnuXd(), m_horizontal_velocity_quantum); - m_enu_yd = quantize(builder.GetGroundSpeedEnuYd(), m_horizontal_velocity_quantum); - m_altitude_rate = quantize(builder.GetAltitudeRate(), m_vertical_velocity_quantum); - } -} - -ADSBSVReport::Builder::Builder(int unique_acid, Units::Time timestamp) : id_(unique_acid), timestamp_(timestamp) {} - -ADSBSVReport ADSBSVReport::Builder::Build() { return ADSBSVReport{*this}; } - -ADSBSVReport::Builder *ADSBSVReport::Builder::NACp(int nacp) { - nacp_ = nacp; - return this; -} - -ADSBSVReport::Builder *ADSBSVReport::Builder::NACv(int nacv) { - nacv_ = nacv; - return this; -} - -ADSBSVReport::Builder *ADSBSVReport::Builder::NICp(int nicp) { - nicp_ = nicp; - return this; -} - -ADSBSVReport::Builder *ADSBSVReport::Builder::NICv(int nicv) { - nicv_ = nicv; - return this; -} - -ADSBSVReport::Builder *ADSBSVReport::Builder::Position(Units::FeetLength enu_x, Units::FeetLength enu_y) { - position_x_ = enu_x; - position_y_ = enu_y; - has_position_ = true; - return this; -} - -ADSBSVReport::Builder *ADSBSVReport::Builder::GeodeticPosition(Units::Angle latitude, Units::Angle longitude) { - latitude_ = latitude; - longitude_ = longitude; - has_position_ = true; - return this; -} - -ADSBSVReport::Builder *ADSBSVReport::Builder::AltitudeMsl(Units::FeetLength altitude_msl) { - altitude_msl_ = altitude_msl; - return this; -} - -ADSBSVReport::Builder *ADSBSVReport::Builder::GroundSpeed(Units::FeetPerSecondSpeed enu_xd, - Units::FeetPerSecondSpeed enu_yd) { - xd_ = enu_xd; - yd_ = enu_yd; - has_velocity_ = true; - return this; -} - -ADSBSVReport::Builder *ADSBSVReport::Builder::AltitudeRate(Units::FeetPerSecondSpeed altitude_rate) { - altitude_rate_ = altitude_rate; - return this; -} - -ADSBSVReport::Builder *ADSBSVReport::Builder::HorizontalPositionQuantum(Units::Length quantum) { - horizontal_position_quantum_ = quantum; - return this; -} - -ADSBSVReport::Builder *ADSBSVReport::Builder::VerticalPositionQuantum(Units::Length quantum) { - vertical_position_quantum_ = quantum; - return this; -} - -ADSBSVReport::Builder *ADSBSVReport::Builder::HorizontalVelocityQuantum(Units::Speed quantum) { - horizontal_velocity_quantum_ = quantum; - return this; -} - -ADSBSVReport::Builder *ADSBSVReport::Builder::VerticalVelocityQuantum(Units::Speed quantum) { - vertical_velocity_quantum_ = quantum; - return this; -} - -} // namespace aaesim::open_source diff --git a/Public/AchieveObserver.cpp b/Public/AchieveObserver.cpp deleted file mode 100644 index b148523..0000000 --- a/Public/AchieveObserver.cpp +++ /dev/null @@ -1,89 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// 2022 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include -#include "public/AchieveObserver.h" - -using namespace std; - -AchieveObserver::AchieveObserver() - : - m_iteration(-1), - m_id(-1) { - m_time = Units::SecondsTime(-99999.0); - m_targ_ttg_to_ach = Units::SecondsTime(-99999.0); - m_own_ttg_to_ach = Units::SecondsTime(-99999.0); - m_curr_dist = Units::MetersLength(-99999.0); - m_ref_dist = Units::MetersLength(-99999.0); -} - - -AchieveObserver::AchieveObserver(const int iter, - const int aircraft_id, - const double tm, - const double target_ttg_to_ach, - const double own_ttg_to_ach, - const double curr_distance, - const double reference_distance) - : - m_iteration(iter), - m_id(aircraft_id), - m_time(tm) { - m_targ_ttg_to_ach = Units::SecondsTime(target_ttg_to_ach); - m_own_ttg_to_ach = Units::SecondsTime(own_ttg_to_ach); - m_curr_dist = Units::MetersLength(curr_distance); - m_ref_dist = Units::MetersLength(reference_distance); -} - - -AchieveObserver::~AchieveObserver() { - // Destructor. -} - - -const std::string AchieveObserver::Hdr() { - // Creates output header for csv file output. - - const string str = "Iteration,AircrafId,Time(s),Targ_TTG_to_Ach(s),Own_TTG_to_Ach(s),CurrDistance(m),RefDistance(m)"; - - return str; -} - - -string AchieveObserver::ToString() { - // Creates string of object for csv file output. - - string str; - - char *txt = new char[301]; - - sprintf(txt, "%d,%d,%lf,%lf,%lf,%lf,%lf", - m_iteration, m_id, - Units::SecondsTime(m_time).value(), - Units::SecondsTime(m_targ_ttg_to_ach).value(), - Units::SecondsTime(m_own_ttg_to_ach).value(), - Units::MetersLength(m_curr_dist).value(), - Units::MetersLength(m_ref_dist).value()); - - str = txt; - - delete[] txt; - - return str; -} diff --git a/Public/Aircraft.cpp b/Public/Aircraft.cpp deleted file mode 100644 index 3f96182..0000000 --- a/Public/Aircraft.cpp +++ /dev/null @@ -1,28 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// 2022 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include "public/Aircraft.h" - -Aircraft::Aircraft() { - // Do nothing -} - -Aircraft::~Aircraft() { - // Do nothing -} diff --git a/Public/AircraftCalculations.cpp b/Public/AircraftCalculations.cpp deleted file mode 100644 index 1b763b2..0000000 --- a/Public/AircraftCalculations.cpp +++ /dev/null @@ -1,482 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include "public/AircraftCalculations.h" - -#include -#include -#include -#include - -#include "public/CoreUtils.h" -#include "public/PositionCalculator.h" - -using namespace std; -using namespace aaesim::open_source; - -log4cplus::Logger AircraftCalculations::logger = log4cplus::Logger::getInstance("AircraftCalculations"); - -bool AircraftCalculations::LegacyGetPositionFromPathLength(const Units::Length &distance_to_go, - const std::vector &horizontal_trajectory, - Units::Length &x_position, Units::Length &y_position, - Units::UnsignedAngle &course, int &traj_index) { - PositionCalculator position_calculator(horizontal_trajectory, TrajectoryIndexProgressionDirection::UNDEFINED); - bool is_valid = - position_calculator.CalculatePositionFromAlongPathDistance(distance_to_go, x_position, y_position, course); - - traj_index = static_cast(position_calculator.GetCurrentTrajectoryIndex()); - if (distance_to_go < Units::zero()) { - traj_index = 0; - } - - return is_valid; -} - -void AircraftCalculations::LegacyGetPathLengthFromPosition(const Units::Length x, const Units::Length y, - const vector &horizontal_trajectory, - Units::Length &distance_along_path, Units::Angle &course) { - vector::size_type ignored_trajectory_index; - const std::vector::size_type default_start_index = 0; - CalculateDistanceAlongPathFromPosition(x, y, horizontal_trajectory, default_start_index, distance_along_path, course, - ignored_trajectory_index); -} - -vector AircraftCalculations::ComputePathDistances( - const Units::Length x, const Units::Length y, const std::vector::size_type &starting_index, - const vector &hTraj) { - // returns distances and cross track errors for all points in a vector - // ordered ascending by distance. - - vector path_distances; - - for (auto i = starting_index; i < hTraj.size(); ++i) { - PathDistance pd; - - Units::MetersLength d = sqrt(Units::sqr(x - Units::MetersLength(hTraj[i].GetXPositionMeters())) + - Units::sqr(y - Units::MetersLength(hTraj[i].GetYPositionMeters()))); - pd.m_distance_to_path_node = d; - pd.m_horizontal_path_index = i; - - if (std::isnan(d.value())) { - string msg = "undefined distance (NaN) computed for path distance"; - LOG4CPLUS_FATAL(logger, msg); - } - - if (path_distances.empty()) { - path_distances.push_back(pd); - } else { - auto path_distance_iter = path_distances.begin(); - - while (path_distance_iter < path_distances.end()) { - if ((*path_distance_iter).m_distance_to_path_node > pd.m_distance_to_path_node) { - break; - } - - path_distance_iter++; - } - - path_distances.insert(path_distance_iter, pd); - } - } - - return path_distances; -} - -Units::NauticalMilesLength AircraftCalculations::PtToPtDist(Units::Length x0, Units::Length y0, Units::Length x1, - Units::Length y1) { - // Computes distance between points. - // - // x0:x component of first point in feet. - // y0:y component of first point in feet. - // x1:x component of last point in feet. - // y1:y component of last point in feet. - // - // returns distance between (x0,y0) to (x1,y1) in nmi. - - Units::NauticalMilesLength dist = sqrt(Units::sqr(x1 - x0) + Units::sqr(y1 - y0)); - - return dist; -} - -void AircraftCalculations::CrossTrackError(const Units::Length position_enu_x, const Units::Length position_enu_y, - int current_trajectory_index, - const vector &horizontal_trajectory, - int &next_trajectory_index, Units::Length &cross_track_error) { - // Calculates the cross track for a segment leading to a horizontal - // trajectory point. The calculation is performed differently for a - // turn segment and for a straight segment. - // - // Turn Segment: - // Position is tested to see if it is in the space formed by the two - // directed line segments that form the turn arc. - // First, determine the turn orientation: see if the points center of - // turn, start turn, and end turn are clockwise or counter-clockwise. - // If the orientation of the points center of turn, start turn, position - // do not have the same orientation, then return a large number for cte. - // If the orientation of the points center of turn, turn stop, position - // does not have the opposite orientation, return a large number for cte. - // If the conditions are met, then return the distance from the turn - // center minus the radius for cte. It is possible for two sequential - // turn segments to be slightly misaligned so that a position could be - // after one segment and before another segment. In this case, the - // position will be considered as on the next segment. - // - // Straight Segment: - // Straight segments could have a small turn, and still be considered - // straight. A perpendicular to the m_path_course segment through the - // position is calculated. If this perpendicular intersects the - // segment, its distance from the segment is returned. If the position - // is after the horizontal trajectory point, a large value for cte is - // returned. If the position is before the beginning of the segment, - // the distance from the position to the beginning of the segment - // (preceding horizontal trajectory point) is returned for cte and the - // horizontal trajectory point is returned for nextTrajIx. - // - // In either case, the preceding segment could be of the other type and - // a gap could exist. Must test if in the gap and not within the - // preceding segment. - - cross_track_error = Units::Infinity(); - next_trajectory_index = INT_MIN; - - if (current_trajectory_index >= horizontal_trajectory.size() - 1) { - // No segment going to first point on route (last trajectory point) - return; - } - - if (horizontal_trajectory[current_trajectory_index].m_segment_type == HorizontalPath::SegmentType::TURN) { - // Compute cross track error for turn. - const double x0 = horizontal_trajectory[current_trajectory_index].m_turn_info.x_position_meters; // turn center - const double y0 = horizontal_trajectory[current_trajectory_index].m_turn_info.y_position_meters; - const double x1 = horizontal_trajectory[current_trajectory_index + 1].GetXPositionMeters(); // start turn - const double y1 = horizontal_trajectory[current_trajectory_index + 1].GetYPositionMeters(); - const double x2 = horizontal_trajectory[current_trajectory_index].GetXPositionMeters(); // stop turn - const double y2 = horizontal_trajectory[current_trajectory_index].GetYPositionMeters(); - - const double dx = Units::MetersLength(position_enu_x).value(); - const double dy = Units::MetersLength(position_enu_y).value(); - - // determine orientation of turn. If crossproduct is zero, then points are colinear - // sign(P0, P1, p2) i.e., center, start, end. - double crossProdSign0 = (y0 - y1) * x2 + (x1 - x0) * y2 + (x0 * y1 - x1 * y0); - if (crossProdSign0 == 0) { // zero length turn - return; - } - - // determine orientation of turn center, start turn, position - const double crossProdSign1 = (y0 - y1) * dx + (x1 - x0) * dy + (x0 * y1 - x1 * y0); - if (((crossProdSign0 > 0) && (crossProdSign1 < 0)) || ((crossProdSign0 < 0) && (crossProdSign1 > 0))) { - // position is before start turn - // need to test if not in previous segment, i.e. in the V between two segments (see AAES-360) - if (horizontal_trajectory[current_trajectory_index + 1].m_segment_type == HorizontalPath::SegmentType::TURN) { - const double x3 = horizontal_trajectory[current_trajectory_index + 1].m_turn_info.x_position_meters; - const double y3 = horizontal_trajectory[current_trajectory_index + 1].m_turn_info.y_position_meters; - const double crossProdSign3 = (y3 - y1) * dx + (x1 - x3) * dy + (x3 * y1 - x1 * y3); - if (((crossProdSign3 > 0) && (crossProdSign1 < 0)) || - ((crossProdSign3 < 0) && (crossProdSign1 > 0))) { // not in previous section - so calculate error - next_trajectory_index = current_trajectory_index; - cross_track_error = - abs(Units::MetersLength(sqrt(pow(x0 - dx, 2) + pow(y0 - dy, 2))) - - Units::MetersLength(horizontal_trajectory[current_trajectory_index].m_turn_info.radius)); - } // otherwise return not in segment - } else { // previous segment is straight - // Use Pythagorean Theorem - if (current_trajectory_index >= horizontal_trajectory.size() - 2) { - return; - } - const double x3 = horizontal_trajectory[current_trajectory_index + 2].GetXPositionMeters(); - const double y3 = horizontal_trajectory[current_trajectory_index + 2].GetYPositionMeters(); - if (pow(x3 - dx, 2) + pow(y3 - dy, 2) > - (pow(x1 - dx, 2) + pow(y1 - dy, 2) + pow(x3 - x1, 2) + pow(y3 - y1, 2))) { - // not in preceding segment so calculate cte - cross_track_error = abs(Units::MetersLength(sqrt(pow(x1 - dx, 2) + pow(y1 - dy, 2)))); - next_trajectory_index = current_trajectory_index; - } - } - return; // position is before start turn - } - - const double crossProdSign2 = (y0 - y2) * dx + (x2 - x0) * dy + (x0 * y2 - x2 * y0); - if (((crossProdSign1 > 0) && (crossProdSign2 > 0)) || ((crossProdSign1 < 0) && (crossProdSign2 < 0))) { - if (current_trajectory_index == 0) { - Units::MetersLength endDistance = abs(Units::MetersLength(sqrt(pow(x2 - dx, 2) + pow(y2 - dy, 2)))); - if (endDistance < Units::MetersLength(500)) { - cross_track_error = endDistance; - next_trajectory_index = current_trajectory_index; - } - } - return; // position is after stop turn - } - - cross_track_error = abs(Units::MetersLength(sqrt(pow(x0 - dx, 2) + pow(y0 - dy, 2))) - - Units::MetersLength(horizontal_trajectory[current_trajectory_index].m_turn_info.radius)); - next_trajectory_index = current_trajectory_index; - return; - - } else if (horizontal_trajectory[current_trajectory_index].m_segment_type == HorizontalPath::SegmentType::STRAIGHT) { - const double x0 = horizontal_trajectory[current_trajectory_index + 1].GetXPositionMeters(); // start of segment - const double y0 = horizontal_trajectory[current_trajectory_index + 1].GetYPositionMeters(); - const double x1 = horizontal_trajectory[current_trajectory_index].GetXPositionMeters(); // end of segment - const double y1 = horizontal_trajectory[current_trajectory_index].GetYPositionMeters(); - const double dx = Units::MetersLength(position_enu_x).value(); // position - const double dy = Units::MetersLength(position_enu_y).value(); - - const double vx = x1 - x0; - const double vy = y1 - y0; - const double wx = dx - x0; - const double wy = dy - y0; - - // test for zero length leg - if (fabs(vx) < 0.00001 && fabs(vy) < 0.00001) { - // is the query point at the same spot? - if (fabs(wx) < 0.00001 && fabs(wy) < 0.00001) { - cross_track_error = Units::zero(); - next_trajectory_index = current_trajectory_index; - } - return; - } - - const double c1 = vx * wx + vy * wy; - if (c1 < 0) { // before start of segment - // See if in preceding segment - if (current_trajectory_index + 2 < horizontal_trajectory.size()) { - if (horizontal_trajectory[current_trajectory_index + 1].m_segment_type == - HorizontalPath::SegmentType::STRAIGHT) { - const double x2 = horizontal_trajectory[current_trajectory_index + 2].GetXPositionMeters(); - const double y2 = horizontal_trajectory[current_trajectory_index + 2].GetYPositionMeters(); - const double ux = x2 - x0; - const double uy = y2 - y0; - const double c3 = ux * wx + uy * wy; - if (c3 >= 0) { - // position is in the preceding segment - return; - } - } else { // previous segment is turn} - const double x3 = - horizontal_trajectory[current_trajectory_index].m_turn_info.x_position_meters; // turn center - const double y3 = horizontal_trajectory[current_trajectory_index].m_turn_info.y_position_meters; - const double x2 = - horizontal_trajectory[current_trajectory_index + 1].GetXPositionMeters(); // start turn - const double y2 = horizontal_trajectory[current_trajectory_index + 1].GetYPositionMeters(); - const double crossProdSign0 = (y3 - y0) * x2 + (x0 - x3) * y2 + (x3 * y0 - x0 * y3); - if (crossProdSign0 == 0) { // zero length turn - return; - } - const double crossProdSign1 = (y3 - y0) * dx + (x0 - x3) * dy + (x3 * y0 - x0 * y3); - if (((crossProdSign0 > 0) && (crossProdSign1 > 0)) || ((crossProdSign0 < 0) && (crossProdSign1 < 0))) { - return; // in previous section - } - } - } - // Not in preeceding segment so include with this segment - cross_track_error = abs(Units::MetersLength(sqrt(pow(x0 - dx, 2) + pow(y0 - dy, 2)))); - next_trajectory_index = current_trajectory_index; - return; - } - - const double c2 = vx * vx + vy * vy; - if (c2 < c1) { // after end of segment - // check that not at end of route (trajIx == 0) - if (current_trajectory_index > 0) { - return; - } - // Need to include positions after end of route or simulation will - // not terminate properly - cross_track_error = abs(Units::MetersLength(sqrt(pow(x1 - dx, 2) + pow(y1 - dy, 2)))); - next_trajectory_index = current_trajectory_index; - return; - } - - // position is between the end points of the segment - const double b = c1 / c2; - const double xb = x0 + b * vx; - const double yb = y0 + b * vy; - cross_track_error = abs(Units::MetersLength(sqrt(pow(xb - dx, 2) + pow(yb - dy, 2)))); - next_trajectory_index = current_trajectory_index; - return; - - } else { - // Error - throw logic_error("Invalid segment_type condition found in crossTrackError"); - } - -} // crossTrackError() - -Units::SignedRadiansAngle AircraftCalculations::ComputeAngleBetweenVectors( - const Units::Length &xvertex, const Units::Length &yvertex, const Units::Length &x1, const Units::Length &y1, - const Units::Length &x2, const Units::Length &y2) { - // vector 1 is from vertex to x1,y1 - Units::MetersLength dx1 = x1 - xvertex; - Units::MetersLength dy1 = y1 - yvertex; - double norm1 = sqrt(dx1.value() * dx1.value() + dy1.value() * dy1.value()); - - // vector 2 is from turn center to turn end - Units::MetersLength dx2 = x2 - xvertex; - Units::MetersLength dy2 = y2 - yvertex; - double norm2 = sqrt(dx2.value() * dx2.value() + dy2.value() * dy2.value()); - - // theta is acos(dot product) - double dotp = dx1.value() / norm1 * dx2.value() / norm2 + dy1.value() / norm1 * dy2.value() / norm2; - if (fabs(dotp) > 1) { - dotp = CoreUtils::SignOfValue(dotp) * 1.0; - } - Units::SignedRadiansAngle theta(acos(dotp)); // acos is on [0,pi] - return theta; -} - -Units::Area AircraftCalculations::ComputeCrossProduct(const Units::Length &xvertex, const Units::Length &yvertex, - const Units::Length &x1, const Units::Length &y1, - const Units::Length &x2, const Units::Length &y2) { - return (yvertex - y1) * x2 + (x1 - xvertex) * y2 + (xvertex * y1 - x1 * yvertex); -} - -bool AircraftCalculations::CalculateDistanceAlongPathFromPosition( - const Units::Length position_x, const Units::Length position_y, - const std::vector &horizontal_trajectory, - const std::vector::size_type starting_trajectory_index, Units::Length &distance_along_path, - Units::Angle &course, std::vector::size_type &resolved_trajectory_index) { - static const Units::NauticalMilesLength cte_tolerance(2.5); // legacy tolerance. do not change. - return CalculateDistanceAlongPathFromPosition(cte_tolerance, position_x, position_y, horizontal_trajectory, - starting_trajectory_index, distance_along_path, course, - resolved_trajectory_index); -} - -bool AircraftCalculations::CalculateDistanceAlongPathFromPosition( - const Units::Length cross_track_tolerance, const Units::Length position_x, const Units::Length position_y, - const std::vector &horizontal_trajectory, - const std::vector::size_type starting_trajectory_index, Units::Length &distance_along_path, - Units::Angle &course, std::vector::size_type &resolved_trajectory_index) { - LOG4CPLUS_TRACE(logger, - "Calculating distance from (" - << Units::MetersLength(position_x) << "," << Units::MetersLength(position_y) - << ") to hpath starting at (" - << Units::MetersLength(horizontal_trajectory[starting_trajectory_index].GetXPositionMeters()) - << "," - << Units::MetersLength(horizontal_trajectory[starting_trajectory_index].GetYPositionMeters()) - << "):" << starting_trajectory_index << "/" << horizontal_trajectory.size()); - - // Dummy values - distance_along_path = Units::NegInfinity(); - course = Units::RadiansAngle(Units::infinity()); - - // Compute Euclidean distances for all horizontal trajectory points - // and order in ascending sequence. - vector distances = AircraftCalculations::ComputePathDistances( - position_x, position_y, - starting_trajectory_index < 1 ? starting_trajectory_index : starting_trajectory_index - 1, - horizontal_trajectory); - - // Find smallest distance with an acceptable cross track error. - int nextTrajIx = -1; - Units::NauticalMilesLength cte; - for (auto i = 0; ((i < distances.size()) && (nextTrajIx == -1)); ++i) { - int computedNextIx; - AircraftCalculations::CrossTrackError(position_x, position_y, distances[i].m_horizontal_path_index, - horizontal_trajectory, computedNextIx, cte); - - if (cte <= cross_track_tolerance) { - nextTrajIx = computedNextIx; - } - } - - resolved_trajectory_index = static_cast(nextTrajIx); - - if (nextTrajIx == -1) { - /* - * Developers: note that this if logic is no longer a FATAL condition, but we - * must throw. Some callers will catch and handle the situation because - * it is sometimes normal. See AAES-382, AAES-633 - */ - char msg[500]; - snprintf( - msg, sizeof(msg), - "Trajectory point with acceptable cross track error not found %lf nmi\nThis can occur for two " - "reasons:\n\t1. Position cannot be projected onto a route segment (before beginning or after end),\n\t2. " - "Position is farther than horizontal tolerance from horizontal trajectory.\nThe second can occur for " - "clearance type of CAPTURE or MAINTAIN (see Issue AAES-1037)", - cte.value()); - throw logic_error(msg); - } - - // Calculate DTG and course of aircraft. - Units::Length dap; - Units::RadiansAngle theta; - if (horizontal_trajectory[nextTrajIx].m_segment_type == HorizontalPath::SegmentType::STRAIGHT) { - Units::Length d = - sqrt(Units::sqr(position_x - Units::MetersLength(horizontal_trajectory[nextTrajIx].GetXPositionMeters())) + - Units::sqr(position_y - Units::MetersLength(horizontal_trajectory[nextTrajIx].GetYPositionMeters()))); - - course = Units::ToUnsigned(Units::RadiansAngle(horizontal_trajectory[nextTrajIx].m_path_course) + - Units::PI_RADIANS_ANGLE); - - if (fabs(Units::MetersLength(d).value()) < 1e-5) { - dap = Units::MetersLength(0.0); - } else { - Units::MetersLength dx = - Units::MetersLength(horizontal_trajectory[nextTrajIx].GetXPositionMeters()) - position_x; - Units::MetersLength dy = - Units::MetersLength(horizontal_trajectory[nextTrajIx].GetYPositionMeters()) - position_y; - theta = Units::RadiansAngle(atan2(dy.value(), dx.value())); - - Units::SignedAngle deltaTheta = Units::ToSigned(theta - course); - - dap = d * cos(deltaTheta); - } - - } else if (horizontal_trajectory[nextTrajIx].m_segment_type == HorizontalPath::SegmentType::TURN) { - Units::MetersLength dx = - position_x - Units::MetersLength(horizontal_trajectory[nextTrajIx].m_turn_info.x_position_meters); - Units::MetersLength dy = - position_y - Units::MetersLength(horizontal_trajectory[nextTrajIx].m_turn_info.y_position_meters); - - // theta is undefined if dx and dy are both zero - // if within 5 meters of the point, consider at the point - double lx = (Units::MetersLength(position_x)).value(); - double ly = (Units::MetersLength(position_y)).value(); - - if (pow(lx - (horizontal_trajectory[nextTrajIx]).GetXPositionMeters(), 2) + - pow(ly - (horizontal_trajectory[nextTrajIx]).GetYPositionMeters(), 2) < - 9) { - distance_along_path = Units::MetersLength(horizontal_trajectory[nextTrajIx].m_path_length_cumulative_meters); - course = Units::ToUnsigned(Units::RadiansAngle(horizontal_trajectory[nextTrajIx].m_path_course) + - Units::PI_RADIANS_ANGLE); - return true; - } - - theta = Units::UnsignedRadiansAngle(atan2(dy.value(), dx.value())); - - Units::RadiansAngle deltaTheta = - Units::ToSigned(Units::UnsignedRadiansAngle(horizontal_trajectory[nextTrajIx].m_turn_info.q_start) - theta); - - dap = Units::MetersLength(horizontal_trajectory[nextTrajIx].m_turn_info.radius) * fabs(deltaTheta.value()); - - if (CoreUtils::SignOfValue(deltaTheta.value()) > 0) { - course = theta + Units::PI_RADIANS_ANGLE / 2; - } else { - course = theta - Units::PI_RADIANS_ANGLE / 2; - } - - course = Units::ToUnsigned(course); - } else { - throw logic_error("Non straight non turn segment_type in CalculateDistanceAlongPathFromPosition"); - } - - distance_along_path = dap + Units::MetersLength(horizontal_trajectory[nextTrajIx].m_path_length_cumulative_meters); - return true; -} diff --git a/Public/AircraftControl.cpp b/Public/AircraftControl.cpp deleted file mode 100644 index f6ebf1b..0000000 --- a/Public/AircraftControl.cpp +++ /dev/null @@ -1,70 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include "public/AircraftControl.h" - -#include -#include -#include - -#include "nlohmann/json.hpp" - -using namespace aaesim::open_source; - -AircraftControl::AircraftControl( - const std::map, - std::shared_ptr>> &controller_pairs) - : controller_map_(controller_pairs) {} - -void AircraftControl::Initialize( - std::shared_ptr aircraft_performance) { - for (auto &entry : controller_map_) { - const auto &vertical = entry.second.second; - if (vertical) { - vertical->Initialize(aircraft_performance); - } - } -} - -std::pair AircraftControl::CalculateControlCommands( - const Guidance &guidance, const EquationsOfMotionState &equations_of_motion_state, - std::shared_ptr sensed_weather) { - const auto &pair = controller_map_.at(guidance.m_active_guidance_phase); - const auto &lateral_controller = pair.first; - const auto &vertical_controller = pair.second; - - Units::Angle phi_command = - lateral_controller->ComputeRollCommand(guidance, equations_of_motion_state, sensed_weather); - - aaesim::open_source::bada_utils::FlapConfiguration flap_configuration{ - aaesim::open_source::bada_utils::FlapConfiguration::UNDEFINED}; - BoundedValue speed_brake_command{0}; - Units::Force thrust_command{Units::zero()}; - Units::Angle gamma_command{Units::zero()}; - Units::Speed true_airspeed_command{Units::zero()}; - vertical_controller->ComputeVerticalCommands(guidance, equations_of_motion_state, sensed_weather, thrust_command, - gamma_command, true_airspeed_command, speed_brake_command, - flap_configuration); - - return std::make_pair(ControlCommands{phi_command, thrust_command, gamma_command, true_airspeed_command, - speed_brake_command, flap_configuration}, - ControlGains{vertical_controller->GetGammaGain(), vertical_controller->GetThrustGain(), - lateral_controller->GetRollGain(), vertical_controller->GetSpeedBrakeGain()}); -} diff --git a/Public/AircraftIntent.cpp b/Public/AircraftIntent.cpp deleted file mode 100644 index 10d717a..0000000 --- a/Public/AircraftIntent.cpp +++ /dev/null @@ -1,532 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include "public/AircraftIntent.h" - -#include -#include -#include -#include -#include -#include -#include - -#include "public/CoreUtils.h" -#include "public/InvalidIndexException.h" -#include "public/SingleTangentPlaneSequence.h" - -AircraftIntent::AircraftIntent() { Initialize(); } - -AircraftIntent::AircraftIntent(const AircraftIntent &in) { - Initialize(); - Copy(in); -} - -void AircraftIntent::Initialize() { - m_all_waypoints.clear(); - DeleteRouteDataContent(); - m_planned_cruise_altitude = Units::ZERO_LENGTH; -} - -AircraftIntent &AircraftIntent::operator=(const AircraftIntent &in) { - Copy(in); - return *this; -} - -void AircraftIntent::DeleteRouteDataContent() { - route_data_.m_name.clear(); - route_data_.m_x.clear(); - route_data_.m_y.clear(); - route_data_.m_z.clear(); - route_data_.m_nominal_altitude.clear(); - route_data_.m_latitude.clear(); - route_data_.m_longitude.clear(); - route_data_.m_nominal_ias.clear(); - route_data_.m_waypoint_phase_of_flight.clear(); - route_data_.m_leg_type.clear(); - route_data_.m_high_altitude_constraint.clear(); - route_data_.m_low_altitude_constraint.clear(); - route_data_.m_high_speed_constraint.clear(); - route_data_.m_low_speed_constraint.clear(); - route_data_.m_rf_latitude.clear(); - route_data_.m_rf_longitude.clear(); - route_data_.m_rf_radius.clear(); - route_data_.m_rf_latitude.clear(); - route_data_.m_rf_longitude.clear(); - route_data_.m_y_rf_center.clear(); - route_data_.m_x_rf_center.clear(); -} - -void AircraftIntent::ClearAndResetRouteDataContent(const std::vector &ascent_waypoints, - const std::vector &cruise_waypoints, - const std::vector &descent_waypoints) { - m_ascent_waypoints = ascent_waypoints; - m_cruise_waypoints = cruise_waypoints; - m_descent_waypoints = descent_waypoints; - m_all_waypoints.clear(); - - DeleteRouteDataContent(); - AddWaypointsToRouteDataVectors(m_ascent_waypoints, ASCENT); - AddWaypointsToRouteDataVectors(m_cruise_waypoints, CRUISE); - - auto vector_current_index = m_all_waypoints.size(); - auto waypoint_itr = m_descent_waypoints.begin(); - while (waypoint_itr != m_descent_waypoints.end()) { - m_all_waypoints.push_back(*waypoint_itr); - route_data_.m_name.push_back(waypoint_itr->GetName()); - route_data_.m_waypoint_phase_of_flight.push_back(DESCENT); - route_data_.m_nominal_altitude.emplace_back(waypoint_itr->GetAltitude()); - route_data_.m_latitude.emplace_back(waypoint_itr->GetLatitude()); - route_data_.m_longitude.emplace_back(waypoint_itr->GetLongitude()); - route_data_.m_nominal_ias.emplace_back(waypoint_itr->GetNominalIas()); - route_data_.m_leg_type.push_back(m_arinc424_dictionary[waypoint_itr->GetArinc424LegType()]); - route_data_.m_high_altitude_constraint.emplace_back(waypoint_itr->GetAltitudeConstraintHigh()); - route_data_.m_low_altitude_constraint.emplace_back(waypoint_itr->GetAltitudeConstraintLow()); - route_data_.m_high_speed_constraint.emplace_back(waypoint_itr->GetSpeedConstraintHigh()); - route_data_.m_low_speed_constraint.emplace_back(waypoint_itr->GetSpeedConstraintLow()); - route_data_.m_rf_latitude.emplace_back(waypoint_itr->GetRfTurnCenterLatitude()); - route_data_.m_rf_longitude.emplace_back(waypoint_itr->GetRfTurnCenterLongitude()); - route_data_.m_rf_radius.emplace_back(waypoint_itr->GetRfTurnArcRadius()); - - const Units::Speed nominal_ias = waypoint_itr->GetNominalIas(); - - if (vector_current_index == m_all_waypoints.size()) { - if (planned_cruise_mach_ != 0) { - // cruise mach specified; use it - route_data_.m_nominal_ias[vector_current_index] = nominal_ias; - } else { - route_data_.m_nominal_ias[vector_current_index] = nominal_ias; - } - } else { - if (nominal_ias == Units::zero()) { - // use previous mach because ias is unspecified - route_data_.m_nominal_ias[vector_current_index] = Units::ZERO_SPEED; - } else if (nominal_ias != Units::zero()) { - // use ias because it is specified - route_data_.m_nominal_ias[vector_current_index] = nominal_ias; - } else if (route_data_.m_nominal_ias[vector_current_index - 1].value() != 0 && nominal_ias == Units::zero()) { - // use previous ias - route_data_.m_nominal_ias[vector_current_index] = route_data_.m_nominal_ias[vector_current_index - 1]; - } else { - route_data_.m_nominal_ias[vector_current_index] = nominal_ias; - } - } - - ++waypoint_itr; - ++vector_current_index; - } -} - -void AircraftIntent::LoadWaypointsFromList(const std::list &ascent_waypoints, - const std::list &cruise_waypoints, - const std::list &descent_waypoints) { - const bool has_ascent = !ascent_waypoints.empty(); - const bool has_cruise = !cruise_waypoints.empty(); - const bool has_descent = !descent_waypoints.empty(); - std::list cruise_waypoints_with_connection_added, descent_waypoints_with_connection_added; - if (has_ascent) { - if (has_cruise) { - cruise_waypoints_with_connection_added = AddConnectingLeg(ascent_waypoints, cruise_waypoints); - if (has_descent) - descent_waypoints_with_connection_added = - AddConnectingLeg(cruise_waypoints_with_connection_added, descent_waypoints); - } else if (has_descent) { - descent_waypoints_with_connection_added = AddConnectingLeg(ascent_waypoints, descent_waypoints); - } - } else if (has_cruise) { - cruise_waypoints_with_connection_added = cruise_waypoints; - if (has_descent) - descent_waypoints_with_connection_added = - AddConnectingLeg(cruise_waypoints_with_connection_added, descent_waypoints); - } else { - descent_waypoints_with_connection_added = descent_waypoints; - } - - std::list ascent_waypoints_shortened_legs, cruise_waypoints_shortened_legs, - descent_waypoints_shortened_legs; - if (!has_ascent) { - ascent_waypoints_shortened_legs = ascent_waypoints; - } else { - ascent_waypoints_shortened_legs = CoreUtils::ShortenLongLegs(ascent_waypoints); - } - if (cruise_waypoints_with_connection_added.empty()) { - cruise_waypoints_shortened_legs = cruise_waypoints_with_connection_added; - } else { - cruise_waypoints_shortened_legs = CoreUtils::ShortenLongLegs(cruise_waypoints_with_connection_added); - } - if (descent_waypoints_with_connection_added.empty()) { - descent_waypoints_shortened_legs = descent_waypoints_with_connection_added; - } else { - descent_waypoints_shortened_legs = CoreUtils::ShortenLongLegs(descent_waypoints_with_connection_added); - } - - ClearAndResetRouteDataContent(AircraftIntent::ConvertListToVector(ascent_waypoints_shortened_legs), - AircraftIntent::ConvertListToVector(cruise_waypoints_shortened_legs), - AircraftIntent::ConvertListToVector(descent_waypoints_shortened_legs)); - - const auto all_waypoints_as_list = - AircraftIntent::RemoveZeroLengthLegs(AircraftIntent::ConvertVectorToList(m_all_waypoints)); - m_tangent_plane_sequence = - std::shared_ptr(new SingleTangentPlaneSequence(all_waypoints_as_list)); - UpdateXYZFromLatLonWgs84(); - DoRouteDataLogging(); -} - -void AircraftIntent::UpdateXYZFromLatLonWgs84() { - EarthModel::GeodeticPosition geoPosition; - EarthModel::LocalPositionEnu xyPosition; - - route_data_.m_x.clear(); - route_data_.m_y.clear(); - route_data_.m_z.clear(); - route_data_.m_x_rf_center.clear(); - route_data_.m_y_rf_center.clear(); - for (int var = 0; var < route_data_.m_latitude.size(); ++var) { - geoPosition.altitude = Units::ZERO_LENGTH; - geoPosition.latitude = Units::RadiansAngle(route_data_.m_latitude[var]); - geoPosition.longitude = Units::RadiansAngle(route_data_.m_longitude[var]); - m_tangent_plane_sequence->ConvertGeodeticToLocal(geoPosition, xyPosition); - route_data_.m_x.emplace_back(xyPosition.x); - route_data_.m_y.emplace_back(xyPosition.y); - route_data_.m_z.emplace_back(xyPosition.z); - - if (route_data_.m_rf_radius[var] == Units::zero()) { - route_data_.m_x_rf_center.emplace_back(Units::zero()); - route_data_.m_y_rf_center.emplace_back(Units::zero()); - } else { - geoPosition.altitude = Units::FeetLength(0); - geoPosition.latitude = Units::RadiansAngle(route_data_.m_rf_latitude[var]); - geoPosition.longitude = Units::RadiansAngle(route_data_.m_rf_longitude[var]); - m_tangent_plane_sequence->ConvertGeodeticToLocal(geoPosition, xyPosition); - route_data_.m_x_rf_center.emplace_back(xyPosition.x); - route_data_.m_y_rf_center.emplace_back(xyPosition.y); - } - } -} - -int AircraftIntent::GetWaypointIndexByName(const std::string &waypoint_name) const { - int ix = -1; - bool found_waypoint = false; - for (const Waypoint &wp : m_all_waypoints) { - ++ix; - if (wp.GetName().compare(waypoint_name) == 0) { - found_waypoint = true; - break; - } - } - - if (!found_waypoint) ix = -1; - - return ix; -} - -void AircraftIntent::Dump(std::ostream &fileOut) const { - fileOut << "------------" << std::endl; - fileOut << "Intent of aircraft " << m_id << ":" << std::endl; - - for (unsigned int i = 0; i < m_all_waypoints.size(); i++) { - Waypoint wp = GetWaypoint(i); - fileOut << "-----" << std::endl; - fileOut << "Waypoint " << i << ":" << std::endl; - fileOut << "waypoint_name[i] " << GetWaypointName(i) << std::endl; - fileOut << "nominal_IAS_at_waypoint[i] " << Units::FeetPerSecondSpeed(wp.GetNominalIas()).value() << std::endl; - fileOut << "waypoint_Alt[i] " << Units::FeetLength(wp.GetAltitude()).value() << std::endl; - fileOut << "waypoint_x[i] " << route_data_.m_x[i].value() << std::endl; - fileOut << "waypoint_y[i] " << route_data_.m_y[i].value() << std::endl; - } -} - -void AircraftIntent::Copy(const AircraftIntent &in) { - DeleteRouteDataContent(); - m_id = in.m_id; - m_planned_cruise_altitude = in.m_planned_cruise_altitude; - planned_cruise_mach_ = in.planned_cruise_mach_; - m_is_loaded = in.m_is_loaded; - route_data_ = in.route_data_; - m_tangent_plane_sequence = in.m_tangent_plane_sequence; - m_all_waypoints = in.m_all_waypoints; - m_descent_waypoints = in.m_descent_waypoints; - m_cruise_waypoints = in.m_cruise_waypoints; - m_ascent_waypoints = in.m_ascent_waypoints; -} - -void AircraftIntent::DumpParms(const std::string &str) const { - LOG4CPLUS_TRACE(AircraftIntent::m_logger, str); - DoRouteDataLogging(); -} - -void AircraftIntent::GetLatLonFromXYZ(const Units::Length &xMeters, const Units::Length &yMeters, - const Units::Length &zMeters, Units::Angle &lat, Units::Angle &lon) const { - // use the ellipsoidal model - EarthModel::LocalPositionEnu localPos; - localPos.x = xMeters; - localPos.y = yMeters; - localPos.z = zMeters; - - EarthModel::GeodeticPosition geo; - m_tangent_plane_sequence->ConvertLocalToGeodetic(localPos, geo); - lat = geo.latitude; - lon = geo.longitude; -} - -std::pair AircraftIntent::FindCommonWaypoint(const AircraftIntent &intent) const { - /* - * Find the earliest common waypoint (closest to the start of own intent). - * - * Returns -1 if not found. - */ - - int ix = GetNumberOfWaypoints() - 1; - int tx = intent.GetNumberOfWaypoints() - 1; - int thisIndex = -1; - int thatIndex = -1; - - while ((ix >= 0) && (tx >= 0)) { - if (GetWaypointName(ix) == intent.GetWaypointName(tx)) { - thisIndex = ix; - thatIndex = tx; - ix--; - tx--; - } else { - break; - } - } - - return std::make_pair(thisIndex, thatIndex); -} - -void AircraftIntent::InsertPairAtIndex(const std::string &wpname, const Units::Length &x, const Units::Length &y, - const int index) { - /* - * The incoming point must have been validated by the caller. - * - * The point will be converted to a waypoint and inserted into the waypoint list that this object - * was initialized with. Then the object will be re-initialized. - */ - Units::Angle lat, lon; - GetLatLonFromXYZ(x, y, Units::ZERO_LENGTH, lat, lon); - - Waypoint wp; - wp.SetRfTurnArcRadius(Units::ZERO_LENGTH); - wp.SetWaypointLatLon(lat, lon); - wp.SetName(wpname); - - // copy constraints -- high from previous waypoint and low from next - auto wp2 = std::next(m_all_waypoints.begin(), index - 1); - wp.SetAltitudeConstraintHigh(wp2->GetAltitudeConstraintHigh()); - wp.SetSpeedConstraintHigh(wp2->GetSpeedConstraintHigh()); - ++wp2; - wp.SetAltitudeConstraintLow(wp2->GetAltitudeConstraintLow()); - wp.SetSpeedConstraintLow(wp2->GetSpeedConstraintLow()); - - InsertWaypointAtIndex(wp, index); -} - -void AircraftIntent::InsertWaypointAtIndex(const Waypoint &wp, int index) { - if (index < m_ascent_waypoints.size()) { - std::vector new_vector(m_ascent_waypoints); - auto itr = std::next(new_vector.begin(), index); - new_vector.insert(itr, wp); - ClearAndResetRouteDataContent(new_vector, m_cruise_waypoints, m_descent_waypoints); - } else if (index < m_ascent_waypoints.size() + m_cruise_waypoints.size()) { - std::vector new_vector(m_cruise_waypoints); - auto itr = std::next(new_vector.begin(), index - m_ascent_waypoints.size()); - new_vector.insert(itr, wp); - ClearAndResetRouteDataContent(m_ascent_waypoints, new_vector, m_descent_waypoints); - } else { - std::vector new_vector(m_descent_waypoints); - auto itr = std::next(new_vector.begin(), index - m_ascent_waypoints.size() - m_cruise_waypoints.size()); - new_vector.insert(itr, wp); - ClearAndResetRouteDataContent(m_ascent_waypoints, m_cruise_waypoints, new_vector); - } - - UpdateXYZFromLatLonWgs84(); -} - -void AircraftIntent::UpdateWaypoint(const Waypoint &waypoint) { - int update_count(0); - std::vector new_vector(m_all_waypoints); - for (auto it = new_vector.begin(); it != new_vector.end(); ++it) { - if (it->GetName() == waypoint.GetName()) { - (*it) = waypoint; - update_count++; - } - } - if (update_count != 1) { - LOG4CPLUS_FATAL(m_logger, update_count << " waypoints updated."); - throw std::runtime_error("Wrong number of waypoints matched."); - } - - const std::vector empty_vector; - ClearAndResetRouteDataContent(empty_vector, empty_vector, new_vector); - UpdateXYZFromLatLonWgs84(); -} - -void AircraftIntent::ClearWaypoints() { - m_all_waypoints.clear(); - - const std::vector empty_vector; - ClearAndResetRouteDataContent(empty_vector, empty_vector, m_all_waypoints); - UpdateXYZFromLatLonWgs84(); -} - -const Waypoint &AircraftIntent::GetWaypoint(unsigned int i) const { - if (i >= m_all_waypoints.size()) { - LOG4CPLUS_FATAL(m_logger, "Index " << i << " is out of range for size " << m_all_waypoints.size()); - throw InvalidIndexException(i, 0, m_all_waypoints.size() - 1); - } - return m_all_waypoints[i]; -} - -void AircraftIntent::SetNumberOfWaypoints(unsigned int n) { - if (n < m_all_waypoints.size()) { - std::vector::iterator it1 = std::next(m_all_waypoints.begin(), n); // get an iterator pointing to index - std::vector::iterator it2 = m_all_waypoints.end(); - m_all_waypoints.erase(it1, it2); - } -} - -bool AircraftIntent::operator==(const AircraftIntent &obj) const { - if (m_id == obj.m_id && m_planned_cruise_altitude == obj.m_planned_cruise_altitude && - planned_cruise_mach_ == obj.planned_cruise_mach_ && m_is_loaded == obj.m_is_loaded && - route_data_.m_name == obj.route_data_.m_name && route_data_.m_x == obj.route_data_.m_x && - route_data_.m_y == obj.route_data_.m_y && route_data_.m_z == obj.route_data_.m_z && - route_data_.m_latitude == obj.route_data_.m_latitude && route_data_.m_longitude == obj.route_data_.m_longitude && - route_data_.m_nominal_altitude == obj.route_data_.m_nominal_altitude && - route_data_.m_nominal_ias == obj.route_data_.m_nominal_ias && - route_data_.m_high_altitude_constraint == obj.route_data_.m_high_altitude_constraint && - route_data_.m_low_altitude_constraint == obj.route_data_.m_low_altitude_constraint && - route_data_.m_high_speed_constraint == obj.route_data_.m_high_speed_constraint && - route_data_.m_low_speed_constraint == obj.route_data_.m_low_speed_constraint && - route_data_.m_rf_latitude == obj.route_data_.m_rf_latitude && - route_data_.m_rf_longitude == obj.route_data_.m_rf_longitude && - route_data_.m_x_rf_center == obj.route_data_.m_x_rf_center && - route_data_.m_y_rf_center == obj.route_data_.m_y_rf_center && - route_data_.m_rf_radius == obj.route_data_.m_rf_radius) { - return true; - } - return false; -} - -std::ostream &operator<<(std::ostream &out, const AircraftIntent &intent) { - out << "aircraft_intent {" << std::endl; - out << "number_of_waypoints " << intent.GetNumberOfWaypoints() << std::endl; - out << "plannedCruiseMach 0" << std::endl; - out << "plannedCruiseAltitude " << Units::FeetLength(intent.GetPlannedCruiseAltitude()).value() << std::endl; - out << "waypoints {" << std::endl; - for (const Waypoint &wp : intent.m_all_waypoints) { - out << wp; - } - out << " }" << std::endl; - out << "}" << std::endl; - return out; -} - -void AircraftIntent::SetPlannedCruiseAltitude(Units::Length altitude) { this->m_planned_cruise_altitude = altitude; } - -void AircraftIntent::SetPlannedCruiseMach(BoundedValue mach_number) { - planned_cruise_mach_ = (double)mach_number; -} - -const std::string &AircraftIntent::GetWaypointName(unsigned int i) const { return GetWaypoint(i).GetName(); } - -Units::MetersLength AircraftIntent::GetWaypointX(unsigned int i) const { - if (i >= route_data_.m_x.size()) { - LOG4CPLUS_FATAL(m_logger, "Index " << i << " is out of range for size " << route_data_.m_x.size()); - throw InvalidIndexException(i, 0, route_data_.m_x.size() - 1); - } - return route_data_.m_x[i]; -} - -Units::MetersLength AircraftIntent::GetWaypointY(unsigned int i) const { - if (i >= route_data_.m_y.size()) { - LOG4CPLUS_FATAL(m_logger, "Index " << i << " is out of range for size " << route_data_.m_y.size()); - throw InvalidIndexException(i, 0, route_data_.m_y.size() - 1); - } - return route_data_.m_y[i]; -} - -std::list AircraftIntent::RemoveZeroLengthLegs(const std::list &waypoints) { - std::list resolved_waypoints; - if (waypoints.empty()) { - return resolved_waypoints; - } - auto wpt_itr = waypoints.begin(); - auto next_itr = std::next(wpt_itr); - for (; next_itr != waypoints.end(); ++wpt_itr, ++next_itr) { - const auto lat1 = wpt_itr->GetLatitude(); - const auto lon1 = wpt_itr->GetLongitude(); - const auto lat2 = next_itr->GetLatitude(); - const auto lon2 = next_itr->GetLongitude(); - const auto lat_diff = Units::abs(lat1 - lat2); - const auto lon_diff = Units::abs(lon1 - lon2); - const auto tolerance = Units::DegreesAngle(1e-5); - const bool skip_waypoint = lat_diff < tolerance && lon_diff < tolerance; - if (!skip_waypoint) { - resolved_waypoints.push_back(*wpt_itr); - } - } - // always include the last waypoint - resolved_waypoints.push_back(waypoints.back()); - return resolved_waypoints; -} - -AircraftIntent AircraftIntent::CopyAndTrimAfterNamedWaypoint(const AircraftIntent &aircraft_intent, - const std::string &waypoint_name) { - AircraftIntent aircraft_intent_copy(aircraft_intent); - const std::string final_waypoint_name = - aircraft_intent.GetWaypoint(aircraft_intent.GetNumberOfWaypoints() - 1).GetName(); - if (aircraft_intent.ContainsWaypointName(waypoint_name) && final_waypoint_name != waypoint_name) { - auto name_comparator = [&waypoint_name](const Waypoint &waypoint_to_test) { - return waypoint_to_test.GetName() == waypoint_name; - }; - auto trim_vector = [&name_comparator](const std::vector &waypoints) { - std::vector trimmed_vector; - for (const Waypoint &wp : waypoints) { - trimmed_vector.push_back(wp); - if (name_comparator(wp)) { - break; - } - } - return trimmed_vector; - }; - std::vector ascent_waypoints = aircraft_intent.GetAscentWaypoints(); - std::vector cruise_waypoints = aircraft_intent.GetCruiseWaypoints(); - std::vector descent_waypoints = aircraft_intent.GetDescentWaypoints(); - if (std::any_of(ascent_waypoints.rbegin(), ascent_waypoints.rend(), name_comparator)) { - cruise_waypoints.clear(); - descent_waypoints.clear(); - ascent_waypoints = trim_vector(ascent_waypoints); - } else if (std::any_of(cruise_waypoints.rbegin(), cruise_waypoints.rend(), name_comparator)) { - cruise_waypoints = trim_vector(cruise_waypoints); - descent_waypoints.clear(); - } else { - descent_waypoints = trim_vector(descent_waypoints); - } - auto to_list = [](std::vector waypoints) { - std::list dest; - std::copy(waypoints.begin(), waypoints.end(), std::back_inserter(dest)); - return dest; - }; - aircraft_intent_copy.ClearWaypoints(); - aircraft_intent_copy.LoadWaypointsFromList(to_list(ascent_waypoints), to_list(cruise_waypoints), - to_list(descent_waypoints)); - } - return aircraft_intent_copy; -} diff --git a/Public/AircraftIntentLoader.cpp b/Public/AircraftIntentLoader.cpp deleted file mode 100644 index 95e2a7b..0000000 --- a/Public/AircraftIntentLoader.cpp +++ /dev/null @@ -1,77 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include "public/AircraftIntentLoader.h" - -#include -#include - -#include "WaypointLoader.h" - -namespace { -std::list BuildWaypointList(const std::list &waypoint_loaders) { - std::list waypoints; - for (const auto &waypoint_loader : waypoint_loaders) { - waypoints.push_back(waypoint_loader.BuildWaypoint()); - } - return waypoints; -} -} // namespace - -namespace aaesim { -namespace loaders { - -bool AircraftIntentLoader::load(DecodedStream *input) { - set_stream(input); - - std::list descent_waypoint_loaders; - std::list ascent_waypoint_loaders, cruise_waypoint_loaders; - double cruise_mach_loaded; - Units::FeetLength cruise_alt_loaded(0); - - // register all the variables used by the Aircraft Intent - register_var("planned_cruise_mach", &cruise_mach_loaded, true); - register_var("planned_cruise_altitude", &cruise_alt_loaded, true); - register_named_list("descent_waypoints", &descent_waypoint_loaders, false); - register_named_list("cruise_waypoints", &cruise_waypoint_loaders, false); - register_named_list("ascent_waypoints", &ascent_waypoint_loaders, false); - - // do the actual reading: - aircraft_intent_.m_is_loaded = complete(); - - aircraft_intent_.m_planned_cruise_altitude = cruise_alt_loaded; - aircraft_intent_.planned_cruise_mach_ = cruise_mach_loaded; - - const auto descent_waypoints = BuildWaypointList(descent_waypoint_loaders); - const auto ascent_waypoints = BuildWaypointList(ascent_waypoint_loaders); - const auto cruise_waypoints = BuildWaypointList(cruise_waypoint_loaders); - - if (descent_waypoints.empty() && ascent_waypoints.empty() && cruise_waypoints.empty()) { - LOG4CPLUS_ERROR(logger_, - "No waypoints were found in the scenario file. Check the aircraft_intent{} input block."); - throw std::runtime_error("Must provide waypoints."); - } else { - aircraft_intent_.LoadWaypointsFromList(ascent_waypoints, cruise_waypoints, descent_waypoints); - } - - return aircraft_intent_.m_is_loaded; -} - -} // namespace loaders -} // namespace aaesim diff --git a/Public/AircraftSpeed.cpp b/Public/AircraftSpeed.cpp deleted file mode 100644 index 9918adc..0000000 --- a/Public/AircraftSpeed.cpp +++ /dev/null @@ -1,47 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include "public/AircraftSpeed.h" - -#include -#include - -using namespace aaesim::open_source; - -AircraftSpeed::AircraftSpeed() { SetSpeed(UNSPECIFIED_SPEED, INT16_MIN); } - -AircraftSpeed::AircraftSpeed(const SpeedValueType type, const double value) { SetSpeed(type, value); } - -AircraftSpeed::AircraftSpeed(const SpeedValueType type, const Units::Speed speed) { - if (type == MACH_SPEED) { - throw std::logic_error("Cannot construct MACH_SPEED AircraftSpeed using traditional speed units."); - } - SetSpeed(type, Units::KnotsSpeed(speed).value()); -} - -AircraftSpeed::~AircraftSpeed() {} - -SpeedValueType AircraftSpeed::GetSpeedType() const { return m_speed_type; } - -void AircraftSpeed::SetSpeed(const SpeedValueType type, const double value) { - m_speed_type = type; - m_value = value; -} - -double AircraftSpeed::GetValue() const { return m_value; } diff --git a/Public/AircraftState.cpp b/Public/AircraftState.cpp deleted file mode 100644 index f5106e0..0000000 --- a/Public/AircraftState.cpp +++ /dev/null @@ -1,284 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include "public/AircraftState.h" - -#include -#include - -#include "public/CustomMath.h" - -using namespace aaesim::open_source; - -Units::UnsignedRadiansAngle AircraftState::GetHeadingCcwFromEastRadians() const { - double result = atan3(m_yd.value(), m_xd.value()); - return Units::UnsignedRadiansAngle(result); -} - -Units::Speed AircraftState::GetGroundSpeed() const { - return Units::sqrt(Units::sqr(GetSpeedEnuX()) + Units::sqr(GetSpeedEnuY())); -} - -AircraftState AircraftState::FromAdsbReport(const aaesim::open_source::ADSBSVReport &adsb_report) { - return aaesim::open_source::AircraftState::Builder(adsb_report.GetId(), adsb_report.GetTime()) - .Position(adsb_report.GetX(), adsb_report.GetY()) - ->Latitude(adsb_report.GetLatitude()) - ->Longitude(adsb_report.GetLongitude()) - ->AltitudeMsl(adsb_report.GetAltitudeMsl()) - ->GroundSpeed(adsb_report.GetXd(), adsb_report.GetYd()) - ->AltitudeRate(adsb_report.GetVerticalSpeed()) - ->Build(); -} - -/** - * Interpolates between (or extrapolates from) a pair of input states. - * This interpolation is linear on position and velocity fields, - * which is only mathematically consistent when velocity does not change. - * Only id, time, position, and velocity fields are written. - * Other fields, such as acceleration, orientation, and wind, - * are left unchanged. - */ -AircraftState &AircraftState::Interpolate(const AircraftState &a, const AircraftState &b, const double time) { - if (a.GetUniqueId() != b.GetUniqueId()) { - LOG4CPLUS_ERROR(logger, "Interpolating between states that have different ids: " + std::to_string(a.m_id) + - " and " + std::to_string(b.m_id) + "."); - } - - const double baTimeDiff = Units::SecondsTime(b.GetTime() - a.GetTime()).value(); - double aWeight, bWeight; - if (baTimeDiff == 0) { - // avoid divide by zero - aWeight = 1; - bWeight = 0; - LOG4CPLUS_ERROR(logger, "Attempt to interpolate between two states for the same time."); - } else { - aWeight = (b.m_time.value() - time) / baTimeDiff; - bWeight = 1 - aWeight; - } - m_id = a.GetUniqueId(); - m_time = Units::SecondsTime(time); - m_x = a.m_x * aWeight + b.m_x * bWeight; - m_y = a.m_y * aWeight + b.m_y * bWeight; - m_z = a.m_z * aWeight + b.m_z * bWeight; - m_xd = a.m_xd * aWeight + b.m_xd * bWeight; - m_yd = a.m_yd * aWeight + b.m_yd * bWeight; - m_zd = a.m_zd * aWeight + b.m_zd * bWeight; - m_latitude = a.m_latitude * aWeight + b.m_latitude * bWeight; - m_longitude = a.m_longitude * aWeight + b.m_longitude * bWeight; - return *this; -} - -AircraftState &AircraftState::Extrapolate(const AircraftState &in, const Units::SecondsTime &time) { - Units::SecondsTime dt = time - in.m_time; - m_time = time; - m_id = in.m_id; - m_x = in.m_x + in.m_xd * dt; - m_y = in.m_y + in.m_yd * dt; - m_z = in.m_z + in.m_zd * dt; - m_xd = in.m_xd; - m_yd = in.m_yd; - m_zd = in.m_zd; - m_latitude = in.m_latitude + in.m_latitude_rate * dt; - m_longitude = in.m_longitude + in.m_longitude_rate * dt; - return *this; -} - -Units::Speed AircraftState::GetTrueAirspeed() const { - Units::MetersPerSecondSpeed tas_x, tas_y; - tas_x = Units::FeetPerSecondSpeed(m_xd) - Units::MetersPerSecondSpeed(m_sensed_wind_east); - tas_y = Units::FeetPerSecondSpeed(m_yd) - Units::MetersPerSecondSpeed(m_sensed_wind_north); - Units::MetersPerSecondSpeed tas = Units::sqrt(Units::sqr(tas_x) + Units::sqr(tas_y)); - return tas; -} - -AircraftState::AircraftState(const AircraftState::Builder &builder) { - m_id = builder.GetUniqueId(); - m_time = builder.GetTimestamp(); - m_x = builder.GetPositionEnuX(); - m_y = builder.GetPositionEnuY(); - m_z = builder.GetAltitudeMsl(); - m_xd = builder.GetGroundSpeedEast(); - m_yd = builder.GetGroundSpeedNorth(); - m_zd = builder.GetAltitudeRate(); - m_xdd = builder.GetGroundAccelerationEastComponent(); - m_ydd = builder.GetGroundAccelerationNorthComponent(); - m_zdd = builder.GetAltitudeAcceleration(); - m_gamma = builder.GetFlightPathAngle(); - m_sensed_wind_east = builder.GetSensedWindEastComponent(); - m_sensed_wind_north = builder.GetSensedWindNorthComponent(); - m_sensed_wind_parallel = builder.GetSensedWindParallelComponent(); - m_sensed_wind_perpendicular = builder.GetSensedWindPerpendicularComponent(); - m_Vwx_dh = builder.GetVerticalWindDerivativeEastComponent(); - m_Vwy_dh = builder.GetVerticalWindDerivativeNorthComponent(); - m_dynamics_state = builder.GetDynamicsState(); - m_sensed_temperature = builder.GetSensedTemperature(); - m_sensed_density = builder.GetSensedDensity(); - m_sensed_pressure = builder.GetSensedPressure(); - m_latitude = builder.GetLatitude(); - m_longitude = builder.GetLongitude(); - m_latitude_rate = builder.GetLatitudeRate(); - m_longitude_rate = builder.GetLongitudeRate(); - m_psi = builder.GetPsi(); -} - -AircraftState::Builder::Builder(int unique_acid, int time_since_epoch_seconds) - : id_(unique_acid), timestamp_(Units::SecondsTime(time_since_epoch_seconds)) {} - -AircraftState::Builder::Builder(int unique_acid, Units::Time timestamp) : id_(unique_acid), timestamp_(timestamp) {} - -AircraftState::Builder::Builder(const AircraftState &state_to_copy) { - id_ = state_to_copy.GetUniqueId(); - timestamp_ = state_to_copy.GetTime(); - enu_x_ = state_to_copy.GetPositionEnuX(); - enu_y_ = state_to_copy.GetPositionEnuY(); - altitude_msl_ = state_to_copy.GetAltitudeMsl(); - enu_east_ = state_to_copy.GetSpeedEnuX(); - enu_north_ = state_to_copy.GetSpeedEnuY(); - altitude_rate_ = state_to_copy.GetVerticalSpeed(); - enu_accel_east_ = state_to_copy.m_xdd; - enu_accel_north_ = state_to_copy.m_ydd; - altitude_accel_ = state_to_copy.m_zdd; - flight_path_angle_ = state_to_copy.m_gamma; - sensed_wind_east_ = state_to_copy.m_sensed_wind_east; - sensed_wind_north_ = state_to_copy.m_sensed_wind_north; - sensed_wind_parallel_ = state_to_copy.m_sensed_wind_parallel; - sensed_wind_perpendicular_ = state_to_copy.m_sensed_wind_perpendicular; - sensed_wind_vertical_derivative_east_ = state_to_copy.m_Vwx_dh; - sensed_wind_vertical_derivative_north_ = state_to_copy.m_Vwy_dh; - sensed_temperature_ = state_to_copy.m_sensed_temperature; - sensed_density_ = state_to_copy.m_sensed_density; - sensed_pressure_ = state_to_copy.m_sensed_pressure; - latitude_ = state_to_copy.m_latitude; - longitude_ = state_to_copy.m_longitude; - latitude_rate_ = state_to_copy.m_latitude_rate; - longitude_rate_ = state_to_copy.m_longitude_rate; - psi_ = state_to_copy.GetPsi(); - dynamics_state_ = state_to_copy.GetDynamicsState(); -} - -AircraftState AircraftState::Builder::Build() { return AircraftState{*this}; } - -AircraftState::Builder *AircraftState::Builder::Position(Units::FeetLength enu_x, Units::FeetLength enu_y) { - enu_x_ = enu_x; - enu_y_ = enu_y; - return this; -} - -AircraftState::Builder *AircraftState::Builder::AltitudeMsl(Units::FeetLength altitude_msl) { - altitude_msl_ = altitude_msl; - return this; -} - -AircraftState::Builder *AircraftState::Builder::GroundSpeed(Units::FeetPerSecondSpeed enu_east, - Units::FeetPerSecondSpeed enu_north) { - enu_east_ = enu_east; - enu_north_ = enu_north; - return this; -} - -AircraftState::Builder *AircraftState::Builder::AltitudeRate(Units::FeetPerSecondSpeed altitude_rate) { - altitude_rate_ = altitude_rate; - return this; -} - -AircraftState::Builder *AircraftState::Builder::GroundAcceleration(Units::FeetSecondAcceleration enu_east, - Units::FeetSecondAcceleration enu_north) { - enu_accel_east_ = enu_east; - enu_accel_north_ = enu_north; - return this; -} - -AircraftState::Builder *AircraftState::Builder::AltitudeAcceleration( - Units::FeetSecondAcceleration altitude_acceleration) { - altitude_accel_ = altitude_acceleration; - return this; -} - -AircraftState::Builder *AircraftState::Builder::FlightPathAngle(Units::SignedAngle fpa) { - flight_path_angle_ = fpa; - return this; -} - -AircraftState::Builder *AircraftState::Builder::SensedWindComponents(Units::Speed east, Units::Speed north) { - sensed_wind_east_ = east; - sensed_wind_north_ = north; - return this; -} - -AircraftState::Builder *AircraftState::Builder::VerticalWindDerivatives(Units::Frequency east, Units::Frequency north) { - sensed_wind_vertical_derivative_east_ = east; - sensed_wind_vertical_derivative_north_ = north; - return this; -} - -AircraftState::Builder *AircraftState::Builder::SensedTemperature(Units::Temperature temperature) { - sensed_temperature_ = temperature; - return this; -} - -AircraftState::Builder *AircraftState::Builder::SensedDensity(Units::Density density) { - sensed_density_ = density; - return this; -} - -AircraftState::Builder *AircraftState::Builder::SensedPressure(Units::Pressure pressure) { - sensed_pressure_ = pressure; - return this; -} - -AircraftState::Builder *AircraftState::Builder::Latitude(Units::SignedAngle latitude) { - latitude_ = latitude; - return this; -} - -AircraftState::Builder *AircraftState::Builder::LatitudeRate(Units::AngularSpeed latitude_rate) { - latitude_rate_ = latitude_rate; - return this; -} - -AircraftState::Builder *AircraftState::Builder::Longitude(Units::SignedAngle longitude) { - longitude_ = longitude; - return this; -} - -AircraftState::Builder *AircraftState::Builder::LongitudeRate(Units::AngularSpeed longitude_rate) { - longitude_rate_ = longitude_rate; - return this; -} - -AircraftState::Builder *AircraftState::Builder::Psi(Units::Angle psi) { - psi_ = psi; - return this; -} - -AircraftState::Builder *AircraftState::Builder::DynamicsState( - const aaesim::open_source::DynamicsState &dynamics_state) { - dynamics_state_ = dynamics_state; - return this; -} - -AircraftState::Builder *AircraftState::Builder::SensedWindsPerpendicular(Units::Speed wind_perpendicular_component) { - sensed_wind_perpendicular_ = wind_perpendicular_component; - return this; -} - -AircraftState::Builder *AircraftState::Builder::SensedWindsParallel(Units::Speed wind_parallel_component) { - sensed_wind_parallel_ = wind_parallel_component; - return this; -} diff --git a/Public/AlongPathDistanceCalculator.cpp b/Public/AlongPathDistanceCalculator.cpp deleted file mode 100644 index edb0901..0000000 --- a/Public/AlongPathDistanceCalculator.cpp +++ /dev/null @@ -1,168 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include "public/AlongPathDistanceCalculator.h" - -#include - -#include -#include - -using namespace aaesim::open_source; - -log4cplus::Logger AlongPathDistanceCalculator::m_logger = log4cplus::Logger::getInstance("AlongPathDistanceCalculator"); -Units::Length AlongPathDistanceCalculator::CROSS_TRACK_TOLERANCE = Units::NauticalMilesLength(2.5); -Units::Length AlongPathDistanceCalculator::EXTENDED_CROSS_TRACK_TOLERANCE = Units::NauticalMilesLength(20.0); -Units::Length AlongPathDistanceCalculator::CAPTURE_CROSS_TRACK_TOLERANCE = Units::NauticalMilesLength(20.0); - -AlongPathDistanceCalculator::AlongPathDistanceCalculator(const std::vector &horizontal_path, - TrajectoryIndexProgressionDirection expected_index_progression) - : HorizontalPathTracker(horizontal_path, expected_index_progression) { - m_is_first_call = true; - m_cross_track_tolerance = CROSS_TRACK_TOLERANCE; -} - -AlongPathDistanceCalculator::AlongPathDistanceCalculator(const std::vector &horizontal_path, - TrajectoryIndexProgressionDirection expected_index_progression, - bool use_large_cross_track_tolerance) - : HorizontalPathTracker(horizontal_path, expected_index_progression) { - m_is_first_call = true; - if (use_large_cross_track_tolerance) - m_cross_track_tolerance = EXTENDED_CROSS_TRACK_TOLERANCE; - else - m_cross_track_tolerance = CROSS_TRACK_TOLERANCE; -} - -AlongPathDistanceCalculator::AlongPathDistanceCalculator(const std::vector &horizontal_path, - TrajectoryIndexProgressionDirection expected_index_progression, - Units::Length specified_cross_track_tolerance) - : HorizontalPathTracker(horizontal_path, expected_index_progression), - m_is_first_call(true), - m_cross_track_tolerance(specified_cross_track_tolerance) {} - -AlongPathDistanceCalculator AlongPathDistanceCalculator::CreateForCaptureClearance( - const std::vector &horizontal_path) { - AlongPathDistanceCalculator result(horizontal_path, /*TrajectoryIndexProgressionDirection::*/ UNDEFINED, - CAPTURE_CROSS_TRACK_TOLERANCE); - return result; -} - -AlongPathDistanceCalculator::~AlongPathDistanceCalculator() = default; - -bool AlongPathDistanceCalculator::CalculateAlongPathDistanceFromPosition(const Units::Length position_x, - const Units::Length position_y, - Units::Length &distance_along_path, - Units::UnsignedAngle &course) { - Units::UnsignedAngle dummy_course = Units::ZERO_ANGLE; - return CalculateAlongPathDistanceFromPosition(position_x, position_y, distance_along_path, course, dummy_course); -} - -/* - * pt_to_pt_course is used for PERFORMANCE_TURN, only. Otherwise, pt_to_pt_course is the same as course. - * This is used in IMTimeBasedAchieve to determine alignment when clearance type is CAPTURE. - */ -bool AlongPathDistanceCalculator::CalculateAlongPathDistanceFromPosition(const Units::Length position_x, - const Units::Length position_y, - Units::Length &distance_along_path, - Units::UnsignedAngle &course, - Units::UnsignedAngle &pt_to_pt_course) { - std::vector::size_type resolved_index; - Units::Length calculated_distance_along_path; - bool return_boolean = IsPositionOnNode(position_x, position_y, resolved_index); - if (!return_boolean) { - if (m_is_first_call) UpdateCurrentIndex(0); - - return_boolean = AircraftCalculations::CalculateDistanceAlongPathFromPosition( - m_cross_track_tolerance, position_x, position_y, m_extended_horizontal_trajectory, m_current_index, - calculated_distance_along_path, course, resolved_index); - HorizontalTurnPath::TURN_TYPE turn_type = m_extended_horizontal_trajectory[resolved_index].m_turn_info.turn_type; - if (turn_type == HorizontalTurnPath::TURN_TYPE::PERFORMANCE) { - Units::MetersLength half_turn_dist = Units::MetersLength( - (m_extended_horizontal_trajectory[resolved_index].m_path_length_cumulative_meters + - m_extended_horizontal_trajectory[resolved_index + 1].m_path_length_cumulative_meters) / - 2); - if (calculated_distance_along_path > half_turn_dist) // first half of turn - pt_to_pt_course = course; - else - pt_to_pt_course = Units::UnsignedRadiansAngle( - Units::UnsignedRadiansAngle(m_extended_horizontal_trajectory[resolved_index].m_path_course) + - Units::PI_RADIANS_ANGLE); - } else { - // for RADIUS_FIXED only use tangent == course - // for UNKNOWN (straight segment) pt_to_pt == course - pt_to_pt_course = course; - } - - } else { - calculated_distance_along_path = - Units::MetersLength(m_extended_horizontal_trajectory[resolved_index].m_path_length_cumulative_meters); - course = Units::RadiansAngle(m_extended_horizontal_trajectory[resolved_index].m_path_course) + - Units::PI_RADIANS_ANGLE; - pt_to_pt_course = course; - } - - if (m_is_first_call) { - UpdateCurrentIndex(resolved_index); // force validate to succeed - m_is_first_call = false; - } - - // Verify that resolved_index has not become discontinuous and is progressing appropriately - const bool found_index_is_valid = return_boolean && ValidateIndexProgression(resolved_index); - if (found_index_is_valid) { - // resolved_index looks correct. Update class member. - UpdateCurrentIndex(resolved_index); - - // decrement distance-to-go by the amount of the modified horizontal trajectory - distance_along_path = calculated_distance_along_path - EXTENSION_LENGTH; - - m_is_passed_end_of_route = distance_along_path < Units::zero(); - } else { - // resolved_index looks incorrect. Throw. - char msg[300]; - std::snprintf( - msg, sizeof(msg), - "Invalid index progression encountered from CalculatePositionFromDistanceAlongPath(), current_index " - "%lu, resolved_index %lu", - m_current_index, resolved_index); - LOG4CPLUS_FATAL(m_logger, msg); - auto high_index = std::max(m_current_index, resolved_index) + 1; - auto low_index = std::min(m_current_index, resolved_index); - for (auto i = low_index; i <= high_index; i++) { - LOG4CPLUS_TRACE(m_logger, "" << i << ": (" << m_extended_horizontal_trajectory[i].GetXPositionMeters() << "," - << m_extended_horizontal_trajectory[i].GetYPositionMeters() << ")"); - } - throw std::logic_error(msg); - } - - return return_boolean; -} - -AlongPathDistanceCalculator::AlongPathDistanceCalculator() : HorizontalPathTracker(), m_is_first_call(true) {} - -bool AlongPathDistanceCalculator::CalculateAlongPathDistanceFromPosition(const Units::Length position_x, - const Units::Length position_y, - Units::Length &distance_along_path) { - Units::UnsignedAngle ignored_course; - return CalculateAlongPathDistanceFromPosition(position_x, position_y, distance_along_path, ignored_course); -} - -void AlongPathDistanceCalculator::UpdateHorizontalTrajectory(const std::vector &horizontal_trajectory) { - HorizontalPathTracker::UpdateHorizontalTrajectory(horizontal_trajectory); - m_is_first_call = true; -} diff --git a/Public/ArcOnEllipsoid.cpp b/Public/ArcOnEllipsoid.cpp deleted file mode 100644 index 5b66447..0000000 --- a/Public/ArcOnEllipsoid.cpp +++ /dev/null @@ -1,186 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include "public/ArcOnEllipsoid.h" - -#include - -#include "public/GeolibUtils.h" -#include "utility/CustomUnits.h" - -using namespace geolib_idealab; -using namespace aaesim; - -log4cplus::Logger ArcOnEllipsoid::m_logger = log4cplus::Logger::getInstance("ArcOnEllipsoid"); - -ArcOnEllipsoid::ArcOnEllipsoid(const geolib_idealab::Arc &arc) : m_arc_primitive(arc) {} - -Units::Length aaesim::ArcOnEllipsoid::GetShapeLength() const { - /* - * Note: this implementation could call CalculateDistanceFromPointOnShapeToEnd and get the same result, - * but that would also carry multiple extra iterative calls to geolib_idealab methods unnecessarily. In order to - * create a more computationally efficient computation of length, this calls directly into geolib_idealab here - * with all the known arc information. - */ - ErrorSet error_set{ErrorCodes::SUCCESS}; - int steps = INT32_MIN; - double length = - arcLength(m_arc_primitive.centerPoint, m_arc_primitive.radius, m_arc_primitive.startAz, m_arc_primitive.endAz, - m_arc_primitive.dir, &steps, &error_set, GEOLIB_TOLERANCE, GEOLIB_EPSILON); - if (!GeolibUtils::IsSuccess(error_set)) { - LOG4CPLUS_ERROR(m_logger, GeolibUtils::m_basic_error_message << formatErrorMessage(error_set)); - throw std::runtime_error(GeolibUtils::m_basic_error_message); - } - - return Units::NauticalMilesLength(length); -} - -Units::SignedAngle ArcOnEllipsoid::GetArcAngularExtent() const { - return Units::SignedRadiansAngle(m_arc_primitive.subtendedAngle); -} -LatitudeLongitudePoint ArcOnEllipsoid::GetCenterPoint() const { - return LatitudeLongitudePoint::CreateFromGeolibPrimitive(m_arc_primitive.centerPoint); -} -Units::Length ArcOnEllipsoid::GetRadius() const { return Units::NauticalMilesLength(m_arc_primitive.radius); } -Units::SignedAngle ArcOnEllipsoid::GetStartAzimuthEnu() const { - const Units::UnsignedAngle start_azimuth_ned = Units::UnsignedRadiansAngle(m_arc_primitive.startAz); - return GeolibUtils::ConvertCourseFromNedToEnu(start_azimuth_ned); -} -Units::SignedAngle ArcOnEllipsoid::GetEndAzimuthEnu() const { - const Units::UnsignedAngle end_azimuth_ned = Units::UnsignedRadiansAngle(m_arc_primitive.endAz); - return GeolibUtils::ConvertCourseFromNedToEnu(end_azimuth_ned); -} -geolib_idealab::ArcDirection ArcOnEllipsoid::GetArcDirection() const { return m_arc_primitive.dir; } - -bool ArcOnEllipsoid::IsPointOnShape(const LatitudeLongitudePoint &test_point) const { - return GeolibUtils::IsPointOnArc(*this, test_point); -} - -bool ArcOnEllipsoid::IsPointInsideArc(const LatitudeLongitudePoint &test_point) const { - return GeolibUtils::IsPointInsideArcSegment(*this, test_point); -} - -Units::SignedAngle ArcOnEllipsoid::GetCourseEnuTangentToStartPoint() const { - Units::UnsignedDegreesAngle add_this(90); - if (m_arc_primitive.dir == ArcDirection::COUNTERCLOCKWISE) { - // change sign - add_this *= -1; - } - const Units::UnsignedAngle tangent_course_ned = Units::UnsignedRadiansAngle(m_arc_primitive.startAz) + add_this; - return GeolibUtils::ConvertCourseFromNedToEnu(tangent_course_ned); -} - -Units::SignedAngle ArcOnEllipsoid::GetCourseEnuTangentToEndPoint() const { - Units::UnsignedDegreesAngle add_this(90); - if (m_arc_primitive.dir == ArcDirection::COUNTERCLOCKWISE) { - // change sign - add_this *= -1; - } - const Units::UnsignedAngle tangent_course_ned = Units::UnsignedRadiansAngle(m_arc_primitive.endAz) + add_this; - return GeolibUtils::ConvertCourseFromNedToEnu(tangent_course_ned); -} - -LatitudeLongitudePoint ArcOnEllipsoid::GetStartPoint() const { - return LatitudeLongitudePoint::CreateFromGeolibPrimitive(m_arc_primitive.startPoint); -} - -LatitudeLongitudePoint ArcOnEllipsoid::GetEndPoint() const { - return LatitudeLongitudePoint::CreateFromGeolibPrimitive(m_arc_primitive.endPoint); -} - -ShapeOnEllipsoid::kDirectionRelativeToShape ArcOnEllipsoid::GetRelativeDirection( - const LatitudeLongitudePoint &latitude_longitude_point) const { - ShapeOnEllipsoid::kDirectionRelativeToShape return_this = ShapeOnEllipsoid::UNSET; - if (IsPointOnShape(latitude_longitude_point)) { - return_this = ShapeOnEllipsoid::ON_SHAPE; - } else { - /* - * Algorithm: - * If latitude_longitude_point is inside the arc AND arc direction is CLOCKWISE, return RIGHT_OF_SHAPE - * If latitude_longitude_point is outside the arc AND arc direction is CLOCKWISE, return LEFT_OF_SHAPE - * If latitude_longitude_point is inside the arc AND arc direction is COUNTERCLOCKWISE, return LEFT_OF_SHAPE - * If latitude_longitude_point is outside the arc AND arc direction is COUNTERCLOCKWISE, return RIGHT_OF_SHAPE - */ - const bool is_inside_arc = IsPointInsideArc(latitude_longitude_point); - if (is_inside_arc && GetArcDirection() == ArcDirection::CLOCKWISE) { - return_this = ShapeOnEllipsoid::RIGHT_OF_SHAPE; - } else if (!is_inside_arc && GetArcDirection() == ArcDirection::CLOCKWISE) { - return_this = ShapeOnEllipsoid::LEFT_OF_SHAPE; - } else if (is_inside_arc && GetArcDirection() == ArcDirection::COUNTERCLOCKWISE) { - return_this = ShapeOnEllipsoid::LEFT_OF_SHAPE; - } else if (!is_inside_arc && GetArcDirection() == ArcDirection::COUNTERCLOCKWISE) { - return_this = ShapeOnEllipsoid::RIGHT_OF_SHAPE; - } - } - return return_this; -} - -Units::Length ArcOnEllipsoid::GetDistanceToEndPoint(const LatitudeLongitudePoint &latitude_longitude_point) const { - return ShapeOnEllipsoid::GetDistanceToEndPoint(latitude_longitude_point); -} - -LatitudeLongitudePoint ArcOnEllipsoid::GetNearestPointOnShape( - const LatitudeLongitudePoint &latitude_longitude_point) const { - std::pair perp_info = - GeolibUtils::FindNearestPointOnArcUsingPerpendiculorProjection(*this, latitude_longitude_point); - return std::get<1>(perp_info); -} - -Units::Length ArcOnEllipsoid::CalculateDistanceFromPointOnShapeToEnd( - const LatitudeLongitudePoint &point_on_shape) const { - ErrorSet error_set{ErrorCodes::SUCCESS}; - int steps = INT32_MIN; - const Units::SignedAngle start_azimuth_enu = - GetCenterPoint().CalculateRelationshipBetweenPoints(point_on_shape).second; - const Units::UnsignedRadiansAngle start_azimuth_ned = GeolibUtils::ConvertCourseFromEnuToNed(start_azimuth_enu); - double length = - arcLength(m_arc_primitive.centerPoint, m_arc_primitive.radius, start_azimuth_ned.value(), - m_arc_primitive.endAz, m_arc_primitive.dir, &steps, &error_set, GEOLIB_TOLERANCE, GEOLIB_EPSILON); - if (!GeolibUtils::IsSuccess(error_set)) { - LOG4CPLUS_ERROR(m_logger, GeolibUtils::m_basic_error_message << formatErrorMessage(error_set)); - throw std::runtime_error(GeolibUtils::m_basic_error_message); - } - - return Units::NauticalMilesLength(length); -} - -LatitudeLongitudePoint ArcOnEllipsoid::CalculatePointAtDistanceFromStartPoint( - const Units::Length &distance_along_shape_from_start_point) const { - LLPoint calculated_point; - double calculated_subtended_angle; - ErrorSet error_set = arcFromLength(m_arc_primitive.centerPoint, m_arc_primitive.startPoint, m_arc_primitive.dir, - Units::NauticalMilesLength(distance_along_shape_from_start_point).value(), - &calculated_point, &calculated_subtended_angle, GEOLIB_TOLERANCE, GEOLIB_EPSILON); - if (!GeolibUtils::IsSuccess(error_set)) { - LOG4CPLUS_ERROR(m_logger, GeolibUtils::m_basic_error_message << formatErrorMessage(error_set)); - throw std::runtime_error(GeolibUtils::m_basic_error_message); - } - - return LatitudeLongitudePoint::CreateFromGeolibPrimitive(calculated_point); -} - -std::pair ArcOnEllipsoid::CalculateCourseAtDistanceFromStartPoint( - const Units::Length &distance_along_shape_from_start_point) const { - const LatitudeLongitudePoint point_on_arc_at_distance_from_start_point = - CalculatePointAtDistanceFromStartPoint(distance_along_shape_from_start_point); - const ArcOnEllipsoid subarc = GeolibUtils::CreateArcOnEllipsoid( - GetStartPoint(), point_on_arc_at_distance_from_start_point, GetCenterPoint(), GetArcDirection()); - - return std::make_pair(subarc.GetCourseEnuTangentToEndPoint(), point_on_arc_at_distance_from_start_point); -} diff --git a/Public/Atmosphere.cpp b/Public/Atmosphere.cpp deleted file mode 100644 index ca50092..0000000 --- a/Public/Atmosphere.cpp +++ /dev/null @@ -1,41 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include "public/Atmosphere.h" - -#include - -#include -#include - -#include "public/CustomMath.h" - -log4cplus::Logger Atmosphere::m_logger = log4cplus::Logger::getInstance(LOG4CPLUS_TEXT("Atmosphere")); - -void Atmosphere::AirDensity_Log(const Units::MetersLength h, const Units::KelvinTemperature t, - const Units::PascalsPressure p, const Units::KilogramsMeterDensity rho) const { - if (m_logger.getLogLevel() <= log4cplus::TRACE_LOG_LEVEL) { - nlohmann::json j; - j["altitude"] = h.value(); - j["temperature"] = t.value(); - j["pressure"] = p.value(); - j["density"] = rho.value(); - LOG4CPLUS_TRACE(m_logger, j); - } -} diff --git a/Public/BlendWindsVerticallyByAltitude.cpp b/Public/BlendWindsVerticallyByAltitude.cpp deleted file mode 100644 index 4304e97..0000000 --- a/Public/BlendWindsVerticallyByAltitude.cpp +++ /dev/null @@ -1,141 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include "public/BlendWindsVerticallyByAltitude.h" - -#include "public/WindStack.h" - -using namespace aaesim::open_source; - -void BlendWindsVerticallyByAltitude::BlendSensedWithPredicted( - const aaesim::open_source::AircraftState ¤t_state, - aaesim::open_source::WeatherPrediction &weather_prediction) { - /* - * Algorithm Description: - * For all altitudes above and below the current_state altitude by less than - * BLEND_HEIGHT, the incoming predicted wind velocities will be updated - * to smoothly blend sensed (from current_state) into predicted. At the current_state - * altitude, the predicted wind will be the same as the sensed wind. Above and below - * it will smoothly transition to the forecast velocity values using a linear weight - * algorithm. Note that altitude lines must not be repeated in the output. This is - * checked comparing the current altitude with the blended wind matrix to set the size - * of the predicted wind matrices and to set the altitudes and velocities in the predicted - * wind matrices accordingly. - */ - - // Local predicted wind WindStack to operate on. The returned matrices - // will be updated just prior to return - aaesim::open_source::WindStack local_blended_x = weather_prediction.east_west(); - aaesim::open_source::WindStack local_blended_y = weather_prediction.north_south(); - - // Define the limits that we need to use for wind blending - - Units::Length currentAlt = Units::FeetLength(current_state.GetAltitudeMsl()); - Units::Length maxAlt = currentAlt + BLEND_HEIGHT; - - if (maxAlt > MAXIMUM_ALTITUDE_LIMIT) { - maxAlt = MAXIMUM_ALTITUDE_LIMIT; - } - - Units::Length minAlt = (currentAlt - BLEND_HEIGHT); - - if (Units::FeetLength(minAlt) < MINIMUM_ALTITUDE_LIMIT) { - minAlt = MINIMUM_ALTITUDE_LIMIT; - } - - // Loop over the predicted matrices. The altitude values between - // the two should always be in-sync, so we can write one loop - // to iterate over both. - - const Units::Speed Vwx_sensed = current_state.GetSensedWindEast(); - const Units::Speed Vwy_sensed = current_state.GetSensedWindNorth(); - - int iRow; - - Units::Length altFromPrediction; - - for (iRow = local_blended_x.GetMaxRow(); iRow >= 1; --iRow) { - altFromPrediction = local_blended_x.GetAltitude(iRow); // this will go down in altitude from highest to lowest - // value stored in local_blended_x - - if (altFromPrediction > maxAlt || altFromPrediction < minAlt) { - continue; - } // above max altitude or below minAlt, no need to blend winds - - // Blend winds - // NOTE: local_blended_x,y store velocity in knots. The aircraft state object stores sensed wind in meters per - // second. Unit conversions are important. - - Units::Length altDiff = currentAlt - altFromPrediction; - - const double weightValue = 1.0 - (abs(altDiff) / BLEND_HEIGHT); - - const double unityMinusWeightValue = 1.0 - weightValue; - - const Units::Speed Vwx_predicted = local_blended_x.GetSpeed(iRow); - const Units::Speed Vwy_predicted = local_blended_y.GetSpeed(iRow); - const Units::Speed Vwx_update((weightValue * Vwx_sensed) + (unityMinusWeightValue * Vwx_predicted)); - const Units::Speed Vwy_update((weightValue * Vwy_sensed) + (unityMinusWeightValue * Vwy_predicted)); - local_blended_x.Insert(iRow, altFromPrediction, Vwx_update); - local_blended_y.Insert(iRow, altFromPrediction, Vwy_update); - } - - // Add the current location and sensed wind to the matrices also - // The below loop is a very verbose way of updating the predicted wind matrix. However, - // WindStack does not contain update/append operations. Remove the below when WindStack - // is updated. - - // Get correct new bounds. - int currentAltIx = -1; - for (iRow = local_blended_x.GetMinRow(); iRow <= local_blended_x.GetMaxRow() && currentAltIx == -1; ++iRow) { - if (abs(currentAlt - local_blended_x.GetAltitude(iRow)) < Units::FeetLength(0.1)) { - // Current altitude found in blended wind matrix. - currentAltIx = iRow; - } - } - - const int newMaxBound = ((currentAltIx == -1) ? (local_blended_x.GetMaxRow() + 1) : local_blended_x.GetMaxRow()); - - weather_prediction.east_west().SetBounds(1, newMaxBound); // this will delete all data - weather_prediction.north_south().SetBounds(1, newMaxBound); // this will delete all data - for (iRow = local_blended_x.GetMinRow(); iRow <= local_blended_x.GetMaxRow(); iRow++) { - if (iRow != currentAltIx) { - // Take winds from blended matrix. - weather_prediction.east_west().Insert(iRow, local_blended_x.GetAltitude(iRow), local_blended_x.GetSpeed(iRow)); - weather_prediction.north_south().Insert(iRow, local_blended_y.GetAltitude(iRow), - local_blended_y.GetSpeed(iRow)); - - } else { - // Take winds from current altitude. - weather_prediction.east_west().Insert(iRow, currentAlt, Vwx_sensed); - weather_prediction.north_south().Insert(iRow, currentAlt, Vwy_sensed); - } - } - - if (currentAltIx == -1) { - // Add current wind to end - weather_prediction.east_west().Insert(newMaxBound, currentAlt, Vwx_sensed); - weather_prediction.north_south().Insert(newMaxBound, currentAlt, Vwy_sensed); - } - - // Sort before returning - weather_prediction.east_west().SortAltitudesAscending(); - weather_prediction.north_south().SortAltitudesAscending(); - weather_prediction.IncrementUpdateCount(); -} diff --git a/Public/CMakeLists.txt b/Public/CMakeLists.txt deleted file mode 100644 index a52207c..0000000 --- a/Public/CMakeLists.txt +++ /dev/null @@ -1,119 +0,0 @@ -cmake_minimum_required(VERSION 3.14) - - -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fPIC") -include(${PROJECT_SOURCE_DIR}/.cmake/get_cpm.cmake) - -CPMAddPackage( - NAME geolib_idealab - GIT_REPOSITORY https://github.com/mitre/geodetic_library.git - GIT_TAG v3.2.9 -) -if (geolib_idealab_ADDED) - # set a variable for the includes. Use the variable later inside target_include_directories() - set(geolib_idealab_INCLUDE_DIRS ${geolib_idealab_SOURCE_DIR}/include) -endif () - -CPMAddPackage( - NAME minicsv - GIT_REPOSITORY https://github.com/shaovoon/minicsv.git - GIT_TAG v1.8.7 - DOWNLOAD_ONLY TRUE -) -if (minicsv_ADDED) - set(minicsv_INCLUDE_DIR ${minicsv_SOURCE_DIR}) -endif () - -CPMAddPackage( - NAME nlohmann_json - GITHUB_REPOSITORY nlohmann/json - VERSION 3.11.3 -) - -set(LIBRARY_SOURCE_FILES - ADSBSVReport.cpp - AircraftCalculations.cpp - AircraftControl.cpp - AircraftIntent.cpp - AircraftIntentLoader.cpp - AircraftSpeed.cpp - AircraftState.cpp - Atmosphere.cpp - BlendWindsVerticallyByAltitude.cpp - CalcWindGradControl.cpp - ConfigurationFileReader.cpp - ControlCommands.cpp - CoreUtils.cpp - EarthModel.cpp - EllipsoidalEarthModel.cpp - Environment.cpp - EuclideanTrajectoryPredictor.cpp - FlightEnvelopeSpeedLimiter.cpp - HorizontalPath.cpp - HorizontalTurnPath.cpp - KinematicDescent4DPredictor.cpp - KinematicTrajectoryPredictor.cpp - LegacyPositionEstimator.cpp - LocalTangentPlane.cpp - NullSpeedLimiter.cpp - NullWindEvaluator.cpp - PassThroughAssap.cpp - PrecalcConstraint.cpp - PrecalcWaypoint.cpp - ScenarioUtils.cpp - SingleTangentPlaneSequence.cpp - SpeedOnPitchControl.cpp - SpeedOnThrustControl.cpp - StatisticalPilotDelay.cpp - StereographicProjection.cpp - TangentPlaneSequence.cpp - ThreeDOFDynamics.cpp - VectorDifferenceWindEvaluator.cpp - VerticalPath.cpp - VerticalPredictor.cpp - Waypoint.cpp - WaypointLoader.cpp - WeatherEstimate.cpp - WeatherPrediction.cpp - WeatherTruth.cpp - Wind.cpp - WindStack.cpp - HorizontalPathTracker.cpp - PositionCalculator.cpp - AlongPathDistanceCalculator.cpp - DirectionOfFlightCourseCalculator.cpp - WindZero.cpp - DataReader.cpp - TvReader.cpp - GeolibUtils.cpp - LatitudeLongitudePoint.cpp - LineOnEllipsoid.cpp - ArcOnEllipsoid.cpp - EuclideanWaypointMonitor.cpp - CustomMath.cpp - DMatrix.cpp - DVector.cpp - InvalidIndexException.cpp - RandomGenerator.cpp - Wgs84PrecalcWaypoint.cpp - USStandardAtmosphere1976.cpp - ZeroWindTrueWeatherOperator.cpp - FullWindTrueWeatherOperator.cpp - DefaultLateralController.cpp - ClimbPhaseVerticalController.cpp -) - -set(LIBRARY_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/lib) - -add_library(pub STATIC ${LIBRARY_SOURCE_FILES}) -target_include_directories(pub PUBLIC - ${geolib_idealab_INCLUDE_DIRS} - ${aaesim_INCLUDE_DIRS} - ${minicsv_INCLUDE_DIR}) -target_link_libraries(pub - PUBLIC - mitre::fsloader - geolib - log4cplus::log4cplus - nlohmann_json::nlohmann_json - "$<$,$,9.1>>:-lstdc++fs>") diff --git a/Public/CalcWindGradControl.cpp b/Public/CalcWindGradControl.cpp deleted file mode 100644 index 2a0a513..0000000 --- a/Public/CalcWindGradControl.cpp +++ /dev/null @@ -1,71 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include "public/CalcWindGradControl.h" - -#include "public/Environment.h" - -using namespace aaesim::open_source; - -CalcWindGradControl::CalcWindGradControl() - : m_wind_x(), - m_wind_y(), - m_altitude(Units::MetersLength(-999.0)), - m_wind_speed_x(), - m_wind_speed_y(), - m_wind_gradient_x(), - m_wind_gradient_y() {} - -CalcWindGradControl::~CalcWindGradControl() = default; - -void CalcWindGradControl::ComputeWindGradients(const Units::Length &msl_altitude, - const WeatherPrediction &weather_prediction, Units::Speed &wind_speed_x, - Units::Speed &wind_speed_y, Units::Frequency &wind_gradient_x, - Units::Frequency &wind_gradient_y) { - bool computex = ((msl_altitude != m_altitude) || (weather_prediction.east_west() != m_wind_x)) || - std::isnan(Units::KnotsSpeed(m_wind_speed_x).value()); - bool computey = ((msl_altitude != m_altitude) || (weather_prediction.north_south() != m_wind_y)) || - std::isnan(Units::KnotsSpeed(m_wind_speed_y).value()); - - if (computex) { - weather_prediction.east_west().CalculateWindGradientAtAltitude(msl_altitude, m_wind_speed_x, m_wind_gradient_x); - } - - if (computey) { - weather_prediction.north_south().CalculateWindGradientAtAltitude(msl_altitude, m_wind_speed_y, m_wind_gradient_y); - } - - if (msl_altitude != m_altitude) { - m_altitude = msl_altitude; - } - - if (weather_prediction.east_west() != m_wind_x) { - m_wind_x = weather_prediction.east_west(); - } - - if (weather_prediction.north_south() != m_wind_y) { - m_wind_y = weather_prediction.north_south(); - } - - wind_speed_x = m_wind_speed_x; - wind_speed_y = m_wind_speed_y; - - wind_gradient_x = m_wind_gradient_x; - wind_gradient_y = m_wind_gradient_y; -} diff --git a/Public/ClimbPhaseVerticalController.cpp b/Public/ClimbPhaseVerticalController.cpp deleted file mode 100644 index 59a087d..0000000 --- a/Public/ClimbPhaseVerticalController.cpp +++ /dev/null @@ -1,80 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include "public/ClimbPhaseVerticalController.h" - -#include - -void aaesim::open_source::ClimbPhaseVerticalController::ComputeAscentCommands( - const Guidance &guidance, const EquationsOfMotionState &equations_of_motion_state, - std::shared_ptr &sensed_weather, Units::Force &thrust_command, - Units::Angle &gamma_command, Units::Speed &tas_command, - aaesim::open_source::bada_utils::FlapConfiguration &flap_command) { - const Units::Frequency gain_altitude = Units::HertzFrequency(0.20); - const Units::Frequency velocity_gain = Units::sqr(natural_frequency_) / thrust_gain_; - - const Units::Speed hdot_ref = guidance.m_vertical_speed; - const Units::Length alt_ref = guidance.m_reference_altitude; - const Units::Length error_alt = alt_ref - equations_of_motion_state.altitude_msl; - - double temp_gamma = -(hdot_ref + gain_altitude * error_alt) / - equations_of_motion_state.true_airspeed; // calculate change in altitude - // if fabs(change) > 1 set to 1 required for asin calculation - if (temp_gamma > 1.0) { - temp_gamma = 1.0; - } else if (temp_gamma < -1.0) { - temp_gamma = -1.0; - } - gamma_command = Units::RadiansAngle(asin(temp_gamma)); - - // Speed Control - tas_command = - sensed_weather->GetTrueWeather()->CAS2TAS(guidance.m_ias_command, equations_of_motion_state.altitude_msl); - - // Speed Error - Units::Speed error_tas = tas_command - equations_of_motion_state.true_airspeed; - Units::Acceleration vel_dot_com = velocity_gain * error_tas; - - // Estimate kinetic forces for this state - Units::Force lift{}, drag{}; - ConfigureFlapsAndEstimateKineticForces(equations_of_motion_state, sensed_weather, aircraft_performance_, lift, drag, - flap_command); - - // Thrust to maintain speed - // Nominal Thrust (no acceleration) at desired speed - const auto ac_mass = aircraft_performance_->GetAircraftMass(); - Units::Force thrust_nominal = - ac_mass * vel_dot_com + drag - ac_mass * Units::ONE_G_ACCELERATION * sin(equations_of_motion_state.gamma) - - ac_mass * equations_of_motion_state.true_airspeed * - (sensed_weather->GetWindSpeedVerticalDerivativeEast() * cos(equations_of_motion_state.psi_enu) + - sensed_weather->GetWindSpeedVerticalDerivativeNorth() * sin(equations_of_motion_state.psi_enu)) * - sin(equations_of_motion_state.gamma) * cos(equations_of_motion_state.gamma); - thrust_command = thrust_nominal; - - // Thrust Limits - Units::Force max_thrust = Units::NewtonsForce(aircraft_performance_->GetMaxThrust( - equations_of_motion_state.altitude_msl, flap_command, - aaesim::open_source::bada_utils::EngineThrustMode::MAXIMUM_CLIMB, Units::ZERO_CELSIUS)); - Units::Force min_thrust = Units::NewtonsForce(aircraft_performance_->GetMaxThrust( - equations_of_motion_state.altitude_msl, flap_command, - aaesim::open_source::bada_utils::EngineThrustMode::DESCENT, Units::ZERO_CELSIUS)); - - DoLogging(error_alt, thrust_command, equations_of_motion_state.thrust, min_thrust, max_thrust, flap_command, - error_tas, tas_command, gamma_command); -}; diff --git a/Public/ClosestPointMetric.cpp b/Public/ClosestPointMetric.cpp deleted file mode 100644 index 0208911..0000000 --- a/Public/ClosestPointMetric.cpp +++ /dev/null @@ -1,85 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// 2022 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include "public/ClosestPointMetric.h" -#include "public/AircraftCalculations.h" - - -ClosestPointMetric::ClosestPointMetric(void) { - m_im_ac_id = 0; - m_target_ac_id = 0; - m_report_metrics = false; - mMinDist = Units::infinity(); -} - - -ClosestPointMetric::~ClosestPointMetric(void) { -} - - -void ClosestPointMetric::update(double imx, - double imy, - double targx, - double targy) { - - // Computes the distance between im and target aircraft based on the input - // positions and replaces the minimum distance if the new distance closer. - // Distance is in nmi. - // - // imx,imy:position of IM aircraft. - // targx,targy:position of target aircraft. - - Units::Length dist = AircraftCalculations::PtToPtDist( - Units::FeetLength(imx), - Units::FeetLength(imy), - Units::FeetLength(targx), - Units::FeetLength(targy)); - - if (dist < mMinDist) { - mMinDist = dist; - } -} - - -Units::Length ClosestPointMetric::getMinDist() { - - // Gets minimum distance. - // - // returns minimum distance. - - return mMinDist; -} - -void ClosestPointMetric::SetAcIds(int im_ac_id, int target_ac_id) { - m_im_ac_id = im_ac_id; - m_target_ac_id = target_ac_id; - m_report_metrics = (im_ac_id != target_ac_id && target_ac_id >= 0); -} - -int ClosestPointMetric::GetImAcId() const { - return m_im_ac_id; -} - -bool ClosestPointMetric::IsReportMetrics() const { - return m_report_metrics; -} - -int ClosestPointMetric::GetTargetAcId() const { - return m_target_ac_id; -} diff --git a/Public/ConfigurationFileReader.cpp b/Public/ConfigurationFileReader.cpp deleted file mode 100644 index 0396585..0000000 --- a/Public/ConfigurationFileReader.cpp +++ /dev/null @@ -1,72 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include "public/ConfigurationFileReader.h" - -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -namespace fs = std::filesystem; - -log4cplus::Logger aaesim::open_source::ConfigurationFileReader::m_logger = - log4cplus::Logger::getInstance(LOG4CPLUS_TEXT("ConfigurationFileReader")); - -const std::vector aaesim::open_source::ConfigurationFileReader::LoadConfigurationFile( - const std::string &suggested_filename) { - std::string configuration_filename(""); - if (suggested_filename.find('/') != std::string::npos) { - configuration_filename = suggested_filename; - } else { - configuration_filename = "../Run_Files/"; - configuration_filename += suggested_filename.c_str(); - } - - std::ifstream file(configuration_filename); - std::vector scenario_filenames; - if (!file.is_open()) { - std::string error_msg = "Configuration file " + configuration_filename + " not found."; - LOG4CPLUS_FATAL(m_logger, error_msg); - throw std::runtime_error(error_msg); - } else { - std::string ignored_first_line; - std::string scenario_filename; - getline(file, ignored_first_line); - while (getline(file, scenario_filename)) { - if (scenario_filename.empty()) continue; - fs::path fp = fs::absolute(scenario_filename); - if (fs::exists(fp)) { - scenario_filenames.push_back(fp); - } else { - std::string error_msg = "Scenario file " + std::string(scenario_filename) + " not found."; - LOG4CPLUS_FATAL(m_logger, error_msg); - throw std::runtime_error(error_msg); - } - } - file.close(); - } - return scenario_filenames; -} diff --git a/Public/ControlCommands.cpp b/Public/ControlCommands.cpp deleted file mode 100644 index 49f7474..0000000 --- a/Public/ControlCommands.cpp +++ /dev/null @@ -1,20 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// 2023 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include "public/ControlCommands.h" diff --git a/Public/CoreUtils.cpp b/Public/CoreUtils.cpp deleted file mode 100644 index 33534c9..0000000 --- a/Public/CoreUtils.cpp +++ /dev/null @@ -1,164 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include "public/CoreUtils.h" - -#include -#include -#include -#include -#include -#include -#include - -#include "public/GeolibUtils.h" -#include "public/LatitudeLongitudePoint.h" -#include "public/SimulationTime.h" - -using namespace std; - -int CoreUtils::FindNearestIndex(const double &value_to_find, const vector &vector_to_search) { - int idx; - if (value_to_find > vector_to_search.back()) { - // Special handling that upper_bound() won't accomplish - idx = static_cast(vector_to_search.size() - 1); - } else { - auto itr = std::upper_bound(vector_to_search.begin(), vector_to_search.end(), value_to_find); - idx = static_cast(itr - vector_to_search.begin()); - } - - return idx; -} - -double CoreUtils::LinearlyInterpolate(int upper_index, double x_interpolation_value, - const std::vector &x_values, const std::vector &y_values) { - if (upper_index < 1 || upper_index >= x_values.size()) { - char msg[200]; - snprintf(msg, sizeof(msg), "upper_index (%d) is not between 1 and %d", upper_index, - static_cast(x_values.size() - 1)); - LOG4CPLUS_FATAL(m_logger, msg); - throw out_of_range(msg); - } - - const double v2 = x_values[upper_index]; - const double v1 = x_values[upper_index - 1]; - const double o2 = y_values[upper_index]; - const double o1 = y_values[upper_index - 1]; - - if ((x_interpolation_value - v1) * (x_interpolation_value - v2) > 0) { - char msg[200]; - snprintf(msg, sizeof(msg), "ratio (%lf) is not between %lf and %lf.", x_interpolation_value, v1, v2); - - double ratio = (x_interpolation_value - v1) / (x_interpolation_value - v2); - if (upper_index + 1 == x_values.size() && (ratio < .1 || ratio > 10)) { - LOG4CPLUS_WARN(m_logger, msg); - } else { - LOG4CPLUS_FATAL(m_logger, msg); - throw domain_error(msg); - } - } - - return ((o2 - o1) / (v2 - v1)) * (x_interpolation_value - v1) + o1; -} - -Units::Speed CoreUtils::LinearlyInterpolate(int upper_index, Units::Length x_interpolation_value, - const std::vector &x_values, - const std::vector &y_values) { - std::vector y_values_as_double{}; - auto insert_speed_as_double = [&y_values_as_double](Units::Speed speed_value) { - y_values_as_double.push_back(Units::MetersPerSecondSpeed(speed_value).value()); - }; - std::for_each(y_values.begin(), y_values.end(), insert_speed_as_double); - return Units::MetersPerSecondSpeed(LinearlyInterpolate( - upper_index, Units::MetersLength(x_interpolation_value).value(), x_values, y_values_as_double)); -} - -const Units::Length CoreUtils::CalculateEuclideanDistance(const std::pair &xyLoc1, - const std::pair &xyLoc2) { - Units::Length xdiff = xyLoc1.first - xyLoc2.first; - Units::Length ydiff = xyLoc1.second - xyLoc2.second; - const Units::Length eucldist = sqrt((xdiff * xdiff) + (ydiff * ydiff)); - return eucldist; -} - -const double CoreUtils::LimitOnInterval(double value, double low_limit, double high_limit) { - return (value < low_limit ? low_limit : (value > high_limit ? high_limit : value)); -} - -const int CoreUtils::SignOfValue(double value) { return (((value) == (0)) ? 0 : (((value) > (0)) ? (1) : (-1))); } - -std::list CoreUtils::ShortenLongLegs(const std::list &ordered_waypoints, - Units::Length maximum_allowable_length) { - using namespace geolib_idealab; - using namespace aaesim; - - std::list replacement_waypoints = {}; - Waypoint previous_waypoint = (ordered_waypoints).front(); - for (const Waypoint &next_waypoint : ordered_waypoints) { - if (previous_waypoint.GetName() != next_waypoint.GetName()) { - if (next_waypoint.GetRfTurnArcRadius() == Units::zero()) { - const LatitudeLongitudePoint point1 = LatitudeLongitudePoint::CreateFromWaypoint(previous_waypoint); - const LatitudeLongitudePoint point2 = LatitudeLongitudePoint::CreateFromWaypoint(next_waypoint); - const LineOnEllipsoid line_on_ellipsoid = LineOnEllipsoid::CreateFromPoints(point1, point2); - - if (line_on_ellipsoid.GetShapeLength() > maximum_allowable_length) { - auto intermediate_waypoints = - GetIntermediateWaypointsForLongLeg(line_on_ellipsoid, maximum_allowable_length); - replacement_waypoints.insert(replacement_waypoints.end(), intermediate_waypoints.begin(), - intermediate_waypoints.end()); - } - } - } - replacement_waypoints.push_back(next_waypoint); - previous_waypoint = next_waypoint; - } - - return std::list(replacement_waypoints); -} - -std::list CoreUtils::GetIntermediateWaypointsForLongLeg(const aaesim::LineOnEllipsoid &line_on_ellipsoid, - Units::Length maximum_allowable_single_leg_distance) { - using namespace geolib_idealab; - using namespace aaesim; - - Units::NauticalMilesLength distance_to_end_point(line_on_ellipsoid.GetShapeLength()); - - std::list intermediate_waypoints; - int loop_counter = 1; - static const Units::NauticalMilesLength five_nm(5); - // static const Units::NauticalMilesLength two_nm(2); - while (distance_to_end_point > maximum_allowable_single_leg_distance) { - LatitudeLongitudePoint new_intermediate_point_on_line = line_on_ellipsoid.CalculatePointAtDistanceFromStartPoint( - maximum_allowable_single_leg_distance * static_cast(loop_counter)); - Waypoint new_waypoint(INTERMEDIATE_WAYPOINT_ROOT_NAME + std::to_string(loop_counter), - new_intermediate_point_on_line.GetLatitude(), - new_intermediate_point_on_line.GetLongitude()); - distance_to_end_point = line_on_ellipsoid.GetDistanceToEndPoint(new_intermediate_point_on_line); - if (distance_to_end_point < five_nm) { - // if (distance_to_end_point < two_nm) { - // too close to the end point. exit loop - break; - } else { - intermediate_waypoints.push_back(new_waypoint); - loop_counter++; - } - } - - return intermediate_waypoints; -} diff --git a/Public/CrossTrackObserver.cpp b/Public/CrossTrackObserver.cpp deleted file mode 100644 index afd3bfa..0000000 --- a/Public/CrossTrackObserver.cpp +++ /dev/null @@ -1,38 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// 2022 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include "public/CrossTrackObserver.h" - - -CrossTrackObserver::CrossTrackObserver(void) { - time = -99999.0; - x = 0.0; - y = 0.0; - dynamic_cross = 0.0; - commanded_cross = 0.0; - unmodified_cross = 0.0; - psi_command = 0.0; - phi = 0.0; - limited_phi = 0.0; - reported_distance = 0.0; -} - - -CrossTrackObserver::~CrossTrackObserver(void) { -} diff --git a/Public/CustomMath.cpp b/Public/CustomMath.cpp deleted file mode 100644 index e479f36..0000000 --- a/Public/CustomMath.cpp +++ /dev/null @@ -1,247 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include "public/CustomMath.h" - -#include -#include - -#include -#include - -#include "utility/UtilityConstants.h" - -using namespace aaesim::open_source::constants; - -// generate a uniform random number between 0 and 1 -// From "Numerical Recipe" - -double atan3(double x, double y) { - // returns arc tangent as an angle measured from north in the range 0, 2pi - - double temp; - - temp = (double)atan2(x, y); - - if (temp < 0.0) { - temp = temp + 2.0 * PI; - } - - return (temp); - -} // atan3 - -double quantize(double value, double lsb) { - // quantizes value to lsb (least significant bit) - if (lsb == 0) return value; - double r = round(value / lsb); - if (r == -0) { - r = 0; - } - return (lsb * r); -} // quantize - -Units::Length quantize(Units::Length value, Units::Length lsb) { - // quantizes value to lsb (least significant bit) - if (lsb == Units::zero()) return value; - double r = round(value / lsb); - if (r == -0) { - r = 0; - } - return (lsb * r); -} - -Units::Speed quantize(Units::Speed value, Units::Speed lsb) { - // quantizes value to lsb (least significant bit) - if (lsb == Units::zero()) return value; - double r = round(value / lsb); - if (r == -0) { - r = 0; - } - return (lsb * r); -} - -Units::Time quantize(Units::Time value, Units::Time lsb) { - // quantizes value to lsb (least significant bit) - if (lsb == Units::zero()) return value; - double r = round(value / lsb); - if (r == -0) { - r = 0; - } - return (lsb * r); -} - -double subtract_headings(double hd1, double hd2) { - // subtract heading 2 from heading 1 with the following convention: - // negative (counterclockwise) deltas are indicated by being greater than pi. - // positive (clockwise) deltas are less than pi. - - double t; - - t = hd1 - hd2; - - if (t < 0.) { - t = TWO_PI + t; - } - - return (t); - -} // subtract_headings - -// inverse = inverse(in) -/* Gauss-Jordan elimination from Numerical recipe:*/ -bool inverse(DMatrix &in, int n, DMatrix &out) { - int irow = -1, icol = -1; - - DVector indxc(1, n); - DVector indxr(1, n); - DVector ipiv(1, n); - DMatrix a(1, n, 1, n); - - // copy the "in" matrix into the "a" matrix: - int in_min_row = in.GetMinRow(); - int in_min_column = in.GetMinColumn(); - for (int i = 1; i <= n; i++) { - for (int j = 1; j <= n; j++) { - a.Set(i, j, in.Get(i - 1 + in_min_row, j - 1 + in_min_column)); - } - } - - for (int j = 1; j <= n; j++) { - ipiv.Set(j, 0.); - } - - for (int i = 1; i <= n; i++) { - double big = 0.0; - for (int j = 1; j <= n; j++) { - if (ipiv.Get(j) != 1.) { - for (int k = 1; k <= n; k++) { - if (ipiv.Get(k) == 0.0) { - if (fabs(a.Get(j, k)) >= big) { - big = fabs(a.Get(j, k)); - irow = j; - icol = k; - } - } else if (ipiv.Get(k) > 1.) { - // singular matrix - printf("\nWarning: Inversion of a singular matrix in the inverse() function (> 1 val).\n"); - return false; - } - } // end for(int k=1; k<=n; k++) - } // end if(ipiv.get(j) != 1.) - } // end for(int j=1; i<=n; j++) - ipiv.Set(icol, ipiv.Get(icol) + 1); - if (irow != icol) { - // swap - for (int l = 1; l <= n; l++) { - double temp_swap; - temp_swap = a.Get(irow, l); - a.Set(irow, l, a.Get(icol, l)); - a.Set(icol, l, temp_swap); - } // end for(int l=1; l<=n; l++) - } // end if(irow != icol) - indxr.Set(i, (double)irow); - indxc.Set(i, (double)icol); - if (a.Get(icol, icol) == 0.0) { - // singular matrix - printf("\nWarning: Inversion of a singular matrix in the inverse() function (0 val).\n"); - return false; - } - double pivinv = 1.0 / a.Get(icol, icol); - a.Set(icol, icol, 1.); - for (int l = 1; l <= n; l++) { - a.Set(icol, l, pivinv * a.Get(icol, l)); - } // end for(int l=1; l<=n; l++) - - for (int ll = 1; ll <= n; ll++) { - if (ll != icol) { - double dum = a.Get(ll, icol); - a.Set(ll, icol, 0.); - for (int l = 1; l <= n; l++) { - a.Set(ll, l, a.Get(ll, l) - dum * a.Get(icol, l)); - } // end for(int l=1; l<=n; l++) - } // end if(ll != icol) - } // end for(int ll=1; ll<=n; ll++) - } // end for(int i=1; i<=n; i++) - - for (int l = n; l >= 1; l--) { - if (indxr.Get(l) != indxc.Get(l)) { - for (int k = 1; k <= n; k++) { - // swap: - double temp; - temp = a.Get(k, (int)indxr.Get(l)); - a.Set(k, (int)indxr.Get(l), a.Get(k, (int)indxc.Get(l))); - a.Set(k, (int)indxc.Get(l), temp); - } - } // end if(indxr.get(l) != indxc.get(l)) - } // end for(int l=n; l>=1; l--) - - // copy the "a" matrix into the "out" matrix: - int out_min_row = out.GetMinRow(); - int out_min_column = out.GetMinColumn(); - for (int i = 1; i <= n; i++) { - for (int j = 1; j <= n; j++) { - out.Set(i - 1 + out_min_row, j - 1 + out_min_column, a.Get(i, j)); - } - } - return true; -} - -void matrix_times_vector(DMatrix &matrix_in, DVector &vector_in, int n, DVector &vector_out) { - for (int i = 0; i < n; i++) { - int ii = i + vector_out.GetMin(); - vector_out[ii] = 0.0; - for (int j = 0; j < n; j++) { - vector_out[ii] += - matrix_in[i + matrix_in.GetMinRow()][j + matrix_in.GetMinColumn()] * vector_in[j + vector_in.GetMin()]; - } - } -} - -/** - * Create a matrix which executes a 3-D rotation of a - * point around a vector when a single-row - * matrix [x y z] is post-multiplied by the rotation - * matrix. - */ -DMatrix &CreateRotationMatrix(double l, double m, double n, const Units::Angle theta) { - // basic formula acquired from: - // https://en.wikipedia.org/wiki/Transformation_matrix#Rotation_2 - // Wikipedia uses T * coord_column, while we use coord_row * T. - // Therefore, we must transpose the matrix. - - // we need a unit vector - double mag2 = l * l + m * m + n * n; - if (mag2 != 1) { - double mag = sqrt(mag2); - l /= mag; - m /= mag; - n /= mag; - } - - double cosT = cos(theta); - double sinT = sin(theta); - double cosT1 = 1 - cosT; - - double a[3][3] = {{l * l * cosT1 + cosT, m * l * cosT1 + n * sinT, n * l * cosT1 - m * sinT}, - {l * m * cosT1 - n * sinT, m * m * cosT1 + cosT, n * m * cosT1 + l * sinT}, - {l * n * cosT1 + m * sinT, m * n * cosT1 - l * sinT, n * n * cosT1 + cosT}}; - DMatrix *result = new DMatrix((double **)&a, 0, 2, 0, 2); - return *result; -} diff --git a/Public/DMatrix.cpp b/Public/DMatrix.cpp deleted file mode 100644 index 3c795b8..0000000 --- a/Public/DMatrix.cpp +++ /dev/null @@ -1,251 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include "public/DMatrix.h" - -#include - -#include "public/InvalidIndexException.h" - -char *DMatrix::MULTIPLICATION_DIMENSIONS_MESSAGE = (char *)"Cannot multiply DMatrix unless inner dimensions match."; - -DMatrix::DMatrix() { - m_min_row = 0; - m_max_row = -1; - m_rows = NULL; -} - -DMatrix::~DMatrix() { - delete[] m_rows; - m_rows = NULL; -} - -DMatrix::DMatrix(const DMatrix &in) { - int size = in.m_max_row - in.m_min_row + 1; - - m_max_row = in.m_max_row; - m_min_row = in.m_min_row; - - m_rows = new DVector[size]; - - for (int loop = 0; loop < size; loop++) { - m_rows[loop] = in.m_rows[loop]; - } -} - -DMatrix::DMatrix(int inRowMin, int inRowMax, int inColMin, int inColMax) { - // calculates the size of the DMatrix - int size = inRowMax - inRowMin + 1; - - // sets row min/max - m_max_row = inRowMax; - m_min_row = inRowMin; - - // allocates the new DMatrix (array of DVectors) - m_rows = new DVector[size]; - - // loop to set the colomn size (DVector min/max) - for (int loop = 0; loop < size; loop++) { - m_rows[loop].SetBounds(inColMin, inColMax); - } -} - -DMatrix::DMatrix(double **array_in, int inRowMin, int inRowMax, int inColMin, int inColMax) { - // calculates the size of the DMatrix - int size = inRowMax - inRowMin + 1; - - // sets row min/max - m_max_row = inRowMax; - m_min_row = inRowMin; - - // allocates the new DMatrix (array of DVectors) - m_rows = new DVector[size]; - - // loop to set the colomn size (DVector min/max) - for (int loop = 0; loop < size; loop++) { - m_rows[loop].SetBounds(inColMin, inColMax); - } - - // NOTE this is dangerous code, the array MUST be the same size as the given row/colomn information - // failure to match the bounds given will cause dangerous memory access!!! - // this only works because 2d arrays are stored in sequential memory addresses - // if the array is dynamically allocated this WILL cause dangerous memory access!!! - double *memory_pointer; // pointer to point to the memory address of the 2d array - memory_pointer = (double *)array_in; // accesses the the memory address of the first element - for (int outer = 0; outer < size; outer++) { - for (int inner = inColMin; inner <= inColMax; inner++) { - m_rows[outer].Set(inner, (*memory_pointer)); - memory_pointer++; // iterates the memory address - } - } -} - -double DMatrix::Get(const int row, const int column) const { - // check if in valid range of DMatrix - if (InRange(row)) { - // uses the DVectors overloaded array operator to get the value - return m_rows[row - m_min_row][column]; - } - - // if not in valid range throw Invalid Index Exception - throw InvalidIndexException(row, m_min_row, m_max_row); -} - -void DMatrix::Set(const int row, const int column, const double value) { - // check if in valid range of DMatrix - if (InRange(row)) { - // uses the DVector overloaded array operator to set the value - m_rows[row - m_min_row][column] = value; - } else { - // else not in range throw Invalid Index Exception - throw InvalidIndexException(row, m_min_row, m_max_row); - } -} - -void DMatrix::SetBounds(int row_min, int row_max, int column_min, int column_max) { - // calculates the size of the DMatrix - int size = row_max - row_min + 1; - - // sets row min/max - m_max_row = row_max; - m_min_row = row_min; - - // allocates the new DMatrix (array of DVectors) - delete[] m_rows; - m_rows = new DVector[size]; - - // loop to set the colomn size (DVector min/max) - for (int loop = 0; loop < size; loop++) { - m_rows[loop].SetBounds(column_min, column_max); - } -} - -bool DMatrix::InRange(const int row, const int colomn) const { - // initializes results to false - bool results = false; - - // check if row is in the range of [min,max] inclusive - if (row >= m_min_row && row <= m_max_row) { - // if in rows range call the DVector of that row and check range - results = m_rows[row].IsIndexInRange(colomn); - } - - return results; -} - -bool DMatrix::InRange(const int row) const { - // initializes results to false - bool results = false; - - // check if row is in the range of [min,max] inclusive - if (row >= m_min_row && row <= m_max_row) { - // if in rows range then results is true - results = true; - } - - return results; -} - -DVector &DMatrix::operator[](int row) { - // check if in valid range of DMatrix - if (InRange(row)) { - // returns the DVector of the given row - return m_rows[row - m_min_row]; - } - - // if not in valid range throw Invalid Index Exception - throw InvalidIndexException(row, m_min_row, m_max_row); -} - -const DVector &DMatrix::operator[](int row) const { - // check if in valid range of DMatrix - if (InRange(row)) { - // returns the DVector of the given row - return m_rows[row - m_min_row]; - } - - // if not in valid range throw Invalid Index Exception - throw InvalidIndexException(row, m_min_row, m_max_row); -} - -DMatrix &DMatrix::operator=(const DMatrix &in) { - if (this != &in) { - // calculates size of Matrix - int size = in.m_max_row - in.m_min_row + 1; - - // sets row min/max - m_max_row = in.m_max_row; - m_min_row = in.m_min_row; - - delete[] m_rows; - - // allocates the new DMatrix (array of DVectors) - m_rows = new DVector[size]; - - // loop to copy the values of the given DMatrix - for (int loop = 0; loop < size; loop++) { - m_rows[loop] = in.m_rows[loop]; - } - } - - return *this; -} - -DMatrix &DMatrix::operator*(const DMatrix &that) const { - int rowStart1 = GetMinRow(); - int rowEnd1 = GetMaxRow(); - int colStart1 = GetMinColumn(); - int colEnd1 = GetMaxColumn(); - int rowStart2 = that.GetMinRow(); - int rowEnd2 = that.GetMaxRow(); - int colStart2 = that.GetMinColumn(); - int colEnd2 = that.GetMaxColumn(); - if (colStart1 != rowStart2 || colEnd1 != rowEnd2) { - // cannot be multiplied because inner dimensions don't match - throw IncompatibleDimensionsException(MULTIPLICATION_DIMENSIONS_MESSAGE); - } - DMatrix *result = new DMatrix(rowStart1, rowEnd1, colStart2, colEnd2); - for (int i = rowStart1; i <= rowEnd1; i++) { - for (int j = colStart2; j <= colEnd2; j++) { - double x = 0; - for (int k = colStart1; k <= colEnd1; k++) { - x += (*this)[i][k] * that[k][j]; - } - result->Set(i, j, x); - } - } - return *result; -} - -void DMatrix::AscendSort() { std::sort(&m_rows[0], &m_rows[m_max_row - m_min_row + 1]); } - -int DMatrix::GetMinRow() const { return m_min_row; } - -int DMatrix::GetMaxRow() const { return m_max_row; } - -int DMatrix::GetMinColumn() const { return m_rows[0].GetMin(); } - -int DMatrix::GetMaxColumn() const { return m_rows[0].GetMax(); } - -DMatrix::IncompatibleDimensionsException::IncompatibleDimensionsException(char *explanation) - : exception(), m_explanation(explanation) {} - -DMatrix::IncompatibleDimensionsException::~IncompatibleDimensionsException() throw() {} - -const char *DMatrix::IncompatibleDimensionsException::what() const throw() { return m_explanation; } diff --git a/Public/DVector.cpp b/Public/DVector.cpp deleted file mode 100644 index 099ac96..0000000 --- a/Public/DVector.cpp +++ /dev/null @@ -1,133 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include "public/DVector.h" - -#include - -#include "public/InvalidIndexException.h" - -using std::cout; -using std::endl; - -DVector::DVector() { - m_min_index = 0; - m_max_index = -1; - m_vector = NULL; -} - -// NOTE: max index is inclusive -DVector::DVector(int min, int max) { - int size = max - min + 1; - - m_min_index = min; - m_max_index = max; - - m_vector = new double[size]; -} - -DVector::DVector(const DVector &in) { - int size = in.m_max_index - in.m_min_index + 1; - m_min_index = in.m_min_index; - m_max_index = in.m_max_index; - m_vector = new double[size]; - - for (int loop = 0; loop < size; loop++) { - m_vector[loop] = in.m_vector[loop]; - } -} - -DVector::~DVector() { - delete[] m_vector; - m_vector = NULL; -} - -double DVector::Get(int index) { - if (IsIndexInRange(index)) { - return m_vector[index - m_min_index]; - } - - throw InvalidIndexException(index, m_min_index, m_max_index); -} - -void DVector::Set(int index, double value) { - if (IsIndexInRange(index)) { - m_vector[index - m_min_index] = value; - } else { - throw InvalidIndexException(index, m_min_index, m_max_index); - } -} - -// NOTE: the max index is inclusive -bool DVector::IsIndexInRange(int index) const { - bool result = false; - - if (index >= m_min_index && index <= m_max_index) { - result = true; - } - - return result; -} - -// NOTE: max index is inclusive -void DVector::SetBounds(int min, int max) { - int size0 = m_max_index - m_min_index + 1; - m_min_index = min; - m_max_index = max; - - int size = max - min + 1; - - if (size != size0) { - delete[] m_vector; - m_vector = new double[size]; - } -} - -double &DVector::operator[](int index) { - if (IsIndexInRange(index)) { - return m_vector[index - m_min_index]; - } - throw InvalidIndexException(index, m_min_index, m_max_index); -} - -const double &DVector::operator[](int index) const { - if (IsIndexInRange(index)) { - return m_vector[index - m_min_index]; - } - throw InvalidIndexException(index, m_min_index, m_max_index); -} - -DVector &DVector::operator=(const DVector &in) { - if (this != &in) { - SetBounds(in.m_min_index, in.m_max_index); - int size = m_max_index - m_min_index + 1; - - for (int loop = 0; loop < size; loop++) { - m_vector[loop] = in.m_vector[loop]; - } - } - - return *this; -} - -bool DVector::operator<(const DVector &other) const { return m_vector[0] < other.m_vector[0]; } - -int DVector::GetMin() { return m_min_index; } - -int DVector::GetMax() { return m_max_index; } diff --git a/Public/DataReader.cpp b/Public/DataReader.cpp deleted file mode 100644 index a73d8ba..0000000 --- a/Public/DataReader.cpp +++ /dev/null @@ -1,128 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -/* - * DataReader.cpp - * - * Created on: Mar 23, 2019 - * Author: klewis - */ - -#include "public/DataReader.h" - -#include - -#include -#include -#include - -namespace aaesim { -namespace open_source { - -const Units::SecondsTime DataReader::UNDEFINED_TIME(-9999); - -log4cplus::Logger DataReader::m_logger = - log4cplus::Logger::getInstance(LOG4CPLUS_TEXT("aaesim::open_source::DataReader")); - -DataReader::DataReader(const std::string &file_name, int header_lines, size_t expected_columns) - : m_expected_column_count(expected_columns) { - OpenFile(file_name, header_lines); -} - -DataReader::DataReader(std::shared_ptr input_stream, int header_lines, size_t expected_columns) - : m_expected_column_count(expected_columns) { - OpenStream(input_stream, header_lines); -} - -DataReader::~DataReader() {} - -void DataReader::OpenFile(std::string file_name, int header_lines) { - std::shared_ptr input_stream(new std::ifstream(file_name)); - OpenStream(input_stream, header_lines); -} - -void DataReader::OpenStream(std::shared_ptr input_stream, int header_lines) { - m_input_stream = input_stream; - SkipLines(header_lines); -} - -void DataReader::SkipLines(int header_lines) { - for (int i = 0; i < header_lines; i++) { - m_input_stream->ignore(1000, '\n'); - } -} - -bool DataReader::Advance() { - m_csv_row.ReadNextRow(*m_input_stream); - if (m_csv_row.Size() == 0) { - // end of stream - return false; - } - - if ((m_expected_column_count != 0) && (m_csv_row.Size() != m_expected_column_count)) { - std::cout << "Unexpected column count in line in data file: " << m_csv_row.Size() << std::endl; - } - - return true; -} - -double DataReader::GetDouble(int column) const { - std::istringstream iss(m_csv_row[column]); - double val = 0; - iss >> val; - return val; -} - -std::string DataReader::GetString(int column) const { return m_csv_row[column]; } - -size_t DataReader::GetColumnCount() const { return m_csv_row.Size(); } - -void DataReader::SetExpectedColumnCount(size_t expected_column_count) { - m_expected_column_count = expected_column_count; -} - -void DataReader::BuildColumnIndex() { - // build the column index - for (int i = 0; i < GetColumnCount(); i++) { - std::string name(GetString(i)); - size_t crpos(name.rfind('\r')); - if (crpos != std::string::npos) { - name.erase(crpos, 1); - } - m_column_index[name] = i; - } - - int column_count0 = m_column_index.size(); - if (column_count0 != GetColumnCount()) { - throw std::runtime_error("Duplicate column names in CSV file"); - } -} - -int DataReader::GetColumnNumber(const std::string &column_name) { - int column_count0 = m_column_index.size(); - int index = m_column_index[column_name]; - int column_count1 = m_column_index.size(); - if (column_count0 != column_count1) { - LOG4CPLUS_WARN(m_logger, "Column \"" << column_name << "\" not found. Will use " << index); - } - return index; -} - -} // namespace open_source -} // namespace aaesim diff --git a/Public/DefaultLateralController.cpp b/Public/DefaultLateralController.cpp deleted file mode 100644 index a2d4b23..0000000 --- a/Public/DefaultLateralController.cpp +++ /dev/null @@ -1,92 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include "public/DefaultLateralController.h" - -#include - -#include "public/CoreUtils.h" - -Units::Angle aaesim::open_source::DefaultLateralController::ComputeRollCommand( - const Guidance &guidance, const EquationsOfMotionState &equations_of_motion_state, - std::shared_ptr &sensed_weather) { - const Units::InvertedLength k_xtrk = Units::PerMeterInvertedLength(5e-4); // meters^-1 - const double k_trk = 3; // unitless - - // States - const Units::Speed tas = equations_of_motion_state.true_airspeed; // true airspeed - const Units::Angle gamma = equations_of_motion_state.gamma; // flight-path angle - const Units::Angle psi = equations_of_motion_state.psi_enu; // heading angle measured from east counter-clockwise - - const Units::Speed Vw_para = sensed_weather->GetWindSpeedEast() * cos(guidance.m_enu_track_angle) + - sensed_weather->GetWindSpeedNorth() * sin(guidance.m_enu_track_angle); - const Units::Speed Vw_perp = -sensed_weather->GetWindSpeedEast() * sin(guidance.m_enu_track_angle) + - sensed_weather->GetWindSpeedNorth() * cos(guidance.m_enu_track_angle); - - const Units::Speed wind_magnitude = - sqrt(Units::sqr(sensed_weather->GetWindSpeedEast()) + Units::sqr(sensed_weather->GetWindSpeedNorth())); - const Units::Speed estimated_ground_speed = sqrt(Units::sqr(tas * cos(gamma)) - Units::sqr(Vw_perp)) + Vw_para; - - double temp = (Units::sqr(tas * cos(gamma)) + Units::sqr(estimated_ground_speed) - Units::sqr(wind_magnitude)) / - (tas * 2 * cos(gamma) * estimated_ground_speed); - - // Limit temp so acos function doesn't give undefined value. - if (temp > 1.0) { - temp = 1.0; - } else if (temp < -1.0) { - temp = -1.0; - } - - const Units::Angle beta = - Units::RadiansAngle(acos(temp)) * -1.0 * CoreUtils::SignOfValue(Units::MetersPerSecondSpeed(Vw_perp).value()); - - // Convert track guidance to heading using winds (beta is the Wind Correction Angle) - Units::Angle heading_command = guidance.m_enu_track_angle + beta; - - // Error in heading angle - Units::SignedAngle e_trk = heading_command - psi; - e_trk.normalize(); - - // Along-path distance and Cross-track Error - Units::Length e_xtrk = Units::zero(); - - // check if guidance has cross track error and use it if so - if (guidance.m_use_cross_track) { - if (guidance.m_reference_bank_angle != Units::ZERO_ANGLE) { - e_xtrk = guidance.m_cross_track_error - (guidance.m_reference_bank_angle / k_xtrk); - } else { - e_xtrk = guidance.m_cross_track_error; - } - } - - // Calculate commanded roll angle - // We had to add a conversion from unitless to radians in the formula for roll_angle_command. - Units::Angle roll_angle_command = -k_xtrk * e_xtrk * Units::ONE_RADIAN_ANGLE - k_trk * e_trk; - const double unlimited_roll_angle_command = Units::RadiansAngle(roll_angle_command).value(); - - // Limit the commanded roll angle - double sign_roll_command = CoreUtils::SignOfValue(unlimited_roll_angle_command); - if (roll_angle_command * sign_roll_command > max_bank_angle_) { - roll_angle_command = max_bank_angle_ * sign_roll_command; - } - - DoLogging(e_xtrk, e_trk, roll_angle_command); - - return roll_angle_command; -}; diff --git a/Public/DirectionOfFlightCourseCalculator.cpp b/Public/DirectionOfFlightCourseCalculator.cpp deleted file mode 100644 index 4cb3efb..0000000 --- a/Public/DirectionOfFlightCourseCalculator.cpp +++ /dev/null @@ -1,183 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include "public/DirectionOfFlightCourseCalculator.h" - -#include -#include - -#include "public/CoreUtils.h" - -using namespace aaesim::open_source; - -log4cplus::Logger DirectionOfFlightCourseCalculator::m_logger = - log4cplus::Logger::getInstance("DirectionOfFlightCourseCalculator"); - -DirectionOfFlightCourseCalculator::DirectionOfFlightCourseCalculator() {} - -DirectionOfFlightCourseCalculator::DirectionOfFlightCourseCalculator( - const std::vector &horizontal_path, - TrajectoryIndexProgressionDirection expected_index_progression) - : HorizontalPathTracker(horizontal_path, expected_index_progression), - m_end_course(Units::RadiansAngle(horizontal_path.front().m_path_course) + Units::PI_RADIANS_ANGLE), - m_start_course(Units::RadiansAngle(horizontal_path.back().m_path_course) + Units::PI_RADIANS_ANGLE) {} - -DirectionOfFlightCourseCalculator::~DirectionOfFlightCourseCalculator() = default; - -bool DirectionOfFlightCourseCalculator::CalculateCourseAtAlongPathDistance(const Units::Length &distance_along_path, - Units::UnsignedAngle &forward_course) { - std::vector::size_type resolved_index; - Units::Angle ignored_turn_theta; - Units::Length ignored_turn_radius; - bool return_value = CalculateForwardCourse(distance_along_path + EXTENSION_LENGTH, m_extended_horizontal_trajectory, - m_current_index, forward_course, ignored_turn_theta, ignored_turn_radius, - resolved_index); - - // Check for end of route is based on passed in distance_along_path - switch (m_index_progression_direction) { - case TrajectoryIndexProgressionDirection::DECREMENTING: - if (!m_is_passed_end_of_route) { - m_is_passed_end_of_route = distance_along_path < Units::zero(); - } - break; - - case TrajectoryIndexProgressionDirection::INCREMENTING: - if (m_is_passed_end_of_route) { - m_is_passed_end_of_route = distance_along_path < Units::zero(); - } - break; - - case TrajectoryIndexProgressionDirection::UNDEFINED: - m_is_passed_end_of_route = distance_along_path < Units::zero(); - break; - - default: - break; - } - - // Verify that resolved_index has not become discontinuous and is progressing appropriately - const bool found_index_is_valid = return_value && ValidateIndexProgression(resolved_index); - if (found_index_is_valid) { - // resolved_index looks correct. Update class member. - UpdateCurrentIndex(resolved_index); - - } else if (distance_along_path + EXTENSION_LENGTH > - Units::MetersLength(m_extended_horizontal_trajectory.back().m_path_length_cumulative_meters)) { - // distance_along_path is very large so off the back of the path. The old code allowed this situation to quietly - // happen. For now, it helps a lot to allow this. But, we should consider this deprecated behavior and throw in - // the future. - char msg[300]; - std::snprintf(msg, sizeof(msg), - "Very long distance_along_path encountered. Too long for path. Allowing for now: %f", - Units::MetersLength(distance_along_path).value()); - LOG4CPLUS_ERROR(m_logger, msg); - - } else { - // resolved_index looks incorrect. Throw. - char msg[300]; - std::snprintf(msg, sizeof(msg), - "Invalid index progression encountered from CalculateCourseAtAlongPathDistance(), current_index " - "%lu, resolved_index %lu", - m_current_index, resolved_index); - LOG4CPLUS_FATAL(m_logger, msg); - throw std::logic_error(msg); - } - - return return_value; -} - -bool DirectionOfFlightCourseCalculator::CalculateForwardCourse( - const Units::Length &distance_along_path, const std::vector &horizontal_trajectory, - const std::vector::size_type starting_trajectory_index, Units::UnsignedAngle &forward_course, - Units::Angle &turn_theta, Units::Length &turn_radius, - std::vector::size_type &resolved_trajectory_index) { - std::vector::size_type index = 0; // stores the index last position < current distance - bool found = false; - const double distance_to_find_meters = Units::MetersLength(distance_along_path).value(); - - // loop to find the distance - static const Units::MetersLength on_node_tol(1e-10); - const std::vector::size_type decremented_starting_index = - starting_trajectory_index < 1 ? 0 : starting_trajectory_index - 1; - for (std::vector::size_type loop = decremented_starting_index; - loop < horizontal_trajectory.size() && !found; ++loop) { - if (std::abs(distance_to_find_meters - horizontal_trajectory[loop].m_path_length_cumulative_meters) < - on_node_tol.value()) { - found = true; - index = loop; - } else if (distance_to_find_meters < horizontal_trajectory[loop].m_path_length_cumulative_meters) { - found = true; - if (loop > 0) { // this if prevents loop from being decremented if at zero - index = loop - 1; - } else { - index = 0; - } - } - } - - // See AAES-639 for explanation. In FAS scenario, distance_along_path is different from total - // m_path_length_cumulative_meters in the 9th decimal place. - if (!found && (distance_to_find_meters < - (horizontal_trajectory[horizontal_trajectory.size() - 1].m_path_length_cumulative_meters + 0.0001))) { - found = true; - index = horizontal_trajectory.size() - 1; - } - - turn_radius = Units::zero(); - turn_theta = Units::zero(); - if (found) { - // calculate position based on if it's a straight or turning path - if (horizontal_trajectory[index].m_segment_type == HorizontalPath::SegmentType::STRAIGHT) { - const Units::Angle crs = Units::RadiansAngle( - horizontal_trajectory[index].m_path_course); // get the forward_course for the given index - - // calculate output values - forward_course = Units::ToUnsigned(crs + Units::PI_RADIANS_ANGLE); - } else if (horizontal_trajectory[index].m_segment_type == HorizontalPath::SegmentType::TURN) { - if ((distance_along_path - Units::MetersLength(horizontal_trajectory[index].m_path_length_cumulative_meters)) < - Units::MetersLength(3)) { - forward_course = - Units::UnsignedRadiansAngle(horizontal_trajectory[index].m_path_course) + Units::PI_RADIANS_ANGLE; - } else { - turn_radius = Units::MetersLength(horizontal_trajectory[index].m_turn_info.radius); - Units::UnsignedAngle start = Units::UnsignedRadiansAngle(horizontal_trajectory[index].m_turn_info.q_start); - Units::UnsignedAngle end = Units::UnsignedRadiansAngle(horizontal_trajectory[index].m_turn_info.q_end); - - // calculate forward_course change between the start and end of turn - Units::SignedRadiansAngle course_change = Units::ToSigned(end - start); - - // calculate difference in distance - Units::Angle delta = Units::RadiansAngle( - (distance_along_path - - Units::MetersLength(horizontal_trajectory[index].m_path_length_cumulative_meters)) / - turn_radius); - - // calculate the theta of the turn - turn_theta = start + delta * CoreUtils::SignOfValue(course_change.value()); - forward_course = Units::ToUnsigned(turn_theta - Units::PI_RADIANS_ANGLE / 2.0 * - CoreUtils::SignOfValue(course_change.value())); - } - } - resolved_trajectory_index = index; - } else { - resolved_trajectory_index = -1; - } - - return found; -} diff --git a/Public/DynamicsObserver.cpp b/Public/DynamicsObserver.cpp deleted file mode 100644 index e039b4f..0000000 --- a/Public/DynamicsObserver.cpp +++ /dev/null @@ -1,43 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// 2022 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include "public/DynamicsObserver.h" - -DynamicsObserver::DynamicsObserver(void) { - iter = -1; - id = -1; - time = -99999.0; - achieved_groundspeed = 0.0; - speed_command = 0.0; - IAS_command = 0.0; -} - -DynamicsObserver::~DynamicsObserver(void) { -} - -// < operator to enable sorting of Dynamic Observer objects -bool DynamicsObserver::operator<(const DynamicsObserver &dyn_in) const { - bool result = false; - - if (this->iter <= dyn_in.iter && this->id < dyn_in.id) { - result = true; - } - - return result; -} diff --git a/Public/EarthModel.cpp b/Public/EarthModel.cpp deleted file mode 100644 index 2166354..0000000 --- a/Public/EarthModel.cpp +++ /dev/null @@ -1,42 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -/* - * EarthModel.cpp - * - * Created on: Jun 25, 2015 - * Author: klewis - */ - -#include "public/EarthModel.h" - -EarthModel::EarthModel() {} - -EarthModel::~EarthModel() {} - -std::ostream &operator<<(std::ostream &out, const EarthModel::GeodeticPosition &geo) { - out << "(" << Units::DegreesAngle(geo.latitude) << "," << Units::DegreesAngle(geo.longitude) << ")"; - return out; -} - -std::ostream &operator<<(std::ostream &out, const EarthModel::LocalPositionEnu &local) { - out << "(" << Units::MetersLength(local.x) << "," << Units::MetersLength(local.y) << "," - << Units::MetersLength(local.z) << ")"; - return out; -} diff --git a/Public/EllipsoidalEarthModel.cpp b/Public/EllipsoidalEarthModel.cpp deleted file mode 100644 index 166078b..0000000 --- a/Public/EllipsoidalEarthModel.cpp +++ /dev/null @@ -1,81 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -/* - * EllipsoidalEarthModel.cpp - * - * Created on: Jun 30, 2015 - * Author: klewis - */ - -#include "public/EllipsoidalEarthModel.h" - -#include -#include - -using namespace aaesim::open_source; - -void EllipsoidalEarthModel::ConvertGeodeticToAbsolute(const EarthModel::GeodeticPosition &geo, - EarthModel::AbsolutePositionEcef &ecef) const { - const double sinLat = sin(geo.latitude); - const double cosLat = cos(geo.latitude); - const Units::Length N = kWgs84SemiMajorAxis / sqrt(1.0 - kWgs84EccentricitySquared * sinLat * sinLat); - ecef.x = N * cosLat * cos(geo.longitude); - ecef.y = N * cosLat * sin(geo.longitude); - ecef.z = N * (1.0 - kWgs84EccentricitySquared) * sinLat; -} - -void EllipsoidalEarthModel::ConvertAbsoluteToGeodetic(const EarthModel::AbsolutePositionEcef &ecef, - EarthModel::GeodeticPosition &geo) const { - // Convert ECEF to Geodetic - Units::Length z = ecef.z; - - // Ferrari's Solution (Wikipedia) - double zeta = (1 - kWgs84EccentricitySquared) * z * z / m_semi_major_radius_squared; - Units::Length p = sqrt(ecef.x * ecef.x + ecef.y * ecef.y); - double s = (m_eccentricity_4 * zeta * p * p) / (m_semi_major_radius_squared * 4); - double rho = (p * p / m_semi_major_radius_squared + zeta - m_eccentricity_4) / 6; - double rhocubed = rho * rho * rho; - double t = pow(rhocubed + s + sqrt(s * (s + 2 * rhocubed)), 0.333333333333); - double u = rho + t + (rho * rho) / t; - double v = sqrt(u * u + m_eccentricity_4 * zeta); - double w = kWgs84EccentricitySquared * (u + v - zeta) / (2 * v); - double kappa = 1 + (kWgs84EccentricitySquared * (sqrt(u + v + w * w) + w)) / (u + v); - - // Now solve for lat & lon - double latRadians = atan(kappa * z / p); - double lonRadians = atan2(Units::MetersLength(ecef.y).value(), Units::MetersLength(ecef.x).value()); - geo.latitude = Units::SignedRadiansAngle(latRadians); - geo.longitude = Units::SignedRadiansAngle(lonRadians); - geo.altitude = Units::MetersLength(0); -} - -std::shared_ptr EllipsoidalEarthModel::MakeEnuConverter( - const GeodeticPosition &pointOfTangencyGeo, const LocalPositionEnu &pointOfTangencyEnu) const { - EarthModel::AbsolutePositionEcef ecef; - ConvertGeodeticToAbsolute(pointOfTangencyGeo, ecef); - - std::shared_ptr converter = std::make_shared(this, ecef, pointOfTangencyEnu); - converter->InitializeRotationForGeodeticOrigin(); - // rotate around the -y axis to the specified latitude - converter->RotateEnuFrame(0, -1, 0, pointOfTangencyGeo.latitude); - // rotate around the z axis to the specified longitude - converter->RotateEnuFrame(0, 0, 1, pointOfTangencyGeo.longitude); - return converter; -} diff --git a/Public/EnvReader.cpp b/Public/EnvReader.cpp deleted file mode 100644 index cd53ece..0000000 --- a/Public/EnvReader.cpp +++ /dev/null @@ -1,70 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// 2022 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -/* - * EnvReader.cpp - * - * Created on: Mar 23, 2019 - * Author: klewis - */ - -#include "public/EnvReader.h" -#include "utility/constants.h" -#include -#include -#include - -namespace testvector { - -const size_t EnvReader::EXPECTED_ENV_COLUMN_COUNT(7); - -/* - * FIXME -- There is currently an issue with the ENV files - * in the test vectors. Tv02 has 5-column files vs. all - * other vectors have 7-columns. For now, we are setting - * expected_columns to 0 in the superclass constructor, - * which inhibits column-count checking. - */ -EnvReader::EnvReader(std::string file_name, int header_lines) : - DataReader(file_name, header_lines, 0 /* EXPECTED_ENV_COLUMN_COUNT */) { -} - -EnvReader::EnvReader(std::shared_ptr input_stream, int header_lines) : - DataReader(input_stream, header_lines, 0 /* EXPECTED_ENV_COLUMN_COUNT */) { -} - -EnvReader::~EnvReader() { -} - -bool EnvReader::Advance() { - bool result = DataReader::Advance(); - if (result) { - m_time = Units::SecondsTime(GetDouble(0)); - } - else { - m_time = DataReader::UNDEFINED_TIME; - } - return result; -} - -const Units::SecondsTime EnvReader::GetTime() const { - return m_time; -} - -} // namespace testvector diff --git a/Public/Environment.cpp b/Public/Environment.cpp deleted file mode 100644 index 1d7784d..0000000 --- a/Public/Environment.cpp +++ /dev/null @@ -1,37 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include "public/Environment.h" - -#include - -#include "public/EllipsoidalEarthModel.h" - -std::unique_ptr Environment::m_instance = nullptr; - -Environment::Environment() : m_earth_model(new EllipsoidalEarthModel()) {} - -Environment *Environment::GetInstance() { - if (m_instance == NULL) { - m_instance = std::unique_ptr(new Environment()); - } - return m_instance.get(); -} - -EarthModel *Environment::GetEarthModel() const { return m_earth_model.get(); } diff --git a/Public/EuclideanThreeDofDynamics.cpp b/Public/EuclideanThreeDofDynamics.cpp deleted file mode 100644 index 4753729..0000000 --- a/Public/EuclideanThreeDofDynamics.cpp +++ /dev/null @@ -1,58 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// 2023 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include "public/EuclideanThreeDofDynamics.h" - -#include "public/AircraftState.h" -#include "public/Guidance.h" -#include "public/Wind.h" - -using namespace aaesim::open_source; - -log4cplus::Logger EuclideanThreeDofDynamics::m_logger = log4cplus::Logger::getInstance("EuclideanThreeDofDynamics"); - -void EuclideanThreeDofDynamics::CalculateEnvironmentalWind(WindStack &wind_east, WindStack &wind_north, - Units::Frequency &dVwx_dh, Units::Frequency &dVwy_dh) { - // Have to do update whether using wind or not. Otherwise density and pressure are not updated during descent. - EarthModel::LocalPositionEnu localPosition; - localPosition.x = m_equations_of_motion_state.enu_x; - localPosition.y = m_equations_of_motion_state.enu_y; - localPosition.z = m_equations_of_motion_state.altitude_msl; - m_tangent_plane_sequence->convertLocalToGeodetic(localPosition, m_equations_of_motion_state.geodetic_position); - m_true_weather->LoadConditionsAt(m_equations_of_motion_state.geodetic_position.latitude, - m_equations_of_motion_state.geodetic_position.longitude, - m_equations_of_motion_state.altitude_msl); - - ThreeDOFDynamics::CalculateEnvironmentalWind(wind_east, wind_north, dVwx_dh, dVwy_dh); -} - -void EuclideanThreeDofDynamics::Initialize( - std::shared_ptr aircraft_performance, - const Waypoint &initial_position, std::shared_ptr tangent_plane_sequence, - Units::Length initial_altitude_msl, Units::Speed initial_true_airspeed, Units::Angle initial_ground_course_enu, - double initial_mass_fraction, std::shared_ptr true_weather) { - this->m_tangent_plane_sequence = std::move(tangent_plane_sequence); - EarthModel::LocalPositionEnu initial_enu_position; - m_tangent_plane_sequence->convertGeodeticToLocal(EarthModel::GeodeticPosition::CreateFromWaypoint(initial_position), - initial_enu_position); - m_equations_of_motion_state.geodetic_position = EarthModel::GeodeticPosition::CreateFromWaypoint(initial_position); - - ThreeDOFDynamics::Initialize(aircraft_performance, initial_enu_position, initial_altitude_msl, initial_true_airspeed, - initial_ground_course_enu, initial_mass_fraction, true_weather); -} diff --git a/Public/EuclideanTrajectoryPredictor.cpp b/Public/EuclideanTrajectoryPredictor.cpp deleted file mode 100644 index 8f68b81..0000000 --- a/Public/EuclideanTrajectoryPredictor.cpp +++ /dev/null @@ -1,1123 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include "public/EuclideanTrajectoryPredictor.h" - -#include -#include - -#include -#include -#include -#include -#include -#include - -#include "public/LawOfSinesResolver.h" -#include "public/Wind.h" - -using namespace std; -using namespace aaesim::open_source::constants; -using namespace aaesim::open_source; - -log4cplus::Logger EuclideanTrajectoryPredictor::m_logger = - log4cplus::Logger::getInstance(LOG4CPLUS_TEXT("EuclideanTrajectoryPredictor")); - -// returns whether three points are counter-clockwise (positive sign), -// colinear (zero), or clockwise (negative sign) -double EuclideanTrajectoryPredictor::CounterClockwise(const double ax, const double ay, const double bx, - const double by, const double cx, const double cy) { - return (bx - ax) * (cy - ay) - (by - ay) * (cx - ax); -} - -// Returns whether (0,0) -> (ax, ay) -> (bx, by) is counterclockwise, clockwise, -// or colinear. -double EuclideanTrajectoryPredictor::CounterClockwise(const double ax, const double ay, const double bx, - const double by) { - return ax * by - bx * ay; -} - -// returns true if the trajectory point and waypoint are at the same location -bool EuclideanTrajectoryPredictor::SamePoint(const PrecalcWaypoint &wp, const HorizontalPath &hp) { - return (wp.m_x_pos_meters.value() == hp.GetXPositionMeters() && - wp.m_y_pos_meters.value() == hp.GetYPositionMeters()); -} - -// given a start point and destination point, finds the point that is distance -// radius from both points on the same side of the line from the start point -// to the destination point as the originally given turn point center. -// Returns the distance from the original turn point center to the calculated -// point. -double EuclideanTrajectoryPredictor::FindCenterPoint(double fromX, double fromY, double toX, double toY, double gcpX, - double gcpY, double radius, double &cpX, double &cpY) { - double deltaX = toX - fromX; - double deltaY = toY - fromY; - double leg_distance = sqrt(pow(deltaX, 2) + pow(deltaY, 2)); - double half_leg = leg_distance / 2.0; - double midX = (toX + fromX) / 2.0; - double midY = (toY + fromY) / 2.0; - double p_distance = sqrt(pow(radius, 2) - pow(half_leg, 2)); - double direction = CounterClockwise(fromX, fromY, toX, toY, gcpX, gcpY); - double deltaCX = deltaY * p_distance / leg_distance; - double deltaCY = deltaX * p_distance / leg_distance; - if (direction > 0) { // ccw - cpX = midX - deltaCX; - cpY = midY + deltaCY; - } else { - cpX = midX + deltaCX; - cpY = midY - deltaCY; - } - - deltaX = cpX - gcpX; - deltaY = cpY - gcpY; - return (sqrt(pow(deltaX, 2) + pow(deltaY, 2))); -} - -// Returns half of hte arc-distance of a turn in meters -double EuclideanTrajectoryPredictor::HalfTurn(HorizontalTurnPath turn) { - Units::SignedRadiansAngle course_change = Units::ToSigned(Units::RadiansAngle(turn.q_end - turn.q_start)); - return (Units::MetersLength(turn.radius).value() * fabs(course_change.value()) / 2.0); -} - -EuclideanTrajectoryPredictor::EuclideanTrajectoryPredictor() { - m_waypoint_vector.clear(); - m_horizontal_path.clear(); - m_distance_calculator = AlongPathDistanceCalculator(); - m_position_calculator = PositionCalculator(); - m_tight_turn_resolver = std::make_shared(); -} - -void EuclideanTrajectoryPredictor::CalculateWaypoints( - const AircraftIntent &aircraft_intent, const aaesim::open_source::WeatherPrediction &weather_prediction) { - m_aircraft_intent = aircraft_intent; - - // Set altitude at FAF to final altitude in feet. - m_altitude_at_final_waypoint = Units::FeetLength(Units::MetersLength( - aircraft_intent.GetRouteData().m_nominal_altitude[(aircraft_intent.GetNumberOfWaypoints() - 1)])); - - // set waypoints - m_waypoint_vector.clear(); // empties the waypoint vector for Aircraft Intent Waypoints - - double prev_dist = 0; - - // begin turn - double delta_Bx; - double delta_By; - // end turn - double delta_Ex; - double delta_Ey; - // straight m_path_course - double delta_x; - double delta_y; - // for calculating new center point of turn - double turnPtX; - double turnPtY; - - Units::RadiansAngle bTperp = Units::ZERO_ANGLE; // Direction vector from center of turn to begin of turn waypoint - Units::RadiansAngle eTperp; // Direction vector from center of turn to end of turn waypoint - - double leg_length; - Units::RadiansAngle course; - double ccw; - - PrecalcWaypoint new_waypoint; - // loop to translate all of the intent waypoints into Precalc Waypoints, works from - // back to front since precalc starts from the endpoint - for (int loop = aircraft_intent.GetNumberOfWaypoints() - 1; loop > 0; loop--) { - delta_x = - Units::MetersLength(aircraft_intent.GetRouteData().m_x[loop - 1] - aircraft_intent.GetRouteData().m_x[loop]) - .value(); // from - to, to get m_path_course correct - delta_y = aircraft_intent.GetRouteData().m_y[loop - 1].value() - aircraft_intent.GetRouteData().m_y[loop].value(); - - // Note: radius is entered to the nearest 10th of a mile = 185.2 meters - if (aircraft_intent.GetRouteData().m_rf_radius[loop].value() > 0.00001) { // This is an RF leg - bool radiusTooLarge = false; - // find distance from this point to center of turn - delta_Bx = aircraft_intent.GetRouteData().m_x[loop - 1].value() - - aircraft_intent.GetRouteData().m_x_rf_center[loop].value(); - delta_By = aircraft_intent.GetRouteData().m_y[loop - 1].value() - - aircraft_intent.GetRouteData().m_y_rf_center[loop].value(); - // TODO compare square so we do not have to take square root - double radBloop = sqrt(pow(delta_Bx, 2) + pow(delta_By, 2)); - // radius is input to nearest 10th of a mile. - if (fabs(radBloop - aircraft_intent.GetRouteData().m_rf_radius[loop].value()) > 100.0) { // difference of 100 - // meters - radiusTooLarge = true; - } - delta_Ex = aircraft_intent.GetRouteData().m_x[loop].value() - - aircraft_intent.GetRouteData().m_x_rf_center[loop].value(); - delta_Ey = aircraft_intent.GetRouteData().m_y[loop].value() - - aircraft_intent.GetRouteData().m_y_rf_center[loop].value(); - // TODO compare square so we do not have to take square root - double radEloop = sqrt(pow(delta_Ex, 2) + pow(delta_Ey, 2)); - if (fabs(radEloop - aircraft_intent.GetRouteData().m_rf_radius[loop].value()) > 100.0) { // difference of 100 - // meters - radiusTooLarge = true; - } - if (radiusTooLarge) { - double miss_dist = FindCenterPoint( - Units::MetersLength(aircraft_intent.GetRouteData().m_x[loop - 1]).value(), - aircraft_intent.GetRouteData().m_y[loop - 1].value(), - aircraft_intent.GetRouteData().m_x[loop].value(), aircraft_intent.GetRouteData().m_y[loop].value(), - aircraft_intent.GetRouteData().m_x_rf_center[loop].value(), - aircraft_intent.GetRouteData().m_y_rf_center[loop].value(), - aircraft_intent.GetRouteData().m_rf_radius[loop].value(), turnPtX, turnPtY); - - LOG4CPLUS_INFO(m_logger, "Calculated turn center point at " << aircraft_intent.GetWaypointName(loop) - << " is " << miss_dist - << " meters different than input."); - - new_waypoint.m_rf_leg_center_x = Units::MetersLength(turnPtX); - new_waypoint.m_rf_leg_center_y = Units::MetersLength(turnPtY); - } else { - new_waypoint.m_rf_leg_center_x = aircraft_intent.GetRouteData().m_x_rf_center[loop]; - new_waypoint.m_rf_leg_center_y = aircraft_intent.GetRouteData().m_y_rf_center[loop]; - } - - ccw = CounterClockwise(delta_Bx, delta_By, delta_Ex, delta_Ey); - - if (fabs(ccw) < 1.0e-10) { // Consider to be colinear - printf("Colinear points loop: %d, x: %f, y:%f, prev_x:%f, prev_y:%f, cp_x:%f, cp_y:%f\n", loop, - aircraft_intent.GetRouteData().m_x[loop].value(), aircraft_intent.GetRouteData().m_y[loop].value(), - aircraft_intent.GetRouteData().m_x[loop - 1].value(), - aircraft_intent.GetRouteData().m_y[loop - 1].value(), - aircraft_intent.GetRouteData().m_x_rf_center[loop].value(), - aircraft_intent.GetRouteData().m_y_rf_center[loop].value()); - LOG4CPLUS_ERROR(m_logger, "RF Leg Center of Turn is colinear with Waypoints"); - } - new_waypoint.m_name = aircraft_intent.GetRouteData().m_name[loop]; - new_waypoint.m_x_pos_meters = aircraft_intent.GetRouteData().m_x[loop]; - new_waypoint.m_y_pos_meters = aircraft_intent.GetRouteData().m_y[loop]; - new_waypoint.m_radius_rf_leg = aircraft_intent.GetRouteData().m_rf_radius[loop]; - - double directDist = sqrt(pow(delta_x, 2) + pow(delta_y, 2)); - double radius = aircraft_intent.GetRouteData().m_rf_radius[loop].value(); - leg_length = 2 * asin(directDist / (2 * radius)) * radius; - - eTperp = Units::RadiansAngle(atan2(delta_Ey, delta_Ex)); - bTperp = Units::RadiansAngle(atan2(delta_By, delta_Bx)); - - if (ccw < 0.0) { // right turn - course = eTperp + Units::DegreesAngle(90); - } else { // left turn - course = eTperp - Units::DegreesAngle(90); - } - } else { // straight leg - leg_length = sqrt(pow(delta_x, 2) + pow(delta_y, 2)); - - // calculate Psi m_path_course - course = Units::RadiansAngle(atan2(delta_y, delta_x)); - - new_waypoint.m_name = aircraft_intent.GetRouteData().m_name[loop]; - new_waypoint.m_x_pos_meters = aircraft_intent.GetRouteData().m_x[loop]; - new_waypoint.m_y_pos_meters = aircraft_intent.GetRouteData().m_y[loop]; - new_waypoint.m_rf_leg_center_x = aircraft_intent.GetRouteData().m_x_rf_center[loop]; - new_waypoint.m_rf_leg_center_y = aircraft_intent.GetRouteData().m_y_rf_center[loop]; - new_waypoint.m_radius_rf_leg = aircraft_intent.GetRouteData().m_rf_radius[loop]; - } - - prev_dist += leg_length; - new_waypoint.m_leg_length = Units::MetersLength(leg_length); - new_waypoint.m_course_angle = course; - new_waypoint.m_precalc_constraints.constraint_along_path_distance = Units::MetersLength(prev_dist); - new_waypoint.m_precalc_constraints.constraint_altHi = - aircraft_intent.GetRouteData().m_high_altitude_constraint[loop - 1]; - new_waypoint.m_precalc_constraints.constraint_altLow = - aircraft_intent.GetRouteData().m_low_altitude_constraint[loop - 1]; - new_waypoint.m_precalc_constraints.constraint_speedHi = - aircraft_intent.GetRouteData().m_high_speed_constraint[loop - 1]; - new_waypoint.m_precalc_constraints.constraint_speedLow = - aircraft_intent.GetRouteData().m_low_speed_constraint[loop - 1]; - - m_waypoint_vector.push_back(new_waypoint); - } - - new_waypoint.m_leg_length = Units::zero(); - new_waypoint.m_name = aircraft_intent.GetRouteData().m_name[0]; - new_waypoint.m_x_pos_meters = aircraft_intent.GetRouteData().m_x[0]; - new_waypoint.m_y_pos_meters = aircraft_intent.GetRouteData().m_y[0]; - new_waypoint.m_precalc_constraints.constraint_along_path_distance = Units::MetersLength(prev_dist); - new_waypoint.m_precalc_constraints.constraint_altHi = aircraft_intent.GetRouteData().m_high_altitude_constraint[0]; - new_waypoint.m_precalc_constraints.constraint_altLow = aircraft_intent.GetRouteData().m_low_altitude_constraint[0]; - new_waypoint.m_precalc_constraints.constraint_speedHi = aircraft_intent.GetRouteData().m_high_speed_constraint[0]; - new_waypoint.m_precalc_constraints.constraint_speedLow = aircraft_intent.GetRouteData().m_low_speed_constraint[0]; - if (aircraft_intent.GetRouteData().m_rf_radius[0].value() < 0.000001) { // straight leg - new_waypoint.m_radius_rf_leg = Units::MetersLength(0); - new_waypoint.m_course_angle = m_waypoint_vector.back().m_course_angle; - new_waypoint.m_rf_leg_center_x = Units::MetersLength(0); - new_waypoint.m_rf_leg_center_y = Units::MetersLength(0); - } else { - new_waypoint.m_rf_leg_center_x = m_waypoint_vector.back().m_rf_leg_center_x; - new_waypoint.m_rf_leg_center_y = m_waypoint_vector.back().m_rf_leg_center_x; - ccw = CounterClockwise(new_waypoint.m_x_pos_meters.value(), new_waypoint.m_y_pos_meters.value(), - m_waypoint_vector.back().m_x_pos_meters.value(), - m_waypoint_vector.back().m_y_pos_meters.value(), new_waypoint.m_rf_leg_center_x.value(), - new_waypoint.m_rf_leg_center_y.value()); - new_waypoint.m_radius_rf_leg = aircraft_intent.GetRouteData().m_rf_radius[0]; - - if (ccw < 0.0) { // right turn - new_waypoint.m_course_angle = bTperp + Units::DegreesAngle(90); - } else { // left turn (ignore colinear) - new_waypoint.m_course_angle = bTperp - Units::DegreesAngle(90); - } - } - m_waypoint_vector.push_back(new_waypoint); - - Units::KnotsSpeed start_speed = aircraft_intent.GetRouteData().m_nominal_ias[0]; - AdjustConstraints(start_speed); -} - -vector EuclideanTrajectoryPredictor::CalculateTurnAnticipation( - const HorizontalTrajOption option) { - vector turnAnticipation{}; - aaesim::open_source::TurnAnticipation straightTurnAnticipation{}; - // first point has zero turn anticipation - turnAnticipation.push_back(straightTurnAnticipation); - // loop through all but last waypoint (starting point for route) - for (unsigned int loop = 1; loop < m_waypoint_vector.size() - 1; ++loop) { - // if this is an RF leg, then no turn anticipation - double alt_at_turn = 0; - double gspeed_at_turn_mps = 0; - - // loop to sum leg lengths used for both altitude approximations - double leg_sum = 0; - for (unsigned int curr_leg = 0; curr_leg < loop; ++curr_leg) { - leg_sum += Units::MetersLength(m_waypoint_vector[curr_leg].m_leg_length).value(); - } - - if (m_waypoint_vector[loop].m_radius_rf_leg.value() > 0.000001) { - turnAnticipation.push_back(straightTurnAnticipation); - // If an RF Leg, need to calculate bank angle and groundspeed - // Consider moving this loop to a separate method - if (option == SECOND_PASS) { - // loop to find the location in the Precalculated Descent to get Ground Speed - unsigned int curr_index = 0; - bool found = false; // index of distance found flag - for (unsigned int next_index = 0; - next_index < m_vertical_predictor->GetVerticalPath().along_path_distance_m.size() && !found; - ++next_index) { - if (fabs(m_vertical_predictor->GetVerticalPath().along_path_distance_m[next_index]) > leg_sum) { - found = true; - } - - curr_index = next_index; - } - - if (curr_index != 0 && found) { - alt_at_turn = m_vertical_predictor->GetVerticalPath().altitude_m[curr_index]; - gspeed_at_turn_mps = m_vertical_predictor->GetVerticalPath().gs_mps[curr_index]; - } else { - const string msg = "Unable to find position in vertical prediction"; - LOG4CPLUS_FATAL(m_logger, msg); - throw runtime_error(msg); - } - - m_waypoint_vector[loop].m_ground_speed = Units::MetersPerSecondSpeed(gspeed_at_turn_mps); - m_waypoint_vector[loop].m_bank_angle = Units::UnsignedRadiansAngle( - atan(gspeed_at_turn_mps * gspeed_at_turn_mps / - (GRAVITY_METERS_PER_SECOND * m_waypoint_vector[loop].m_radius_rf_leg.value()))); - } - continue; - } - // if the next leg (previously calculated leg) is an RF leg, no anticipation - if (m_waypoint_vector[loop - 1].m_radius_rf_leg.value() > 0.0000001) { - turnAnticipation.push_back(straightTurnAnticipation); - continue; - } - // calculate change in waypoint m_path_course - double course_change = - Units::ToSigned(m_waypoint_vector[loop].m_course_angle - m_waypoint_vector[loop - 1].m_course_angle) - .value(); - if (course_change == 0.0) // straight segment - { - turnAnticipation.push_back(straightTurnAnticipation); - continue; - } - - // check options - // if option 1 calculate approximate altitude at turn using 3-deg fpa - if (option == FIRST_PASS) { - alt_at_turn = leg_sum * tan(3 * PI / 180) + - Units::MetersLength(m_vertical_predictor->GetAltitudeAtEndOfRoute()).value(); - - if (alt_at_turn > Units::MetersLength(m_vertical_predictor->GetCruiseAltitude()).value()) { - alt_at_turn = Units::MetersLength(m_vertical_predictor->GetCruiseAltitude()).value(); - } - - // check if altitude is more or less than the Transition Altitude to calculate groundspeed at turn - if (alt_at_turn < Units::MetersLength(m_vertical_predictor->GetTransitionAltitude()).value()) { - gspeed_at_turn_mps = - Units::MetersPerSecondSpeed(m_atmosphere->CAS2TAS(m_vertical_predictor->GetTransitionIas(), - Units::MetersLength(alt_at_turn))) - .value(); // ignore wind for this estimate - } else { - Units::KelvinTemperature t = - m_atmosphere->GetTemperature(Units::MetersLength(alt_at_turn)); // gets temperature - gspeed_at_turn_mps = m_vertical_predictor->GetTransitionMach() * - sqrt(kGamma * R.value() * t.value()); // ignore wind for this estimate - } - } // END if FIRST_PASS - // else if option 2 calculate approximate altitude at turn from 1st pass Vertical Trajectory - else if (option == SECOND_PASS) { - // loop to find the location in the Precalculated Descent to get Ground Speed - unsigned int curr_index = 0; - bool found = false; // index of distance found flag - for (unsigned int next_index = 0; - next_index < m_vertical_predictor->GetVerticalPath().along_path_distance_m.size() && !found; - ++next_index) { - if (fabs(m_vertical_predictor->GetVerticalPath().along_path_distance_m[next_index]) > leg_sum) { - found = true; - } - curr_index = next_index; - } - - if (curr_index != 0 && found) { - alt_at_turn = m_vertical_predictor->GetVerticalPath().altitude_m[curr_index]; - gspeed_at_turn_mps = m_vertical_predictor->GetVerticalPath().gs_mps[curr_index]; - } else { - VerticalPath newTrajectory = m_vertical_predictor->GetVerticalPath(); - ofstream gout("newVerticalTrajectory.csv"); - gout << "Iteration,AC_ID,TimeToGo(sec),DistanceToGo(meters),Altitude(feet),IAS_Speed(knots),Altitude_" - "Change(fpm),Velocity_Change(mps2),Theta(deg),TAS_GroundSpeed(knots),Mass" - << endl; - gout << fixed; - for (int i = 0; i < newTrajectory.along_path_distance_m.size(); ++i) { - gout << "0,0," << setprecision(2) << newTrajectory.time_to_go_sec[i] << "," << setprecision(6) - << Units::MetersLength(newTrajectory.along_path_distance_m[i]).value() << "," - << Units::FeetLength(Units::MetersLength(newTrajectory.altitude_m[i])).value() << "," - << Units::KnotsSpeed(Units::MetersPerSecondSpeed(newTrajectory.cas_mps[i])).value() << "," - << Units::FeetPerMinuteSpeed(Units::MetersPerSecondSpeed(newTrajectory.altitude_rate_mps[i])) - .value() - << "," << Units::KnotsSpeed(newTrajectory.true_airspeed[i]).value() << "," - << Units::MetersSecondAcceleration(newTrajectory.tas_rate_mps[i]).value() << "," - << Units::DegreesAngle(Units::RadiansAngle(newTrajectory.theta_radians[i])).value() << "," - << Units::KnotsSpeed(Units::MetersPerSecondSpeed(newTrajectory.gs_mps[i])).value() << "," - << newTrajectory.mass_kg[i] << endl; - } - gout.close(); - - const string msg = "Unable to find position in vertical prediction"; - LOG4CPLUS_FATAL(m_logger, msg); - throw runtime_error(msg); - } - } // END of SECOND_PASS - - aaesim::open_source::TurnAnticipation thisTurnAnticipation{}; - // Calculate Turn Radius - double bankangle = 0.5 * fabs(course_change); - thisTurnAnticipation.groundspeed = gspeed_at_turn_mps; - - if (alt_at_turn < 19500.0 * FEET_TO_METERS) { - thisTurnAnticipation.maxAngle = Units::RadiansAngle(m_bank_angle).value(); - if (bankangle > Units::RadiansAngle(m_bank_angle).value()) { - bankangle = Units::RadiansAngle(m_bank_angle).value(); - } else if (bankangle < 10.0 / DEGREES_PER_RADIAN) { - bankangle = 10.0 / DEGREES_PER_RADIAN; - } - } else { - thisTurnAnticipation.maxAngle = 15.0 / DEGREES_PER_RADIAN; - if (bankangle > 15.0 / DEGREES_PER_RADIAN) { - bankangle = 15.0 / DEGREES_PER_RADIAN; - } else if (bankangle < 5.0 / DEGREES_PER_RADIAN) { - bankangle = 5.0 / DEGREES_PER_RADIAN; - } - } - thisTurnAnticipation.bankAngle = bankangle; - - m_waypoint_vector[loop].m_bank_angle = Units::UnsignedRadiansAngle(bankangle); - m_waypoint_vector[loop].m_ground_speed = Units::MetersPerSecondSpeed(gspeed_at_turn_mps); - - const auto turn_radius = (pow(gspeed_at_turn_mps, 2)) / (GRAVITY_METERS_PER_SECOND * tan(bankangle)); - thisTurnAnticipation.radius = turn_radius; - - const auto half_course_change = fabs(course_change) / 2.0; - - thisTurnAnticipation.distance = turn_radius * tan(half_course_change); - - if (thisTurnAnticipation.distance <= (0.2 * NAUTICAL_MILES_TO_METERS)) // 1200 feet - { - turnAnticipation.push_back(straightTurnAnticipation); // handle as straight - continue; - } - - turnAnticipation.push_back(thisTurnAnticipation); - - } // end for (loop through waypoints to calculate turnAnticipation - - // add turnAnticipation for route start point - turnAnticipation.push_back(straightTurnAnticipation); - - if (turnAnticipation.size() != m_waypoint_vector.size()) { - LOG4CPLUS_ERROR(m_logger, "Turn Anticipation vector not the same size as waypoint_vector"); - } - - return turnAnticipation; -} - -void EuclideanTrajectoryPredictor::CalculateHorizontalTrajectory(const HorizontalTrajOption option) { - vector results; - HorizontalPath temp; - double radius; - double turnDist; - const Units::DegreesAngle epsilon(5.0); - - if (m_waypoint_vector.size() == 0) { - m_horizontal_path = results; - return; - } - - // do initialization of first 2 elements - results.push_back(temp); - results.push_back(temp); - - results[0].SetXYPositionMeters(m_waypoint_vector[0].m_x_pos_meters.value(), - m_waypoint_vector[0].m_y_pos_meters.value()); - - if (m_waypoint_vector.size() == 1) { - results[1].SetXYPositionMeters(m_waypoint_vector[0].m_x_pos_meters.value(), - m_waypoint_vector[0].m_y_pos_meters.value()); - results[0].m_segment_type = HorizontalPath::SegmentType::STRAIGHT; - results[0].m_path_course = m_waypoint_vector[0].m_course_angle.value(); - results[0].m_path_length_cumulative_meters = 0.0; - results[1].m_path_length_cumulative_meters = 0.0; - results[1].m_segment_type = HorizontalPath::SegmentType::STRAIGHT; - results[1].m_path_course = m_waypoint_vector[0].m_course_angle.value(); - m_horizontal_path = results; - return; - } - - // Calculate turn anticipation for each turn point - vector turnAnticipation = CalculateTurnAnticipation(option); - - // loop to create horizontal trajectory points - - // Use the turn anticipation information to generate the horizontal path - int makeconstturn = 1; // flag for which part of turn is being computed - // 1 - normal turn or first part of tight turn, - // 2 - second part of tight turn - int counter = 1; // index of last element in results - - // Next turn anticipation must be less than remaining leg length - double remainingLegDistance = Units::MetersLength(m_waypoint_vector[0].m_leg_length).value(); - - double course_change; - double turn_radius; - for (unsigned int loop = 1; loop < m_waypoint_vector.size(); ++loop) { - if (turnAnticipation[loop].distance == 0) // straight segment_type, or this is RF leg, or previous was RF leg - { - results[counter].SetXYPositionMeters(m_waypoint_vector[loop].m_x_pos_meters.value(), - m_waypoint_vector[loop].m_y_pos_meters.value()); - if (m_waypoint_vector[loop - 1].m_radius_rf_leg.value() < 0.000001) { - results[counter - 1].m_segment_type = HorizontalPath::SegmentType::STRAIGHT; - } else { // RFLeg - results[counter - 1].m_segment_type = HorizontalPath::SegmentType::TURN; - results[counter - 1].m_turn_info.x_position_meters = m_waypoint_vector[loop - 1].m_rf_leg_center_x.value(); - results[counter - 1].m_turn_info.y_position_meters = m_waypoint_vector[loop - 1].m_rf_leg_center_y.value(); - results[counter - 1].m_turn_info.radius = Units::MetersLength(m_waypoint_vector[loop - 1].m_radius_rf_leg); - results[counter - 1].m_turn_info.groundspeed = m_waypoint_vector[loop - 1].m_ground_speed; - results[counter - 1].m_turn_info.bankAngle = m_waypoint_vector[loop - 1].m_bank_angle; - results[counter - 1].m_turn_info.turn_type = HorizontalTurnPath::TURN_TYPE::RADIUS_FIXED; - - results[counter - 1].m_turn_info.q_start = Units::UnsignedRadiansAngle(atan2( - (results[counter - 1].GetYPositionMeters() - results[counter - 1].m_turn_info.y_position_meters), - (results[counter - 1].GetXPositionMeters() - results[counter - 1].m_turn_info.x_position_meters))); - results[counter - 1].m_turn_info.q_end = Units::UnsignedRadiansAngle( - atan2((results[counter].GetYPositionMeters() - results[counter - 1].m_turn_info.y_position_meters), - (results[counter].GetXPositionMeters() - results[counter - 1].m_turn_info.x_position_meters))); - } - if (m_waypoint_vector[loop].m_radius_rf_leg.value() < 0.000001) { - remainingLegDistance = Units::MetersLength(m_waypoint_vector[loop].m_leg_length).value(); - } else { - remainingLegDistance = 0.0; - } - - results[counter - 1].m_path_course = m_waypoint_vector[loop - 1].m_course_angle.value(); - } else { // turn segment - course_change = - Units::ToSigned(m_waypoint_vector[loop].m_course_angle - m_waypoint_vector[loop - 1].m_course_angle) - .value(); - - if (makeconstturn == 2) // continue previous turn (KITE or LawOfSines algorithm) - // radius and turnDist are set in previous loop - { - // first check to see if next turn will fit. If not, then we need to do a KITE or Law of Sines turn from - // here - if (turnAnticipation[loop].distance + turnAnticipation[loop + 1].distance > - Units::MetersLength(m_waypoint_vector[loop].m_leg_length).value()) { - // if next leg is straight, then KITE or Law of Sines is not tight enough - if (turnAnticipation[loop + 1].distance == 0) { - turnAnticipation[loop].distance = Units::MetersLength(m_waypoint_vector[loop].m_leg_length).value(); - turn_radius = turnAnticipation[loop].distance / tan(fabs(course_change) / 2.0); - makeconstturn = 1; - } else { - // law of sines - double next_course_change = Units::ToSigned(m_waypoint_vector[loop + 1].m_course_angle - - m_waypoint_vector[loop].m_course_angle) - .value(); - - m_tight_turn_resolver->ResolveTightTurnGeometry( - course_change, next_course_change, - Units::MetersLength(m_waypoint_vector[loop].m_leg_length).value(), radius, turnDist); - - if (radius < 0) // should not happen with Law-of-Sines - { - turnAnticipation[loop].distance = - Units::MetersLength(m_waypoint_vector[loop].m_leg_length / 2.0).value(); - if (turnAnticipation[loop + 1].distance > turnAnticipation[loop].distance) { - turnAnticipation[loop + 1].distance = turnAnticipation[loop].distance; - } - turn_radius = turnAnticipation[loop].distance / tan(fabs(course_change) / 2.0); - } else { - turn_radius = radius; - if (turnDist > remainingLegDistance) { // need tighter turn than Law Of Sines - turnAnticipation[loop].distance = remainingLegDistance; - turn_radius = turnAnticipation[loop].distance / tan(fabs(course_change) / 2.0); - makeconstturn = 1; - } else { - // probably need a straight segment_type here; turn anticipation for Law Of Sines < remaining - // distance - turn_radius = radius; - turnAnticipation[loop].distance = turnDist; - - // for second part of turn - turnDist = Units::MetersLength(m_waypoint_vector[loop].m_leg_length).value() - turnDist; - remainingLegDistance = turnDist; - makeconstturn = 2; - } - } - } - - } else { - turn_radius = radius; - turnAnticipation[loop].distance = turnDist; - makeconstturn = 3; - } - } - // makeconstturn == 1 - else if (turnAnticipation[loop].distance + turnAnticipation[loop + 1].distance > - Units::MetersLength(m_waypoint_vector[loop].m_leg_length).value()) { - if (turnAnticipation[loop + 1].distance == 0) { - turnAnticipation[loop].distance = Units::MetersLength(m_waypoint_vector[loop].m_leg_length).value(); - turn_radius = turnAnticipation[loop].distance / tan(fabs(course_change) / 2.0); - turnAnticipation[loop].radius = turn_radius; - turnAnticipation[loop].bankAngle = - atan(pow(turnAnticipation[loop].groundspeed, 2) / (GRAVITY_METERS_PER_SECOND * turn_radius)); - // To Do, reset bank angle - } else { - double next_course_change = Units::ToSigned(m_waypoint_vector[loop + 1].m_course_angle - - m_waypoint_vector[loop].m_course_angle) - .value(); - - m_tight_turn_resolver->ResolveTightTurnGeometry( - course_change, next_course_change, - Units::MetersLength(m_waypoint_vector[loop].m_leg_length).value(), radius, turnDist); - - if (radius < 0) // should not happen with Law-of-Sines - { - turnAnticipation[loop].distance = - Units::MetersLength(m_waypoint_vector[loop].m_leg_length / 2.0).value(); - if (turnAnticipation[loop + 1].distance > turnAnticipation[loop].distance) { - turnAnticipation[loop + 1].distance = turnAnticipation[loop].distance; - } - turn_radius = turnAnticipation[loop].distance / tan(fabs(course_change) / 2.0); - } else { - if (turnDist > remainingLegDistance) { - turnAnticipation[loop].distance = remainingLegDistance; - turnDist = remainingLegDistance; - turn_radius = turnAnticipation[loop].distance / tan(fabs(course_change) / 2.0); - turnAnticipation[loop].radius = turn_radius; - turnAnticipation[loop].bankAngle = - atan(pow(turnAnticipation[loop].groundspeed, 2) / (GRAVITY_METERS_PER_SECOND * turn_radius)); - } else { - // probably need a straight segment_type here - turn_radius = radius; - turnAnticipation[loop].radius = turn_radius; - turnAnticipation[loop].distance = turnDist; - turnAnticipation[loop].bankAngle = - atan(pow(turnAnticipation[loop].groundspeed, 2) / (GRAVITY_METERS_PER_SECOND * turn_radius)); - - // for second part of turn - turnDist = Units::MetersLength(m_waypoint_vector[loop].m_leg_length).value() - turnDist; - remainingLegDistance = turnDist; - makeconstturn = 2; - } - } - } - } else // normal turn - { - if (turnAnticipation[loop].distance > remainingLegDistance) // not enough room to make planned turn - { - turnAnticipation[loop].distance = remainingLegDistance; - } - turn_radius = turnAnticipation[loop].distance / tan(fabs(course_change) / 2.0); - } - - // Calculate X, Y positions for start and end of turn - double startTurnx = m_waypoint_vector[loop].m_x_pos_meters.value() - - turnAnticipation[loop].distance * cos(m_waypoint_vector[loop - 1].m_course_angle); - double startTurny = m_waypoint_vector[loop].m_y_pos_meters.value() - - turnAnticipation[loop].distance * sin(m_waypoint_vector[loop - 1].m_course_angle); - double stopTurnx = m_waypoint_vector[loop].m_x_pos_meters.value() + - turnAnticipation[loop].distance * cos(m_waypoint_vector[loop].m_course_angle); - double stopTurny = m_waypoint_vector[loop].m_y_pos_meters.value() + - turnAnticipation[loop].distance * sin(m_waypoint_vector[loop].m_course_angle); - - // Check if last trajectory point is within 5 meters of start of this turn - if (pow(results[counter - 1].GetXPositionMeters() - startTurnx, 2) + - pow(results[counter - 1].GetYPositionMeters() - startTurny, 2) < - 25) { // do not add a straight segment_type and move previous trajectory point to start turn point - counter = counter - 1; - } else // insert a straight leg - { - results[counter - 1].m_segment_type = HorizontalPath::SegmentType::STRAIGHT; - results[counter].SetXYPositionMeters(startTurnx, startTurny); - results[counter - 1].m_path_course = m_waypoint_vector[loop - 1].m_course_angle.value(); - results.push_back(temp); - } - - // process end of turn - counter++; - results[counter - 1].m_segment_type = HorizontalPath::SegmentType::TURN; - results[counter - 1].m_turn_info.turn_type = HorizontalTurnPath::TURN_TYPE::PERFORMANCE; - results[counter].SetXYPositionMeters(stopTurnx, stopTurny); - results[counter - 1].m_path_course = (m_waypoint_vector[loop - 1].m_course_angle).value(); - results[counter - 1].m_turn_info.radius = Units::MetersLength(turn_radius); - - double gs = m_waypoint_vector[loop].m_ground_speed.value(); - results[counter - 1].m_turn_info.groundspeed = m_waypoint_vector[loop].m_ground_speed; - results[counter - 1].m_turn_info.bankAngle = - Units::UnsignedRadiansAngle(atan(gs * gs / (GRAVITY_METERS_PER_SECOND * turn_radius))); - - // Check for bank angle in excess of max bank angle (option 2 only) - if (option == SECOND_PASS) { - if (results[counter - 1].m_turn_info.bankAngle > m_bank_angle + epsilon / 2) { - // Getting close to maximum epsilon. Start warning user. - ostringstream msg_strm; - msg_strm << "Turn at waypoint " << loop << " uses bank angle " - << Units::DegreesAngle(results[counter - 1].m_turn_info.bankAngle) - << " which exceeds maximum allowable bank: " << Units::DegreesAngle(m_bank_angle); - string msg = msg_strm.str(); - LOG4CPLUS_WARN(m_logger, msg); - } else if (results[counter - 1].m_turn_info.bankAngle > (m_bank_angle + epsilon)) { - // beyond epsilon. Throw. - ostringstream msg_strm; - msg_strm << "Turn at waypoint " << loop << " uses bank angle " - << Units::DegreesAngle(results[counter - 1].m_turn_info.bankAngle) - << " which exceeds our internal tolerance of allowable bank error: " - << Units::DegreesAngle(m_bank_angle + epsilon) - << ". This looks like an unflyable waypoint sequence."; - string msg = msg_strm.str(); - LOG4CPLUS_FATAL(m_logger, msg); - throw logic_error(msg); - } - } - if (course_change < 0) // left turn - { - results[counter - 1].m_turn_info.q_start = - Units::ToUnsigned(m_waypoint_vector[loop - 1].m_course_angle + Units::PI_RADIANS_ANGLE / 2.0); - results[counter - 1].m_turn_info.q_end = - Units::ToUnsigned(m_waypoint_vector[loop].m_course_angle + Units::PI_RADIANS_ANGLE / 2.0); - results[counter - 1].m_turn_info.x_position_meters = - results[counter - 1].GetXPositionMeters() + - turn_radius * cos(m_waypoint_vector[loop - 1].m_course_angle - (Units::PI_RADIANS_ANGLE / 2.0)); - results[counter - 1].m_turn_info.y_position_meters = - results[counter - 1].GetYPositionMeters() + - turn_radius * sin(m_waypoint_vector[loop - 1].m_course_angle - (Units::PI_RADIANS_ANGLE / 2.0)); - results[counter].SetXYPositionMeters( - results[counter - 1].m_turn_info.x_position_meters + - turn_radius * cos(m_waypoint_vector[loop].m_course_angle + (Units::PI_RADIANS_ANGLE / 2.0)), - results[counter - 1].m_turn_info.y_position_meters + - turn_radius * sin(m_waypoint_vector[loop].m_course_angle + (Units::PI_RADIANS_ANGLE / 2.0))); - } else // right turn - { - results[counter - 1].m_turn_info.q_start = - Units::ToUnsigned(m_waypoint_vector[loop - 1].m_course_angle - Units::PI_RADIANS_ANGLE / 2.0); - results[counter - 1].m_turn_info.q_end = - Units::ToUnsigned(m_waypoint_vector[loop].m_course_angle - Units::PI_RADIANS_ANGLE / 2.0); - results[counter - 1].m_turn_info.x_position_meters = - results[counter - 1].GetXPositionMeters() + - turn_radius * cos(m_waypoint_vector[loop - 1].m_course_angle + (Units::PI_RADIANS_ANGLE / 2.0)); - results[counter - 1].m_turn_info.y_position_meters = - results[counter - 1].GetYPositionMeters() + - turn_radius * sin(m_waypoint_vector[loop - 1].m_course_angle + (Units::PI_RADIANS_ANGLE / 2.0)); - results[counter].SetXYPositionMeters( - results[counter - 1].m_turn_info.x_position_meters + - turn_radius * cos(m_waypoint_vector[loop].m_course_angle - (Units::PI_RADIANS_ANGLE / 2.0)), - results[counter - 1].m_turn_info.y_position_meters + - turn_radius * sin(m_waypoint_vector[loop].m_course_angle - (Units::PI_RADIANS_ANGLE / 2.0))); - } - - // prepare for next segment_type - remainingLegDistance = - sqrt(pow(results[counter].GetXPositionMeters() - m_waypoint_vector[loop + 1].m_x_pos_meters.value(), 2) + - pow(results[counter].GetYPositionMeters() - m_waypoint_vector[loop + 1].m_y_pos_meters.value(), 2)); - } // END turn segment_type - - results.push_back(temp); - counter++; - - } // END for waypoint_vector[loop] for horizontal trajectory - - // delete the blank element at the end - results.pop_back(); - counter--; - // fill in remaining fields of previous element - results[counter].m_segment_type = HorizontalPath::SegmentType::STRAIGHT; - results[counter].m_path_course = m_waypoint_vector[m_waypoint_vector.size() - 1].m_course_angle.value(); - - // calculate path length - double seg_length = 0.0; - results[0].m_path_length_cumulative_meters = 0.0; - for (unsigned int loop = 0; loop < results.size() - 1; ++loop) { - if (results[loop].m_segment_type == HorizontalPath::SegmentType::STRAIGHT) { - seg_length = sqrt(pow(results[loop + 1].GetXPositionMeters() - results[loop].GetXPositionMeters(), 2) + - pow(results[loop + 1].GetYPositionMeters() - results[loop].GetYPositionMeters(), 2)); - results[loop + 1].m_path_length_cumulative_meters = results[loop].m_path_length_cumulative_meters + seg_length; - } else if (results[loop].m_segment_type == HorizontalPath::SegmentType::TURN) { - Units::SignedRadiansAngle course_change = Units::ToSigned( - Units::RadiansAngle(results[loop].m_turn_info.q_end - results[loop].m_turn_info.q_start)); - seg_length = Units::MetersLength(results[loop].m_turn_info.radius).value() * fabs(course_change.value()); - results[loop + 1].m_path_length_cumulative_meters = results[loop].m_path_length_cumulative_meters + seg_length; - } - } - - // set waypoint constraint distances - unsigned int iTraj = 0; - for (unsigned int loop = 0; loop < m_waypoint_vector.size() - 1; ++loop) { - if (SamePoint(m_waypoint_vector[loop], results[iTraj])) { - iTraj++; - if (SamePoint(m_waypoint_vector[loop + 1], results[iTraj])) { - // Cases A, C - m_waypoint_vector[loop].m_precalc_constraints.constraint_along_path_distance = - Units::MetersLength(results[iTraj].m_path_length_cumulative_meters); - continue; - } - if (results[iTraj - 1].m_segment_type == HorizontalPath::SegmentType::STRAIGHT) { - // Case B - m_waypoint_vector[loop].m_precalc_constraints.constraint_along_path_distance = Units::MetersLength( - results[iTraj].m_path_length_cumulative_meters + HalfTurn(results[iTraj].m_turn_info)); - iTraj++; - continue; - } - // Case D - m_waypoint_vector[loop].m_precalc_constraints.constraint_along_path_distance = Units::MetersLength( - results[iTraj - 1].m_path_length_cumulative_meters + HalfTurn(results[iTraj - 1].m_turn_info)); - continue; - } - - if (SamePoint(m_waypoint_vector[loop + 1], results[iTraj])) { - // Case E - m_waypoint_vector[loop].m_precalc_constraints.constraint_along_path_distance = - Units::MetersLength(results[iTraj - 1].m_path_length_cumulative_meters); - continue; - } - - if (results[iTraj].m_segment_type == HorizontalPath::SegmentType::TURN) { - // Case G - m_waypoint_vector[loop].m_precalc_constraints.constraint_along_path_distance = Units::MetersLength( - results[iTraj].m_path_length_cumulative_meters + HalfTurn(results[iTraj].m_turn_info)); - iTraj++; - continue; - } - - iTraj++; - if (SamePoint(m_waypoint_vector[loop + 1], results[iTraj])) { - // Case F - m_waypoint_vector[loop].m_precalc_constraints.constraint_along_path_distance = - Units::MetersLength(results[iTraj].m_path_length_cumulative_meters); - continue; - } - - // Case H - m_waypoint_vector[loop].m_precalc_constraints.constraint_along_path_distance = - Units::MetersLength(results[iTraj].m_path_length_cumulative_meters + HalfTurn(results[iTraj].m_turn_info)); - iTraj++; - } - m_waypoint_vector[m_waypoint_vector.size() - 1].m_precalc_constraints.constraint_along_path_distance = - m_waypoint_vector[m_waypoint_vector.size() - 2].m_precalc_constraints.constraint_along_path_distance; - - // Set the class member as the result of the horizontal planning operation - m_horizontal_path = results; - DoHorizontalPathLogging(m_logger, option); -} - -void EuclideanTrajectoryPredictor::BuildTrajectoryPrediction( - aaesim::open_source::WeatherPrediction &weather, const std::shared_ptr &position_converter, - Units::Length start_altitude) { - BuildTrajectoryPrediction(weather, position_converter, start_altitude, Units::infinity()); -} - -void EuclideanTrajectoryPredictor::BuildTrajectoryPrediction( - aaesim::open_source::WeatherPrediction &weather, const std::shared_ptr &position_converter, - Units::Length start_altitude, Units::Length aircraft_distance_to_go) { - m_aircraft_distance_to_go = aircraft_distance_to_go; - SetAtmosphere(weather.getAtmosphere()); - - // calculation the positions of the Precalculation Waypoints - DefineRoute(); - - if (m_vertical_predictor != NULL) { - CalculateHorizontalTrajectory(FIRST_PASS); - } else { - string msg = string("NULL vertical predictor encountered calling Calculate_Horizontal_Traj(FIRST_PASS)\n") + - string("Check trajectory class constructor"); - LOG4CPLUS_FATAL(m_logger, msg); - throw logic_error(msg); - } - - if (m_vertical_predictor != NULL) { - m_vertical_predictor->BuildVerticalPrediction(m_horizontal_path, m_waypoint_vector, weather, start_altitude, - aircraft_distance_to_go); - DoVerticalPathLogging(m_logger, FIRST_PASS); - } else { - string msg = string("NULL vertical predictor encountered calling buildVerticalPrediction (first pass)\n") + - string("Check trajectory class constructor"); - LOG4CPLUS_FATAL(m_logger, msg); - throw logic_error(msg); - } - - UpdateWeatherPrediction(weather, position_converter); - - if (m_vertical_predictor != NULL) { - CalculateHorizontalTrajectory(SECOND_PASS); - } else { - string msg = string("NULL vertical predictor encountered calling Calculate_Horizontal_Traj(SECOND_PASS)\n") + - string("Check trajectory class constructor"); - LOG4CPLUS_FATAL(EuclideanTrajectoryPredictor::m_logger, msg); - throw logic_error(msg); - } - - if (m_vertical_predictor != NULL) { - m_vertical_predictor->BuildVerticalPrediction(m_horizontal_path, m_waypoint_vector, weather, start_altitude, - aircraft_distance_to_go); - DoVerticalPathLogging(m_logger, SECOND_PASS); - } else { - string msg = string("NULL vertical predictor encountered calling buildVerticalPrediction (second pass)") + - string("Check trajectory class constructor"); - - LOG4CPLUS_FATAL(EuclideanTrajectoryPredictor::m_logger, msg); - throw logic_error(msg); - } - - m_distance_calculator = - AlongPathDistanceCalculator(m_horizontal_path, TrajectoryIndexProgressionDirection::UNDEFINED); - m_position_calculator = PositionCalculator(m_horizontal_path, TrajectoryIndexProgressionDirection::UNDEFINED); -} - -// the method to calculate Guidance based on the 4D Trajectory -aaesim::open_source::Guidance EuclideanTrajectoryPredictor::Update( - const aaesim::open_source::AircraftState &state, const aaesim::open_source::Guidance ¤t_guidance) { - aaesim::open_source::Guidance result; - static const Units::DegreesPerSecondAngularSpeed roll_rate(3.0); // roll rate for guidance (AAES-668) - - // Calculate Psi command and cross-track error if the aircraft is turning - // get the distance based on aircraft position - Units::MetersLength distance_to_go; - Units::UnsignedRadiansAngle course_at_dtg; - m_distance_calculator.CalculateAlongPathDistanceFromPosition(state.GetPositionEnuX(), state.GetPositionEnuY(), - distance_to_go, course_at_dtg); - if (!m_distance_calculator.IsPassedEndOfRoute()) { - result = m_vertical_predictor->Update( - state, current_guidance, - distance_to_go); // calls the 4D Precalculated Descent and issues altitude and speed Guidance - - // get aircraft position based on that calculated distance - Units::MetersLength x_pos; - Units::MetersLength y_pos; - Units::UnsignedRadiansAngle course_at_position; - m_position_calculator.CalculatePositionFromAlongPathDistance(distance_to_go, x_pos, y_pos, course_at_position); - std::vector::size_type traj_index = m_position_calculator.GetCurrentTrajectoryIndex(); - if (traj_index == m_horizontal_path.size() - 1) { - traj_index--; - } - - // error check - Units::SignedRadiansAngle check_cross_track = Units::ToSigned(course_at_position - course_at_dtg); - if (fabs(check_cross_track.value()) > 0.001) { - LOG4CPLUS_WARN(m_logger, "AC" << state.GetUniqueId() << " Course angles from getPosFromPathLength(" - << course_at_position << ") and getPathLengthFromPos (" << course_at_dtg - << ") do not agree. DTG: " << distance_to_go); - } - - Units::FeetLength distToTrajPoint = - distance_to_go - Units::MetersLength(m_horizontal_path[traj_index].m_path_length_cumulative_meters); - Units::SecondsTime timeToTrajPoint = distToTrajPoint / result.m_ground_speed; - - result.m_enu_track_angle = course_at_position; // set the m_path_course command result - - // calculate cross track as difference between actual and precalculated position - Units::MetersLength cross_track = - sqrt(Units::sqr(state.GetPositionEnuX() - x_pos) + Units::sqr(state.GetPositionEnuY() - y_pos)); - - // generate cross-track sign based on distance from turn center and change in m_path_course - double center_dist = sqrt(pow(Units::MetersLength(state.GetPositionEnuX()).value() - - m_horizontal_path[traj_index].m_turn_info.x_position_meters, - 2) + - pow(Units::MetersLength(state.GetPositionEnuY()).value() - - m_horizontal_path[traj_index].m_turn_info.y_position_meters, - 2)); - - // calculate the cross track error based on distance from center point and m_path_course change if turning - if (m_horizontal_path[traj_index].m_segment_type == HorizontalPath::SegmentType::TURN) { - // see if need to roll out - double rollFactor = 1; // dimensionless - if (traj_index > 0 && (m_horizontal_path[traj_index - 1].m_turn_info.radius.value() < 1)) { // next leg is - // straight - Units::SecondsTime timeToBank = m_horizontal_path[traj_index].m_turn_info.bankAngle / roll_rate; - // double timeToBank = h_traj[traj_index].m_turn_info.bankAngle.value() / 0.05235980; - if (timeToTrajPoint <= timeToBank) { - rollFactor = timeToTrajPoint / timeToBank; - } - } - Units::UnsignedRadiansAngle acCourse = Units::UnsignedRadiansAngle( - Units::RadiansAngle(m_horizontal_path[traj_index].m_path_course) + Units::PI_RADIANS_ANGLE); - Units::SignedRadiansAngle courseChange = Units::ToUnsigned(course_at_dtg - acCourse); - // courseChange - positive is left turn, neg is right turn - // if left turn, distance < radius is left of m_path_course, distance > radius is right of m_path_course - // if right turn, distance < radius is right of m_path_course, distance > radius is left of m_path_course - if (courseChange > Units::SignedRadiansAngle(0.0)) // left turn (AC is actually turning right) - { - // added for guidance in turns - result.m_reference_bank_angle = rollFactor * m_horizontal_path[traj_index].m_turn_info.bankAngle; - - if (center_dist < - Units::MetersLength(m_horizontal_path[traj_index].m_turn_info.radius).value()) // left of m_path_course - { - result.m_cross_track_error = cross_track; - } else // right of m_path_course - { - result.m_cross_track_error = -cross_track; - } - } else { - result.m_reference_bank_angle = -rollFactor * m_horizontal_path[traj_index].m_turn_info.bankAngle; - - if (center_dist < Units::MetersLength(m_horizontal_path[traj_index].m_turn_info.radius).value()) { - result.m_cross_track_error = -cross_track; - } else { - result.m_cross_track_error = cross_track; - } - } - - result.m_use_cross_track = true; // redundant - } - // else do straight track trajectory cross-track calculation - else { - if (traj_index + 1 >= m_horizontal_path.size()) { - std::vector::size_type traj_index1 = m_horizontal_path.size() - 2; - LOG4CPLUS_WARN(m_logger, "traj_index reset from " << traj_index << " to " << traj_index1); - traj_index = traj_index1; - } - - // if next segment_type a turn, calculate roll-in time at 3 degrees bank per second - // then calculate distance to next point and apply roll in to guidance - - if (traj_index > 0 && (m_horizontal_path[traj_index - 1].m_turn_info.radius.value() > - 1)) { // roll rate is 3 degrees per second or 0.05235988 radians per second - Units::SecondsTime timeToBank = m_horizontal_path[traj_index - 1].m_turn_info.bankAngle / roll_rate; - if (timeToTrajPoint <= timeToBank) // roll in - { - Units::UnsignedRadiansAngle turnAmount = (m_horizontal_path[traj_index - 1].m_turn_info.q_start - - m_horizontal_path[traj_index - 1].m_turn_info.q_end); - Units::SignedRadiansAngle courseChange = Units::ToUnsigned(turnAmount); - // right turn is positive, left turn is negative - double rollFactor = (timeToTrajPoint / timeToBank); - if (courseChange > Units::ZERO_ANGLE) { // right turn - result.m_reference_bank_angle = - -(1.0 - rollFactor) * m_horizontal_path[traj_index - 1].m_turn_info.bankAngle; - } else { // left turn - result.m_reference_bank_angle = - (1.0 - rollFactor) * m_horizontal_path[traj_index - 1].m_turn_info.bankAngle; - } - } - } - - result.m_cross_track_error = Units::MetersLength(-(Units::MetersLength(state.GetPositionEnuY()).value() - - m_horizontal_path[traj_index + 1].GetYPositionMeters()) * - cos(course_at_dtg) + - (Units::MetersLength(state.GetPositionEnuX()).value() - - m_horizontal_path[traj_index + 1].GetXPositionMeters()) * - sin(course_at_dtg)); - } - result.m_use_cross_track = true; - } // not passed end of route - else { - // off the guidance path so no guidance can be calculated. Set the references to reasonable values from current - // state. - result.m_reference_altitude = Units::FeetLength(state.GetAltitudeMsl()); - result.m_ground_speed = Units::FeetPerSecondSpeed(state.GetGroundSpeed()); - result.m_ias_command = m_vertical_predictor->GetIasAtEndOfRoute(); - result.SetValid(true); - } - - return result; -} - -void EuclideanTrajectoryPredictor::DefineRoute() { - // loop to process all of the waypoint positions - for (unsigned int loop = 0; loop < m_waypoint_vector.size(); ++loop) { - m_waypoint_vector[loop].m_course_angle = Units::ToUnsigned(m_waypoint_vector[loop].m_course_angle); - } -} - -void EuclideanTrajectoryPredictor::AdjustConstraints(Units::Speed start_speed) { - // if the constraint for first leg is "unconstrained" then set to start speed - if (m_waypoint_vector[m_waypoint_vector.size() - 1].m_precalc_constraints.constraint_speedHi >= - Waypoint::MAX_SPEED_CONSTRAINT - Units::MetersPerSecondSpeed(1)) - m_waypoint_vector[m_waypoint_vector.size() - 1].m_precalc_constraints.constraint_speedHi = start_speed; - for (auto loop = m_waypoint_vector.size() - 1; loop > 0; --loop) { - Units::MetersLength H1_High = m_waypoint_vector[loop].m_precalc_constraints.constraint_altHi; - Units::MetersLength H0_High = m_waypoint_vector[loop - 1].m_precalc_constraints.constraint_altHi; - if (H1_High < H0_High) { - m_waypoint_vector[loop - 1].m_precalc_constraints.constraint_altHi = H1_High; - } - Units::MetersPerSecondSpeed s1_high = m_waypoint_vector[loop].m_precalc_constraints.constraint_speedHi; - Units::MetersPerSecondSpeed s0_high = m_waypoint_vector[loop - 1].m_precalc_constraints.constraint_speedHi; - if (s1_high < s0_high) { - m_waypoint_vector[loop - 1].m_precalc_constraints.constraint_speedHi = s1_high; - } - } - - for (auto loop = 1; loop < m_waypoint_vector.size(); ++loop) { - Units::MetersLength H1_Low = m_waypoint_vector[loop].m_precalc_constraints.constraint_altLow; - Units::MetersLength H0_Low = m_waypoint_vector[loop - 1].m_precalc_constraints.constraint_altLow; - if (H1_Low < H0_Low) { - m_waypoint_vector[loop].m_precalc_constraints.constraint_altLow = H0_Low; - } - } -} - -const AircraftIntent &EuclideanTrajectoryPredictor::GetAircraftIntent() const { return m_aircraft_intent; } - -const vector &EuclideanTrajectoryPredictor::GetHorizontalPath() const { return m_horizontal_path; } - -void EuclideanTrajectoryPredictor::UpdateWeatherPrediction( - aaesim::open_source::WeatherPrediction &weather, - const std::shared_ptr &position_converter) const {} - -void EuclideanTrajectoryPredictor::SetAtmosphere(std::shared_ptr atmosphere) { - m_atmosphere = atmosphere; - m_vertical_predictor->SetAtmosphere(atmosphere); -} - -const std::vector EuclideanTrajectoryPredictor::EstimateHorizontalTrajectory( - const aaesim::open_source::WeatherPrediction &weather_prediction) { - SetAtmosphere(weather_prediction.getAtmosphere()); - CalculateHorizontalTrajectory(FIRST_PASS); - return GetHorizontalPath(); -} diff --git a/Public/EuclideanWaypointMonitor.cpp b/Public/EuclideanWaypointMonitor.cpp deleted file mode 100644 index 3539271..0000000 --- a/Public/EuclideanWaypointMonitor.cpp +++ /dev/null @@ -1,77 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include "public/EuclideanWaypointMonitor.h" - -#include - -#include "public/AircraftCalculations.h" - -aaesim::open_source::EuclideanWaypointMonitor::EuclideanWaypointMonitor( - const aaesim::LatitudeLongitudePoint &lat_lon_point) - : m_point_to_monitor( - aaesim::LatitudeLongitudePoint::CreateFromGeolibPrimitive(lat_lon_point.GetGeolibPrimitiveLLPoint())) {}; - -void aaesim::open_source::EuclideanWaypointMonitor::Update(const aaesim::LatitudeLongitudePoint &position, - const Units::SignedAngle &ground_course_enu) { - // Treat the lat/lon values as y/x values - Units::Length current_position_x(Units::infinity()), current_position_y(Units::infinity()); - PerformFakeTranslationToEuclidean(position, current_position_x, current_position_y); - - Units::Length predicted_next_x(Units::infinity()), predicted_next_y(Units::infinity()); - const LatitudeLongitudePoint projected_lat_lon = - position.ProjectDistanceAlongCourse(Units::MetersLength(500), ground_course_enu); - PerformFakeTranslationToEuclidean(projected_lat_lon, predicted_next_x, predicted_next_y); - - Units::Length monitored_point_x(Units::infinity()), monitored_point_y(Units::infinity()); - PerformFakeTranslationToEuclidean(m_point_to_monitor, monitored_point_x, monitored_point_y); - - const Units::SignedAngle angle = - AircraftCalculations::ComputeAngleBetweenVectors(current_position_x, current_position_y, predicted_next_x, - predicted_next_y, monitored_point_x, monitored_point_y); - m_is_passed_waypoint = Units::abs(angle) > Units::PI_RADIANS_ANGLE / 2; -} - -void aaesim::open_source::EuclideanWaypointMonitor::PerformFakeTranslationToEuclidean( - const aaesim::LatitudeLongitudePoint &lat_lon_point, Units::Length &x, Units::Length &y) { - x = Units::MetersLength(Units::SignedRadiansAngle(lat_lon_point.GetLongitude()).value()); - y = Units::MetersLength(Units::SignedRadiansAngle(lat_lon_point.GetLatitude()).value()); -} - -std::shared_ptr - aaesim::open_source::EuclideanWaypointMonitor::OfWgs84PrecalcWaypoint( - const aaesim::open_source::Wgs84PrecalcWaypoint &waypoint) { - return std::make_shared( - aaesim::open_source::EuclideanWaypointMonitor(waypoint.m_position)); -} - -std::shared_ptr - aaesim::open_source::EuclideanWaypointMonitor::OfEllipsoidalPoint( - const aaesim::LatitudeLongitudePoint &ellipsoidal_point) { - return std::make_shared( - aaesim::open_source::EuclideanWaypointMonitor(ellipsoidal_point)); -} - -std::shared_ptr - aaesim::open_source::EuclideanWaypointMonitor::OfGeodeticPoint( - const EarthModel::GeodeticPosition &geodetic_point) { - const LatitudeLongitudePoint monitor_this(geodetic_point.latitude, geodetic_point.longitude); - return std::make_shared( - aaesim::open_source::EuclideanWaypointMonitor(monitor_this)); -} diff --git a/Public/FlightEnvelopeSpeedLimiter.cpp b/Public/FlightEnvelopeSpeedLimiter.cpp deleted file mode 100644 index bb09912..0000000 --- a/Public/FlightEnvelopeSpeedLimiter.cpp +++ /dev/null @@ -1,92 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include "public/FlightEnvelopeSpeedLimiter.h" - -using namespace aaesim::open_source; - -const Units::Speed FlightEnvelopeSpeedLimiter::MINIMUM_IAS_LIMIT = Units::KnotsSpeed(150); -const BoundedValue FlightEnvelopeSpeedLimiter::MINIMUM_MACH_LIMIT = BoundedValue(0.6); - -FlightEnvelopeSpeedLimiter::FlightEnvelopeSpeedLimiter( - const aaesim::open_source::bada_utils::FlapSpeeds &flap_speeds, - const aaesim::open_source::bada_utils::FlightEnvelope &flight_envelope) - : m_flap_speeds(flap_speeds), m_flight_envelope(flight_envelope) {} - -Units::Speed FlightEnvelopeSpeedLimiter::LimitSpeedCommand( - const Units::Speed previous_ias_speed_command, const Units::Speed current_ias_speed_command, - const Units::Speed reference_velocity_mps, const Units::Length speed_quantization_distance, - const Units::Length distance_to_end_of_route, const Units::Length current_altitude, - const aaesim::open_source::bada_utils::FlapConfiguration flap_configuration) { - Units::Speed limited_ias = Units::max(MINIMUM_IAS_LIMIT, current_ias_speed_command); - - switch (flap_configuration) { - case bada_utils::FlapConfiguration::TAKEOFF: - if (limited_ias < m_flap_speeds.cas_takeoff_minimum) { - return m_flap_speeds.cas_takeoff_minimum; - } - return limited_ias; - case bada_utils::FlapConfiguration::INITIAL_CLIMB: - if (limited_ias < m_flap_speeds.cas_climb_minimum) { - return m_flap_speeds.cas_climb_minimum; - } - return limited_ias; - case bada_utils::FlapConfiguration::CRUISE: - if (limited_ias < m_flap_speeds.cas_cruise_minimum) { - return m_flap_speeds.cas_cruise_minimum; - } - return limited_ias; - case bada_utils::FlapConfiguration::APPROACH: - if (limited_ias < m_flap_speeds.cas_approach_minimum) { - return m_flap_speeds.cas_approach_minimum; - } else if (limited_ias > m_flap_speeds.cas_approach_maximum) { - return m_flap_speeds.cas_approach_maximum; - } - return limited_ias; - case bada_utils::FlapConfiguration::LANDING: - if (limited_ias < m_flap_speeds.cas_landing_minimum) { - return m_flap_speeds.cas_landing_minimum; - } else if (limited_ias > m_flap_speeds.cas_landing_maximum) { - return m_flap_speeds.cas_landing_maximum; - } - return limited_ias; - case bada_utils::FlapConfiguration::GEAR_DOWN: - if (limited_ias < m_flap_speeds.cas_gear_out_minimum) { - return m_flap_speeds.cas_gear_out_minimum; - } else if (limited_ias > m_flap_speeds.cas_gear_out_maximum) { - return m_flap_speeds.cas_gear_out_maximum; - } - return limited_ias; - default: - return limited_ias; - } -} - -BoundedValue FlightEnvelopeSpeedLimiter::LimitMachCommand( - const BoundedValue &previous_reference_speed_command_mach, - const BoundedValue ¤t_mach_command, const BoundedValue &nominal_mach, - const Units::Mass ¤t_mass, const Units::Length ¤t_altitude, - const WeatherPrediction &weather_prediction) { - if (current_mach_command > m_flight_envelope.M_mo) { - return BoundedValue(m_flight_envelope.M_mo); - } else if (current_mach_command < MINIMUM_MACH_LIMIT) { - return MINIMUM_MACH_LIMIT; - } - return current_mach_command; -} diff --git a/Public/FmsWaypointSequenceFile.cpp b/Public/FmsWaypointSequenceFile.cpp deleted file mode 100644 index d671efa..0000000 --- a/Public/FmsWaypointSequenceFile.cpp +++ /dev/null @@ -1,84 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// 2022 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include "public/FmsWaypointSequenceFile.h" - -using namespace aaesim::open_source; - -log4cplus::Logger FmsWaypointSequenceFile::logger = - log4cplus::Logger::getInstance(LOG4CPLUS_TEXT("FmsWaypointSequenceFile")); - -FmsWaypointSequenceFile::FmsWaypointSequenceFile() : OutputHandler("", "_fms_waypoint_sequence.csv") { - m_fms_waypoint_data.reserve(10000); -} - -void FmsWaypointSequenceFile::Finish() { - if (!m_fms_waypoint_data.empty()) { - - os.open(filename.c_str()); - - if (!os.is_open()) { - std::string error_msg = "Cannot open " + filename; - LOG4CPLUS_FATAL(FmsWaypointSequenceFile::logger, error_msg); - return; - } - - os.set_delimiter(',', ","); - - // Header - os << "iteration"; - os << "acid"; - os << "time_sec"; - os << "waypoint_name"; - os << NEWLINE; - os.flush(); - - // Important for outputting double data. - os.get_ofstream().precision(12); - - for (const auto &ix : m_fms_waypoint_data) { - os << ix.iteration_number; - os << ix.acid; - os << Units::SecondsTime(ix.simulation_time).value(); - os << ix.waypoint_name; - os << NEWLINE; - os.flush(); - } - - os.close(); - } - - m_fms_waypoint_data.clear(); - m_finished = true; -} - -void FmsWaypointSequenceFile::Gather(const int iteration_number, const SimpleAircraft &aircraft) { - for (const auto &pair : aircraft.GetFms()->GetSequencedWaypoints()) { - auto time = pair.first; - auto wgs84_waypoint = pair.second; - - FmsWaypointData waypoint_data; - waypoint_data.iteration_number = iteration_number; - waypoint_data.acid = aircraft.GetAircraftId(); - waypoint_data.simulation_time = time; - waypoint_data.waypoint_name.assign(wgs84_waypoint.m_name); - - m_fms_waypoint_data.push_back(waypoint_data); - } -} diff --git a/Public/ForeWindReader.cpp b/Public/ForeWindReader.cpp deleted file mode 100644 index 8980dc2..0000000 --- a/Public/ForeWindReader.cpp +++ /dev/null @@ -1,63 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// 2022 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -/* - * ForeWindReader.cpp - * - * Created on: Mar 23, 2019 - * Author: klewis - */ - -#include "public/ForeWindReader.h" - -namespace testvector { - -ForeWindReader::ForeWindReader(std::string file_name, int header_lines) : -DataReader(file_name, header_lines, 0) { -} - -ForeWindReader::ForeWindReader(std::shared_ptr input_stream, int header_lines) : - DataReader(input_stream, header_lines, 0) { -} - -ForeWindReader::~ForeWindReader() { -} - -bool ForeWindReader::ReadWind(WeatherPrediction &weather_prediction) { - const int ALTITUDE_COUNT(5); - - weather_prediction.east_west.SetBounds(1, ALTITUDE_COUNT); - weather_prediction.north_south.SetBounds(1, ALTITUDE_COUNT); - for (int i = 1; i <= ALTITUDE_COUNT; i++) { - Advance(); - if (GetColumnCount() == 0) { - // end of stream - return false; - } - Units::MetersLength altitude(GetDouble(0)); - Units::MetersPerSecondSpeed u(GetDouble(1)); - Units::MetersPerSecondSpeed v(GetDouble(2)); - weather_prediction.east_west.Set(i, altitude, u); - weather_prediction.north_south.Set(i, altitude, v); - } - - return true; -} - -} // namespace testvector diff --git a/Public/FullWindTrueWeatherOperator.cpp b/Public/FullWindTrueWeatherOperator.cpp deleted file mode 100644 index dfadfce..0000000 --- a/Public/FullWindTrueWeatherOperator.cpp +++ /dev/null @@ -1,40 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include "public/FullWindTrueWeatherOperator.h" - -void aaesim::open_source::FullWindTrueWeatherOperator::CalculateEnvironmentalWind( - const EarthModel::GeodeticPosition &position, const Units::Length &altitude_msl) { - m_true_weather->LoadConditionsAt(position.latitude, position.longitude, altitude_msl); - - // Get Winds and Wind Gradients at altitude - m_true_weather->east_west().CalculateWindGradientAtAltitude(altitude_msl, m_wind_speed_east, - m_vertical_derivative_east); - m_true_weather->north_south().CalculateWindGradientAtAltitude(altitude_msl, m_wind_speed_north, - m_vertical_derivative_north); -} - -Units::Speed aaesim::open_source::FullWindTrueWeatherOperator::GetWindSpeedEast() const { return m_wind_speed_east; } -Units::Speed aaesim::open_source::FullWindTrueWeatherOperator::GetWindSpeedNorth() const { return m_wind_speed_north; } -Units::Frequency aaesim::open_source::FullWindTrueWeatherOperator::GetWindSpeedVerticalDerivativeEast() const { - return m_vertical_derivative_east; -} -Units::Frequency aaesim::open_source::FullWindTrueWeatherOperator::GetWindSpeedVerticalDerivativeNorth() const { - return m_vertical_derivative_north; -} diff --git a/Public/GeolibUtils.cpp b/Public/GeolibUtils.cpp deleted file mode 100644 index 60f00c6..0000000 --- a/Public/GeolibUtils.cpp +++ /dev/null @@ -1,366 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include "public/GeolibUtils.h" - -#include -#include -#include -#include -#include - -using namespace aaesim; -using namespace geolib_idealab; - -LatitudeLongitudePoint GeolibUtils::CalculateNewPoint(const LatitudeLongitudePoint &start_point, - const Units::Length &distance, - const Units::SignedAngle &course_enu) { - const Units::UnsignedRadiansAngle angle_from_true_north = ConvertCourseFromEnuToNed(course_enu); - - // Project self to a new location and return that location - LLPoint destination_calculated; - ErrorSet error_set = direct(start_point.GetGeolibPrimitiveLLPoint(), angle_from_true_north.value(), - Units::NauticalMilesLength(distance).value(), &destination_calculated, GEOLIB_EPSILON); - const bool geolib_fail = !GeolibUtils::IsSuccess(error_set); - if (geolib_fail) { - LOG4CPLUS_ERROR(m_logger, m_basic_error_message << formatErrorMessage(error_set)); - throw std::runtime_error(m_basic_error_message); - } - return LatitudeLongitudePoint(Units::RadiansAngle(destination_calculated.latitude), - Units::RadiansAngle(destination_calculated.longitude)); -} - -std::pair GeolibUtils::CalculateRelationshipBetweenPoints( - const LatitudeLongitudePoint &start_point, const LatitudeLongitudePoint &end_point) { - auto info = CalculateRelationshipBetweenPointsExpanded(start_point, end_point); - return std::make_pair(std::get<0>(info), std::get<1>(info)); -} - -const Units::UnsignedAngle GeolibUtils::ConvertCourseFromEnuToNed(const Units::SignedAngle &course_enu) { - const double cos_term = cos(course_enu); - const double sin_term = sin(course_enu); - const double arctan2_result = std::atan2(cos_term, sin_term); - const Units::SignedRadiansAngle tmp(arctan2_result); - return Units::UnsignedAngle(tmp); -} - -const Units::SignedAngle GeolibUtils::ConvertCourseFromNedToEnu(const Units::UnsignedAngle &course_ned) { - const double cos_term = cos(Units::SignedRadiansAngle(course_ned)); - const double sin_term = sin(Units::SignedRadiansAngle(course_ned)); - const double arctan2_result = std::atan2(cos_term, sin_term); - return Units::SignedRadiansAngle(arctan2_result); -} - -const LineOnEllipsoid GeolibUtils::CreateLineOnEllipsoid(const LatitudeLongitudePoint &start_point, - const LatitudeLongitudePoint &end_point) { - Geodesic geolib_geodesic; - ErrorSet error_set = createGeo(&geolib_geodesic, start_point.GetGeolibPrimitiveLLPoint(), - end_point.GetGeolibPrimitiveLLPoint(), LineType::SEGMENT, GEOLIB_EPSILON); - const bool geolib_fail = !GeolibUtils::IsSuccess(error_set); - if (geolib_fail) { - LOG4CPLUS_ERROR(m_logger, m_basic_error_message << formatErrorMessage(error_set)); - throw std::runtime_error(m_basic_error_message); - } - - LineOnEllipsoid line_on_ellipsoid(geolib_geodesic); - return line_on_ellipsoid; -} - -const ArcOnEllipsoid GeolibUtils::CreateArcOnEllipsoid(const LatitudeLongitudePoint &start_point, - const LatitudeLongitudePoint &end_point, - const LatitudeLongitudePoint ¢er_point, - const geolib_idealab::ArcDirection &arc_direction) { - Arc arc_primitive; - ErrorSet error_set = - createArc(&arc_primitive, center_point.GetGeolibPrimitiveLLPoint(), start_point.GetGeolibPrimitiveLLPoint(), - end_point.GetGeolibPrimitiveLLPoint(), arc_direction, GEOLIB_TOLERANCE, GEOLIB_EPSILON); - const bool geolib_fail = !GeolibUtils::IsSuccess(error_set); - if (geolib_fail) { - LOG4CPLUS_ERROR(m_logger, m_basic_error_message << formatErrorMessage(error_set)); - throw std::runtime_error(m_basic_error_message); - } - - ArcOnEllipsoid arc_on_ellipsoid(arc_primitive); - return arc_on_ellipsoid; -} - -const std::tuple > - GeolibUtils::CalculateLineLineIntersectionPoint(const LineOnEllipsoid &line1, const LineOnEllipsoid &line2) { - // Line-Line intersection - double crs31, distance_line1_start_to_intx_point, crs32, distance_line2_start_to_intx_point; - geolib_idealab::LLPoint intersection_point; - ErrorSet error_set = - geoIntx(line1.GetStartPoint().GetGeolibPrimitiveLLPoint(), line1.GetEndPoint().GetGeolibPrimitiveLLPoint(), - line1.GetLineType(), &crs31, &distance_line1_start_to_intx_point, - line2.GetStartPoint().GetGeolibPrimitiveLLPoint(), line2.GetEndPoint().GetGeolibPrimitiveLLPoint(), - line2.GetLineType(), &crs32, &distance_line2_start_to_intx_point, &intersection_point, - GEOLIB_TOLERANCE, GEOLIB_EPSILON); - - if (HasErrorBitSet(error_set, ErrorCodes::NO_INTERSECTION_ERR)) { - LOG4CPLUS_WARN(m_logger, formatErrorMessage(error_set)); - LatitudeLongitudePoint no_point{}; - std::vector no_vector{}; - return std::make_tuple(false, no_point, no_vector); - } - - bool geolib_failed = !GeolibUtils::IsSuccess(error_set); - if (geolib_failed) { - LOG4CPLUS_ERROR(m_logger, m_basic_error_message << formatErrorMessage(error_set)); - throw std::runtime_error(m_basic_error_message); - } - - std::vector distances_to_intersection_point = { - Units::NauticalMilesLength(distance_line1_start_to_intx_point), - Units::NauticalMilesLength(distance_line2_start_to_intx_point)}; - return std::make_tuple(true, LatitudeLongitudePoint::CreateFromGeolibPrimitive(intersection_point), - distances_to_intersection_point); -} -const bool GeolibUtils::IsPointOnLine(const LineOnEllipsoid &line, const LatitudeLongitudePoint &test_point) { - geolib_idealab::ErrorSet error_set{ErrorCodes::SUCCESS}; - int ret = geolib_idealab::ptIsOnGeo( - line.GetStartPoint().GetGeolibPrimitiveLLPoint(), line.GetEndPoint().GetGeolibPrimitiveLLPoint(), - test_point.GetGeolibPrimitiveLLPoint(), line.GetLineType(), &error_set, GEOLIB_TOLERANCE, GEOLIB_EPSILON); - const bool geolib_fail = !GeolibUtils::IsSuccess(error_set); - if (geolib_fail) { - LOG4CPLUS_ERROR(m_logger, m_basic_error_message << formatErrorMessage(error_set)); - throw std::runtime_error(m_basic_error_message); - } - - bool return_this = (ret != 0); - return return_this; -} -const bool GeolibUtils::IsPointOnArc(const ArcOnEllipsoid &arc, const LatitudeLongitudePoint &test_point) { - geolib_idealab::ErrorSet error_set{ErrorCodes::SUCCESS}; - int ret = geolib_idealab::ptIsOnArc( - arc.GetCenterPoint().GetGeolibPrimitiveLLPoint(), Units::NauticalMilesLength(arc.GetRadius()).value(), - Units::UnsignedRadiansAngle(GeolibUtils::ConvertCourseFromEnuToNed(arc.GetStartAzimuthEnu())).value(), - Units::UnsignedRadiansAngle(GeolibUtils::ConvertCourseFromEnuToNed(arc.GetEndAzimuthEnu())).value(), - arc.GetArcDirection(), test_point.GetGeolibPrimitiveLLPoint(), &error_set, GEOLIB_TOLERANCE, GEOLIB_EPSILON); - const bool geolib_fail = !GeolibUtils::IsSuccess(error_set); - if (geolib_fail) { - LOG4CPLUS_ERROR(m_logger, m_basic_error_message << formatErrorMessage(error_set)); - throw std::runtime_error(m_basic_error_message); - } - - bool return_this = (ret == 1); - return return_this; -} - -std::tuple - GeolibUtils::FindNearestPointOnLineUsingPerpendicularProjection(const LineOnEllipsoid &line, - const LatitudeLongitudePoint &point_not_on_line) { - geolib_idealab::LLPoint point_on_line; - double crs_ned_to_line_from_point_not_on_line, distance_along_perpendicular_projection; - ErrorSet error_set = projectToGeo( - line.GetStartPoint().GetGeolibPrimitiveLLPoint(), - Units::UnsignedRadiansAngle(ConvertCourseFromEnuToNed(line.GetForwardCourseEnuAtStartPoint())).value(), - point_not_on_line.GetGeolibPrimitiveLLPoint(), &point_on_line, &crs_ned_to_line_from_point_not_on_line, - &distance_along_perpendicular_projection, GEOLIB_TOLERANCE, GEOLIB_EPSILON); - - const bool geolib_fail = !GeolibUtils::IsSuccess(error_set); - if (geolib_fail) { - LOG4CPLUS_ERROR(m_logger, m_basic_error_message << formatErrorMessage(error_set)); - throw std::runtime_error(m_basic_error_message); - } - - return std::make_tuple( - LatitudeLongitudePoint::CreateFromGeolibPrimitive(point_on_line), - ConvertCourseFromNedToEnu(Units::UnsignedRadiansAngle(crs_ned_to_line_from_point_not_on_line)), - Units::NauticalMilesLength(distance_along_perpendicular_projection)); -} - -const bool GeolibUtils::IsPointInsideArcSegment(const ArcOnEllipsoid &finite_arc, - const LatitudeLongitudePoint &test_point) { - geolib_idealab::ErrorSet error_set{ErrorCodes::SUCCESS}; - int ret = geolib_idealab::ptIsInsideArc( - finite_arc.GetCenterPoint().GetGeolibPrimitiveLLPoint(), - Units::NauticalMilesLength(finite_arc.GetRadius()).value(), - Units::UnsignedRadiansAngle(GeolibUtils::ConvertCourseFromEnuToNed(finite_arc.GetStartAzimuthEnu())).value(), - Units::UnsignedRadiansAngle(GeolibUtils::ConvertCourseFromEnuToNed(finite_arc.GetEndAzimuthEnu())).value(), - finite_arc.GetArcDirection(), test_point.GetGeolibPrimitiveLLPoint(), &error_set, GEOLIB_TOLERANCE, - GEOLIB_EPSILON); - const bool geolib_fail = !GeolibUtils::IsSuccess(error_set); - if (geolib_fail) { - LOG4CPLUS_ERROR(m_logger, m_basic_error_message << formatErrorMessage(error_set)); - throw std::runtime_error(m_basic_error_message); - } - - bool return_this = (ret != 0); - return return_this; -} - -std::pair GeolibUtils::FindNearestPointOnArcUsingPerpendiculorProjection( - const ArcOnEllipsoid &arc, const LatitudeLongitudePoint &point_not_on_arc) { - // line from arc center to point_not_on_arc - LineOnEllipsoid radius_line_through_point = - LineOnEllipsoid::CreateFromPoints(arc.GetCenterPoint(), point_not_on_arc); - - // Get new point at arc-radius distance from center along the line's course - LatitudeLongitudePoint point_on_arc = GeolibUtils::CalculateNewPoint( - arc.GetCenterPoint(), arc.GetRadius(), radius_line_through_point.GetForwardCourseEnuAtStartPoint()); - - // Make sure it is on the arc - bool is_on_arc = arc.IsPointOnShape(point_on_arc); - - return std::pair(is_on_arc, point_on_arc); -} - -bool GeolibUtils::ArePointsMathematicallyEqual(const LatitudeLongitudePoint &point1, - const LatitudeLongitudePoint &point2) { - int ret = geolib_idealab::ptsAreSame(point1.GetGeolibPrimitiveLLPoint(), point2.GetGeolibPrimitiveLLPoint(), - GEOLIB_TOLERANCE); - - bool return_this = (ret != 0); - return return_this; -} - -std::pair GeolibUtils::CreateArcTangentToTwoLines(const LineOnEllipsoid &line1, - const LineOnEllipsoid &line2, - const Units::Length &required_radius) { - LLPoint arc_center_point; - LLPoint arc_start_point; - LLPoint arc_end_point; - geolib_idealab::ArcDirection arc_direction; - ErrorSet error_set = arcTanToTwoGeos( - line1.GetStartPoint().GetGeolibPrimitiveLLPoint(), - Units::UnsignedRadiansAngle(ConvertCourseFromEnuToNed(line1.GetForwardCourseEnuAtStartPoint())).value(), - line2.GetStartPoint().GetGeolibPrimitiveLLPoint(), - Units::UnsignedRadiansAngle(ConvertCourseFromEnuToNed(line2.GetForwardCourseEnuAtStartPoint())).value(), - Units::NauticalMilesLength(required_radius).value(), &arc_center_point, &arc_start_point, &arc_end_point, - &arc_direction, GEOLIB_TOLERANCE, GEOLIB_EPSILON); - - bool one_arc_found = true; - ArcOnEllipsoid arc_to_return; - if (GeolibUtils::IsSuccess(error_set)) { - arc_to_return = - CreateArcOnEllipsoid(LatitudeLongitudePoint::CreateFromGeolibPrimitive(arc_start_point), - LatitudeLongitudePoint::CreateFromGeolibPrimitive(arc_end_point), - LatitudeLongitudePoint::CreateFromGeolibPrimitive(arc_center_point), arc_direction); - } else if (error_set & ErrorCodes::NO_TANGENT_ARC_ERR) { - // this is okay, just return with no formed arc - one_arc_found = false; - } else { - LOG4CPLUS_ERROR(m_logger, m_basic_error_message << formatErrorMessage(error_set)); - throw std::runtime_error(m_basic_error_message); - } - - return std::pair(one_arc_found, arc_to_return); -} - -ArcOnEllipsoid GeolibUtils::CreateArcFromInboundShapeAndEndPoint(const ShapeOnEllipsoid *inbound_shape, - const LatitudeLongitudePoint &end_point) { - geolib_idealab::Arc calculated_arc; - ErrorSet error_set = arcFromStartAndEnd( - inbound_shape->GetEndPoint().GetGeolibPrimitiveLLPoint(), - Units::UnsignedRadiansAngle(ConvertCourseFromEnuToNed(inbound_shape->GetForwardCourseEnuAtEndPoint())).value(), - end_point.GetGeolibPrimitiveLLPoint(), &calculated_arc, GEOLIB_TOLERANCE, GEOLIB_EPSILON); - - if (GeolibUtils::IsSuccess(error_set)) { - return ArcOnEllipsoid(calculated_arc); - } - - LOG4CPLUS_ERROR(m_logger, m_basic_error_message << formatErrorMessage(error_set)); - throw std::runtime_error(m_basic_error_message); -} - -std::vector > GeolibUtils::CalculateLineArcIntersectionPoints( - const LineOnEllipsoid &line, const ArcOnEllipsoid &arc) { - LLPointPair intersection_pairs_on_circle; - int number_of_intersections = INT32_MIN; - ErrorSet error_set = geoArcIntx( - line.GetStartPoint().GetGeolibPrimitiveLLPoint(), - Units::UnsignedRadiansAngle(ConvertCourseFromEnuToNed(line.GetForwardCourseEnuAtStartPoint())).value(), - arc.GetCenterPoint().GetGeolibPrimitiveLLPoint(), Units::NauticalMilesLength(arc.GetRadius()).value(), - intersection_pairs_on_circle, &number_of_intersections, GEOLIB_TOLERANCE, GEOLIB_EPSILON); - - std::vector > return_this; - if (GeolibUtils::IsSuccess(error_set)) { - if (number_of_intersections == 0) { - // no intersections found - auto no_intersection_return = { - std::make_pair(false, LatitudeLongitudePoint(Units::ZERO_ANGLE, Units::ZERO_ANGLE))}; - return no_intersection_return; - } else { - for (auto i = 0; i < number_of_intersections; ++i) { - const LatitudeLongitudePoint intx_point = - LatitudeLongitudePoint::CreateFromGeolibPrimitive(intersection_pairs_on_circle[i]); - const bool is_point_on_line = line.IsPointOnShape(intx_point); - const bool is_point_on_arc = arc.IsPointOnShape(intx_point); - return_this.push_back(std::make_pair(is_point_on_arc && is_point_on_line, intx_point)); - } - } - } else { - LOG4CPLUS_ERROR(m_logger, m_basic_error_message << formatErrorMessage(error_set)); - throw std::runtime_error(m_basic_error_message); - } - - return return_this; -} -const ArcOnEllipsoid GeolibUtils::CreateFullCircleOnEllipsoid(const LatitudeLongitudePoint ¢er_point, - const Units::Length &arc_radius, - const geolib_idealab::ArcDirection &arc_direction) { - geolib_idealab::Arc calculated_arc; - LatitudeLongitudePoint start_point = center_point.ProjectDistanceAlongCourse(arc_radius, Units::ZERO_ANGLE); - LLPoint end_point = start_point.GetGeolibPrimitiveLLPoint(); - ErrorSet error_set = - arcEndFromStartAndCenter(start_point.GetGeolibPrimitiveLLPoint(), 0, center_point.GetGeolibPrimitiveLLPoint(), - arc_direction, end_point, &calculated_arc, GEOLIB_TOLERANCE, GEOLIB_EPSILON); - - ArcOnEllipsoid arc_to_return; - if (GeolibUtils::IsSuccess(error_set)) { - arc_to_return = ArcOnEllipsoid(calculated_arc); - } else { - LOG4CPLUS_ERROR(m_logger, m_basic_error_message << formatErrorMessage(error_set)); - throw std::runtime_error(m_basic_error_message); - } - - return arc_to_return; -} - -LineOnEllipsoid GeolibUtils::CreateLineOfZeroLength(const LatitudeLongitudePoint &location, - const Units::SignedAngle &course_enu) { - geolib_idealab::Geodesic zero_length_geodesic; - zero_length_geodesic.length = 0; - zero_length_geodesic.startPoint = location.GetGeolibPrimitiveLLPoint(); - zero_length_geodesic.endPoint = zero_length_geodesic.startPoint; - zero_length_geodesic.startAz = Units::UnsignedRadiansAngle(ConvertCourseFromEnuToNed(course_enu)).value(); - zero_length_geodesic.endAz = zero_length_geodesic.startAz; - zero_length_geodesic.lineType = SEGMENT; - return LineOnEllipsoid(zero_length_geodesic); -} - -std::tuple - GeolibUtils::CalculateRelationshipBetweenPointsExpanded(const LatitudeLongitudePoint &start_point, - const LatitudeLongitudePoint &end_point) { - double start_crs_radians_ned_unsigned = DBL_MIN; - double end_course_radians_ned_unsigned = DBL_MIN; - double distance_nm = DBL_MIN; - ErrorSet error_set = - inverse(start_point.GetGeolibPrimitiveLLPoint(), end_point.GetGeolibPrimitiveLLPoint(), - &start_crs_radians_ned_unsigned, &end_course_radians_ned_unsigned, &distance_nm, GEOLIB_EPSILON); - const bool geolib_fail = !GeolibUtils::IsSuccess(error_set); - if (geolib_fail) { - LOG4CPLUS_ERROR(m_logger, m_basic_error_message << formatErrorMessage(error_set)); - throw std::runtime_error(m_basic_error_message); - } - - return std::make_tuple(Units::NauticalMilesLength(distance_nm), - ConvertCourseFromNedToEnu(Units::UnsignedRadiansAngle(start_crs_radians_ned_unsigned)), - ConvertCourseFromNedToEnu(Units::UnsignedRadiansAngle(end_course_radians_ned_unsigned))); -} diff --git a/Public/Guidance.cpp b/Public/Guidance.cpp deleted file mode 100644 index dc847ae..0000000 --- a/Public/Guidance.cpp +++ /dev/null @@ -1,44 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// 2023 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include "public/Guidance.h" -#include - -using namespace aaesim::open_source; - -Guidance::Guidance() { - m_ias_command = Units::ZERO_SPEED; - m_ground_speed = Units::ZERO_SPEED; - m_vertical_speed = Units::ZERO_SPEED; - m_reference_altitude = Units::ZERO_LENGTH; - m_cross_track_error = Units::ZERO_LENGTH; - m_enu_track_angle = Units::ZERO_ANGLE; - m_reference_bank_angle = Units::ZERO_ANGLE; - m_mach_command = 0; - - m_use_cross_track = false; - m_valid = false; -} - -Guidance::~Guidance() {} - -int Guidance::GetIasCommandIntegerKnots() const { - double result = round(Units::KnotsSpeed(m_ias_command).value()); - return (int)result; -} diff --git a/Public/HfpReader.cpp b/Public/HfpReader.cpp deleted file mode 100644 index ade6ad9..0000000 --- a/Public/HfpReader.cpp +++ /dev/null @@ -1,150 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This is the copyright work of The MITRE Corporation, and was produced -// for the U. S. Government under Contract Number DTFAWA-10-C-00080, and -// is subject to Federal Aviation Administration Acquisition Management -// System Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV -// (Oct. 1996). No other use other than that granted to the U. S. -// Government, or to those acting on behalf of the U. S. Government, -// under that Clause is authorized without the express written -// permission of The MITRE Corporation. For further information, please -// contact The MITRE Corporation, Contracts Office, 7515 Colshire Drive, -// McLean, VA 22102-7539, (703) 983-6000. -// -// Copyright 2020 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -/* - * HfpReader.cpp - * - * Created on: Sep 5, 2019 - * Author: klewis - */ - -#include "public/HfpReader.h" -#include - -using namespace std; - -namespace testvector { - -HfpReader::HfpReader(std::string file_name, int header_lines) : - DataReader(file_name, 0, 0) { - SetColumnIndexesFromHeader(header_lines); -} - -HfpReader::HfpReader(std::shared_ptr input_stream, int header_lines) : - DataReader(input_stream, 0, 0) { - SetColumnIndexesFromHeader(header_lines); -} - -HfpReader::~HfpReader() { -} - -void HfpReader::SetColumnIndexesFromHeader(const int header_lines) { - if (header_lines < 1) { - throw logic_error("Headers are required for HfpReader."); - } - int line_number(0); - Advance(); // first header line - // Meaningful HPF headers are at least 4 characters; first column is normally 5. - while (GetString(0).length() < 3 && line_number < header_lines) { - line_number++; - Advance(); - } - if (line_number == header_lines) { - throw runtime_error("Suitable header line not found."); - } - - BuildColumnIndex(); - - // Record column indexes. - m_x_column = GetColumnNumber("x[m]"); - m_y_column = GetColumnNumber("y[m]"); - m_dtg_column = GetColumnNumber("DTG[m]"); - m_segment_type_column = GetColumnNumber("Segment Type"); - m_course_column = GetColumnNumber("Course[rad]"); - m_turn_center_x_column = GetColumnNumber("Turn Center x[m]"); - m_turn_center_y_column = GetColumnNumber("Turn Center y[m]"); - m_angle_start_of_turn_column = GetColumnNumber("Angle Start of Turn[rad]"); - m_angle_end_of_turn_column = GetColumnNumber("Angle End of Turn[rad]"); - m_turn_radius_column = GetColumnNumber("R[m]"); - m_ground_speed_column = GetColumnNumber("groundspeed_mps"); - m_bank_angle_column = GetColumnNumber("bank_angle_deg"); - m_latitude_column = GetColumnNumber("Lat[deg]"); - m_longitude_column = GetColumnNumber("Lon[deg]"); - m_turn_center_latitude_column = GetColumnNumber("Turn Center Lat[deg]"); - m_turn_center_longitude_column = GetColumnNumber("Turn Center Lon[deg]"); - - SetExpectedColumnCount(GetColumnCount()); - - // Assume the next line is numeric data, - // even if we didn't read the expected number of header lines. -} - -Units::Length HfpReader::GetX() { - return Units::MetersLength(GetDouble(m_x_column)); -} - -Units::Length HfpReader::GetY() { - return Units::MetersLength(GetDouble(m_y_column)); -} - -Units::Length HfpReader::GetDTG() { - return Units::MetersLength(GetDouble(m_dtg_column)); -} - -std::string HfpReader::GetSegmentType() { - return GetString(m_segment_type_column); -} - -Units::Angle HfpReader::GetCourse() { - return Units::RadiansAngle(GetDouble(m_course_column)); -} - -Units::Length HfpReader::GetTurnCenterX() { - return Units::MetersLength(GetDouble(m_turn_center_x_column)); -} - -Units::Length HfpReader::GetTurnCenterY() { - return Units::MetersLength(GetDouble(m_turn_center_y_column)); -} - -Units::Angle HfpReader::GetAngleStartOfTurn() { - return Units::RadiansAngle(GetDouble(m_angle_start_of_turn_column)); -} - -Units::Angle HfpReader::GetAngleEndOfTurn() { - return Units::RadiansAngle(GetDouble(m_angle_end_of_turn_column)); -} - -Units::Length HfpReader::GetTurnRadius() { - return Units::MetersLength(GetDouble(m_turn_radius_column)); -} - -Units::Speed HfpReader::GetGroundSpeed() { - return Units::MetersPerSecondSpeed(GetDouble(m_ground_speed_column)); -} - -Units::Angle HfpReader::GetBankAngle() { - return Units::DegreesAngle(GetDouble(m_bank_angle_column)); -} - -Units::Angle HfpReader::GetLatitude() { - return Units::DegreesAngle(GetDouble(m_latitude_column)); -} - -Units::Angle HfpReader::GetLongitude() { - return Units::DegreesAngle(GetDouble(m_longitude_column)); -} - -Units::Angle HfpReader::GetTurnCenterLatitude() { - return Units::DegreesAngle(GetDouble(m_turn_center_latitude_column)); -} - -Units::Angle HfpReader::GetTurnCenterLongitude() { - return Units::DegreesAngle(GetDouble(m_turn_center_longitude_column)); -} - -} // namespace testvector diff --git a/Public/HfpReader2020.cpp b/Public/HfpReader2020.cpp deleted file mode 100644 index 9888350..0000000 --- a/Public/HfpReader2020.cpp +++ /dev/null @@ -1,168 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// 2022 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include "public/HfpReader2020.h" -#include -#include -#include - -using namespace testvector; - -HfpReader2020::HfpReader2020(std::string file_name, - int header_lines) - : DataReader(std::move(file_name), 0, 0), - m_x_column(), - m_y_column(), - m_dtg_column(), - m_segment_type_column(), - m_course_column(), - m_turn_center_x_column(), - m_turn_center_y_column(), - m_angle_start_of_turn_column(), - m_angle_end_of_turn_column(), - m_turn_radius_column(), - m_ground_speed_column(), - m_bank_angle_column(), - m_latitude_column(), - m_longitude_column(), - m_turn_center_latitude_column(), - m_turn_center_longitude_column() { - SetColumnIndexesFromHeader(header_lines); -} - -HfpReader2020::HfpReader2020(std::shared_ptr input_stream, - int header_lines) - : DataReader(std::move(input_stream), 0, 0) { - SetColumnIndexesFromHeader(header_lines); -} - -HfpReader2020::~HfpReader2020() = default; - -void HfpReader2020::SetColumnIndexesFromHeader(const int header_lines) { - if (header_lines < 1) { - throw std::logic_error("Headers are required for HfpReader2020."); - } - - int line_number = 0; - Advance(); // first header line - - // Meaningful HPF headers are at least 4 characters; first column is normally 5. - while (GetString(0).length() < 3 && line_number < header_lines) { - line_number++; - Advance(); - } - - if (line_number == header_lines) { - throw std::runtime_error("Suitable header line not found."); - } - - BuildColumnIndex(); - - // Record column indexes. - m_x_column = GetColumnNumber("x[m]"); - m_y_column = GetColumnNumber("y[m]"); - m_dtg_column = GetColumnNumber("DTG[m]"); - m_segment_type_column = GetColumnNumber("Segment_Type"); - m_course_column = GetColumnNumber("Course[rad]"); - m_turn_center_x_column = GetColumnNumber("Turn_Center_x[m]"); - m_turn_center_y_column = GetColumnNumber("Turn_Center_y[m]"); - m_angle_start_of_turn_column = GetColumnNumber("Angle_Start_of_Turn[rad]"); - m_angle_end_of_turn_column = GetColumnNumber("Angle_End_of_Turn[rad]"); - m_turn_radius_column = GetColumnNumber("R[m]"); - m_ground_speed_column = GetColumnNumber("GS[m/s]"); - m_bank_angle_column = GetColumnNumber("Bank_Angle[deg]"); - m_latitude_column = GetColumnNumber("Lat[deg]"); - m_longitude_column = GetColumnNumber("Lon[deg]"); - m_turn_center_latitude_column = GetColumnNumber("Turn_Center_Lat[deg]"); - m_turn_center_longitude_column = GetColumnNumber("Turn_Center_Lon[deg]"); - - SetExpectedColumnCount(GetColumnCount()); -} - -Units::Length HfpReader2020::GetX() { - return Units::MetersLength(GetDouble(m_x_column)); -} - -Units::Length HfpReader2020::GetY() { - return Units::MetersLength(GetDouble(m_y_column)); -} - -Units::Length HfpReader2020::GetDTG() { - return Units::MetersLength(GetDouble(m_dtg_column)); -} - -HorizontalPath::SegmentType HfpReader2020::GetSegmentType() { - const std::string segment_type = GetString(m_segment_type_column); - - if (segment_type == "straight") { - return HorizontalPath::STRAIGHT; - } else if (segment_type == "turn") { - return HorizontalPath::TURN; - } else { - return HorizontalPath::UNSET; - } -} - -Units::Angle HfpReader2020::GetCourse() { - return Units::RadiansAngle(GetDouble(m_course_column)); -} - -Units::Length HfpReader2020::GetTurnCenterX() { - return Units::MetersLength(GetDouble(m_turn_center_x_column)); -} - -Units::Length HfpReader2020::GetTurnCenterY() { - return Units::MetersLength(GetDouble(m_turn_center_y_column)); -} - -Units::Angle HfpReader2020::GetAngleStartOfTurn() { - return Units::RadiansAngle(GetDouble(m_angle_start_of_turn_column)); -} - -Units::Angle HfpReader2020::GetAngleEndOfTurn() { - return Units::RadiansAngle(GetDouble(m_angle_end_of_turn_column)); -} - -Units::Length HfpReader2020::GetTurnRadius() { - return Units::MetersLength(GetDouble(m_turn_radius_column)); -} - -Units::Speed HfpReader2020::GetGroundSpeed() { - return Units::MetersPerSecondSpeed(GetDouble(m_ground_speed_column)); -} - -Units::Angle HfpReader2020::GetBankAngle() { - return Units::DegreesAngle(GetDouble(m_bank_angle_column)); -} - -Units::Angle HfpReader2020::GetLatitude() { - return Units::DegreesAngle(GetDouble(m_latitude_column)); -} - -Units::Angle HfpReader2020::GetLongitude() { - return Units::DegreesAngle(GetDouble(m_longitude_column)); -} - -Units::Angle HfpReader2020::GetTurnCenterLatitude() { - return Units::DegreesAngle(GetDouble(m_turn_center_latitude_column)); -} - -Units::Angle HfpReader2020::GetTurnCenterLongitude() { - return Units::DegreesAngle(GetDouble(m_turn_center_longitude_column)); -} diff --git a/Public/HfpReaderPre2020.cpp b/Public/HfpReaderPre2020.cpp deleted file mode 100644 index 89f0ad4..0000000 --- a/Public/HfpReaderPre2020.cpp +++ /dev/null @@ -1,152 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// 2022 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -/* - * HfpReaderPre2020.cpp - * - * Created on: Sep 5, 2019 - * Author: klewis - */ - -#include "public/HfpReaderPre2020.h" -#include - -using namespace std; - -namespace testvector { - -HfpReaderPre2020::HfpReaderPre2020(std::string file_name, int header_lines) : - DataReader(file_name, 0, 0) { - SetColumnIndexesFromHeader(header_lines); -} - -HfpReaderPre2020::HfpReaderPre2020(std::shared_ptr input_stream, int header_lines) : - DataReader(input_stream, 0, 0) { - SetColumnIndexesFromHeader(header_lines); -} - -HfpReaderPre2020::~HfpReaderPre2020() { -} - -void HfpReaderPre2020::SetColumnIndexesFromHeader(const int header_lines) { - if (header_lines < 1) { - throw logic_error("Headers are required for HfpReaderPre2020."); - } - int line_number(0); - Advance(); // first header line - // Meaningful HPF headers are at least 4 characters; first column is normally 5. - while (GetString(0).length() < 3 && line_number < header_lines) { - line_number++; - Advance(); - } - if (line_number == header_lines) { - throw runtime_error("Suitable header line not found."); - } - - BuildColumnIndex(); - - // Record column indexes. - m_x_column = GetColumnNumber("x[m]"); - m_y_column = GetColumnNumber("y[m]"); - m_dtg_column = GetColumnNumber("DTG[m]"); - m_segment_type_column = GetColumnNumber("Segment Type"); - m_course_column = GetColumnNumber("Course[rad]"); - m_turn_center_x_column = GetColumnNumber("Turn Center x[m]"); - m_turn_center_y_column = GetColumnNumber("Turn Center y[m]"); - m_angle_start_of_turn_column = GetColumnNumber("Angle Start of Turn[rad]"); - m_angle_end_of_turn_column = GetColumnNumber("Angle End of Turn[rad]"); - m_turn_radius_column = GetColumnNumber("R[m]"); - m_ground_speed_column = GetColumnNumber("groundspeed_mps"); - m_bank_angle_column = GetColumnNumber("bank_angle_deg"); - m_latitude_column = GetColumnNumber("Lat[deg]"); - m_longitude_column = GetColumnNumber("Lon[deg]"); - m_turn_center_latitude_column = GetColumnNumber("Turn Center Lat[deg]"); - m_turn_center_longitude_column = GetColumnNumber("Turn Center Lon[deg]"); - - SetExpectedColumnCount(GetColumnCount()); - - // Assume the next line is numeric data, - // even if we didn't read the expected number of header lines. -} - -Units::Length HfpReaderPre2020::GetX() { - return Units::MetersLength(GetDouble(m_x_column)); -} - -Units::Length HfpReaderPre2020::GetY() { - return Units::MetersLength(GetDouble(m_y_column)); -} - -Units::Length HfpReaderPre2020::GetDTG() { - return Units::MetersLength(GetDouble(m_dtg_column)); -} - -std::string HfpReaderPre2020::GetSegmentType() { - return GetString(m_segment_type_column); -} - -Units::Angle HfpReaderPre2020::GetCourse() { - return Units::RadiansAngle(GetDouble(m_course_column)); -} - -Units::Length HfpReaderPre2020::GetTurnCenterX() { - return Units::MetersLength(GetDouble(m_turn_center_x_column)); -} - -Units::Length HfpReaderPre2020::GetTurnCenterY() { - return Units::MetersLength(GetDouble(m_turn_center_y_column)); -} - -Units::Angle HfpReaderPre2020::GetAngleStartOfTurn() { - return Units::RadiansAngle(GetDouble(m_angle_start_of_turn_column)); -} - -Units::Angle HfpReaderPre2020::GetAngleEndOfTurn() { - return Units::RadiansAngle(GetDouble(m_angle_end_of_turn_column)); -} - -Units::Length HfpReaderPre2020::GetTurnRadius() { - return Units::MetersLength(GetDouble(m_turn_radius_column)); -} - -Units::Speed HfpReaderPre2020::GetGroundSpeed() { - return Units::MetersPerSecondSpeed(GetDouble(m_ground_speed_column)); -} - -Units::Angle HfpReaderPre2020::GetBankAngle() { - return Units::DegreesAngle(GetDouble(m_bank_angle_column)); -} - -Units::Angle HfpReaderPre2020::GetLatitude() { - return Units::DegreesAngle(GetDouble(m_latitude_column)); -} - -Units::Angle HfpReaderPre2020::GetLongitude() { - return Units::DegreesAngle(GetDouble(m_longitude_column)); -} - -Units::Angle HfpReaderPre2020::GetTurnCenterLatitude() { - return Units::DegreesAngle(GetDouble(m_turn_center_latitude_column)); -} - -Units::Angle HfpReaderPre2020::GetTurnCenterLongitude() { - return Units::DegreesAngle(GetDouble(m_turn_center_longitude_column)); -} - -} // namespace testvector diff --git a/Public/HorizontalPath.cpp b/Public/HorizontalPath.cpp deleted file mode 100644 index 23d7b02..0000000 --- a/Public/HorizontalPath.cpp +++ /dev/null @@ -1,34 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include "public/HorizontalPath.h" - -double aaesim::open_source::HorizontalPath::GetXPositionMeters() const { return m_x_position_meters; } - -double aaesim::open_source::HorizontalPath::GetYPositionMeters() const { return m_y_position_meters; } - -void aaesim::open_source::HorizontalPath::SetXYPositionMeters(double x_position_meters, double y_position_meters) { - m_x_position_meters = x_position_meters; - m_y_position_meters = y_position_meters; -} - -bool aaesim::open_source::HorizontalPath::operator==(const HorizontalPath &that) const { - return (this->m_x_position_meters == that.m_x_position_meters) && - (this->m_y_position_meters == that.m_y_position_meters) && (this->m_segment_type == that.m_segment_type); -} diff --git a/Public/HorizontalPathTracker.cpp b/Public/HorizontalPathTracker.cpp deleted file mode 100644 index 824d2d6..0000000 --- a/Public/HorizontalPathTracker.cpp +++ /dev/null @@ -1,250 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include - -#include - -aaesim::open_source::HorizontalPathTracker::HorizontalPathTracker( - const std::vector &horizontal_trajectory, - TrajectoryIndexProgressionDirection expected_index_progression) - : m_extended_horizontal_trajectory(ExtendHorizontalTrajectory(horizontal_trajectory)), - m_unmodified_horizontal_trajectory(horizontal_trajectory), - m_is_passed_end_of_route(expected_index_progression == TrajectoryIndexProgressionDirection::INCREMENTING), - m_index_progression_direction(expected_index_progression) { - InitializeStartingIndex(); -} - -std::vector aaesim::open_source::HorizontalPathTracker::ExtendHorizontalTrajectory( - const std::vector &horizontal_trajectory) { - // add one more straight segment to end - std::vector extended_trajectory; - Units::RadiansAngle crs(horizontal_trajectory[0].m_path_course); - aaesim::open_source::HorizontalPath hp; - hp.m_segment_type = aaesim::open_source::HorizontalPath::SegmentType::STRAIGHT; - hp.SetXYPositionMeters(horizontal_trajectory[0].GetXPositionMeters() - - Units::MetersLength(EXTENSION_LENGTH).value() * Units::cos(crs), - horizontal_trajectory[0].GetYPositionMeters() - - Units::MetersLength(EXTENSION_LENGTH).value() * Units::sin(crs)); // meter - hp.m_path_length_cumulative_meters = 0; - hp.m_path_course = horizontal_trajectory[0].m_path_course; - extended_trajectory.push_back(hp); - - // extend all lengths - for (auto itr = horizontal_trajectory.begin(); itr < horizontal_trajectory.end(); ++itr) { - aaesim::open_source::HorizontalPath element = itr.operator*(); - element.m_path_length_cumulative_meters += Units::MetersLength(EXTENSION_LENGTH).value(); - extended_trajectory.push_back(element); - } - - // add one more straigt segment to the beginning - Units::RadiansAngle crs_back(horizontal_trajectory.back().m_path_course); - aaesim::open_source::HorizontalPath hp_beginning; - hp_beginning.m_segment_type = aaesim::open_source::HorizontalPath::SegmentType::STRAIGHT; - hp_beginning.SetXYPositionMeters( - horizontal_trajectory.back().GetXPositionMeters() + - Units::MetersLength(EXTENSION_LENGTH).value() * Units::cos(crs_back), - horizontal_trajectory.back().GetYPositionMeters() + - Units::MetersLength(EXTENSION_LENGTH).value() * Units::sin(crs_back)); // meter - hp_beginning.m_path_length_cumulative_meters = horizontal_trajectory.back().m_path_length_cumulative_meters + - 2 * Units::MetersLength(EXTENSION_LENGTH).value(); - hp_beginning.m_path_course = horizontal_trajectory.back().m_path_course; - extended_trajectory.push_back(hp_beginning); - - return extended_trajectory; -} - -void aaesim::open_source::HorizontalPathTracker::InitializeStartingIndex() { - switch (m_index_progression_direction) { - case TrajectoryIndexProgressionDirection::DECREMENTING: - if (m_extended_horizontal_trajectory.size() > 1) { - UpdateCurrentIndex(m_extended_horizontal_trajectory.size() - 2); - } else { - UpdateCurrentIndex(0); - } - break; - - case TrajectoryIndexProgressionDirection::UNDEFINED: - case TrajectoryIndexProgressionDirection::INCREMENTING: - UpdateCurrentIndex(0); - break; - - default: - // The code should never get here. - UpdateCurrentIndex(INT32_MAX); - break; - } -} - -bool aaesim::open_source::HorizontalPathTracker::ValidateIndexProgression( - std::vector::size_type index_to_check) { - bool is_progress_valid; - switch (m_index_progression_direction) { - case TrajectoryIndexProgressionDirection::DECREMENTING: - is_progress_valid = m_current_index == index_to_check || m_current_index - 1 == index_to_check; - break; - - case TrajectoryIndexProgressionDirection::UNDEFINED: - is_progress_valid = true; - break; - - case TrajectoryIndexProgressionDirection::INCREMENTING: - is_progress_valid = m_current_index == index_to_check || m_current_index + 1 == index_to_check; - break; - - default: - is_progress_valid = false; - break; - } - - return is_progress_valid; -} - -void aaesim::open_source::HorizontalPathTracker::UpdateHorizontalTrajectory( - const std::vector &horizontal_trajectory) { - const auto hp_to_find = m_extended_horizontal_trajectory[m_current_index]; - m_unmodified_horizontal_trajectory = horizontal_trajectory; - m_extended_horizontal_trajectory = ExtendHorizontalTrajectory(horizontal_trajectory); - - auto find_result = - std::find(m_extended_horizontal_trajectory.begin(), m_extended_horizontal_trajectory.end(), hp_to_find); - if (find_result == m_extended_horizontal_trajectory.end()) { - InitializeStartingIndex(); - return; - } - m_current_index = std::distance(m_extended_horizontal_trajectory.begin(), find_result); -} - -bool aaesim::open_source::HorizontalPathTracker::IsPositionOnNode( - const Units::Length position_x, const Units::Length position_y, - std::vector::size_type &node_index) { - bool is_on_node = - Units::abs(Units::MetersLength(m_extended_horizontal_trajectory[m_current_index].GetXPositionMeters()) - - position_x) < ON_NODE_TOLERANCE && - Units::abs(Units::MetersLength(m_extended_horizontal_trajectory[m_current_index].GetYPositionMeters()) - - position_y) < ON_NODE_TOLERANCE; - - if (!is_on_node) { - std::vector::size_type next_index; - switch (m_index_progression_direction) { - case TrajectoryIndexProgressionDirection::DECREMENTING: - if (m_current_index > 0) { - next_index = m_current_index - 1; - auto x_diff = - Units::abs(Units::MetersLength(m_extended_horizontal_trajectory[next_index].GetXPositionMeters()) - - position_x); - auto y_diff = - Units::abs(Units::MetersLength(m_extended_horizontal_trajectory[next_index].GetYPositionMeters()) - - position_y); - is_on_node = x_diff < ON_NODE_TOLERANCE && y_diff < ON_NODE_TOLERANCE; - if (is_on_node) node_index = next_index; - } - break; - - case TrajectoryIndexProgressionDirection::INCREMENTING: - if (m_current_index < m_extended_horizontal_trajectory.size() - 1) { - next_index = m_current_index + 1; - auto x_diff = - Units::abs(Units::MetersLength(m_extended_horizontal_trajectory[next_index].GetXPositionMeters()) - - position_x); - auto y_diff = - Units::abs(Units::MetersLength(m_extended_horizontal_trajectory[next_index].GetYPositionMeters()) - - position_y); - is_on_node = x_diff < ON_NODE_TOLERANCE && y_diff < ON_NODE_TOLERANCE; - if (is_on_node) node_index = next_index; - } - break; - - case TrajectoryIndexProgressionDirection::UNDEFINED: - for (auto index = 0; index < m_extended_horizontal_trajectory.size(); ++index) { - is_on_node = - Units::abs(Units::MetersLength(m_extended_horizontal_trajectory[index].GetXPositionMeters()) - - position_x) < ON_NODE_TOLERANCE && - Units::abs(Units::MetersLength(m_extended_horizontal_trajectory[index].GetYPositionMeters()) - - position_y) < ON_NODE_TOLERANCE; - if (is_on_node) { - node_index = index; - break; - } - } - break; - - default: - is_on_node = false; - break; - } - } else { - node_index = m_current_index; - } - - return is_on_node; -} - -bool aaesim::open_source::HorizontalPathTracker::IsDistanceAlongPathOnNode( - const Units::Length distance_along_path, - std::vector::size_type &node_index) { - const Units::Length distance_to_check = distance_along_path + EXTENSION_LENGTH; - bool is_on_node = - Units::abs( - Units::MetersLength(m_extended_horizontal_trajectory[m_current_index].m_path_length_cumulative_meters) - - distance_to_check) < ON_NODE_TOLERANCE; - - if (!is_on_node) { - std::vector::size_type next_index; - switch (m_index_progression_direction) { - case TrajectoryIndexProgressionDirection::DECREMENTING: - next_index = m_current_index - 1; - is_on_node = - Units::abs(Units::MetersLength( - m_extended_horizontal_trajectory[next_index].m_path_length_cumulative_meters) - - distance_to_check) < ON_NODE_TOLERANCE; - if (is_on_node) node_index = next_index; - break; - - case TrajectoryIndexProgressionDirection::INCREMENTING: - next_index = m_current_index + 1; - is_on_node = - Units::abs(Units::MetersLength( - m_extended_horizontal_trajectory[next_index].m_path_length_cumulative_meters) - - distance_to_check) < ON_NODE_TOLERANCE; - if (is_on_node) node_index = next_index; - break; - - case TrajectoryIndexProgressionDirection::UNDEFINED: - for (int index = 0; index < m_extended_horizontal_trajectory.size(); ++index) { - is_on_node = Units::abs(Units::MetersLength( - m_extended_horizontal_trajectory[index].m_path_length_cumulative_meters) - - distance_to_check) < ON_NODE_TOLERANCE; - if (is_on_node) { - node_index = index; - break; - } - } - break; - - default: - is_on_node = false; - break; - } - } else { - node_index = m_current_index; - } - - return is_on_node; -} diff --git a/Public/HorizontalTurnPath.cpp b/Public/HorizontalTurnPath.cpp deleted file mode 100644 index 2d4112c..0000000 --- a/Public/HorizontalTurnPath.cpp +++ /dev/null @@ -1,43 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include "public/HorizontalTurnPath.h" - -#include "public/HorizontalPath.h" - -/** - * Determine whether this is a right or left turn by using the - * two previous points in the trajectory to establish a line - * and finding whether the turn center is on the left or right. - * The HorizontalPath object which owns this HorizontalTurnPath - * would be p2. - */ -aaesim::open_source::HorizontalTurnPath::TURN_DIRECTION aaesim::open_source::HorizontalTurnPath::GetTurnDirection( - const HorizontalPath &p0, const HorizontalPath &p1) const { - if (turn_type == UNKNOWN) return NO_TURN; - - double dx1 = p1.GetXPositionMeters() - p0.GetXPositionMeters(); - double dy1 = p1.GetYPositionMeters() - p0.GetYPositionMeters(); - double dx2 = x_position_meters - p1.GetXPositionMeters(); - double dy2 = y_position_meters - p1.GetYPositionMeters(); - - double cross_product = dx1 * dy2 - dy1 * dx2; - - return (cross_product > 0) ? LEFT_TURN : RIGHT_TURN; -} diff --git a/Public/IMCommandObserver.cpp b/Public/IMCommandObserver.cpp deleted file mode 100644 index 122f6d9..0000000 --- a/Public/IMCommandObserver.cpp +++ /dev/null @@ -1,55 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// 2022 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include "public/IMCommandObserver.h" - -IMCommandObserver::IMCommandObserver(void) { - id = -1; - time = 0.0; - distance_to_go = 0.0; - state_altitude = 0.0; - state_TAS = 0.0; - state_groundspeed = 0.0; - IAS_command = 0.0; - unmodified_IAS = 0.0; - TAS_command = 0.0; - reference_velocity = 0.0; - reference_distance = 0.0; - predictedDistance = 0.0; - distance_difference = 0.0; - trueDistance = 0.0; - iteration = 0; -} - -IMCommandObserver::~IMCommandObserver(void) { -} - -// operator < for sort algorithm -bool IMCommandObserver::operator<(const IMCommandObserver &im_in) const { - bool result = false; - - // if a.id < b.id OR a.id == b.id && a.time < b.time then a is < than b - if (this->id < im_in.id) { - result = true; - } else if (this->id == im_in.id && this->time < im_in.time) { - result = true; - } - - return result; -} diff --git a/Public/InternalObserver.cpp b/Public/InternalObserver.cpp deleted file mode 100644 index 95015ec..0000000 --- a/Public/InternalObserver.cpp +++ /dev/null @@ -1,1072 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// 2022 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include "public/InternalObserver.h" -#include "public/Environment.h" -#include "public/StereographicProjection.h" -#include - -using namespace std; - -InternalObserver *InternalObserver::mInstance = NULL; -log4cplus::Logger InternalObserver::logger = log4cplus::Logger::getInstance(LOG4CPLUS_TEXT("InternalObserver")); - -InternalObserver *InternalObserver::getInstance() { - if (mInstance == NULL) { - mInstance = new InternalObserver(); - } - return mInstance; -} - -void InternalObserver::clearInstance() { - if (mInstance != NULL) { - delete mInstance; - mInstance = NULL; // blow away the instance - } -} - -InternalObserver::InternalObserver(void) { - m_save_maintain_metrics = true; - m_scenario_iter = 0; - mOwnKinVertPathObs = NULL; - mTargKinVertPathObs = NULL; - outputNMFiles = true; -} - -InternalObserver::~InternalObserver(void) { - - if (mOwnKinVertPathObs != NULL) { - delete mOwnKinVertPathObs; - } - - if (mTargKinVertPathObs != NULL) { - delete mTargKinVertPathObs; - } - -} - -void InternalObserver::reset(void) { - //reset for each iteration: - -} - -void InternalObserver::process(void) { - - outputStateModel(); - dumpPredictedWind(); - process_IM_command(); - process_NM_stats(); - process_speed_command_count(); - processMaintainMetrics(); - processFinalGS(); - processMergePointMetric(); - processClosestPointMetric(); - process_ptis_b_reports(); - - dumpAchieveList(); - -} - -void InternalObserver::set_scenario_name(string in) { - scenario_name = in; -} - -void InternalObserver::storeStateModel(aaesim::open_source::AircraftState asv, - int flapsConfig, - float speed_brake, - double ias) { - // Stores data for state model output as string. - // - // asv:Aircraft state vector data. - // flapsConfig:Configuration of flaps. - - while (m_scenario_iter >= (int) stateModelOutput.size()) { - vector > strings; - stateModelOutput.push_back(strings); - } - - // Add vector for current aircraft if needed. - - while (asv.m_id >= (int) stateModelOutput[m_scenario_iter].size()) { - vector strings; - stateModelOutput[m_scenario_iter].push_back(strings); - } - - - // Store state model string. - - stateModelOutput[m_scenario_iter][asv.m_id].push_back(stateModelString(asv, flapsConfig, speed_brake, ias)); -} - - -string InternalObserver::stateModelString(aaesim::open_source::AircraftState asv, - int flapsConfig, - float speed_brake, - double ias) { - // Formats state model data string. - // - // asv:Aircraft state vector object. - // flapsConfig:flaps configuration - // - // returns state model report data string. - - stringstream strm; - - // get current speed in FPS - - double currSpeed = sqrt(pow(asv.m_xd, 2) + pow(asv.m_yd, 2)); - - strm << m_scenario_iter << ","; // iteration number - strm << asv.m_id << ","; // id - strm << asv.m_time << ","; // time - strm << asv.m_x << ","; // x value in feet - strm << asv.m_y << ","; // y value in feet - strm << asv.m_z << ","; // altitude value in feet - strm << asv.m_xd << ","; // x velocity in FPS - strm << asv.m_yd << ","; // y velocity in FPS - strm << asv.m_zd * 60 << ","; // altitude velocity int Feet per Minute - - // Note:These two are the exactly the same calculation except for a units conversion. - - strm << currSpeed << ","; // ground speed in FPS - strm << Units::KnotsSpeed(asv.GetGroundSpeed()).value() << ","; - - strm << asv.m_xdd << ","; // x acceleration FPS^2 - strm << asv.m_ydd << ","; // y acceleration FPS^2 - strm << asv.m_zdd * 60 << ","; // z acceleration in Feet per Minute per Second - strm << asv.m_Vwx << ","; // true wind Vwx in MPS - strm << asv.m_Vwy << ","; // true wind Vwy in MPS - strm << asv.m_Vw_para << ","; // true wind Vw_para in MPS - strm << asv.m_Vw_perp << ","; // true wind Vw_perp in MPS - strm << flapsConfig << ","; // configuration for flaps (mainly debug) - strm << speed_brake << ","; - strm << ias; - - string str; - - strm >> str; - - return str; -} - -MergePointMetric& InternalObserver::GetMergePointMetric(int id) { - return m_aircraft_iteration_stats[id].m_merge_point_metric; -} - -MaintainMetric& InternalObserver::GetMaintainMetric(int id) { - return m_aircraft_iteration_stats[id].m_maintain_metric; -} - -ClosestPointMetric& InternalObserver::GetClosestPointMetric(int id) { - return m_aircraft_iteration_stats[id].m_closest_point_metric; -} - -NMObserver& InternalObserver::GetNMObserver(int id) { - return m_aircraft_scenario_stats[id].m_nm_observer; -} - -int InternalObserver::GetScenarioIter() const { - return m_scenario_iter; -} - -void InternalObserver::SetScenarioIter(int scenario_iter) { - this->m_scenario_iter = scenario_iter; -} - -string InternalObserver::stateModelHdr() { - // Returns state model string. - - string str = "Iteration,AC_ID,Time,X(feet),Y(feet),Alt(feet),X_Vel(FPS),Y_Vel(FPS),Alt_Vel(FPM),GroundSpeed(FPS),GroundSpeed(Knots),X_Accel(FPSS),Y_Accel(FPSS),Alt_Accel(FPMPS),True_Wind_Vwx(MPS),True_Wind_Vwy(MPS),Vw_para(MPS),Vw_perp(MPS),distToGo(M),FlapsConfig,SpeedBrake,IAS(Knots)"; - - return str; -} - - -void InternalObserver::outputStateModel() { - // Writes state model report. - - string fname = scenario_name + "-state-model-output.csv"; - ofstream out; - out.open(fname.c_str()); - - if (out.is_open()) { - // Header - - out << stateModelHdr().c_str() << endl; - - for (unsigned int i = 0; i < stateModelOutput.size(); i++) { - // Loop through aircraft and output all records. - - for (unsigned int j = 0; j < stateModelOutput[i].size(); j++) { - for (unsigned int k = 0; k < stateModelOutput[i][j].size(); k++) { - - out << stateModelOutput[i][j][k].c_str() << endl; - } - } - } - } - - out.close(); - - - // Clear everything - stateModelOutput.clear(); - -} - - -// output the IM commands -void InternalObserver::IM_command_output(int id_in, - double time_in, - double state_alt, - double state_TAS, - double state_groundspeed_in, - double ias_command_in, - double unmod_ias, - double tas_command_in, - double ref_vel_in, - double ref_dist_in, - double predDistIn, - double trueDistIn) { - IMCommandObserver new_command; - new_command.iteration = this->m_scenario_iter; - new_command.id = id_in; - new_command.time = time_in; - new_command.state_altitude = state_alt * FEET_TO_METERS; - new_command.distance_to_go = fabs(predDistIn); // Distance is in meters - new_command.state_TAS = state_TAS; // TAS is in MPS - new_command.state_groundspeed = state_groundspeed_in; // Ground Speed is in meters - new_command.IAS_command = ias_command_in; // IAS command in meters - new_command.unmodified_IAS = unmod_ias; // unmodified IAS command in meters - new_command.TAS_command = tas_command_in; // TAS command in meters - new_command.reference_velocity = ref_vel_in; // reference velocity in meters - new_command.reference_distance = ref_dist_in; - new_command.predictedDistance = predDistIn; - new_command.distance_difference = ref_dist_in - predDistIn; - new_command.trueDistance = trueDistIn; - - im_commands.push_back(new_command); -} - -void InternalObserver::process_IM_command() { - // open report data file - string output_file_name = scenario_name + "-IM-command-output.csv"; - ofstream out; - out.open(output_file_name.c_str()); - -// sort(im_commands.begin(), im_commands.end()); // sort the list by aircraft and time - - //if file opens successfully, process the output - if (out.is_open() == true && im_commands.empty() == false) { - out << - "Iteration,Aircraft_ID,Time,Aircraft_True_Distance(Meters),Aircraft_Predicted_Distance(Meters),State_Altitude(Meters),State_TAS(MPS),State_GroundSpeed(MPS),IAS_Speed_Command(MPS),Unmodified_IAS_Command(MPS),TAS_Speed_Command(MPS)" - << - endl; - - for (unsigned int loop = 0; loop < im_commands.size(); loop++) { - out << im_commands[loop].iteration << ","; - out << im_commands[loop].id << ","; - out << im_commands[loop].time << ","; - out << im_commands[loop].trueDistance << ","; - out << im_commands[loop].predictedDistance << ","; - out << im_commands[loop].state_altitude << ","; - out << im_commands[loop].state_TAS << ","; - out << std::setprecision(15) << im_commands[loop].state_groundspeed << ","; - out << std::setprecision(15) << im_commands[loop].IAS_command << ","; - out << std::setprecision(15) << im_commands[loop].unmodified_IAS << ","; - out << std::setprecision(15) << im_commands[loop].TAS_command << endl; - } - - out.close(); - } -} - -// outputs the Nautical Mile report for all aircraft -void InternalObserver::process_NM_aircraft() { - if (outputNM()) { - // Write NM report if we are outputting NM files. - - // loop to process all of the aircraft NM reports - for (auto ix = m_aircraft_scenario_stats.begin(); ix != m_aircraft_scenario_stats.end(); ++ix) { - NMObserver &nm_observer = ix->second.m_nm_observer; - // if the current aircraft has Nautical Mile output entries output them - if (!nm_observer.entry_list.empty()) { - char *temp = new char[10]; - - sprintf(temp, "%d", ix->first); - - string output_file_name = scenario_name + "_AC" + temp + "-NM-output.csv"; - delete[] temp; - - ofstream out; - - // if first iteration create file, otherwise append file - if (m_scenario_iter == 0) { - out.open(output_file_name.c_str()); - } else { - out.open(output_file_name.c_str(), ios::out | ios::app); - } - - // if file opens properly and entry list isn't empty output the Nautical Mile results - if (out.is_open()) { - // if first iteration create header - - if (m_scenario_iter == 0) { - out << - "AC_ID,Iteration,Predicted_Distance(NM),True_Distance(NM),Time,Own_Command_IAS(Knots),Own_Current_GroundSpeed(Knots),Target_GroundSpeed(Knots),Min_IAS_Command(Knots),Max_IAS_Command(Knots),Min_GS_Command(Knots),Max_GS_Command(Knots)" - << - endl; - } - - nm_observer.initialize_stats(); // initialize the statistics to the size of the entry list - - // loop to process all aircraft entries - for (unsigned int index = 0; index < nm_observer.entry_list.size(); index++) { - // output the report - out << ix->first << ","; - out << m_scenario_iter << ","; - out << nm_observer.entry_list[index].predictedDistance / NAUTICAL_MILES_TO_METERS << ","; - out << nm_observer.entry_list[index].trueDistance / NAUTICAL_MILES_TO_METERS << ","; - out << nm_observer.entry_list[index].time << ","; - out << nm_observer.entry_list[index].acIAS / KNOTS_TO_METERS_PER_SECOND << ","; - out << nm_observer.entry_list[index].acGS / KNOTS_TO_METERS_PER_SECOND << ","; - out << nm_observer.entry_list[index].targetGS / KNOTS_TO_METERS_PER_SECOND << ","; - out << nm_observer.entry_list[index].minIAS / KNOTS_TO_METERS_PER_SECOND << ","; - out << nm_observer.entry_list[index].maxIAS / KNOTS_TO_METERS_PER_SECOND << ","; - out << nm_observer.entry_list[index].minTAS / KNOTS_TO_METERS_PER_SECOND << ","; - out << nm_observer.entry_list[index].maxTAS / KNOTS_TO_METERS_PER_SECOND << endl; - - // add entries to Statistics class - nm_observer.predictedDistance[index] = - nm_observer.entry_list[index].predictedDistance / NAUTICAL_MILES_TO_METERS; - nm_observer.trueDistance[index] = - nm_observer.entry_list[index].trueDistance / NAUTICAL_MILES_TO_METERS; - nm_observer.ac_IAS_stats[index].Insert( - nm_observer.entry_list[index].acIAS / KNOTS_TO_METERS_PER_SECOND); - nm_observer.ac_GS_stats[index].Insert( - nm_observer.entry_list[index].acGS / KNOTS_TO_METERS_PER_SECOND); - nm_observer.target_GS_stats[index].Insert( - nm_observer.entry_list[index].targetGS / KNOTS_TO_METERS_PER_SECOND); - nm_observer.min_IAS_stats[index].Insert( - nm_observer.entry_list[index].minIAS / KNOTS_TO_METERS_PER_SECOND); - nm_observer.max_IAS_stats[index].Insert( - nm_observer.entry_list[index].maxIAS / KNOTS_TO_METERS_PER_SECOND); - } - - nm_observer.entry_list.clear(); - nm_observer.curr_NM = -2; // resets the current NM value - out.close(); - } - } - } - } -} - -void InternalObserver::process_NM_stats() { - - // Write NM stats output NM files being processed. - if (outputNM()) { - - // loop to process all of the aircraft NM reports - for (auto ix = m_aircraft_scenario_stats.begin(); ix != m_aircraft_scenario_stats.end(); ++ix) { - NMObserver &nm_observer = ix->second.m_nm_observer; - - if (nm_observer.predictedDistance.size() > 0) { - char *temp = new char[10]; - - sprintf(temp, "%d", ix->first); - - string output_file_name = scenario_name + "_AC" + temp + "-stats-NM-output.csv"; - - delete[] temp; - - ofstream out; - - out.open(output_file_name.c_str(), ios::out); - - // if file opens properly and entry list isn't empty output the Nautical Mile statistics - if (out.is_open()) { - out << - "Predicted_Distance,True_Distance,AC_IAS_Mean,AC_IAS_Dev,AC_GS_Mean,AC_GS_Dev,Target_GS_Mean,Target_GS_Dev,Min_Mean,Min_Dev,Max_Mean,Max_Dev" - << - endl; - - // loop to process all distance entry statistics - for (unsigned int index = 0; index < nm_observer.predictedDistance.size(); index++) { - out << nm_observer.predictedDistance[index] << ","; - out << nm_observer.trueDistance[index] << ","; - out << nm_observer.ac_IAS_stats[index].GetMean() << ","; - out << nm_observer.ac_IAS_stats[index].ComputeStandardDeviation() << ","; - out << nm_observer.ac_GS_stats[index].GetMean() << ","; - out << nm_observer.ac_GS_stats[index].ComputeStandardDeviation() << ","; - out << nm_observer.target_GS_stats[index].GetMean() << ","; - out << nm_observer.target_GS_stats[index].ComputeStandardDeviation() << ","; - out << nm_observer.min_IAS_stats[index].GetMean() << ","; - out << nm_observer.min_IAS_stats[index].ComputeStandardDeviation() << ","; - out << nm_observer.max_IAS_stats[index].GetMean() << ","; - out << nm_observer.max_IAS_stats[index].ComputeStandardDeviation() << endl; - } - - out.close(); - } - - } - } - } -} - - -void InternalObserver::speed_command_count_output(vector speed_change_list) { - aircraft_speed_count_list.push_back(speed_change_list); -} - -void InternalObserver::process_speed_command_count() { - // open report data file - string output_file_name = scenario_name + "-Speed_Command_Count-output.csv"; - ofstream out; - out.open(output_file_name.c_str()); - - if (out.is_open() && aircraft_speed_count_list.size() > 0) { - // generate header information - out << "Iteration"; - for (unsigned int loop = 0; loop < aircraft_speed_count_list[0].size(); loop++) { - out << ",AC" << loop; - } - out << endl; - - // output the data for each iteration - for (unsigned int iter_loop = 0; iter_loop < aircraft_speed_count_list.size(); iter_loop++) { - out << iter_loop; - for (unsigned int ac_loop = 0; ac_loop < aircraft_speed_count_list[iter_loop].size(); ac_loop++) { - out << "," << aircraft_speed_count_list[iter_loop][ac_loop]; - } - out << endl; - } - - out.close(); - } -} - -void InternalObserver::initializeIteration(int number_of_aircraft_in_scenario) { - m_aircraft_iteration_stats.clear(); - predWinds.clear(); -} - -void InternalObserver::outputMaintainMetrics() { - // Post processes the maintain metric data after each iteration, - // forming a string for each iteration and placing it in a local - // string vector. - - string body; - char bfr[121]; - - - // Add header. - - if (maintainOutput.size() == 0) { - body = "Iteration"; - - for (auto ix = m_aircraft_iteration_stats.begin(); ix != m_aircraft_iteration_stats.end(); ++ix) { - int acid = ix->first; - MaintainMetric &maintain_metric = ix->second.m_maintain_metric; - if (!maintain_metric.IsOutputEnabled()) continue; - sprintf(bfr, ",ac %d-mean,ac %d-stdev,ac %d-95bound,ac %d-maintainTime,ac %d-timeGreaterThan10", - acid, acid, acid, acid, acid); - body = body + bfr; - } - - maintainOutput.push_back(body); - } - - - // Add body. - sprintf(bfr, "%d", ((int) maintainOutput.size() - 1)); // Iteration - body = bfr; - - for (auto ix = m_aircraft_iteration_stats.begin(); ix != m_aircraft_iteration_stats.end(); ++ix) { - MaintainMetric &maintain_metric = ix->second.m_maintain_metric; - if (!maintain_metric.IsOutputEnabled()) continue; - if (maintain_metric.hasSamples()) { - sprintf(bfr, ",%f,%f,%f,%f,%d", maintain_metric.getMeanErr(), - maintain_metric.getStdErr(), maintain_metric.getBound95(), - maintain_metric.getTotMaintain(), maintain_metric.getNumCycles()); - } else { - sprintf(bfr, ",No samples,,,,"); - } - - body = body + bfr; - } - - maintainOutput.push_back(body); -} - -void InternalObserver::processMaintainMetrics() { - - // Output maintain metrics .csv file. - - string output_file_name = scenario_name + "-Maintain-Metrics.csv"; - ofstream out; - out.open(output_file_name.c_str()); - - if (out.is_open()) { - for (size_t ix = 0; ix < maintainOutput.size(); ix++) { - out << maintainOutput[ix] << endl; - } - - out.close(); - } - - - // Clear report vector. - - maintainOutput.clear(); -} - -void InternalObserver::updateFinalGS(int id, - double gs) { - - // Stores/replaces final ground speed for a aircraft. - // - // id:id of aircraft. - // gs:ground speed. - - if (id >= 0) { - m_aircraft_iteration_stats[id].finalGS = gs; - } - -} - -void InternalObserver::outputFinalGS() { - - // Post processes final ground speed data after each iteration, - // forming a string for each iteration and placing it in a local - // string vector. - - string body; - char bfr[51]; - - - // Add header. - - if (finalGSOutput.size() == 0) { - body = "Iteration"; - - for (auto ix = m_aircraft_iteration_stats.begin(); ix != m_aircraft_iteration_stats.end(); ++ix) { - sprintf(bfr, ",ac %d-gs", ix->first); - body = body + bfr; - } - - finalGSOutput.push_back(body); - } - - - // Add body. - sprintf(bfr, "%d", ((int) finalGSOutput.size() - 1)); // Iteration // TODO:A better way of determining iteration. - body = bfr; - - for (auto ix = m_aircraft_iteration_stats.begin(); ix != m_aircraft_iteration_stats.end(); ++ix) { - sprintf(bfr, ",%f", ix->second.finalGS); - body = body + bfr; - } - - - // Add string to output. - - finalGSOutput.push_back(body); - -} - -void InternalObserver::processFinalGS() { - - // Outputs the final groundspeed .csv file. - - // Open file - - string output_file_name = scenario_name + "-Final-Groundspeed.csv"; - ofstream out; - out.open(output_file_name.c_str()); - - - if (out.is_open()) { - for (size_t ix = 0; ix < finalGSOutput.size(); ix++) { - out << finalGSOutput[ix] << endl; - } - - out.close(); - } - - - // Clear report vector. - - finalGSOutput.clear(); -} - -void InternalObserver::outputMergePointMetric() { - - // Creates report for merge point metric, first a column header - // and then for each iteration, one line with merge point stats. - // Each line is a string stored in an output vector. - - string body; - char bfr[61]; - - // Add header. - - if (mergePointOutput.size() == 0) { - body = "Iteration"; - - for (auto ix = m_aircraft_iteration_stats.begin(); ix != m_aircraft_iteration_stats.end(); ++ix) { - MergePointMetric &merge_point_metric = ix->second.m_merge_point_metric; - if (merge_point_metric.willReportMetrics()) { - int id1 = merge_point_metric.GetImAcId(); - int id0 = merge_point_metric.GetTargetAcId(); - sprintf(bfr, ",ac %d-mergePt,ac %d-distTo ac %d", id1, id1, id0); - body = body + bfr; - } - } - - mergePointOutput.push_back(body); - } - - - // Add body. - sprintf(bfr, "%d", ((int) mergePointOutput.size() - 1)); // Iteration - body = bfr; - - for (auto ix = m_aircraft_iteration_stats.begin(); ix != m_aircraft_iteration_stats.end(); ++ix) { - MergePointMetric &merge_point_metric = ix->second.m_merge_point_metric; - if (merge_point_metric.willReportMetrics()) { - sprintf(bfr, ",%s,%f", merge_point_metric.getMergePoint().c_str(), - Units::NauticalMilesLength(merge_point_metric.getDist()).value()); - body = body + bfr; - } - } - - mergePointOutput.push_back(body); - -} - -void InternalObserver::processMergePointMetric() { - - // Output merge point metric to a .csv file. - - string output_file_name = scenario_name + "-Merge-Point-Metric.csv"; - ofstream out; - out.open(output_file_name.c_str()); - - if (out.is_open()) { - for (size_t ix = 0; ix < mergePointOutput.size(); ix++) { - out << mergePointOutput[ix] << endl; - } - - out.close(); - } - - - // Clear report vector. - - mergePointOutput.clear(); -} - -void InternalObserver::outputClosestPointMetric() { - - // Creates report text for the closest point metric. - // A column header is created the first time through. - // A line containing the closest point metric stats is - // created for each iteration. - - - string body; - char bfr[61]; - - // Add header. - - if (closestPointOutput.size() == 0) { - body = "Iteration"; - for (auto ix = m_aircraft_iteration_stats.begin(); ix != m_aircraft_iteration_stats.end(); ++ix) { - ClosestPointMetric &closest_point_metric = ix->second.m_closest_point_metric; - if (closest_point_metric.IsReportMetrics()) { - sprintf(bfr, ",ac %u-smallestDistTo ac %u", - closest_point_metric.GetImAcId(), closest_point_metric.GetTargetAcId()); - body = body + bfr; - } - } - - closestPointOutput.push_back(body); - } - - - // Add body. - sprintf(bfr, "%d", ((int) closestPointOutput.size() - 1)); // Iteration - body = bfr; - - for (auto ix = m_aircraft_iteration_stats.begin(); ix != m_aircraft_iteration_stats.end(); ++ix) { - ClosestPointMetric &closest_point_metric = ix->second.m_closest_point_metric; - if (closest_point_metric.IsReportMetrics()) { - sprintf(bfr, ",%f", - Units::NauticalMilesLength(closest_point_metric.getMinDist()).value()); - body = body + bfr; - } - } - - closestPointOutput.push_back(body); - -} - -void InternalObserver::processClosestPointMetric() { - - // Output closest point metric to a .csv file. - - string output_file_name = scenario_name + "-Closest-Point-Metric.csv"; - ofstream out; - out.open(output_file_name.c_str()); - - if (out.is_open()) { - for (size_t ix = 0; ix < closestPointOutput.size(); ix++) { - out << closestPointOutput[ix] << endl; - } - - out.close(); - } - - - // Clear report vector. - - closestPointOutput.clear(); -} - -// collect pTIS_B reports into this class -void InternalObserver::collect_ptis_b_report(Sensor::ADSB::ADSBSVReport adsb_sv_report) { - ptis_b_report_list.push_back(adsb_sv_report); -} - -void InternalObserver::process_ptis_b_reports() // process the ADS-B reports -{ - //Figure the maximum id out of all the IDs of in the receiver_id field in ptis_b_ether_with_receiver_ID_list - int max_id = -100; - //loop through the receiver_id fields in ptis_b_ether_with_receiver_ID_list - for (size_t i = 0; i < ptis_b_report_list.size(); i++) { - int this_id = ptis_b_report_list[i].GetId(); - if (this_id > max_id) { - max_id = this_id; - } - } - - //loop through all ac ids - for (int ac_id = 0; ac_id <= max_id; ac_id++) { - - // open report data file - std::ostringstream ostr_ac_id; //output string stream - ostr_ac_id << ac_id; //use the string stream to convert ac_id into an output string stream - string ac_id_string = ostr_ac_id.str(); //convert to string - - string output_file_name = scenario_name + "-TIS-B-Report-output-TargetACID-" + ac_id_string + ".csv"; - ofstream out; - out.open(output_file_name.c_str()); - - //if file opens successfully, process the output - if (out.is_open() == true && ptis_b_report_list.empty() == false) { - //print the header - out << "TOA,24bitAddress,Lat,Lon,Alt,EWVel,NSVel,NACp,NIC,NACv,SIL,SDA,VertRate" << endl; - - for (size_t i = 0; i < ptis_b_report_list.size(); i++) { - Sensor::ADSB::ADSBSVReport return_report; - return_report = ptis_b_report_list[i]; - if (return_report.GetId() == ac_id) { - // print out current record - out << return_report.GetTime().value() << ","; // outputs the TOA - out << return_report.GetId() << ","; // output id - Units::DegreesAngle lat_out, long_out; - StereographicProjection::xy_to_ll( - Units::FeetLength(return_report.GetX()), - Units::FeetLength(return_report.GetY()), - lat_out, long_out); // call the Stereographic Projection to convert the aircraft X/Y to Lat/Long - out.precision(10); - out << lat_out.value() << ","; // output the lat in degrees - out << long_out.value() << ","; // output lon in degrees - out << return_report.GetZ().value() << ","; // output the current altitude value in feet - out << Units::KnotsSpeed(return_report.GetXd()).value() << - ","; // output the current x velocity in knots; the unit of return_report.getxd is assumed to be feet/second - out << Units::KnotsSpeed(return_report.GetYd()).value() << - ","; // output the current y velocity in knots; the unit of return_report.getyd is assumed to be feet/second - out << return_report.GetNacp() << ","; // output the NACp - out << return_report.GetNicp() << ","; // output the NICp - out << return_report.GetNacv() << ","; // output the NACv - out << 2 << ","; // output the SIL (set at 2) - out << 2 << ","; // output the SDA (set at 2) - out << Units::FeetPerMinuteSpeed(return_report.GetZd()).value() << - endl; // output the current vertical velocity feet per minute; the unit of return_report.zd is assumed to be feet/second - } //end if(return_report.id == ac_id) - } //end for(int i = 0; i < ptis_b_ether_with_receiver_ID_list.size(); i++) - out.close(); - } //end if( out.is_open() == true && ads_b_ether_list.empty() == false) - } //end for(int ac_id = 0; ac_id <= max_id; ac_id++) - - -} - -void InternalObserver::addPredictedWind(int id, const WeatherPrediction &weatherPrediction) { - // Adds predicted wind entry for an aircraft. - // - // id:aircraft id. - // weatherPrediction.east_west, weatherPrediction.north_south:predicted wind data for aircraft. - // altitudes in feet, wind speeds in knots. - - // Add header. - if (predWinds.size() == 0) { - predWinds.push_back(predWindsHeading(weatherPrediction.east_west.GetMaxRow())); - } - - // Add altitudes, x speed, y speed. - predWinds.push_back(predWindsData(id, 1, "Alt(feet)", weatherPrediction.east_west)); - predWinds.push_back(predWindsData(id, 2, "XSpeed(Knots)", weatherPrediction.east_west)); - predWinds.push_back(predWindsData(id, 2, "YSpeed(Knots)", weatherPrediction.north_south)); - predWinds.push_back(predTempData(id, "Temperature(C)", weatherPrediction)); -} - -string InternalObserver::predWindsHeading(int numVals) { - // Formats header for predicted winds metric. - // - // numVals:number of values in the predicted winds matrices where - // each value is an altitude, speed pair. - // - // returns header line. - - string hdr = "Aircraft_id,Field"; - - // Only aircraft id for now. This will need clarification from Lesley. - // Still need to add a blank column title for each column. - - for (int i = 1; i <= numVals; i++) { - hdr += ","; - } - - return hdr; -} - -string InternalObserver::predWindsData(int id, - int col, - string field, - const WindStack &mat) { - // Formats data line for predicted winds metric for an aircraft - // for a data row. With respect between the data line output and - // how the wind matrices are setup, the rows and columns are - // inverted. Altitudes output in meters, speeds in meters/second. - // - // id:aircraft id. - // field:field name. - // col:col of data being formatted-1 for altitude, 2 for speed. - // mat:matrix containing data to format into string. - // - // returns data line. - - string str; - - char *txt = new char[31]; - - - // Aircraft id - - sprintf(txt, "%d", id); - str = txt; - - - // Field - - str += ","; - str += field.c_str(); - - - // Data line-all in meters, meters/second. - - for (int i = 1; i <= mat.GetMaxRow(); i++) { - switch (col) { - case 1: - sprintf(txt, ",%lf", mat.GetAltitude(i).value()); - break; - case 2: - sprintf(txt, ",%lf", mat.GetSpeed(i).value()); - - } - str += txt; - } - - delete[] txt; - - return str; -} - -string InternalObserver::predTempData(int id, - string field, - const WeatherPrediction &weatherPrediction) { - // Formats data line for predicted winds metric for an aircraft - // for a data row. With respect between the data line output and - // how the wind matrices are setup, the rows and columns are - // inverted. Altitudes output in meters, speeds in meters/second. - // - // id:aircraft id. - // field:field name. - // col:col of data being formatted-1 for altitude, 2 for speed. - // mat:matrix containing data to format into string. - // - // returns data line. - - string str; - - char *txt = new char[31]; - - - // Aircraft id - - sprintf(txt, "%d", id); - str = txt; - - - // Field - - str += ","; - str += field.c_str(); - - - // Data line-all in meters, meters/second. - const WindStack &mat(weatherPrediction.east_west); - for (int i = 1; i <= mat.GetMaxRow(); i++) { - Units::Length alt = mat.GetAltitude(i); - Units::KelvinTemperature temperature = weatherPrediction.GetForecastAtmosphere()->GetTemperature(alt); - sprintf(txt, ",%lf", temperature.value() - 273.15); - str += txt; - } - - delete[] txt; - - return str; -} - -void InternalObserver::dumpPredictedWind() { - // Outputs predicted winds .csv file. - - string fileName = scenario_name + "-Predicted-Winds.csv"; - ofstream out; - out.open(fileName.c_str()); - - if (out.is_open()) { - for (size_t ix = 0; ix < predWinds.size(); ix++) { - out << predWinds[ix] << endl; - } - - out.close(); - } - - - // Clear predicted winds output. - predWinds.clear(); -} - -void InternalObserver::addAchieveRcd(size_t aircraftId, - double tm, - double target_ttg_to_ach, - double own_ttg_to_ach, - double curr_distance, - double reference_distance) { - // Adds data record for achieve algorithms. - // - // aircraftId:aircraft id. - // tm:time (seconds). - // target_ttg_to_ach:target time to go to achieve (seconds). - // own_ttg_to_ach:own time to go to achieve (seconds). - // curr_distance:current distance (meters). - // reference_distance:reference distance (meters). - - AchieveObserver achievercd(this->m_scenario_iter, aircraftId, tm, target_ttg_to_ach, own_ttg_to_ach, - curr_distance, reference_distance); - m_aircraft_scenario_stats[aircraftId].m_achieve_list.push_back(achievercd); -} - - -void InternalObserver::dumpAchieveList() { - // Writes output from achieve algorithms to time to go .csv file. - - string fileName = scenario_name + "-time-to-go.csv"; - ofstream out; - - bool needHdr = true; - - for (auto ix = m_aircraft_scenario_stats.begin(); ix != m_aircraft_scenario_stats.end(); ++ix) { - vector &achieve_list = ix->second.m_achieve_list; - - if (achieve_list.empty()) { - continue; - } // Nada for this ac - - // Header - if (needHdr) { - out.open(fileName.c_str()); - if (!out.is_open()) { - LOG4CPLUS_ERROR(logger, "Cannot open " << fileName << " for achieve list output."); - return; - } - out << achieve_list[0].Hdr().c_str() << endl; - needHdr = false; - } - - for (size_t ix = 0; ix < achieve_list.size(); ix++) { - out << achieve_list[ix].ToString().c_str() << endl; - } - } - if (out.is_open()) - out.close(); - -} - - -void InternalObserver::setNMOutput(bool NMflag) { - // Sets flag to output NM data. - // - // NMFlag:NM output flag. - // true-output NM data. - // false-don't output NM data. - - this->outputNMFiles = NMflag; -} - - -bool InternalObserver::outputNM(void) { - // Determines whether NM data being output or not. - // - // returns true if outputting NM data. - // false if not outputting NM data. - - return this->outputNMFiles; -} - -void InternalObserver::SetRecordMaintainMetrics(bool new_value) { - m_save_maintain_metrics = new_value; -} - -const bool InternalObserver::GetRecordMaintainMetrics() const { - return m_save_maintain_metrics; -} - -CrossTrackObserver& InternalObserver::GetCrossEntry() { - return m_cross_entry; -} - -InternalObserver::AircraftIterationStats::AircraftIterationStats() : - finalGS(-1.0) { -} diff --git a/Public/InvalidIndexException.cpp b/Public/InvalidIndexException.cpp deleted file mode 100644 index 0f630f6..0000000 --- a/Public/InvalidIndexException.cpp +++ /dev/null @@ -1,32 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include "public/InvalidIndexException.h" - -#include -#include - -#include - -InvalidIndexException::InvalidIndexException(const int value, const int low_limit, const int high_limit) : exception() { - log4cplus::Logger logger = log4cplus::Logger::getInstance(LOG4CPLUS_TEXT("InvalidIndexException")); - LOG4CPLUS_ERROR(logger, "attempted to access index " << value << " between " << low_limit << " and " << high_limit); -} - -InvalidIndexException::InvalidIndexException(char *value) : exception() { std::cout << value << std::endl; } diff --git a/Public/KinematicDescent4DPredictor.cpp b/Public/KinematicDescent4DPredictor.cpp deleted file mode 100644 index f983b30..0000000 --- a/Public/KinematicDescent4DPredictor.cpp +++ /dev/null @@ -1,1512 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include "public/KinematicDescent4DPredictor.h" - -#include -#include -#include -#include - -#include "public/SimulationTime.h" -#include "public/Waypoint.h" - -using namespace std; -using namespace aaesim::open_source; -using namespace aaesim::open_source::constants; - -log4cplus::Logger KinematicDescent4DPredictor::m_logger = - log4cplus::Logger::getInstance(LOG4CPLUS_TEXT("KinematicDescent4DPredictor")); - -const Units::Length KinematicDescent4DPredictor::m_vertical_tolerance_distance = Units::FeetLength(400); - -KinematicDescent4DPredictor::KinematicDescent4DPredictor() - : m_kinematic_descent_type(CONSTRAINED), - m_altitude_at_end_of_route(Units::zero()), - m_deceleration_mps(0.5 * KNOTS_TO_METERS_PER_SECOND), - m_deceleration_level_mps(0.75 * KNOTS_TO_METERS_PER_SECOND), - m_deceleration_fpa_mps(0.3 * KNOTS_TO_METERS_PER_SECOND), - m_const_gamma_cas_term_rad(2.9 * DEGREES_TO_RADIAN), - m_const_gamma_cas_er_rad(3.1 * DEGREES_TO_RADIAN), - m_const_gamma_mach_rad(4.0 * DEGREES_TO_RADIAN), - m_prediction_too_low(false), - m_prediction_too_high(false) {} - -KinematicDescent4DPredictor::~KinematicDescent4DPredictor() = default; - -void KinematicDescent4DPredictor::SetMembers(const double &mach_descent, const Units::Speed ias_descent, - const Units::Length cruise_altitude, - const Units::Length transition_altitude) { - m_transition_mach = mach_descent; - m_transition_ias = ias_descent; - m_cruise_altitude_msl = cruise_altitude; - - if (IsCruiseMachValid()) { - m_transition_altitude_msl = transition_altitude; - } else { - m_transition_altitude_msl = Units::Infinity(); - } -} - -void KinematicDescent4DPredictor::BuildVerticalPrediction(vector &horizontal_path, - vector &precalc_waypoints, - const WeatherPrediction &weather_prediction, - const Units::Length &start_altitude, - const Units::Length &aircraft_distance_to_go) { - m_start_altitude_msl = start_altitude; - m_prediction_too_low = false; - m_prediction_too_high = false; - HorizontalPath start_pos(horizontal_path.back()); - LOG4CPLUS_TRACE(m_logger, "Building vertical prediction from (" - << start_pos.GetXPositionMeters() << "," << start_pos.GetYPositionMeters() - << "), alt=" << m_start_altitude_msl - << " dtg: " << Units::MetersLength(aircraft_distance_to_go).value()); - m_course_calculator = - DirectionOfFlightCourseCalculator(horizontal_path, TrajectoryIndexProgressionDirection::UNDEFINED); - - ConstrainedVerticalPath(horizontal_path, precalc_waypoints, m_deceleration_mps, m_const_gamma_cas_term_rad, - m_const_gamma_cas_er_rad, m_const_gamma_mach_rad, weather_prediction, - aircraft_distance_to_go); - - TrimDuplicatesFromVerticalPath(); - - m_current_trajectory_index = m_vertical_path.along_path_distance_m.size() - 1; -} - -/* - * Build prediction to current position and then level flight. Use aircraft distance to go to determine - * the segment that contains aircraft position. If the prediction is above or below the aircraft altitude - * by more than the allowable amount at the same distance to go as the aircraft, then do an FPA from the - * last waypoint of the prediction that has a positive FPA angle. - */ -void KinematicDescent4DPredictor::ConstrainedVerticalPath(vector &horizontal_path, - vector &precalc_waypoints, - double deceleration, double const_gamma_cas_term, - double const_gamma_cas_er, double const_gamma_mach, - const WeatherPrediction &weather_prediction, - const Units::Length &aircraft_distance_to_go) { - m_vertical_path_waypoint_index.clear(); - VerticalPath trajTemp; - trajTemp.mass_kg.push_back(-1.0); - trajTemp.time_to_go_sec.push_back(Units::SecondsTime(m_descent_start_time).value()); - trajTemp.along_path_distance_m.push_back(0); - trajTemp.altitude_m.push_back(Units::MetersLength(m_altitude_at_end_of_route).value()); - trajTemp.cas_mps.push_back(Units::MetersPerSecondSpeed(m_ias_at_end_of_route).value()); - trajTemp.mach.push_back(Units::MetersPerSecondSpeed(weather_prediction.GetForecastAtmosphere()->IASToMach( - Units::MetersPerSecondSpeed(m_ias_at_end_of_route), - Units::MetersLength(m_altitude_at_end_of_route))) - .value()); - trajTemp.altitude_rate_mps.push_back(0); - trajTemp.true_airspeed.push_back(weather_prediction.CAS2TAS(m_ias_at_end_of_route, m_altitude_at_end_of_route)); - trajTemp.tas_rate_mps.push_back(0); - trajTemp.theta_radians.push_back(0); - trajTemp.flap_setting.push_back(aaesim::open_source::bada_utils::FlapConfiguration::UNDEFINED); - - Units::Speed vwpara; - Units::Speed vwperp; - Units::Speed Vwx, Vwy; - Units::UnsignedAngle course = m_course_calculator.GetCourseAtPathEnd(); - ComputeWindCoefficients(m_altitude_at_end_of_route, Units::RadiansAngle(course), weather_prediction, vwpara, vwperp, - Vwx, Vwy); - - Units::Speed initialgs = sqrt(Units::sqr(weather_prediction.getAtmosphere()->CAS2TAS(m_ias_at_end_of_route, - m_altitude_at_end_of_route)) - - Units::sqr(vwperp)) + - vwpara; - - trajTemp.gs_mps.push_back(Units::MetersPerSecondSpeed(initialgs).value()); - trajTemp.wind_velocity_east.push_back(Vwx); - trajTemp.wind_velocity_north.push_back(Vwy); - - trajTemp.algorithm_type.push_back(VerticalPath::PredictionAlgorithmType::UNDETERMINED); - - m_vertical_path = trajTemp; - m_vertical_path_waypoint_index.push_back(0); // index for vertical path at first precalc waypoint - - VerticalPath last_state = m_vertical_path; - VerticalPath last_waypoint_state = m_vertical_path; - - double FPA; - double alt1 = min(Units::MetersLength(m_transition_altitude_msl).value(), - Units::MetersLength(m_cruise_altitude_msl).value()); - - if (aircraft_distance_to_go < Units::NauticalMilesLength(1)) { - LOG4CPLUS_WARN(m_logger, - "Attempting to perform constrained vertical path with less than 1 nautical mile to go. " - "Calculating level path, instead."); - m_vertical_path = LevelVerticalPath( - m_vertical_path, - Units::MetersLength( - precalc_waypoints[precalc_waypoints.size() - 1].m_precalc_constraints.constraint_along_path_distance) - .value(), - horizontal_path, weather_prediction, aircraft_distance_to_go); - return; - } - - while (m_vertical_path.altitude_m.back() < alt1) { - if (m_vertical_path.along_path_distance_m.back() > horizontal_path.back().m_path_length_cumulative_meters) { - break; - } - - if (m_vertical_path.altitude_m.back() < 10000 * FEET_TO_METERS) { - m_vertical_path = ConstantCasVerticalPath(m_vertical_path, alt1, horizontal_path, precalc_waypoints, - const_gamma_cas_term, weather_prediction, aircraft_distance_to_go); - } else { - m_vertical_path = ConstantCasVerticalPath(m_vertical_path, alt1, horizontal_path, precalc_waypoints, - const_gamma_cas_er, weather_prediction, aircraft_distance_to_go); - } - if (m_prediction_too_low || m_prediction_too_high) { - break; - } - - if (m_precalculated_constraints.violation_flag) { - if (m_precalculated_constraints.active_flag == ActiveFlagType::SEG_END_LOW_ALT) { - FPA = atan2((Units::MetersLength(m_precalculated_constraints.constraint_altLow).value() - - last_waypoint_state.altitude_m.back()), - (Units::MetersLength(m_precalculated_constraints.constraint_along_path_distance).value() - - last_waypoint_state.along_path_distance_m.back())); - m_vertical_path = ConstantFpaDecelerationVerticalPath( - last_waypoint_state, Units::MetersLength(m_precalculated_constraints.constraint_altLow).value(), - m_deceleration_fpa_mps, - Units::MetersPerSecondSpeed(m_precalculated_constraints.constraint_speedHi).value(), FPA, - horizontal_path, precalc_waypoints, weather_prediction, aircraft_distance_to_go); - if (m_prediction_too_low || m_prediction_too_high) { - break; - } - - m_vertical_path = ConstantGeometricFpaVerticalPath( - m_vertical_path, Units::MetersLength(m_precalculated_constraints.constraint_altLow).value(), FPA, - horizontal_path, precalc_waypoints, weather_prediction, aircraft_distance_to_go); - - } else if (m_precalculated_constraints.active_flag == ActiveFlagType::AT_ALT_ON_SPEED) { - FPA = atan2(Units::MetersLength(m_precalculated_constraints.constraint_altHi).value() - - last_state.altitude_m.back(), - Units::MetersLength(m_precalculated_constraints.constraint_along_path_distance).value() - - last_state.along_path_distance_m.back()); - - if (FPA > 0.10 * PI / 180.0) { - m_vertical_path = ConstantGeometricFpaVerticalPath( - last_state, Units::MetersLength(m_precalculated_constraints.constraint_altHi).value(), FPA, - horizontal_path, precalc_waypoints, weather_prediction, aircraft_distance_to_go); - if (m_prediction_too_low || m_prediction_too_high) { - break; - } - } - - m_vertical_path = LevelVerticalPath( - m_vertical_path, - Units::MetersLength(m_precalculated_constraints.constraint_along_path_distance).value(), - horizontal_path, weather_prediction, aircraft_distance_to_go); - - } else if (m_precalculated_constraints.active_flag == ActiveFlagType::BELOW_ALT_SLOW) { - if (m_precalculated_constraints.index < precalc_waypoints.size()) { - m_vertical_path = ConstantDecelerationVerticalPath( - m_vertical_path, m_precalculated_constraints.constraint_along_path_distance, - m_precalculated_constraints.constraint_altHi, deceleration, - Units::MetersPerSecondSpeed(m_precalculated_constraints.constraint_speedHi).value(), - horizontal_path, weather_prediction, aircraft_distance_to_go); - } - - if (m_prediction_too_low || m_prediction_too_high) { - break; - } - - if ((m_vertical_path.along_path_distance_m.back() > - Units::MetersLength(m_precalculated_constraints.constraint_along_path_distance).value()) && - (m_vertical_path.altitude_m.back() < - Units::MetersLength(m_precalculated_constraints.constraint_altLow).value())) { - // If idle-descent acceleration is below low altitude constraint- - // redo with with a constant FPA deceleration trajectory. - FPA = atan2((Units::MetersLength(m_precalculated_constraints.constraint_altLow).value() - - last_state.altitude_m.back()), - (Units::MetersLength(m_precalculated_constraints.constraint_along_path_distance).value() - - last_state.along_path_distance_m.back())); - Units::DegreesAngle uFPA = Units::RadiansAngle(FPA); - if (FPA > Units::RadiansAngle(DESCENT_ANGLE_MAX).value()) - LOG4CPLUS_WARN(m_logger, "prediction FPA is " << uFPA.value() << " which is greater than " - << DESCENT_ANGLE_WARNING.value()); - if (uFPA < DESCENT_ANGLE_MAX) { - m_vertical_path = ConstantFpaDecelerationVerticalPath( - last_state, Units::MetersLength(m_precalculated_constraints.constraint_altLow).value(), - m_deceleration_fpa_mps, - Units::MetersPerSecondSpeed(m_precalculated_constraints.constraint_speedHi).value(), FPA, - horizontal_path, precalc_waypoints, weather_prediction, aircraft_distance_to_go); - } - } - } else if (m_precalculated_constraints.active_flag == ActiveFlagType::AT_ALT_SLOW) { - if (m_precalculated_constraints.index < precalc_waypoints.size()) { - m_vertical_path = LevelDecelerationVerticalPath( - m_vertical_path, m_precalculated_constraints.constraint_along_path_distance, - m_deceleration_level_mps, - Units::MetersPerSecondSpeed(m_precalculated_constraints.constraint_speedHi).value(), - horizontal_path, weather_prediction, aircraft_distance_to_go); - } - } - - if (m_prediction_too_low || m_prediction_too_high) { - break; - } - } - - last_state = m_vertical_path; - - if (m_vertical_path.along_path_distance_m.back() > - Units::MetersLength(m_precalculated_constraints.constraint_along_path_distance).value()) { - if (last_waypoint_state == m_vertical_path) { - LOG4CPLUS_ERROR(m_logger, "Infinite loop detected...trying to progress"); - return; - } - - last_waypoint_state = m_vertical_path; - while (m_vertical_path_waypoint_index.back() >= last_waypoint_state.altitude_m.size()) { - m_vertical_path_waypoint_index.pop_back(); - } - if (m_vertical_path_waypoint_index.back() < last_waypoint_state.altitude_m.size() - 1) { - m_vertical_path_waypoint_index.push_back(last_waypoint_state.altitude_m.size() - 1); - } - - if (m_vertical_path.altitude_m.back() > m_start_altitude_msl.value()) { - break; - } - } - } - - // Constant Mach segment - last_state = m_vertical_path; - - if (!m_prediction_too_low && !m_prediction_too_high) { - while ((m_vertical_path.altitude_m.back() < m_start_altitude_msl.value()) && - (m_vertical_path.along_path_distance_m.back() <= horizontal_path.back().m_path_length_cumulative_meters)) { - m_vertical_path = - ConstantMachVerticalPath(last_state, m_start_altitude_msl.value(), horizontal_path, precalc_waypoints, - const_gamma_mach, weather_prediction, aircraft_distance_to_go); - - if (m_prediction_too_low || m_prediction_too_high) { - break; - } - - if (m_precalculated_constraints.violation_flag) { - if (m_precalculated_constraints.active_flag == ActiveFlagType::SEG_END_LOW_ALT) { - FPA = atan2(Units::MetersLength(m_precalculated_constraints.constraint_altLow).value() - - last_state.altitude_m.back(), - Units::MetersLength(m_precalculated_constraints.constraint_along_path_distance).value() - - last_state.along_path_distance_m.back()); - - // if unable to reach low altitude constraint due to excessive FPA, then continue - // constantMachVerticalPath - if (FPA > 10 * PI / 180.0) { - LOG4CPLUS_WARN( - m_logger, - "constrainedVerticalPath prediction in mach segment cannot reach low altitude constraint"); - } else if (FPA > 0.10 * PI / 180.0) { - m_vertical_path = ConstantGeometricFpaVerticalPath( - last_state, Units::MetersLength(m_precalculated_constraints.constraint_altLow).value(), FPA, - horizontal_path, precalc_waypoints, weather_prediction, aircraft_distance_to_go); - } else { - m_vertical_path = LevelVerticalPath( - last_state, - Units::MetersLength(m_precalculated_constraints.constraint_along_path_distance).value(), - horizontal_path, weather_prediction, aircraft_distance_to_go); - } - - if (m_prediction_too_low || m_prediction_too_high) { - break; - } - - m_vertical_path = LevelVerticalPath( - m_vertical_path, - Units::MetersLength(m_precalculated_constraints.constraint_along_path_distance).value(), - horizontal_path, weather_prediction, aircraft_distance_to_go); - } else if (m_precalculated_constraints.active_flag == ActiveFlagType::AT_ALT_ON_SPEED) { - FPA = atan2(Units::MetersLength(m_precalculated_constraints.constraint_altHi).value() - - last_state.altitude_m.back(), - Units::MetersLength(m_precalculated_constraints.constraint_along_path_distance).value() - - last_state.along_path_distance_m.back()); - - if (FPA > 0.10 * PI / 180.0) { - m_vertical_path = ConstantGeometricFpaVerticalPath( - last_state, Units::MetersLength(m_precalculated_constraints.constraint_altHi).value(), FPA, - horizontal_path, precalc_waypoints, weather_prediction, aircraft_distance_to_go); - } else { - m_vertical_path = LevelVerticalPath( - last_state, - Units::MetersLength(m_precalculated_constraints.constraint_along_path_distance).value(), - horizontal_path, weather_prediction, aircraft_distance_to_go); - } - m_vertical_path = LevelVerticalPath( - m_vertical_path, - Units::MetersLength(m_precalculated_constraints.constraint_along_path_distance).value(), - horizontal_path, weather_prediction, aircraft_distance_to_go); - } - } - last_state = m_vertical_path; - } - } - - if (m_prediction_too_low) { - LOG4CPLUS_TRACE(m_logger, "Constrained prediction too low. Replanning with FPA."); - } - - if (m_prediction_too_high) { - LOG4CPLUS_TRACE(m_logger, "Constrained prediction too high. Replanning with FPA."); - } - - if (m_prediction_too_low || m_prediction_too_high) { - // Prediction is at aircraft position but not at aircraft altitude. Need to replan using FPA. - int start_index = -1; - bool fpa_start_found = false; - while (!m_vertical_path_waypoint_index.empty()) { - // Find a waypoint at which the predicted altitude allows an FPA to the current position of the aircraft - start_index = m_vertical_path_waypoint_index.back(); - m_vertical_path_waypoint_index.pop_back(); - double distance_to_aircraft_m = Units::MetersLength(aircraft_distance_to_go).value() - - m_vertical_path.along_path_distance_m[start_index]; - double altitude_to_aircraft_m = - Units::MetersLength(m_start_altitude_msl).value() - m_vertical_path.altitude_m[start_index]; - double FPA = atan2(altitude_to_aircraft_m, distance_to_aircraft_m); - if (FPA < Units::RadiansAngle(DESCENT_ANGLE_MAX).value()) { - // found a point where the FPA is not too large. Check if FPA will violate altitude constraints - fpa_start_found = true; - for (unsigned int loop = start_index + 1; loop < precalc_waypoints.size(); loop++) { - if (aircraft_distance_to_go < - precalc_waypoints[loop].m_precalc_constraints.constraint_along_path_distance) - break; - double pred_alt_m = - m_vertical_path.altitude_m[start_index] + - Units::MetersLength(altitude_to_aircraft_m / distance_to_aircraft_m).value() * - Units::MetersLength( - precalc_waypoints[loop].m_precalc_constraints.constraint_along_path_distance) - .value(); - if (pred_alt_m > - Units::MetersLength(precalc_waypoints[loop].m_precalc_constraints.constraint_altHi).value() || - pred_alt_m < - Units::MetersLength(precalc_waypoints[loop].m_precalc_constraints.constraint_altLow).value()) { - fpa_start_found = false; - break; - } - } - if (fpa_start_found) break; - } - } - - if (!fpa_start_found) { - LOG4CPLUS_WARN(m_logger, "Constrained prediction unable to reach aircraft altitude at aircraft position"); - } else { - TrimVerticalPath(m_vertical_path, start_index); - m_vertical_path = - ConstantFpaToCurrentPositionVerticalPath(m_vertical_path, horizontal_path, precalc_waypoints, - const_gamma_mach, weather_prediction, aircraft_distance_to_go); - } - - if (m_vertical_path.altitude_m.back() < Units::MetersLength(m_start_altitude_msl).value()) { - LOG4CPLUS_TRACE(m_logger, - "Did not complete FPA above transition altitude. " << m_vertical_path.altitude_m.back()); - } - - if (m_vertical_path.along_path_distance_m.back() < Units::MetersLength(aircraft_distance_to_go).value()) { - LOG4CPLUS_TRACE(m_logger, "Did not complete FPA above transition altitude"); - } - } - - // Level segment to end of prediction - if (m_vertical_path.altitude_m.back() < Units::MetersLength(m_transition_altitude_msl).value()) { - while (m_vertical_path.along_path_distance_m.back() < horizontal_path.back().m_path_length_cumulative_meters) { - m_precalculated_constraints = - FindActiveConstraint(m_vertical_path.along_path_distance_m.back(), precalc_waypoints); - m_precalculated_constraints = - CheckActiveConstraint(m_vertical_path.along_path_distance_m.back(), m_vertical_path.altitude_m.back(), - m_vertical_path.cas_mps.back(), m_precalculated_constraints, - Units::MetersLength(m_transition_altitude_msl).value()); - if (m_precalculated_constraints.violation_flag && - (m_precalculated_constraints.active_flag == ActiveFlagType::BELOW_ALT_SLOW || - m_precalculated_constraints.active_flag == ActiveFlagType::AT_ALT_SLOW)) { - m_vertical_path = LevelDecelerationVerticalPath( - m_vertical_path, m_deceleration_level_mps, - Units::MetersPerSecondSpeed(m_precalculated_constraints.constraint_speedHi).value(), horizontal_path, - weather_prediction, aircraft_distance_to_go); - } - m_vertical_path = LevelVerticalPath(m_vertical_path, horizontal_path.back().m_path_length_cumulative_meters, - horizontal_path, weather_prediction, aircraft_distance_to_go); - } - } - m_vertical_path = LevelVerticalPath(m_vertical_path, horizontal_path.back().m_path_length_cumulative_meters, - horizontal_path, weather_prediction, aircraft_distance_to_go); -} - -VerticalPath KinematicDescent4DPredictor::ConstantCasVerticalPath( - const VerticalPath &vertical_path, double altitude_at_end, vector &horizontal_path, - vector &precalc_waypoints, double CAS_gamma, const WeatherPrediction &weather_prediction, - const Units::Length &aircraft_distance_to_go) { - VerticalPath result; - - const double delta_t = -TIME_STEP_SECONDS; - double dist = vertical_path.along_path_distance_m[vertical_path.along_path_distance_m.size() - 1]; - double v_cas = vertical_path.cas_mps[vertical_path.cas_mps.size() - 1]; - double h = vertical_path.altitude_m[vertical_path.altitude_m.size() - 1]; - - bool bracket_found = false; - - result = vertical_path; - - m_precalculated_constraints = FindActiveConstraint(dist, precalc_waypoints); - m_precalculated_constraints = CheckActiveConstraint(dist, h, v_cas, m_precalculated_constraints, - Units::MetersLength(m_transition_altitude_msl).value()); - - while (h < altitude_at_end && m_precalculated_constraints.active_flag <= ActiveFlagType::BELOW_ALT_ON_SPEED) { - double v_tas = Units::MetersPerSecondSpeed(weather_prediction.getAtmosphere()->CAS2TAS( - Units::MetersPerSecondSpeed(v_cas), Units::MetersLength(h))) - .value(); - - double esf; - double dh_dt; - double dv_dh; - double dv_dt; - double h_new; - double dist_new; - double v_tas_new; - double v_cas_new; - double curr_time; - - Units::UnsignedRadiansAngle course; - m_course_calculator.CalculateCourseAtAlongPathDistance(Units::MetersLength(dist), course); - - Units::Speed vwpara; - Units::Speed vwperp; - Units::Speed Vwx, Vwy; - - ComputeWindCoefficients(Units::MetersLength(h), Units::RadiansAngle(course), weather_prediction, vwpara, vwperp, - Vwx, Vwy); - - esf = CalculateEsfUsingConstantCAS(v_tas, h, - weather_prediction.getAtmosphere()->GetTemperature(Units::MetersLength(h))); - - // climb/descent rate - dh_dt = -v_tas * sin(CAS_gamma); - - // change in speed with respect to altitude - dv_dh = (1 / esf - 1) * (GRAVITY_METERS_PER_SECOND / v_tas); - - // acceleration rate - dv_dt = dv_dh * dh_dt; - - h_new = dh_dt * delta_t + h; - v_tas_new = dv_dt * delta_t + v_tas; - - v_cas_new = Units::MetersPerSecondSpeed(weather_prediction.getAtmosphere()->TAS2CAS( - Units::MetersPerSecondSpeed(v_tas_new), Units::MetersLength(h_new))) - .value(); - - double gsnew = sqrt(pow(v_tas_new * cos(CAS_gamma), 2) - pow(Units::MetersPerSecondSpeed(vwperp).value(), 2)) + - Units::MetersPerSecondSpeed(vwpara).value(); - - dist_new = dist - delta_t * gsnew; - - const double mach = weather_prediction.GetForecastAtmosphere()->IASToMach(Units::MetersPerSecondSpeed(v_cas_new), - Units::MetersLength(h_new)); - - result.along_path_distance_m.push_back(dist_new); - result.cas_mps.push_back(v_cas_new); - result.mach.push_back(mach); - result.altitude_m.push_back(h_new); - result.altitude_rate_mps.push_back(dh_dt); - result.true_airspeed.push_back(Units::MetersPerSecondSpeed(v_tas_new)); - result.tas_rate_mps.push_back(dv_dt); - result.theta_radians.push_back(CAS_gamma); - result.gs_mps.push_back(gsnew); - result.wind_velocity_east.push_back(Vwx); - result.wind_velocity_north.push_back(Vwy); - result.algorithm_type.push_back(VerticalPath::PredictionAlgorithmType::CONSTANT_CAS); - result.mass_kg.push_back(-1.0); - curr_time = result.time_to_go_sec.back(); - result.time_to_go_sec.push_back(curr_time + fabs(delta_t)); - result.flap_setting.push_back(aaesim::open_source::bada_utils::FlapConfiguration::UNDEFINED); - - if (!bracket_found && dist_new > Units::MetersLength(aircraft_distance_to_go).value()) { - bracket_found = true; - - if ((h - Units::MetersLength(m_vertical_tolerance_distance).value()) > - Units::MetersLength(m_start_altitude_msl).value()) { - LOG4CPLUS_TRACE(m_logger, "Prediction alt too high. pred: " - << h_new - << " start_alt: " << Units::MetersLength(m_start_altitude_msl).value()); - m_prediction_too_high = true; - return result; - } - if ((h_new + Units::MetersLength(m_vertical_tolerance_distance).value()) < - Units::MetersLength(m_start_altitude_msl).value()) { - LOG4CPLUS_TRACE(m_logger, "Prediction alt too low. pred: " - << h_new - << " start_alt: " << Units::MetersLength(m_start_altitude_msl).value()); - m_prediction_too_low = true; - return result; - } - } - - dist = dist_new; // in meters - h = h_new; // in meters - v_cas = v_cas_new; // in meters per second - - if (m_precalculated_constraints.active_flag == ActiveFlagType::BELOW_ALT_ON_SPEED) { - m_precalculated_constraints = CheckActiveConstraint(dist, h, v_cas, m_precalculated_constraints, - Units::MetersLength(m_transition_altitude_msl).value()); - } - } - - if ((aircraft_distance_to_go < Units::MetersLength(Units::Infinity())) && - (h - Units::MetersLength(m_vertical_tolerance_distance).value()) > - Units::MetersLength(m_start_altitude_msl).value()) { - LOG4CPLUS_TRACE(m_logger, "Prediction alt too high. pred: " << h << " start_alt: " - << Units::MetersLength(m_start_altitude_msl).value()); - m_prediction_too_high = true; - } - - return result; -} - -VerticalPath KinematicDescent4DPredictor::ConstantMachVerticalPath( - const VerticalPath &vertical_path, double altitude_at_end, vector &horizontal_path, - vector &precalc_waypoints, double gamma, const WeatherPrediction &weather_prediction, - const Units::Length &aircraft_distance_to_go) { - VerticalPath result; - - const double delta_t = -TIME_STEP_SECONDS; - double dist = vertical_path.along_path_distance_m[vertical_path.along_path_distance_m.size() - 1]; - double v_cas = vertical_path.cas_mps[vertical_path.cas_mps.size() - 1]; - double h = vertical_path.altitude_m[vertical_path.altitude_m.size() - 1]; - - result = vertical_path; - - m_precalculated_constraints = FindActiveConstraint(dist, precalc_waypoints); - m_precalculated_constraints = CheckActiveConstraint(dist, h, v_cas, m_precalculated_constraints, - Units::MetersLength(m_transition_altitude_msl).value()); - - while ((h < altitude_at_end) && (m_precalculated_constraints.active_flag <= ActiveFlagType::BELOW_ALT_ON_SPEED || - m_precalculated_constraints.active_flag == ActiveFlagType::BELOW_ALT_SLOW || - m_precalculated_constraints.active_flag == ActiveFlagType::AT_ALT_SLOW)) { - double v_tas = Units::MetersPerSecondSpeed(weather_prediction.getAtmosphere()->CAS2TAS( - Units::MetersPerSecondSpeed(v_cas), Units::MetersLength(h))) - .value(); - double esf; - double dh_dt; - double dv_dh; - double dv_dt; - double h_new; - double dist_new; - double v_tas_new; - double v_cas_new; - double curr_time; - - bool bracket_found = false; - - Units::Speed vwpara; - Units::Speed vwperp; - Units::Speed Vwx, Vwy; - Units::UnsignedRadiansAngle course; - m_course_calculator.CalculateCourseAtAlongPathDistance(Units::MetersLength(dist), course); - ComputeWindCoefficients(Units::MetersLength(h), Units::RadiansAngle(course), weather_prediction, vwpara, vwperp, - Vwx, Vwy); - - esf = CalculateEsfUsingConstantMach(v_tas, h, - weather_prediction.getAtmosphere()->GetTemperature(Units::MetersLength(h))); - - // climb/descent rate - dh_dt = -v_tas * sin(gamma); - - // change in speed with respect to altitude - dv_dh = (1 / esf - 1) * (GRAVITY_METERS_PER_SECOND / v_tas); - - // acceleration rate - dv_dt = dv_dh * dh_dt; - - h_new = dh_dt * delta_t + h; - - v_tas_new = dv_dt * delta_t + v_tas; - - v_cas_new = Units::MetersPerSecondSpeed(weather_prediction.getAtmosphere()->TAS2CAS( - Units::MetersPerSecondSpeed(v_tas_new), Units::MetersLength(h_new))) - .value(); - - double gsnew = sqrt(pow(v_tas_new * cos(gamma), 2) - pow(Units::MetersPerSecondSpeed(vwperp).value(), 2)) + - Units::MetersPerSecondSpeed(vwpara).value(); - - dist_new = dist - delta_t * gsnew; - - const double mach = weather_prediction.GetForecastAtmosphere()->IASToMach(Units::MetersPerSecondSpeed(v_cas_new), - Units::MetersLength(h_new)); - - result.along_path_distance_m.push_back(dist_new); - result.cas_mps.push_back(v_cas_new); - result.mach.push_back(mach); - result.altitude_m.push_back(h_new); - result.altitude_rate_mps.push_back(dh_dt); - result.true_airspeed.push_back(Units::MetersPerSecondSpeed(v_tas_new)); - result.tas_rate_mps.push_back(dv_dt); - result.theta_radians.push_back(gamma); - result.gs_mps.push_back(gsnew); - result.wind_velocity_east.push_back(Vwx); - result.wind_velocity_north.push_back(Vwy); - result.algorithm_type.push_back(VerticalPath::PredictionAlgorithmType::CONSTANT_MACH); - result.mass_kg.push_back(-1.0); - result.flap_setting.push_back(aaesim::open_source::bada_utils::FlapConfiguration::UNDEFINED); - - curr_time = result.time_to_go_sec.back(); - - result.time_to_go_sec.push_back(curr_time + fabs(delta_t)); // adds last time +0.5 to the end since - // fabs(delta_t) is 0.5 - - if (!bracket_found && dist_new > Units::MetersLength(aircraft_distance_to_go).value()) { - bracket_found = true; - if ((h - Units::MetersLength(m_vertical_tolerance_distance).value()) > - Units::MetersLength(m_start_altitude_msl).value()) { - LOG4CPLUS_TRACE(m_logger, "Prediction alt too high. pred: " - << h_new - << " start_alt: " << Units::MetersLength(m_start_altitude_msl).value()); - m_prediction_too_high = true; - return result; - } - if ((h_new + Units::MetersLength(m_vertical_tolerance_distance).value()) < - Units::MetersLength(m_start_altitude_msl).value()) { - LOG4CPLUS_TRACE(m_logger, "Prediction alt too low. pred: " - << h_new - << " start_alt: " << Units::MetersLength(m_start_altitude_msl).value()); - m_prediction_too_low = true; - return result; - } - } - - dist = dist_new; // in meters - h = h_new; // in meters - v_cas = v_cas_new; // in meters per second - - if (m_precalculated_constraints.active_flag == ActiveFlagType::BELOW_ALT_ON_SPEED || - m_precalculated_constraints.active_flag == ActiveFlagType::BELOW_ALT_SLOW || - m_precalculated_constraints.active_flag == ActiveFlagType::AT_ALT_SLOW) { - m_precalculated_constraints = CheckActiveConstraint(dist, h, v_cas, m_precalculated_constraints, - Units::MetersLength(m_transition_altitude_msl).value()); - } - } - - if ((aircraft_distance_to_go < Units::MetersLength(Units::Infinity())) && - (h - Units::MetersLength(m_vertical_tolerance_distance).value()) > - Units::MetersLength(m_start_altitude_msl).value()) { - LOG4CPLUS_TRACE(m_logger, "Prediction alt too high. pred: " << h << " start_alt: " - << Units::MetersLength(m_start_altitude_msl).value()); - m_prediction_too_high = true; - } - - return result; -} - -VerticalPath KinematicDescent4DPredictor::ConstantGeometricFpaVerticalPath( - const VerticalPath &vertical_path, double altitude_at_end, double flight_path_angle, - vector &horizontal_path, vector &precalc_waypoints, - const WeatherPrediction &weather_prediction, const Units::Length &aircraft_distance_to_go) { - VerticalPath result = vertical_path; - - const double delta_t = -TIME_STEP_SECONDS; - double v_cas = vertical_path.cas_mps[vertical_path.cas_mps.size() - 1]; - double dist = vertical_path.along_path_distance_m[vertical_path.along_path_distance_m.size() - 1]; - double h = vertical_path.altitude_m[vertical_path.altitude_m.size() - 1]; - double theta = vertical_path.theta_radians[vertical_path.theta_radians.size() - 1]; - - bool bracket_found = false; - - while (h < altitude_at_end) { - double v_tas; - double esf; - double dh_dt; - double dv_dh; - double dv_dt; - double h_new; - double dist_new; - double v_tas_new; - double v_cas_new; - double curr_time; - - Units::Speed vwpara; - Units::Speed vwperp; - Units::Speed Vwx, Vwy; - Units::UnsignedRadiansAngle course; - m_course_calculator.CalculateCourseAtAlongPathDistance(Units::MetersLength(dist), course); - ComputeWindCoefficients(Units::MetersLength(h), Units::RadiansAngle(course), weather_prediction, vwpara, vwperp, - Vwx, Vwy); - v_tas = Units::MetersPerSecondSpeed(weather_prediction.getAtmosphere()->CAS2TAS( - Units::MetersPerSecondSpeed(v_cas), Units::MetersLength(h))) - .value(); - - if (h <= Units::MetersLength(m_transition_altitude_msl).value()) { - esf = CalculateEsfUsingConstantCAS(v_tas, h, - weather_prediction.getAtmosphere()->GetTemperature(Units::MetersLength(h))); - } else { - esf = CalculateEsfUsingConstantMach( - v_tas, h, weather_prediction.getAtmosphere()->GetTemperature(Units::MetersLength(h))); - } - - // climb/descent rate - dh_dt = -v_tas * sin(theta); - - // change in speed with respect to altitude - dv_dh = (1 / esf - 1) * (GRAVITY_METERS_PER_SECOND / v_tas); - - // acceleration rate - dv_dt = dv_dh * dh_dt; - - h_new = dh_dt * delta_t + h; - - v_tas_new = dv_dt * delta_t + v_tas; - - v_cas_new = Units::MetersPerSecondSpeed(weather_prediction.getAtmosphere()->TAS2CAS( - Units::MetersPerSecondSpeed(v_tas_new), Units::MetersLength(h_new))) - .value(); - - double gsnew = sqrt(pow(v_tas_new * cos(theta), 2) - pow(Units::MetersPerSecondSpeed(vwperp).value(), 2)) + - Units::MetersPerSecondSpeed(vwpara).value(); - - dist_new = gsnew * (-delta_t) + dist; - - double dh_dt_update = -gsnew * tan(flight_path_angle); - - if (fabs(dh_dt_update) > v_tas_new) { - string msg = string("ConstantGeometricVerticalPath is about to take asin() of a number greater than 1.0\n") + - string("This will result in theta becoming NaN"); - LOG4CPLUS_FATAL(m_logger, msg); - throw logic_error(msg); - } - double theta_new = asin(-dh_dt_update / v_tas_new); - - const double mach = weather_prediction.GetForecastAtmosphere()->IASToMach(Units::MetersPerSecondSpeed(v_cas_new), - Units::MetersLength(h_new)); - - result.along_path_distance_m.push_back(dist_new); - result.cas_mps.push_back(v_cas_new); - result.mach.push_back(mach); - result.altitude_m.push_back(h_new); - result.altitude_rate_mps.push_back(dh_dt); - result.true_airspeed.push_back(Units::MetersPerSecondSpeed(v_tas_new)); - result.tas_rate_mps.push_back(dv_dt); - result.theta_radians.push_back(theta_new); - result.gs_mps.push_back(gsnew); - result.wind_velocity_east.push_back(Vwx); - result.wind_velocity_north.push_back(Vwy); - result.algorithm_type.push_back(VerticalPath::PredictionAlgorithmType::FPA); - result.mass_kg.push_back(-1.0); - result.flap_setting.push_back(aaesim::open_source::bada_utils::FlapConfiguration::UNDEFINED); - - curr_time = result.time_to_go_sec.back(); - result.time_to_go_sec.push_back(curr_time + fabs(delta_t)); // adds last time +0.5 to the end since - // fabs(delta_t) is 0.5 - - if (!bracket_found && dist_new > Units::MetersLength(aircraft_distance_to_go).value()) { - bracket_found = true; - if (h - Units::MetersLength(m_vertical_tolerance_distance).value() > - Units::MetersLength(m_start_altitude_msl).value()) { - LOG4CPLUS_TRACE(m_logger, "Prediction alt too high. pred: " - << h_new - << " start_alt: " << Units::MetersLength(m_start_altitude_msl).value()); - m_prediction_too_high = true; - return result; - } - if ((h_new + Units::MetersLength(m_vertical_tolerance_distance).value()) < - Units::MetersLength(m_start_altitude_msl).value()) { - LOG4CPLUS_TRACE(m_logger, "Prediction alt too low. pred: " - << h_new - << " start_alt: " << Units::MetersLength(m_start_altitude_msl).value()); - m_prediction_too_low = true; - return result; - } - } - - dist = dist_new; // in meters - h = h_new; // in meters - v_cas = v_cas_new; // in meters per second - theta = theta_new; - } - - if ((aircraft_distance_to_go < Units::MetersLength(Units::Infinity())) && - (h - Units::MetersLength(m_vertical_tolerance_distance).value()) > - Units::MetersLength(m_start_altitude_msl).value()) { - LOG4CPLUS_TRACE(m_logger, "Prediction alt too high. pred: " << h << " start_alt: " - << Units::MetersLength(m_start_altitude_msl).value()); - m_prediction_too_high = true; - } - - return result; -} - -VerticalPath KinematicDescent4DPredictor::ConstantFpaDecelerationVerticalPath( - const VerticalPath &vertical_path, double altitude_at_end, double deceleration_mps, double velocity_cas_end, - double flight_path_angle, vector &horizontal_path, vector &precalc_waypoints, - const WeatherPrediction &weather_prediction, const Units::Length &aircraft_distance_to_go) { - VerticalPath result = vertical_path; - - const double delta_t = -TIME_STEP_SECONDS; - double v_cas = vertical_path.cas_mps[vertical_path.cas_mps.size() - 1]; - double dist = vertical_path.along_path_distance_m[vertical_path.along_path_distance_m.size() - 1]; - double h = vertical_path.altitude_m[vertical_path.altitude_m.size() - 1]; - double theta = vertical_path.theta_radians[vertical_path.theta_radians.size() - 1]; - - while ((h < altitude_at_end) && (v_cas < velocity_cas_end)) { - double v_tas = Units::MetersPerSecondSpeed(weather_prediction.getAtmosphere()->CAS2TAS( - Units::MetersPerSecondSpeed(v_cas), Units::MetersLength(h))) - .value(); - Units::KilogramsMeterDensity rho; - Units::Pressure p; - double dh_dt; - double dv_dt; - double h_new; - double dist_new; - double v_tas_new; - double v_cas_new; - double curr_time; - - bool bracket_found = false; - - Units::MetersPerSecondSpeed Vwx, Vwy; - Units::HertzFrequency dVwx_dh, dVwy_dh; - Units::UnsignedRadiansAngle course; - m_course_calculator.CalculateCourseAtAlongPathDistance(Units::MetersLength(dist), course); - m_wind_calculator.ComputeWindGradients(Units::MetersLength(h), weather_prediction, Vwx, Vwy, dVwx_dh, dVwy_dh); - - double Vw_para = Vwx.value() * cos(course) + Vwy.value() * sin(course); - double Vw_perp = -Vwx.value() * sin(course) + Vwy.value() * cos(course); - - weather_prediction.getAtmosphere()->AirDensity(Units::MetersLength(h), rho, p); - - double gs_new = sqrt(pow(v_tas * cos(theta), 2) - pow(Vw_perp, 2)) + Vw_para; - - // climb/descent rate - dh_dt = -gs_new * tan(flight_path_angle); - dv_dt = -deceleration_mps; - - h_new = dh_dt * delta_t + h; - v_tas_new = dv_dt * delta_t + v_tas; - v_cas_new = Units::MetersPerSecondSpeed(weather_prediction.getAtmosphere()->TAS2CAS( - Units::MetersPerSecondSpeed(v_tas_new), Units::MetersLength(h_new))) - .value(); - dist_new = gs_new * (-delta_t) + dist; - if (fabs(dh_dt) > v_tas_new) { - string msg = string( - "ConstantFpaDecelerationVerticalPath is about to take asin() of a number greater " - "than 1.0\n") + - string("This will result in theta becoming NaN"); - LOG4CPLUS_FATAL(m_logger, msg); - throw logic_error(msg); - } - theta = asin((-dh_dt) / v_tas_new); - - const double mach = weather_prediction.GetForecastAtmosphere()->IASToMach(Units::MetersPerSecondSpeed(v_cas_new), - Units::MetersLength(h_new)); - - result.along_path_distance_m.push_back(dist_new); - result.cas_mps.push_back(v_cas_new); - result.mach.push_back(mach); - result.altitude_m.push_back(h_new); - result.altitude_rate_mps.push_back(dh_dt); - result.true_airspeed.push_back(Units::MetersPerSecondSpeed(v_tas_new)); - result.tas_rate_mps.push_back(dv_dt); - result.theta_radians.push_back(theta); - result.gs_mps.push_back(gs_new); - result.mass_kg.push_back(-1.0); - result.wind_velocity_east.push_back(Vwx); - result.wind_velocity_north.push_back(Vwy); - result.algorithm_type.push_back(VerticalPath::PredictionAlgorithmType::FPA_DECEL); - result.flap_setting.push_back(aaesim::open_source::bada_utils::FlapConfiguration::UNDEFINED); - curr_time = result.time_to_go_sec.back(); - result.time_to_go_sec.push_back(curr_time + fabs(delta_t)); // adds last time +0.5 to the end since - // fabs(delta_t) is 0.5 - - if (!bracket_found && dist_new > Units::MetersLength(aircraft_distance_to_go).value()) { - bracket_found = true; - if ((h - Units::MetersLength(m_vertical_tolerance_distance).value()) > - Units::MetersLength(m_start_altitude_msl).value()) { - LOG4CPLUS_TRACE(m_logger, "Prediction alt too high. pred: " - << h_new - << " start_alt: " << Units::MetersLength(m_start_altitude_msl).value()); - m_prediction_too_high = true; - return result; - } - if ((h_new + Units::MetersLength(m_vertical_tolerance_distance).value()) < - Units::MetersLength(m_start_altitude_msl).value()) { - LOG4CPLUS_TRACE(m_logger, "Prediction alt too low. pred: " - << h_new - << " start_alt: " << Units::MetersLength(m_start_altitude_msl).value()); - m_prediction_too_low = true; - return result; - } - } - - dist = dist_new; // in meters - h = h_new; // in meters - v_cas = v_cas_new; // in meters per second - } - - if ((aircraft_distance_to_go < Units::MetersLength(Units::Infinity())) && - (h - Units::MetersLength(m_vertical_tolerance_distance).value()) > - Units::MetersLength(m_start_altitude_msl).value()) { - LOG4CPLUS_TRACE(m_logger, "Prediction alt too high. pred: " << h << " start_alt: " - << Units::MetersLength(m_start_altitude_msl).value()); - m_prediction_too_high = true; - } - - return result; -} - -VerticalPath KinematicDescent4DPredictor::LevelDecelerationVerticalPath(const VerticalPath &vertical_path, - double deceleration, double velocity_cas_end, - vector &horizontal_path, - const WeatherPrediction &weather_prediction, - const Units::Length &aircraft_distance_to_go) { - VerticalPath result = vertical_path; - - const double delta_t = -TIME_STEP_SECONDS; - double v_cas = vertical_path.cas_mps[vertical_path.cas_mps.size() - 1]; - double dist = vertical_path.along_path_distance_m[vertical_path.along_path_distance_m.size() - 1]; - double h = vertical_path.altitude_m[vertical_path.altitude_m.size() - 1]; - - while (v_cas < velocity_cas_end) { - double v_tas = Units::MetersPerSecondSpeed(weather_prediction.getAtmosphere()->CAS2TAS( - Units::MetersPerSecondSpeed(v_cas), Units::MetersLength(h))) - .value(); - double dh_dt; - double dv_dt; - double h_new; - double dist_new; - double v_tas_new; - double v_cas_new; - double curr_time; - - Units::Speed vwpara; - Units::Speed vwperp; - Units::Speed Vwx, Vwy; - Units::UnsignedRadiansAngle course; - m_course_calculator.CalculateCourseAtAlongPathDistance(Units::MetersLength(dist), course); - ComputeWindCoefficients(Units::MetersLength(h), Units::RadiansAngle(course), weather_prediction, vwpara, vwperp, - Vwx, Vwy); - - // climb/descent rate - dh_dt = 0.0; - double theta_new = 0.0; - - // acceleration rate - dv_dt = -deceleration; - - h_new = dh_dt * delta_t + h; - v_tas_new = dv_dt * delta_t + v_tas; - v_cas_new = Units::MetersPerSecondSpeed(weather_prediction.getAtmosphere()->TAS2CAS( - Units::MetersPerSecondSpeed(v_tas_new), Units::MetersLength(h_new))) - .value(); - - double gsnew = sqrt(pow(v_tas_new * cos(theta_new), 2) - pow(Units::MetersPerSecondSpeed(vwperp).value(), 2)) + - Units::MetersPerSecondSpeed(vwpara).value(); - - dist_new = dist - delta_t * gsnew; - - const double mach = weather_prediction.GetForecastAtmosphere()->IASToMach(Units::MetersPerSecondSpeed(v_cas_new), - Units::MetersLength(h_new)); - - result.along_path_distance_m.push_back(dist_new); - result.cas_mps.push_back(v_cas_new); - result.mach.push_back(mach); - result.altitude_m.push_back(h_new); - result.altitude_rate_mps.push_back(dh_dt); - result.true_airspeed.push_back(Units::MetersPerSecondSpeed(v_tas_new)); - result.tas_rate_mps.push_back(dv_dt); - result.theta_radians.push_back(theta_new); - result.gs_mps.push_back(gsnew); - result.mass_kg.push_back(-1.0); - result.wind_velocity_east.push_back(Vwx); - result.wind_velocity_north.push_back(Vwy); - result.algorithm_type.push_back(VerticalPath::PredictionAlgorithmType::LEVEL_DECEL1); - result.flap_setting.push_back(aaesim::open_source::bada_utils::FlapConfiguration::UNDEFINED); - - curr_time = result.time_to_go_sec.back(); - result.time_to_go_sec.push_back(curr_time + fabs(delta_t)); // adds last time +0.5 to the end since - // fabs(delta_t) is 0.5 - - dist = dist_new; // in meters - h = h_new; // in meters - v_cas = v_cas_new; // in meters per second - } - - if ((result.along_path_distance_m.back() > Units::MetersLength(aircraft_distance_to_go).value()) && - ((h + Units::MetersLength(m_vertical_tolerance_distance).value()) < - Units::MetersLength(m_start_altitude_msl).value())) { - LOG4CPLUS_TRACE(m_logger, "Prediction alt too low. pred: " << h << " start_alt: " - << Units::MetersLength(m_start_altitude_msl).value()); - m_prediction_too_low = true; - } - - return result; -} - -VerticalPath KinematicDescent4DPredictor::LevelDecelerationVerticalPath(const VerticalPath &vertical_path, - Units::Length distance_to_go, - double deceleration, double velocity_cas_end, - vector &horizontal_path, - const WeatherPrediction &weather_prediction, - const Units::Length &aircraft_distance_to_go) { - VerticalPath result = vertical_path; - - const double delta_t = -TIME_STEP_SECONDS; - double v_cas = vertical_path.cas_mps[vertical_path.cas_mps.size() - 1]; - double dist = vertical_path.along_path_distance_m[vertical_path.along_path_distance_m.size() - 1]; - double h = vertical_path.altitude_m[vertical_path.altitude_m.size() - 1]; - - double distEnd = Units::MetersLength(distance_to_go).value(); - - while (v_cas < velocity_cas_end && dist <= distEnd) { - double v_tas = Units::MetersPerSecondSpeed(weather_prediction.getAtmosphere()->CAS2TAS( - Units::MetersPerSecondSpeed(v_cas), Units::MetersLength(h))) - .value(); - double dh_dt; - double dv_dt; - double h_new; - double dist_new; - double v_tas_new; - double v_cas_new; - double curr_time; - - Units::Speed vwpara; - Units::Speed vwperp; - Units::Speed Vwx, Vwy; - Units::UnsignedRadiansAngle course; - m_course_calculator.CalculateCourseAtAlongPathDistance(Units::MetersLength(dist), course); - ComputeWindCoefficients(Units::MetersLength(h), Units::RadiansAngle(course), weather_prediction, vwpara, vwperp, - Vwx, Vwy); - - // climb/descent rate - dh_dt = 0.0; - double theta_new = 0.0; - - // acceleration rate - dv_dt = -deceleration; - - h_new = dh_dt * delta_t + h; - v_tas_new = dv_dt * delta_t + v_tas; - v_cas_new = Units::MetersPerSecondSpeed(weather_prediction.getAtmosphere()->TAS2CAS( - Units::MetersPerSecondSpeed(v_tas_new), Units::MetersLength(h_new))) - .value(); - - double gsnew = sqrt(pow(v_tas_new * cos(theta_new), 2) - pow(Units::MetersPerSecondSpeed(vwperp).value(), 2)) + - Units::MetersPerSecondSpeed(vwpara).value(); - - dist_new = dist - delta_t * gsnew; - - const double mach = weather_prediction.GetForecastAtmosphere()->IASToMach(Units::MetersPerSecondSpeed(v_cas_new), - Units::MetersLength(h_new)); - - result.along_path_distance_m.push_back(dist_new); - result.cas_mps.push_back(v_cas_new); - result.mach.push_back(mach); - result.altitude_m.push_back(h_new); - result.altitude_rate_mps.push_back(dh_dt); - result.true_airspeed.push_back(Units::MetersPerSecondSpeed(v_tas_new)); - result.tas_rate_mps.push_back(dv_dt); - result.theta_radians.push_back(theta_new); - result.gs_mps.push_back(gsnew); - result.mass_kg.push_back(-1.0); - result.wind_velocity_east.push_back(Vwx); - result.wind_velocity_north.push_back(Vwy); - result.algorithm_type.push_back(VerticalPath::PredictionAlgorithmType::LEVEL_DECEL2); - result.flap_setting.push_back(aaesim::open_source::bada_utils::FlapConfiguration::UNDEFINED); - - curr_time = result.time_to_go_sec.back(); - result.time_to_go_sec.push_back(curr_time + fabs(delta_t)); // adds last time +0.5 to the end since - // fabs(delta_t) is 0.5 - - dist = dist_new; // in meters - h = h_new; // in meters - v_cas = v_cas_new; // in meters per second - } - - if ((result.along_path_distance_m.back() > Units::MetersLength(aircraft_distance_to_go).value()) && - ((h + Units::MetersLength(m_vertical_tolerance_distance).value()) < - Units::MetersLength(m_start_altitude_msl).value())) { - LOG4CPLUS_TRACE(m_logger, "Prediction alt too low. pred: " << h << " start_alt: " - << Units::MetersLength(m_start_altitude_msl).value()); - m_prediction_too_low = true; - } - - return result; -} - -VerticalPath KinematicDescent4DPredictor::LevelVerticalPath(const VerticalPath &vertical_path, double x_end, - vector &horizontal_path, - const WeatherPrediction &weather_prediction, - const Units::Length &aircraft_distance_to_go) { - VerticalPath result; - - const double delta_t = -TIME_STEP_SECONDS; - double v_cas = vertical_path.cas_mps[vertical_path.cas_mps.size() - 1]; - double h = vertical_path.altitude_m[vertical_path.altitude_m.size() - 1]; - double dist = vertical_path.along_path_distance_m[vertical_path.along_path_distance_m.size() - 1]; - - result = vertical_path; - - while (fabs(dist) < fabs(x_end)) { - double v_tas = Units::MetersPerSecondSpeed(weather_prediction.getAtmosphere()->CAS2TAS( - Units::MetersPerSecondSpeed(v_cas), Units::MetersLength(h))) - .value(); - double dh_dt; - double dv_dt; - double h_new; - double dist_new; - double v_tas_new; - double v_cas_new; - double theta_new; - double curr_time; - double gsnew; - - Units::Speed vwpara; - Units::Speed vwperp; - Units::Speed Vwx, Vwy; - Units::UnsignedRadiansAngle course; - m_course_calculator.CalculateCourseAtAlongPathDistance(Units::MetersLength(dist), course); - ComputeWindCoefficients(Units::MetersLength(h), Units::RadiansAngle(course), weather_prediction, vwpara, vwperp, - Vwx, Vwy); - - // climb/descent rate - dh_dt = 0.0; - - // acceleration rate - dv_dt = 0.0; - - theta_new = 0.0; - - h_new = dh_dt * delta_t + h; - - v_tas_new = dv_dt * delta_t + v_tas; - - v_cas_new = Units::MetersPerSecondSpeed(weather_prediction.getAtmosphere()->TAS2CAS( - Units::MetersPerSecondSpeed(v_tas_new), Units::MetersLength(h_new))) - .value(); - - gsnew = sqrt(pow(v_tas_new * cos(theta_new), 2) - pow(Units::MetersPerSecondSpeed(vwperp).value(), 2)) + - Units::MetersPerSecondSpeed(vwpara).value(); - - dist_new = gsnew * (-delta_t) + dist; - - const double mach = weather_prediction.GetForecastAtmosphere()->IASToMach(Units::MetersPerSecondSpeed(v_cas_new), - Units::MetersLength(h_new)); - - result.along_path_distance_m.push_back(dist_new); - result.cas_mps.push_back(v_cas_new); - result.mach.push_back(mach); - result.altitude_m.push_back(h_new); - result.altitude_rate_mps.push_back(dh_dt); - result.true_airspeed.push_back(Units::MetersPerSecondSpeed(v_tas_new)); - result.tas_rate_mps.push_back(dv_dt); - result.theta_radians.push_back(theta_new); - result.gs_mps.push_back(gsnew); - result.wind_velocity_east.push_back(Vwx); - result.wind_velocity_north.push_back(Vwy); - result.algorithm_type.push_back(VerticalPath::PredictionAlgorithmType::LEVEL); - result.flap_setting.push_back(aaesim::open_source::bada_utils::FlapConfiguration::UNDEFINED); - result.mass_kg.push_back(-1.0); - - curr_time = result.time_to_go_sec.back(); - result.time_to_go_sec.push_back(curr_time + fabs(delta_t)); // adds last time +0.5 to the end since - // fabs(delta_t) is 0.5 - - // set values for next iteration of the loop - dist = dist_new; - h = h_new; - v_cas = v_cas_new; - } - - if ((result.along_path_distance_m.back() > Units::MetersLength(aircraft_distance_to_go).value()) && - ((h + Units::MetersLength(m_vertical_tolerance_distance).value()) < - Units::MetersLength(m_start_altitude_msl).value())) { - LOG4CPLUS_TRACE(m_logger, "Prediction alt too low. pred: " << h << " start_alt: " - << Units::MetersLength(m_start_altitude_msl).value()); - m_prediction_too_low = true; - } - - return result; -} - -// TODO: change velocity_cas_end to Units -VerticalPath KinematicDescent4DPredictor::ConstantDecelerationVerticalPath( - const VerticalPath &vertical_path, Units::Length distance_to_go, Units::Length altitude_high, double deceleration, - double velocity_cas_end, vector &horizontal_path, const WeatherPrediction &weather_prediction, - const Units::Length &aircraft_distance_to_go) { - VerticalPath result = vertical_path; - - const double delta_t = -TIME_STEP_SECONDS; - double dist = vertical_path.along_path_distance_m[vertical_path.along_path_distance_m.size() - 1]; - double v_cas = vertical_path.cas_mps[vertical_path.cas_mps.size() - 1]; - double h = vertical_path.altitude_m[vertical_path.altitude_m.size() - 1]; - - double distEnd = Units::MetersLength(distance_to_go).value(); - double altEnd = Units::MetersLength(altitude_high).value(); - - bool bracket_found = false; - - while (v_cas < velocity_cas_end && h < altEnd && dist < distEnd) { - double v_tas = Units::MetersPerSecondSpeed(weather_prediction.getAtmosphere()->CAS2TAS( - Units::MetersPerSecondSpeed(v_cas), Units::MetersLength(h))) - .value(); - double dh_dt; - double dv_dt; - double h_new; - double dist_new; - double v_tas_new; - double v_cas_new; - double curr_time; - - Units::Speed vwpara; - Units::Speed vwperp; - Units::Speed Vwx, Vwy; - Units::UnsignedRadiansAngle course; - m_course_calculator.CalculateCourseAtAlongPathDistance(Units::MetersLength(dist), course); - ComputeWindCoefficients(Units::MetersLength(h), Units::RadiansAngle(course), weather_prediction, vwpara, vwperp, - Vwx, Vwy); - - // One degree descent - double theta_new = PI / 180.0; - - // climb/descent rate - dh_dt = -v_tas * sin(theta_new); - - // acceleration rate - dv_dt = -deceleration; - - h_new = dh_dt * delta_t + h; - - v_tas_new = dv_dt * delta_t + v_tas; - - v_cas_new = Units::MetersPerSecondSpeed(weather_prediction.getAtmosphere()->TAS2CAS( - Units::MetersPerSecondSpeed(v_tas_new), Units::MetersLength(h_new))) - .value(); - - double gsnew = sqrt(pow(v_tas_new * cos(theta_new), 2) - pow(Units::MetersPerSecondSpeed(vwperp).value(), 2)) + - Units::MetersPerSecondSpeed(vwpara).value(); - - dist_new = dist - delta_t * gsnew; - - const double mach = weather_prediction.GetForecastAtmosphere()->IASToMach(Units::MetersPerSecondSpeed(v_cas_new), - Units::MetersLength(h_new)); - - result.along_path_distance_m.push_back(dist_new); - result.cas_mps.push_back(v_cas_new); - result.mach.push_back(mach); - result.altitude_m.push_back(h_new); - result.altitude_rate_mps.push_back(dh_dt); - result.true_airspeed.push_back(Units::MetersPerSecondSpeed(v_tas_new)); - result.tas_rate_mps.push_back(dv_dt); - result.theta_radians.push_back(theta_new); - result.gs_mps.push_back(gsnew); - result.wind_velocity_east.push_back(Vwx); - result.wind_velocity_north.push_back(Vwy); - result.algorithm_type.push_back(VerticalPath::PredictionAlgorithmType::CONSTANT_DECEL); - result.flap_setting.push_back(aaesim::open_source::bada_utils::FlapConfiguration::UNDEFINED); - result.mass_kg.push_back(-1.0); - - curr_time = result.time_to_go_sec.back(); - - result.time_to_go_sec.push_back(curr_time + fabs(delta_t)); // adds last time +0.5 to the end since - // fabs(delta_t) is 0.5 - - if (!bracket_found && dist_new > Units::MetersLength(aircraft_distance_to_go).value()) { - bracket_found = true; - if ((h - Units::MetersLength(m_vertical_tolerance_distance).value()) > - Units::MetersLength(m_start_altitude_msl).value()) { - LOG4CPLUS_TRACE(m_logger, "Prediction alt too high. pred: " - << h_new - << " start_alt: " << Units::MetersLength(m_start_altitude_msl).value()); - m_prediction_too_high = true; - return result; - } - if ((h_new + Units::MetersLength(m_vertical_tolerance_distance).value()) < - Units::MetersLength(m_start_altitude_msl).value()) { - LOG4CPLUS_TRACE(m_logger, "Prediction alt too low. pred: " - << h_new - << " start_alt: " << Units::MetersLength(m_start_altitude_msl).value()); - m_prediction_too_low = true; - return result; - } - } - - // set values for next iteration of the loop - dist = dist_new; // in meters - h = h_new; // in meters - v_cas = v_cas_new; // in meters per second - } - - if ((aircraft_distance_to_go < Units::MetersLength(Units::Infinity())) && - (h - Units::MetersLength(m_vertical_tolerance_distance).value()) > - Units::MetersLength(m_start_altitude_msl).value()) { - LOG4CPLUS_TRACE(m_logger, "Prediction alt too high. pred: " << h << " start_alt: " - << Units::MetersLength(m_start_altitude_msl).value()); - m_prediction_too_high = true; - } - - return result; -} - -VerticalPath KinematicDescent4DPredictor::ConstantFpaToCurrentPositionVerticalPath( - const VerticalPath &vertical_path, std::vector &horizontal_path, - std::vector &precalc_waypoints, double const_gamma_mach, - const WeatherPrediction &weather_prediction, const Units::Length &aircraft_distance_to_go) { - Units::Length distance_to_plan = aircraft_distance_to_go; - - // Should probably throw exception if trying to re-predict with aircraft distance to go set to infinite - if (aircraft_distance_to_go == Units::MetersLength(Units::infinity())) { - LOG4CPLUS_ERROR(m_logger, - "Attempting to re-predict constrained vertical path with infinite aircraft distance to go."); - distance_to_plan = Units::MetersLength(horizontal_path.back().m_path_length_cumulative_meters); - } - - VerticalPath result = vertical_path; - result.algorithm_type.back() = VerticalPath::FPA_TO_CURRENT_POS; - - double dist = vertical_path.along_path_distance_m[vertical_path.along_path_distance_m.size() - 1]; - double v_cas = vertical_path.cas_mps[vertical_path.cas_mps.size() - 1]; - double h = vertical_path.altitude_m[vertical_path.altitude_m.size() - 1]; - - const double descent_ratio = - (m_start_altitude_msl.value() - h) / (Units::MetersLength(aircraft_distance_to_go).value() - dist); - double fpa = atan2(m_start_altitude_msl.value() - h, Units::MetersLength(aircraft_distance_to_go).value() - dist); - - while (dist < Units::MetersLength(aircraft_distance_to_go).value() && - h < Units::MetersLength(m_transition_altitude_msl).value()) { - PrecalcConstraint constraints = FindActiveConstraint(dist, precalc_waypoints); - constraints = - CheckActiveConstraint(dist, h, v_cas, constraints, Units::MetersLength(m_transition_altitude_msl).value()); - - double distance_left = Units::MetersLength(constraints.constraint_along_path_distance).value(); - if (distance_left > Units::MetersLength(aircraft_distance_to_go).value()) { - distance_left = Units::MetersLength(aircraft_distance_to_go).value(); - } - - const double altitude_at_end = h + (distance_left - dist) * descent_ratio; - // The first step in ConstantFPADecelerationVerticalPath() and ConstantGeometricVerticalPath() uses the - // previous value of theta. A new theta is not calculated until the second step. If the aircraft - // position is close to the waypoint calculation of theta can result in NaN or an FPA angle too high. - // See Issue AAES-1025. This should rarely occur. - - if (descent_ratio > 0.2) { - LOG4CPLUS_TRACE(m_logger, "Aircraft position too close to waypoint for adequate FPA. See Issue AAES-1025"); - LOG4CPLUS_TRACE(m_logger, "dist_left: " << (distance_left - dist) << ", alt_change: " - << (m_start_altitude_msl.value() - h) << ", fpa: " << fpa); - if (fpa > Units::RadiansAngle(DESCENT_ANGLE_MAX).value()) { - fpa = Units::RadiansAngle(DESCENT_ANGLE_MAX).value(); - } - result = ConstantGeometricFpaVerticalPath(result, altitude_at_end, fpa, horizontal_path, precalc_waypoints, - weather_prediction, Units::Length(Units::infinity())); - result.altitude_m.back() = altitude_at_end; - if (result.altitude_m.size() > 2) { - result.altitude_m[result.altitude_m.size() - 2] = altitude_at_end; - } - h = result.altitude_m.back(); - dist = result.along_path_distance_m.back(); - } - - while (dist < Units::MetersLength(constraints.constraint_along_path_distance).value() && h < altitude_at_end) { - if (v_cas < Units::MetersPerSecondSpeed(constraints.constraint_speedHi).value()) { - if (m_prediction_too_high || m_prediction_too_low) { - result = ConstantFpaDecelerationVerticalPath( - result, altitude_at_end, m_deceleration_fpa_mps, - Units::MetersPerSecondSpeed(constraints.constraint_speedHi).value(), fpa, horizontal_path, - precalc_waypoints, weather_prediction, Units::Length(Units::infinity())); - } else { - result = ConstantFpaDecelerationVerticalPath( - result, altitude_at_end, m_deceleration_mps, - Units::MetersPerSecondSpeed(constraints.constraint_speedHi).value(), fpa, horizontal_path, - precalc_waypoints, weather_prediction, Units::Length(Units::infinity())); - } - } - result = ConstantGeometricFpaVerticalPath(result, altitude_at_end, fpa, horizontal_path, precalc_waypoints, - weather_prediction, Units::Length(Units::infinity())); - - dist = result.along_path_distance_m.back(); - v_cas = result.cas_mps.back(); - h = result.altitude_m.back(); - if (dist > Units::MetersLength(aircraft_distance_to_go).value()) { - break; - } - } - if (h > altitude_at_end && dist < Units::MetersLength(constraints.constraint_along_path_distance).value()) { - result = LevelVerticalPath(result, Units::MetersLength(constraints.constraint_along_path_distance).value(), - horizontal_path, weather_prediction, Units::Length(Units::infinity())); - dist = result.along_path_distance_m.back(); - v_cas = result.cas_mps.back(); - h = result.altitude_m.back(); - } - } - - // either at transition altitude or prediction goes past aircraft distance to go - if (result.altitude_m.back() < Units::MetersLength(m_start_altitude_msl).value()) { - result = ConstantMachVerticalPath(result, m_start_altitude_msl.value(), horizontal_path, precalc_waypoints, - const_gamma_mach, weather_prediction, Units::Length(Units::infinity())); - } - - double prediction_dist = - Units::MetersLength( - precalc_waypoints[precalc_waypoints.size() - 1].m_precalc_constraints.constraint_along_path_distance) - .value(); - result = LevelVerticalPath(result, prediction_dist, horizontal_path, weather_prediction, - Units::Length(Units::infinity())); - return result; -}; - -void KinematicDescent4DPredictor::ComputeWindCoefficients(Units::Length altitude, Units::Angle course, - const WeatherPrediction &weather_prediction, - Units::Speed ¶llel_wind_velocity, - Units::Speed &perpendicular_wind_velocity, - Units::Speed &wind_velocity_x, - Units::Speed &wind_velocity_y) { - Units::HertzFrequency dVwx_dh, dVwy_dh; - - m_wind_calculator.ComputeWindGradients(altitude, weather_prediction, wind_velocity_x, wind_velocity_y, dVwx_dh, - dVwy_dh); - - parallel_wind_velocity = wind_velocity_x * cos(course) + wind_velocity_y * sin(course); - perpendicular_wind_velocity = -wind_velocity_x * sin(course) + wind_velocity_y * cos(course); -}; - -void KinematicDescent4DPredictor::TrimVerticalPath(VerticalPath &vertical_path, int path_index) { - if (path_index >= vertical_path.along_path_distance_m.size()) { - LOG4CPLUS_WARN(m_logger, "path_index greater than vertical path size: unable to trim it"); - return; - } - vertical_path.along_path_distance_m.erase(vertical_path.along_path_distance_m.begin() + path_index + 1, - vertical_path.along_path_distance_m.end()); - vertical_path.algorithm_type.erase(vertical_path.algorithm_type.begin() + path_index + 1, - vertical_path.algorithm_type.end()); - vertical_path.altitude_m.erase(vertical_path.altitude_m.begin() + path_index + 1, vertical_path.altitude_m.end()); - vertical_path.altitude_rate_mps.erase(vertical_path.altitude_rate_mps.begin() + path_index + 1, - vertical_path.altitude_rate_mps.end()); - vertical_path.tas_rate_mps.erase(vertical_path.tas_rate_mps.begin() + path_index + 1, - vertical_path.tas_rate_mps.end()); - vertical_path.cas_mps.erase(vertical_path.cas_mps.begin() + path_index + 1, vertical_path.cas_mps.end()); - vertical_path.gs_mps.erase(vertical_path.gs_mps.begin() + path_index + 1, vertical_path.gs_mps.end()); - vertical_path.mass_kg.erase(vertical_path.mass_kg.begin() + path_index + 1, vertical_path.mass_kg.end()); - vertical_path.theta_radians.erase(vertical_path.theta_radians.begin() + path_index + 1, - vertical_path.theta_radians.end()); - vertical_path.time_to_go_sec.erase(vertical_path.time_to_go_sec.begin() + path_index + 1, - vertical_path.time_to_go_sec.end()); - vertical_path.true_airspeed.erase(vertical_path.true_airspeed.begin() + path_index + 1, - vertical_path.true_airspeed.end()); - vertical_path.wind_velocity_east.erase(vertical_path.wind_velocity_east.begin() + path_index + 1, - vertical_path.wind_velocity_east.end()); - vertical_path.wind_velocity_north.erase(vertical_path.wind_velocity_north.begin() + path_index + 1, - vertical_path.wind_velocity_north.end()); - vertical_path.mass_kg.erase(vertical_path.mass_kg.begin() + path_index + 1, vertical_path.mass_kg.end()); -} diff --git a/Public/KinematicTrajectoryPredictor.cpp b/Public/KinematicTrajectoryPredictor.cpp deleted file mode 100644 index aa11018..0000000 --- a/Public/KinematicTrajectoryPredictor.cpp +++ /dev/null @@ -1,77 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include "public/KinematicTrajectoryPredictor.h" - -#include - -using namespace std; -using namespace aaesim::open_source; - -log4cplus::Logger KinematicTrajectoryPredictor::m_logger = - log4cplus::Logger::getInstance(LOG4CPLUS_TEXT("KinematicTrajectoryPredictor")); - -KinematicTrajectoryPredictor::KinematicTrajectoryPredictor() { - m_vertical_predictor = std::shared_ptr(new KinematicDescent4DPredictor()); -} - -KinematicTrajectoryPredictor::KinematicTrajectoryPredictor(Units::Angle maximum_bank_angle, Units::Speed transition_ias, - double transition_mach, - Units::Length transition_altitude_msl, - Units::Length cruise_altitude_msl) { - m_bank_angle = maximum_bank_angle; - KinematicDescent4DPredictor *kinematic_descent_predictor = new KinematicDescent4DPredictor(); - kinematic_descent_predictor->SetMembers(transition_mach, transition_ias, cruise_altitude_msl, - transition_altitude_msl); - m_vertical_predictor = std::shared_ptr(kinematic_descent_predictor); -} - -KinematicTrajectoryPredictor::KinematicTrajectoryPredictor(const KinematicTrajectoryPredictor &obj) { operator=(obj); } - -void KinematicTrajectoryPredictor::CalculateWaypoints(const AircraftIntent &aircraft_intent, - const WeatherPrediction &weather_prediction) { - Units::Length altitude_at_faf = Units::MetersLength( - aircraft_intent.GetRouteData().m_nominal_altitude[aircraft_intent.GetNumberOfWaypoints() - 1]); - Units::Speed nominal_ias_at_faf = Units::FeetPerSecondSpeed( - aircraft_intent.GetRouteData().m_nominal_ias[aircraft_intent.GetNumberOfWaypoints() - 1]); - - GetKinematicDescent4dPredictor()->SetConditionsAtEndOfRoute(altitude_at_faf, nominal_ias_at_faf); - EuclideanTrajectoryPredictor::CalculateWaypoints(aircraft_intent, WeatherPrediction()); -} - -KinematicTrajectoryPredictor &KinematicTrajectoryPredictor::operator=(const KinematicTrajectoryPredictor &obj) { - if (this != &obj) { - EuclideanTrajectoryPredictor::operator=(obj); - - std::shared_ptr kin = obj.GetKinematicDescent4dPredictor(); - - if (kin == NULL) { - m_vertical_predictor = std::shared_ptr((KinematicDescent4DPredictor *)NULL); - } else { - m_vertical_predictor = std::shared_ptr(new KinematicDescent4DPredictor(*kin)); - } - } - - return *this; -} - -std::shared_ptr KinematicTrajectoryPredictor::GetKinematicDescent4dPredictor() const { - return std::shared_ptr( - static_pointer_cast(m_vertical_predictor)); -} diff --git a/Public/LatitudeLongitudePoint.cpp b/Public/LatitudeLongitudePoint.cpp deleted file mode 100644 index 472154e..0000000 --- a/Public/LatitudeLongitudePoint.cpp +++ /dev/null @@ -1,77 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include "public/LatitudeLongitudePoint.h" - -#include - -#include - -#include "public/GeolibUtils.h" - -using namespace aaesim; - -log4cplus::Logger LatitudeLongitudePoint::m_logger = - log4cplus::Logger::getInstance(LOG4CPLUS_TEXT("LatitudeLongitudePoint")); - -LatitudeLongitudePoint::LatitudeLongitudePoint(const Units::SignedAngle &wgs84_latitude, - const Units::SignedAngle &wgs84_longitude) { - m_llpoint.latitude = Units::SignedRadiansAngle(wgs84_latitude).value(); - m_llpoint.longitude = Units::SignedRadiansAngle(wgs84_longitude).value(); -} - -Units::SignedAngle LatitudeLongitudePoint::GetLatitude() const { return Units::SignedRadiansAngle(m_llpoint.latitude); } - -Units::SignedAngle LatitudeLongitudePoint::GetLongitude() const { - return Units::SignedRadiansAngle(m_llpoint.longitude); -} - -const geolib_idealab::LLPoint &LatitudeLongitudePoint::GetGeolibPrimitiveLLPoint() const { return m_llpoint; } - -LatitudeLongitudePoint LatitudeLongitudePoint::CreateFromGeolibPrimitive(geolib_idealab::LLPoint ll_point) { - return LatitudeLongitudePoint(Units::SignedRadiansAngle(ll_point.latitude), - Units::SignedRadiansAngle(ll_point.longitude)); -} - -LatitudeLongitudePoint LatitudeLongitudePoint::ProjectDistanceAlongCourse(Units::Length projection_distance, - Units::SignedAngle course_enu) const { - return GeolibUtils::CalculateNewPoint(*this, projection_distance, course_enu); -} - -LatitudeLongitudePoint LatitudeLongitudePoint::CreateFromWaypoint(const Waypoint &wgs84_waypoint) { - return LatitudeLongitudePoint(wgs84_waypoint.GetLatitude(), wgs84_waypoint.GetLongitude()); -} - -LatitudeLongitudePoint LatitudeLongitudePoint::CreateFromGeodeticPosition( - const EllipsoidalEarthModel::GeodeticPosition &geodetic_position) { - return LatitudeLongitudePoint(geodetic_position.latitude, geodetic_position.longitude); -} - -std::pair LatitudeLongitudePoint::CalculateRelationshipBetweenPoints( - const LatitudeLongitudePoint &other_point) const { - return GeolibUtils::CalculateRelationshipBetweenPoints(*this, other_point); -} -bool LatitudeLongitudePoint::ArePointsEqual(const LatitudeLongitudePoint &test_point) const { - return GeolibUtils::ArePointsMathematicallyEqual(*this, test_point); -} - -bool LatitudeLongitudePoint::operator==(const LatitudeLongitudePoint &rhs) const { - return GetLatitude() == rhs.GetLatitude() && GetLongitude() == rhs.GetLongitude(); -} -bool LatitudeLongitudePoint::operator!=(const LatitudeLongitudePoint &rhs) const { return !(rhs == *this); } diff --git a/Public/LegacyPositionEstimator.cpp b/Public/LegacyPositionEstimator.cpp deleted file mode 100644 index 74a7a20..0000000 --- a/Public/LegacyPositionEstimator.cpp +++ /dev/null @@ -1,44 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include "public/LegacyPositionEstimator.h" - -using namespace aaesim::open_source; - -EarthModel::GeodeticPosition LegacyPositionEstimator::ComputeLatLon(const EquationsOfMotionState &eqm_state) const { - auto local_position = EarthModel::LocalPositionEnu{}; - local_position.x = eqm_state.enu_x; - local_position.y = eqm_state.enu_y; - local_position.z = eqm_state.altitude_msl; - EarthModel::GeodeticPosition geodetic_position; - m_tangent_plane_sequence->ConvertLocalToGeodetic(local_position, geodetic_position); - geodetic_position.altitude = eqm_state.altitude_msl; - return geodetic_position; -} - -void LegacyPositionEstimator::ComputePosition(const SimulationTime &simtime, const EquationsOfMotionState &eqm_state, - const EquationsOfMotionStateDeriv &eqm_state_derivative, - EarthModel::GeodeticPosition &position, LatLonDerivative &position_rate) { - position = ComputeLatLon(eqm_state); - position_rate.latitude_time_derivative = - (position.latitude - m_last_resolved_position.latitude) / simtime.GetSimulationTimeStep(); - position_rate.longitude_time_derivative = - (position.longitude - m_last_resolved_position.longitude) / simtime.GetSimulationTimeStep(); - m_last_resolved_position = position; -} diff --git a/Public/LineOnEllipsoid.cpp b/Public/LineOnEllipsoid.cpp deleted file mode 100644 index 973175b..0000000 --- a/Public/LineOnEllipsoid.cpp +++ /dev/null @@ -1,169 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include "public/LineOnEllipsoid.h" - -#include - -#include -#include - -#include "public/GeolibUtils.h" - -using namespace aaesim; -using namespace geolib_idealab; - -log4cplus::Logger LineOnEllipsoid::m_logger = log4cplus::Logger::getInstance("LineOnEllipsoid"); - -LineOnEllipsoid::LineOnEllipsoid(const geolib_idealab::Geodesic &geodesic) : geolib_geodesic_(geodesic) { - ComputeUnitVectorNormalToLineStartEnd(); -} - -const geolib_idealab::Geodesic &LineOnEllipsoid::GetGeolibPrimitiveGeodesic() const { return geolib_geodesic_; } - -Units::SignedAngle LineOnEllipsoid::GetForwardCourseEnuAtStartPoint() const { - return GeolibUtils::ConvertCourseFromNedToEnu(Units::UnsignedRadiansAngle(geolib_geodesic_.startAz)); -} - -Units::SignedAngle LineOnEllipsoid::GetForwardCourseEnuAtEndPoint() const { - return GeolibUtils::ConvertCourseFromNedToEnu(Units::UnsignedRadiansAngle(geolib_geodesic_.endAz)); -} - -Units::Length LineOnEllipsoid::GetShapeLength() const { return Units::NauticalMilesLength(geolib_geodesic_.length); } - -LineOnEllipsoid LineOnEllipsoid::CreateFromPoints(const LatitudeLongitudePoint &start_point, - const LatitudeLongitudePoint &end_point) { - return GeolibUtils::CreateLineOnEllipsoid(start_point, end_point); -} - -LatitudeLongitudePoint LineOnEllipsoid::GetEndPoint() const { - return LatitudeLongitudePoint::CreateFromGeolibPrimitive(geolib_geodesic_.endPoint); -} - -LatitudeLongitudePoint LineOnEllipsoid::GetStartPoint() const { - return LatitudeLongitudePoint::CreateFromGeolibPrimitive(geolib_geodesic_.startPoint); -} - -const geolib_idealab::LineType LineOnEllipsoid::GetLineType() const { return geolib_idealab::LineType::SEGMENT; } - -bool LineOnEllipsoid::IsPointOnShape(const LatitudeLongitudePoint &test_point) const { - return GeolibUtils::IsPointOnLine(*this, test_point); -} - -LineOnEllipsoid LineOnEllipsoid::CreateExtendedLine(Units::Length extended_distance) const { - const LatitudeLongitudePoint new_end_point = - this->GetEndPoint().ProjectDistanceAlongCourse(extended_distance, this->GetForwardCourseEnuAtEndPoint()); - return LineOnEllipsoid::CreateFromPoints(this->GetStartPoint(), new_end_point); -} - -void LineOnEllipsoid::ComputeUnitVectorNormalToLineStartEnd() { - EllipsoidalEarthModel earth_model; - EllipsoidalEarthModel::GeodeticPosition start_point_position_geodetic; - start_point_position_geodetic.latitude = GetStartPoint().GetLatitude(); - start_point_position_geodetic.longitude = GetStartPoint().GetLongitude(); - EllipsoidalEarthModel::AbsolutePositionEcef start_point_position_absolute; - earth_model.ConvertGeodeticToAbsolute(start_point_position_geodetic, start_point_position_absolute); - const EllipsoidalEarthModel::AbsolutePositionEcef start_point_position_unit_vector = - start_point_position_absolute.ToUnitVector(); - - EllipsoidalEarthModel::GeodeticPosition end_point_position_geodetic; - end_point_position_geodetic.latitude = GetEndPoint().GetLatitude(); - end_point_position_geodetic.longitude = GetEndPoint().GetLongitude(); - EllipsoidalEarthModel::AbsolutePositionEcef end_point_position_absolute; - earth_model.ConvertGeodeticToAbsolute(end_point_position_geodetic, end_point_position_absolute); - const EllipsoidalEarthModel::AbsolutePositionEcef end_point_position_unit_vector = - end_point_position_absolute.ToUnitVector(); - - unit_vector_normal_to_line_start_end_ = - VectorCrossProduct(start_point_position_unit_vector, end_point_position_unit_vector).ToUnitVector(); -} - -ShapeOnEllipsoid::kDirectionRelativeToShape LineOnEllipsoid::GetRelativeDirection( - const LatitudeLongitudePoint &point_not_on_shape) const { - EllipsoidalEarthModel earth_model; - - // Cross product of line start and point not on shape - std::tuple perp_info = - GeolibUtils::FindNearestPointOnLineUsingPerpendicularProjection(*this, point_not_on_shape); - const LatitudeLongitudePoint nearest_point_on_line = std::get<0>(perp_info); - EllipsoidalEarthModel::GeodeticPosition nearest_point_position_geodetic; - nearest_point_position_geodetic.latitude = nearest_point_on_line.GetLatitude(); - nearest_point_position_geodetic.longitude = nearest_point_on_line.GetLongitude(); - EllipsoidalEarthModel::AbsolutePositionEcef nearest_point_position_absolute; - earth_model.ConvertGeodeticToAbsolute(nearest_point_position_geodetic, nearest_point_position_absolute); - - EllipsoidalEarthModel::GeodeticPosition test_point_position_geodetic; - test_point_position_geodetic.latitude = point_not_on_shape.GetLatitude(); - test_point_position_geodetic.longitude = point_not_on_shape.GetLongitude(); - EllipsoidalEarthModel::AbsolutePositionEcef test_point_position_absolute; - earth_model.ConvertGeodeticToAbsolute(test_point_position_geodetic, test_point_position_absolute); - const EllipsoidalEarthModel::AbsolutePositionEcef vector_to_test_point = - VectorDifference(nearest_point_position_absolute, test_point_position_absolute); - const EllipsoidalEarthModel::AbsolutePositionEcef vector_to_test_point_unit_vector = - vector_to_test_point.ToUnitVector(); - - // Use angle between vectors to compute relative direction - const Units::MetersLength dp = - VectorDotProduct(unit_vector_normal_to_line_start_end_, vector_to_test_point_unit_vector); - const double ratio = dp / Units::MetersLength(1.0); - const auto beta = Units::SignedRadiansAngle{std::acos(ratio)}; - if (beta < Units::HALF_PI_RADIANS_ANGLE) { - return ShapeOnEllipsoid::RIGHT_OF_SHAPE; - } else if (beta > Units::HALF_PI_RADIANS_ANGLE) { - return ShapeOnEllipsoid::LEFT_OF_SHAPE; - } - return ShapeOnEllipsoid::ON_SHAPE; -} - -Units::Length LineOnEllipsoid::GetDistanceToEndPoint(const LatitudeLongitudePoint &latitude_longitude_point) const { - return ShapeOnEllipsoid::GetDistanceToEndPoint(latitude_longitude_point); -} - -LatitudeLongitudePoint LineOnEllipsoid::GetNearestPointOnShape(const LatitudeLongitudePoint &point_not_on_shape) const { - std::tuple perp_info = - GeolibUtils::FindNearestPointOnLineUsingPerpendicularProjection(*this, point_not_on_shape); - return std::get<0>(perp_info); -} - -Units::Length LineOnEllipsoid::CalculateDistanceFromPointOnShapeToEnd( - const LatitudeLongitudePoint &point_on_shape) const { - return GetEndPoint().CalculateRelationshipBetweenPoints(point_on_shape).first; -} - -LatitudeLongitudePoint LineOnEllipsoid::CalculatePointAtDistanceFromStartPoint( - const Units::Length &distance_along_shape_from_start_point) const { - LatitudeLongitudePoint point_on_line_at_distance_along_path = GetStartPoint().ProjectDistanceAlongCourse( - distance_along_shape_from_start_point, GetForwardCourseEnuAtStartPoint()); - return point_on_line_at_distance_along_path; -} -std::pair LineOnEllipsoid::CalculateCourseAtDistanceFromStartPoint( - const Units::Length &distance_along_shape_from_start_point) const { - double temp_course_1, temp_course_2, dist_to_point; - ErrorSet error_set{ErrorCodes::SUCCESS}; - LatitudeLongitudePoint point_on_geodesic = - CalculatePointAtDistanceFromStartPoint(distance_along_shape_from_start_point); - double course_ned_at_point = geoCrs(geolib_geodesic_, point_on_geodesic.GetGeolibPrimitiveLLPoint(), &temp_course_1, - &temp_course_2, &dist_to_point, &error_set, GEOLIB_TOLERANCE, GEOLIB_EPSILON); - if (!GeolibUtils::IsSuccess(error_set)) { - LOG4CPLUS_ERROR(m_logger, GeolibUtils::m_basic_error_message << formatErrorMessage(error_set)); - throw std::runtime_error(GeolibUtils::m_basic_error_message); - } - const Units::UnsignedRadiansAngle course_ned_to_return(course_ned_at_point); - return std::make_pair(GeolibUtils::ConvertCourseFromNedToEnu(course_ned_to_return), point_on_geodesic); -} diff --git a/Public/LocalTangentPlane.cpp b/Public/LocalTangentPlane.cpp deleted file mode 100644 index 44991fb..0000000 --- a/Public/LocalTangentPlane.cpp +++ /dev/null @@ -1,143 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -/* - * LocalTangentPlane.cpp - * - * Created on: Jun 25, 2015 - * Author: klewis - */ - -#include "public/LocalTangentPlane.h" - -#include - -#include -#include - -#include "public/CustomMath.h" - -using namespace std; - -log4cplus::Logger LocalTangentPlane::logger = log4cplus::Logger::getInstance(LOG4CPLUS_TEXT("LocalTangentPlane")); -const double LocalTangentPlane::identity3x3[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}; -const double yzx[3][3] = {{0, 0, 1}, {1, 0, 0}, {0, 1, 0}}; -const double zxy[3][3] = {{0, 1, 0}, {0, 0, 1}, {1, 0, 0}}; - -LocalTangentPlane::LocalTangentPlane(const EarthModel *earthModel, - const EarthModel::AbsolutePositionEcef &ecefPointOfTangency, - const EarthModel::LocalPositionEnu &enuPointOfTangency) - : earthModel(earthModel), - pointOfTangencyEcef(ecefPointOfTangency), - pointOfTangencyEnu(enuPointOfTangency), - m_ecef_to_enu(DMatrix((double **)&identity3x3, 0, 2, 0, 2)), - m_enu_to_ecef(DMatrix((double **)&identity3x3, 0, 2, 0, 2)) {} - -LocalTangentPlane::~LocalTangentPlane() {} - -void LocalTangentPlane::InitializeRotationForGeodeticOrigin() { - m_ecef_to_enu = DMatrix((double **)zxy, 0, 2, 0, 2); - m_enu_to_ecef = DMatrix((double **)yzx, 0, 2, 0, 2); -} - -void LocalTangentPlane::RotateEnuFrame(const double x, const double y, const double z, const Units::Angle theta) { - auto rotation = std::unique_ptr(&CreateRotationMatrix(x, y, z, theta)); - auto ecef_to_enu = std::unique_ptr(&(m_ecef_to_enu * *rotation)); - m_ecef_to_enu = *ecef_to_enu; - - auto inverse_rotation = std::unique_ptr(&CreateRotationMatrix(x, y, z, -theta)); - auto enu_to_ecef = std::unique_ptr(&(*inverse_rotation * m_enu_to_ecef)); - m_enu_to_ecef = *enu_to_ecef; -} - -void LocalTangentPlane::ConvertGeodeticToAbsolute(const EarthModel::GeodeticPosition &geo, - EarthModel::AbsolutePositionEcef &ecef) const { - earthModel->ConvertGeodeticToAbsolute(geo, ecef); -} - -void LocalTangentPlane::ConvertAbsoluteToGeodetic(const EarthModel::AbsolutePositionEcef &ecef, - EarthModel::GeodeticPosition &geo) const { - earthModel->ConvertAbsoluteToGeodetic(ecef, geo); -} - -void LocalTangentPlane::printCoordinates(const string &title, Units::Length x, Units::Length y, Units::Length z) { - LOG4CPLUS_TRACE(logger, title << " (" << Units::MetersLength(x).value() << "," << Units::MetersLength(y).value() - << "," << Units::MetersLength(z).value() << ")"); -} - -/** - * Converts local coordinates to absolute. - */ -void LocalTangentPlane::ConvertLocalToAbsolute(const EarthModel::LocalPositionEnu &enu, - EarthModel::AbsolutePositionEcef &ecef) const { - Units::Length x1 = enu.x - pointOfTangencyEnu.x; - Units::Length y1 = enu.y - pointOfTangencyEnu.y; - Units::Length z1 = enu.z - pointOfTangencyEnu.z; - printCoordinates("Local relative: ", x1, y1, z1); - - ecef.x = m_enu_to_ecef[0][0] * x1 + m_enu_to_ecef[0][1] * y1 + m_enu_to_ecef[0][2] * z1; - ecef.y = m_enu_to_ecef[1][0] * x1 + m_enu_to_ecef[1][1] * y1 + m_enu_to_ecef[1][2] * z1; - ecef.z = m_enu_to_ecef[2][0] * x1 + m_enu_to_ecef[2][1] * y1 + m_enu_to_ecef[2][2] * z1; - - printCoordinates("Rotated relative: ", ecef.x, ecef.y, ecef.z); - - ecef.x += pointOfTangencyEcef.x; - ecef.y += pointOfTangencyEcef.y; - ecef.z += pointOfTangencyEcef.z; - - printCoordinates("Absolute: ", ecef.x, ecef.y, ecef.z); -} - -/** - * Converts absolute coordinates to local. - */ -void LocalTangentPlane::ConvertAbsoluteToLocal(const EarthModel::AbsolutePositionEcef &ecef, - EarthModel::LocalPositionEnu &enu) const { - Units::Length x1 = ecef.x - pointOfTangencyEcef.x; - Units::Length y1 = ecef.y - pointOfTangencyEcef.y; - Units::Length z1 = ecef.z - pointOfTangencyEcef.z; - - enu.x = m_ecef_to_enu[0][0] * x1 + m_ecef_to_enu[0][1] * y1 + m_ecef_to_enu[0][2] * z1; - enu.y = m_ecef_to_enu[1][0] * x1 + m_ecef_to_enu[1][1] * y1 + m_ecef_to_enu[1][2] * z1; - enu.z = m_ecef_to_enu[2][0] * x1 + m_ecef_to_enu[2][1] * y1 + m_ecef_to_enu[2][2] * z1; - - enu.x += pointOfTangencyEnu.x; - enu.y += pointOfTangencyEnu.y; - enu.z += pointOfTangencyEnu.z; -} - -void LocalTangentPlane::ConvertGeodeticToLocal(const EarthModel::GeodeticPosition &geo, - EarthModel::LocalPositionEnu &enu) const { - EarthModel::AbsolutePositionEcef temp; - earthModel->ConvertGeodeticToAbsolute(geo, temp); - ConvertAbsoluteToLocal(temp, enu); -} - -void LocalTangentPlane::ConvertLocalToGeodetic(const EarthModel::LocalPositionEnu &enu, - EarthModel::GeodeticPosition &geo) const { - EarthModel::AbsolutePositionEcef temp; - ConvertLocalToAbsolute(enu, temp); - earthModel->ConvertAbsoluteToGeodetic(temp, geo); -} - -const EarthModel::LocalPositionEnu &LocalTangentPlane::getPointOfTangencyEnu() const { return pointOfTangencyEnu; } - -const EarthModel::AbsolutePositionEcef &LocalTangentPlane::getPointOfTangencyEcef() const { - return pointOfTangencyEcef; -} diff --git a/Public/MaintainMetric.cpp b/Public/MaintainMetric.cpp deleted file mode 100644 index d3dc4b4..0000000 --- a/Public/MaintainMetric.cpp +++ /dev/null @@ -1,156 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// 2022 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include "public/MaintainMetric.h" -#include - - -MaintainMetric::MaintainMetric(void) { - - // Constructor - - achieveByTime = -1.0; - totalMaintainTime = 0.0; - numCyclesOutsideThreshold = 0; - m_output_enabled = false; -} - - -MaintainMetric::~MaintainMetric(void) { - - // Destructor -} - - -void MaintainMetric::AddSpacingErrorSec(double err) { - - // Adds data to be added for each pass through an IM::update method. - // Also increments number of cycles if outside of threshold. - // - // err:input spacing error. - - spacingError.Insert(err); - - // TODO:Need to include time step in this if. - - if (fabs(err) > CYCLE_THRESHOLD) { - numCyclesOutsideThreshold++; - } -} - - -void MaintainMetric::SetTimeAtAbp(double aTime) { - - // Sets time aircraft went by achieve by point. - // - // aTime:achieve by time. - - achieveByTime = aTime; -} - - -void MaintainMetric::ComputeTotalMaintainTime(double cTime) { - - // Computes total maintain time subtracting the achieveByTime - // from the current time. - // - // cTime:current time. - - totalMaintainTime = cTime - achieveByTime; -} - - -bool MaintainMetric::TimeAtAbpRecorded() { - - // Boolean to determine if achieveBy set. - // - // return:true if achieve by has valid time. - // false if achieve by does not have valid time. - - return (achieveByTime >= 0.0); -} - - -double MaintainMetric::getMeanErr() { - - // Gets mean spacing error. - // - // returns mean error. - - return spacingError.GetMean(); -} - - -double MaintainMetric::getStdErr() { - - // Gets standard deviation of spacing error. - // - // returns standard deviation of error. - - return spacingError.ComputeStandardDeviation(); -} - - -double MaintainMetric::getBound95() { - - // Gets 95th bound of spacing error. - // - // returns 95th bound of spacing error. - - return spacingError.Get95thBounds(); -} - - -double MaintainMetric::getTotMaintain() { - - // Gets total maintain time. - // - // returns total maintain time. - - return totalMaintainTime; -} - - -int MaintainMetric::getNumCycles() { - - // Gets number of cycles with spacing errors > cycle threshold - // - // returns number of cycles. - - return numCyclesOutsideThreshold; -} - - -bool MaintainMetric::hasSamples() { - - // Determines whether there are any samples collected. - // - // returns true if there are samples - // else false. - - return (spacingError.GetNumberOfSamples() > 0); -} - -bool MaintainMetric::IsOutputEnabled() const { - return m_output_enabled; -} - -void MaintainMetric::SetOutputEnabled(bool output_enabled) { - m_output_enabled = output_enabled; -} diff --git a/Public/MergePointMetric.cpp b/Public/MergePointMetric.cpp deleted file mode 100644 index bea30b7..0000000 --- a/Public/MergePointMetric.cpp +++ /dev/null @@ -1,175 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// 2022 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include "public/MergePointMetric.h" -#include "public/AircraftCalculations.h" -#include -// #include - -using namespace std; -log4cplus::Logger MergePointMetric::logger = log4cplus::Logger::getInstance("MergePointMetric"); - -MergePointMetric::MergePointMetric(void) { - m_im_ac_id = 0; - m_target_ac_id = 0; - mReportMetrics = false; - mMergePointName = ""; - mIMDist = mMergeDist = Units::infinity(); - mTargY = mTargX = mIMY = mIMX = 0; - mMergePointY = mMergePointX = Units::ZERO_LENGTH; -} - - -MergePointMetric::~MergePointMetric(void) { -} - -void MergePointMetric::determineMergePoint(const AircraftIntent &imintent, - const AircraftIntent &targintent) { - - // Determines and sets the merge point between the IM and target aircraft. - // The merge point is defined as the first waypoint in the route where the - // remainder of the IM and target route match - // If no merge point is found, this class will not report any metrics. - // - // imintent:intent of IM aircraft containing the list of waypoints. - // targintent:intent of target aircraft containing the list of waypoints. - - m_im_ac_id = imintent.GetId(); - m_target_ac_id = targintent.GetId(); - - // overwrite these if merge point is found - mReportMetrics = false; - mMergePointName = ""; - - std::pair indices = imintent.FindCommonWaypoint(targintent); - const int index = indices.first; - - if (m_im_ac_id == m_target_ac_id) { - LOG4CPLUS_TRACE(logger, "Target ID matches own for acid " << m_im_ac_id - << ". No merge metrics will be reported."); - } - else if (index == -1) { - LOG4CPLUS_TRACE(logger, "No merge point found for im acid " << imintent.GetId() << - " and target acid " << targintent.GetId() - << ". No merge metrics will be reported."); - } else { - mMergePointName = imintent.GetWaypointName(index); - mMergePointX = imintent.GetWaypointX(index); - mMergePointY = imintent.GetWaypointY(index); - mReportMetrics = true; - } - -} - - -void MergePointMetric::update(double imXNew, - double imYNew, - double targXNew, - double targYNew) { - - // Replaces the current IM and target position if the new IM position is closer to the - // merge point than the current IM point. Distances are computed in nmi. - // - // imXNew,imYNew:the new IM position. - // targXNew,targYNew:the new target position. - - if (!mReportMetrics) { - return; - } // nothing to do - - if (newPointCloser(imXNew, imYNew)) { - // Replace the current information with the new information. - - mIMX = imXNew; - mIMY = imYNew; - mIMDist = AircraftCalculations::PtToPtDist( - mMergePointX, - mMergePointY, - Units::FeetLength(mIMX), - Units::FeetLength(mIMY)); - - mTargX = targXNew; - mTargY = targYNew; - mMergeDist = AircraftCalculations::PtToPtDist( - Units::FeetLength(mIMX), - Units::FeetLength(mIMY), - Units::FeetLength(mTargX), - Units::FeetLength(mTargY)); - } -} - - -string MergePointMetric::getMergePoint() { - - // Gets merge point. - // - // returns name of waypoint which is the merge point. - - return mMergePointName; -} - - -Units::Length MergePointMetric::getDist() { - - // Gets computed distance when IM aircraft was at merge point. - // - // returns distance in nmi. - - return mMergeDist; -} - - -bool MergePointMetric::newPointCloser(double x, - double y) { - - // Checks if newest IM position closer to waypoint than the stored IM position. - // - // x,y:new IM position in feet. - // - // returns true if the new closer to the merge point. - // else false. - - return (AircraftCalculations::PtToPtDist( - mMergePointX, mMergePointY, - Units::FeetLength(x), Units::FeetLength(y)) - < mIMDist); -} - - -bool MergePointMetric::mergePointFound() { - - // Determines whether merge point already found. - // - // returns true if merge point found. - // else false - - return (mMergePointName.length() > 0); -} - -bool MergePointMetric::willReportMetrics() const { - return mReportMetrics; -} - -int MergePointMetric::GetImAcId() const { - return m_im_ac_id; -} - -int MergePointMetric::GetTargetAcId() const { - return m_target_ac_id; -} diff --git a/Public/NMObserver.cpp b/Public/NMObserver.cpp deleted file mode 100644 index 00c5b81..0000000 --- a/Public/NMObserver.cpp +++ /dev/null @@ -1,82 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// 2022 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include "public/NMObserver.h" - - -NMObserver::NMObserver(void) { - curr_NM = -2; -} - - -NMObserver::~NMObserver(void) { -} - -void NMObserver::output_NM_values(double predictedDistance, - double trueDistance, - double time, - double currIAS, - double currGS, - double targetGS, - double minIAS, - double maxIAS, - double minTAS, - double maxTAS) { - // Creates and adds a new entry to the nautical mile observer report. - // - // predictedDistance:predicted distance (current distance from IM algorithms). - // trueDistance:true distance. - // time:time. - // currIAS:current aircraft indicated airspeed. - // currGS:current aircraft ground speed. - // targetGS:target aircraft ground speed. - // minIAS:minimum aircraft indicated airspeed. - // maxIAS:maximum aircraft indicated airspeed. - // minTAS:mininum aircraft true airspeed. - // maxTAS:maximum aircraft true airspeed. - - NMObserverEntry new_entry; - - new_entry.predictedDistance = predictedDistance; - new_entry.trueDistance = trueDistance; - new_entry.time = time; - new_entry.acIAS = currIAS; - new_entry.acGS = currGS; - new_entry.targetGS = targetGS; - new_entry.minIAS = minIAS; - new_entry.maxIAS = maxIAS; - new_entry.minTAS = minTAS; - new_entry.maxTAS = maxTAS; - - entry_list.push_back(new_entry); -} - -void NMObserver::initialize_stats() { - // sets the stats size - while (predictedDistance.size() < entry_list.size()) { - Statistics temp_value; - predictedDistance.push_back(0.0); - trueDistance.push_back(0.0); - ac_IAS_stats.push_back(temp_value); - ac_GS_stats.push_back(temp_value); - target_GS_stats.push_back(temp_value); - min_IAS_stats.push_back(temp_value); - max_IAS_stats.push_back(temp_value); - } -} diff --git a/Public/NMObserverEntry.cpp b/Public/NMObserverEntry.cpp deleted file mode 100644 index 22cbbb2..0000000 --- a/Public/NMObserverEntry.cpp +++ /dev/null @@ -1,38 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// 2022 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include "public/NMObserverEntry.h" - - -NMObserverEntry::NMObserverEntry(void) { - predictedDistance = 0.0; - trueDistance = 0.0; - time = 0.0; - acIAS = 0.0; - acGS = 0.0; - targetGS = 0.0; - minIAS = 0.0; - maxIAS = 0.0; - minTAS = 0.0; - maxTAS = 0.0; -} - - -NMObserverEntry::~NMObserverEntry(void) { -} diff --git a/Public/NullSpeedLimiter.cpp b/Public/NullSpeedLimiter.cpp deleted file mode 100644 index 367cb15..0000000 --- a/Public/NullSpeedLimiter.cpp +++ /dev/null @@ -1,40 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include "public/NullSpeedLimiter.h" - -using namespace aaesim::open_source; - -NullSpeedLimiter::NullSpeedLimiter() = default; - -Units::Speed NullSpeedLimiter::LimitSpeedCommand( - const Units::Speed previous_ias_speed_command, const Units::Speed current_ias_speed_command, - const Units::Speed reference_velocity_mps, const Units::Length speed_quantization_distance, - const Units::Length distance_to_end_of_route, const Units::Length current_altitude, - const aaesim::open_source::bada_utils::FlapConfiguration flap_configuration) { - return current_ias_speed_command; -} - -BoundedValue NullSpeedLimiter::LimitMachCommand( - const BoundedValue &previous_reference_speed_command_mach, - const BoundedValue ¤t_mach_command, const BoundedValue &nominal_mach, - const Units::Mass ¤t_mass, const Units::Length ¤t_altitude, - const WeatherPrediction &weather_prediction) { - return current_mach_command; -} diff --git a/Public/NullWindEvaluator.cpp b/Public/NullWindEvaluator.cpp deleted file mode 100644 index 9e5d144..0000000 --- a/Public/NullWindEvaluator.cpp +++ /dev/null @@ -1,45 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include "public/NullWindEvaluator.h" - -#include - -using namespace aaesim::open_source; - -std::shared_ptr NullWindEvaluator::m_instance; - -const std::shared_ptr NullWindEvaluator::GetInstance() { - if (!m_instance) { - m_instance = std::unique_ptr(new NullWindEvaluator()); - } - return m_instance; -} - -NullWindEvaluator::NullWindEvaluator() = default; - -NullWindEvaluator::~NullWindEvaluator() = default; - -bool NullWindEvaluator::ArePredictedWindsAccurate(const aaesim::open_source::AircraftState &state, - const WeatherPrediction &weather_prediction, - const Units::Speed reference_cas, - const Units::Length reference_altitude, - const std::shared_ptr &sensed_atmosphere) const { - return true; -} diff --git a/Public/OutputHandler.cpp b/Public/OutputHandler.cpp deleted file mode 100644 index d79474b..0000000 --- a/Public/OutputHandler.cpp +++ /dev/null @@ -1,23 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// 2023 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include "public/OutputHandler.h" - -OutputHandler::OutputHandler(const std::string &scenario_name, const std::string &file_suffix) - : m_file_suffix(file_suffix), filename(scenario_name + file_suffix), os(), m_finished(false) {} diff --git a/Public/PassThroughAssap.cpp b/Public/PassThroughAssap.cpp deleted file mode 100644 index 231fecb..0000000 --- a/Public/PassThroughAssap.cpp +++ /dev/null @@ -1,38 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include "public/PassThroughAssap.h" - -#include - -#include "public/NullADSBReceiver.h" - -aaesim::open_source::PassThroughAssap::PassThroughAssap() - : m_adsb_receiver(std::make_shared()) {} - -aaesim::open_source::AircraftState aaesim::open_source::PassThroughAssap::Update( - const aaesim::open_source::AircraftState &state_to_sync_with, - const aaesim::open_source::ADSBSVReport &most_recent_ads_b) const { - return aaesim::open_source::AircraftState::FromAdsbReport(most_recent_ads_b); -} - -void aaesim::open_source::PassThroughAssap::Initialize( - std::shared_ptr adsb_receiver) { - m_adsb_receiver = adsb_receiver; -} diff --git a/Public/PilotDelay.cpp b/Public/PilotDelay.cpp deleted file mode 100644 index a7fcd0a..0000000 --- a/Public/PilotDelay.cpp +++ /dev/null @@ -1,244 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// 2023 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include -#include "public/PilotDelay.h" -#include "public/ScenarioUtils.h" -#include "public/CustomMath.h" - -const double PilotDelay::STANDARD_DEVIATION_LIMIT(3); -log4cplus::Logger PilotDelay::m_logger = log4cplus::Logger::getInstance(LOG4CPLUS_TEXT("PilotDelay")); - -PilotDelay::PilotDelay() - : m_atmosphere(nullptr), - m_pilot_delay_mean(Units::SecondsTime(12.0)), - m_pilot_delay_standard_deviation(Units::SecondsTime(0.0)), - m_pilot_delay_is_on(true), - m_delay_count(0), - m_delay_sum(0), - m_delay_square_sum(0) { - IterationReset(); -} - -PilotDelay::~PilotDelay() { - if (m_delay_count > 0) { - DumpStatistics(); - } -} - -void PilotDelay::IterationReset() { - m_time_to_next_speed_change = Units::SecondsTime(-1.0); - m_guidance_ias = Units::zero(); - m_guidance_mach = 0.0; -} - -/** - * Recomputes and updates mach as necessary according to the time delay - * and recomputes ias accordingly. Manages time to the next speed change. - * - * @param previous_im_speed_command_mach mach command from previous time. - * @param input_im_speed_command_mach mach computed from calling method. - * @param current_altitude altitude flight is at. - * @param altitude_at_end_of_route altitude at the FAF or last point. - * - * @return recomputed guidance ias speed. - */ -Units::Speed PilotDelay::UpdateMach(double previous_im_speed_command_mach, double input_im_speed_command_mach, - Units::Length current_altitude, Units::Length altitude_at_end_of_route) { - if ((input_im_speed_command_mach != previous_im_speed_command_mach) && - (m_time_to_next_speed_change < Units::zero())) { - - // Reset time delay counter and set m_guidance_mach to previous speed. - m_time_to_next_speed_change = ComputeTimeToSpeedChange(current_altitude, altitude_at_end_of_route); - m_guidance_mach = previous_im_speed_command_mach; - } else if (m_guidance_ias == Units::zero()) { - // ias has not changed and guidance ias not set. Compute guidance ias - // from guidance mach. - m_guidance_mach = previous_im_speed_command_mach; - SetInitialIAS(current_altitude, m_atmosphere->MachToIAS(previous_im_speed_command_mach, current_altitude)); - - if (m_time_to_next_speed_change < Units::zero()) { - // past delay time-recompute new delay time and update guidance ias. - m_time_to_next_speed_change = ComputeTimeToSpeedChange(current_altitude, altitude_at_end_of_route); - } - } - - if (m_time_to_next_speed_change == Units::zero()) { - // At time to change speed-set to input mach. - m_guidance_mach = input_im_speed_command_mach; - } - - // Update time delay counter and return guidance speed in ias. - m_time_to_next_speed_change -= Units::SecondsTime(1.0); - - return m_atmosphere->MachToIAS(m_guidance_mach, current_altitude); -} - -/** - * Recomputes and updates ias as necessary. The pilot delay time is also updated. - * - * @param previous_im_speed_command_ias Computed ias from the last time. - * @param input_im_speed_command_ias computed ias from calling method. - * @param current_altitude aircraft altitude. - * @param altitude_at_end_of_route altitude at the FAF or last point. - * - * @return guidance ias speed. - */ -Units::Speed PilotDelay::UpdateIAS(Units::Speed previous_im_speed_command_ias, Units::Speed input_im_speed_command_ias, - Units::Length current_altitude, Units::Length altitude_at_end_of_route) { - - if (input_im_speed_command_ias != previous_im_speed_command_ias) { - // ias has changed. - if (m_time_to_next_speed_change < Units::zero()) { - // past delay time-recompute new delay time and update guidance ias. - m_time_to_next_speed_change = ComputeTimeToSpeedChange(current_altitude, altitude_at_end_of_route); - - if (m_guidance_ias == Units::zero()) { - // compute first guidance ias - SetInitialIAS(current_altitude, previous_im_speed_command_ias); - } else { - // set guidance ias from previous ias. - m_guidance_ias = previous_im_speed_command_ias; - } - } else if ((m_time_to_next_speed_change > Units::zero()) && (m_guidance_ias == Units::zero())) { - // not at delay time yet and guidance ias not set-compute guidance ias - // from guidance mach. - SetInitialIAS(current_altitude, previous_im_speed_command_ias); - } - } else if (m_guidance_ias == Units::zero()) { - // ias has not changed and guidance ias not set-compute guidnace ias - // from guidance mach. - SetInitialIAS(current_altitude, previous_im_speed_command_ias); - } - - if (m_time_to_next_speed_change == Units::zero()) { - // time to process delay-set guidance ias from input ias. - m_guidance_ias = input_im_speed_command_ias; - } - - // update delay time and return guidance ias. - m_time_to_next_speed_change -= Units::SecondsTime(1.0); - - if (m_guidance_ias == Units::zero()) { - throw std::runtime_error("Zero guidance IAS computed."); - } - - return m_guidance_ias; -} - -/** - * Computes time to next speed change. - * - * @param current_altitude altitude flight is at. - * @param altitude_at_end_of_route altitude at FAF or last point. - * - * @return time to next speed change. - */ -Units::Time PilotDelay::ComputeTimeToSpeedChange(Units::Length current_altitude, - Units::Length altitude_at_end_of_route) { - Units::SecondsTime tval; - - if ((current_altitude - altitude_at_end_of_route) > Units::FeetLength(9000.0)) { - tval = aaesim::open_source::ScenarioUtils::RANDOM_NUMBER_GENERATOR.TruncatedGaussianSample( - m_pilot_delay_mean, m_pilot_delay_standard_deviation, STANDARD_DEVIATION_LIMIT); - } else { - tval = aaesim::open_source::ScenarioUtils::RANDOM_NUMBER_GENERATOR.TruncatedGaussianSample( - m_pilot_delay_mean / 2, m_pilot_delay_standard_deviation / 2, STANDARD_DEVIATION_LIMIT); - } - - tval = abs(quantize(tval, Units::SecondsTime(1))); - - m_delay_count++; - double t = tval.value(); - m_delay_sum += t; - m_delay_square_sum += t * t; - m_delay_frequency[t]++; - - return tval; -} - -void PilotDelay::SetPilotDelayParameters(const Units::Time mean, const Units::Time standard_deviation) { - m_pilot_delay_mean = mean; - m_pilot_delay_standard_deviation = standard_deviation; - if (m_pilot_delay_is_on && (m_pilot_delay_standard_deviation * STANDARD_DEVIATION_LIMIT > m_pilot_delay_mean)) { - Units::SecondsTime low = m_pilot_delay_mean - m_pilot_delay_standard_deviation * STANDARD_DEVIATION_LIMIT; - Units::SecondsTime high = m_pilot_delay_mean + m_pilot_delay_standard_deviation * STANDARD_DEVIATION_LIMIT; - LOG4CPLUS_WARN(m_logger, "Pilot delay can range from " - << low << " to " << high << " based on mean=" << m_pilot_delay_mean - << ", standard deviation=" << m_pilot_delay_standard_deviation - << ", and standard deviation cap=" << STANDARD_DEVIATION_LIMIT << "." << std::endl - << "Computed negative delays will be flipped to positive."); - } -} - -/** - * Dumps PilotDelay objects. - * To use this, logger properties level must be set to DEBUG. - * - * @param str Header string for output. - */ -void PilotDelay::DumpParameters(std::string str) const { - LOG4CPLUS_DEBUG(PilotDelay::m_logger, std::endl - << "Pilot delay parms for " << str.c_str() << std::endl - << std::endl); - LOG4CPLUS_DEBUG(PilotDelay::m_logger, "m_pilot_delay_is_on " << m_pilot_delay_is_on << std::endl); - LOG4CPLUS_DEBUG(PilotDelay::m_logger, - "m_pilot_delay_mean " << Units::SecondsTime(m_pilot_delay_mean).value() << std::endl); - - LOG4CPLUS_DEBUG(PilotDelay::m_logger, - "mTimetoNextSpeedChange " << Units::SecondsTime(m_time_to_next_speed_change).value() << std::endl); - LOG4CPLUS_DEBUG(PilotDelay::m_logger, - "m_guidance_ias " << Units::MetersPerSecondSpeed(m_guidance_ias).value() << std::endl); - LOG4CPLUS_DEBUG(PilotDelay::m_logger, "m_guidance_mach " << m_guidance_mach << std::endl); -} - -/** - * Sets the guidance IAS to the converted Mach if known; otherwise the provided fallback. - */ -void PilotDelay::SetInitialIAS(Units::Length current_altitude, Units::Speed fallback_IAS) { - - // Have we been using Mach? - if (m_guidance_mach != 0) { - m_guidance_ias = m_atmosphere->MachToIAS(m_guidance_mach, current_altitude); - } else { - m_guidance_ias = fallback_IAS; - } -} - -void PilotDelay::DumpStatistics() const { - if (m_logger.getLogLevel() == log4cplus::TRACE_LOG_LEVEL) { - LOG4CPLUS_TRACE(m_logger, "****** PilotDelay statistics ******"); - const std::string BAR("************************************************************"); - double max_delay = m_delay_frequency.rbegin()->first; - for (double delay = 0; delay <= max_delay; delay++) { - int count = m_delay_frequency.at(delay); - LOG4CPLUS_TRACE(m_logger, std::setprecision(3) - << delay << ": " << BAR.substr(0, count) << " (" << count << ")"); - } - double mean = m_delay_sum / m_delay_count; - double standard_deviation = sqrt((m_delay_square_sum - m_delay_count * mean * mean) / m_delay_count); - LOG4CPLUS_TRACE(m_logger, "Number of delays (all iterations): " << m_delay_count); - LOG4CPLUS_TRACE(m_logger, "Average delay (all iterations): " << mean << " seconds" - << " (parameter value " << m_pilot_delay_mean - << ")"); - LOG4CPLUS_TRACE(m_logger, "Standard deviation (all iterations): " << standard_deviation << " seconds" - << " (parameter value " - << m_pilot_delay_standard_deviation << ")"); - } -} diff --git a/Public/PositionCalculator.cpp b/Public/PositionCalculator.cpp deleted file mode 100644 index a7b79e6..0000000 --- a/Public/PositionCalculator.cpp +++ /dev/null @@ -1,147 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include "public/PositionCalculator.h" - -#include -#include - -#include "public/CoreUtils.h" - -using namespace aaesim::open_source; - -log4cplus::Logger PositionCalculator::m_logger = log4cplus::Logger::getInstance("PositionCalculator"); - -PositionCalculator::PositionCalculator(const std::vector &horizontal_path, - TrajectoryIndexProgressionDirection expected_index_progression) - : DirectionOfFlightCourseCalculator(horizontal_path, expected_index_progression) {} - -PositionCalculator::PositionCalculator() : DirectionOfFlightCourseCalculator() {} - -PositionCalculator::~PositionCalculator() = default; - -bool PositionCalculator::CalculatePositionFromAlongPathDistance(const Units::Length &distance_along_path, - Units::Length &position_x, Units::Length &position_y, - Units::UnsignedAngle &course) { - std::vector::size_type resolved_index; - bool return_value = CalculatePosition(distance_along_path + EXTENSION_LENGTH, m_extended_horizontal_trajectory, - m_current_index, position_x, position_y, course, resolved_index); - - // Check for end of route is based on passed in distance_along_path - switch (m_index_progression_direction) { - case TrajectoryIndexProgressionDirection::DECREMENTING: - if (!m_is_passed_end_of_route) { - m_is_passed_end_of_route = distance_along_path < Units::zero(); - } - break; - - case TrajectoryIndexProgressionDirection::INCREMENTING: - if (m_is_passed_end_of_route) { - m_is_passed_end_of_route = distance_along_path < Units::zero(); - } - break; - - case TrajectoryIndexProgressionDirection::UNDEFINED: - m_is_passed_end_of_route = distance_along_path < Units::zero(); - break; - - default: - break; - } - - // Verify that resolved_index has not become discontinuous and is progressing appropriately - const bool found_index_is_valid = return_value && ValidateIndexProgression(resolved_index); - if (found_index_is_valid) { - // resolved_index looks correct. Update class member. - UpdateCurrentIndex(resolved_index); - - } else if (distance_along_path + EXTENSION_LENGTH > - Units::MetersLength(m_extended_horizontal_trajectory.back().m_path_length_cumulative_meters)) { - // distance_along_path is very large so off the back of the path. The old code allowed this situation to quietly - // happen. For now, it helps a lot to allow this. But, we should consider this deprecated behavior and throw in - // the future. - char msg[300]; - std::snprintf(msg, sizeof(msg), - "Very long distance_along_path encountered. Too long for path. Allowing for now: %f", - Units::MetersLength(distance_along_path).value()); - LOG4CPLUS_ERROR(m_logger, msg); - - } else { - // resolved_index looks incorrect. Throw. - char msg[300]; - std::snprintf( - msg, sizeof(msg), - "Invalid index progression encountered from CalculatePositionFromDistanceAlongPath(), current_index " - "%lu, resolved_index %lu", - m_current_index, resolved_index); - LOG4CPLUS_FATAL(m_logger, msg); - throw std::logic_error(msg); - } - - return return_value; -} - -bool PositionCalculator::CalculatePosition(const Units::Length &distance_along_path, - const std::vector &horizontal_trajectory, - const std::vector::size_type starting_trajectory_index, - Units::Length &x_position, Units::Length &y_position, - Units::UnsignedAngle &course, - std::vector::size_type &resolved_trajectory_index) { - Units::Angle turn_theta; - Units::Length turn_radius; - const bool found = CalculateForwardCourse(distance_along_path, horizontal_trajectory, starting_trajectory_index, - course, turn_theta, turn_radius, resolved_trajectory_index); - - if (found) { - // calculate position based on if it's a straight or turning path - if (horizontal_trajectory[resolved_trajectory_index].m_segment_type == HorizontalPath::SegmentType::STRAIGHT) { - const Units::Angle crs = Units::RadiansAngle(horizontal_trajectory[resolved_trajectory_index].m_path_course); - - // calculate output values - x_position = Units::MetersLength(horizontal_trajectory[resolved_trajectory_index].GetXPositionMeters()) + - ((distance_along_path - - Units::MetersLength( - horizontal_trajectory[resolved_trajectory_index].m_path_length_cumulative_meters)) * - cos(crs)); - y_position = Units::MetersLength(horizontal_trajectory[resolved_trajectory_index].GetYPositionMeters()) + - ((distance_along_path - - Units::MetersLength( - horizontal_trajectory[resolved_trajectory_index].m_path_length_cumulative_meters)) * - sin(crs)); - } else if (horizontal_trajectory[resolved_trajectory_index].m_segment_type == HorizontalPath::SegmentType::TURN) { - if ((distance_along_path - - Units::MetersLength(horizontal_trajectory[resolved_trajectory_index].m_path_length_cumulative_meters)) < - Units::MetersLength(3)) { - x_position = Units::MetersLength(horizontal_trajectory[resolved_trajectory_index].GetXPositionMeters()); - y_position = Units::MetersLength(horizontal_trajectory[resolved_trajectory_index].GetYPositionMeters()); - } else { - const Units::Length radius = turn_radius; - const Units::Angle theta = turn_theta; - x_position = - Units::MetersLength(horizontal_trajectory[resolved_trajectory_index].m_turn_info.x_position_meters) + - radius * cos(theta); - y_position = - Units::MetersLength(horizontal_trajectory[resolved_trajectory_index].m_turn_info.y_position_meters) + - radius * sin(theta); - } - } - } - - return found; -} diff --git a/Public/PrecalcConstraint.cpp b/Public/PrecalcConstraint.cpp deleted file mode 100644 index 07b41e3..0000000 --- a/Public/PrecalcConstraint.cpp +++ /dev/null @@ -1,61 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include "public/PrecalcConstraint.h" - -using namespace aaesim::open_source; - -bool operator<=(ActiveFlagType l, ActiveFlagType r) { return ((static_cast(l)) <= (static_cast(r))); } - -PrecalcConstraint &PrecalcConstraint::operator=(const PrecalcConstraint &obj) { - if (this != &obj) { - constraint_along_path_distance = obj.constraint_along_path_distance; - constraint_altHi = obj.constraint_altHi; - constraint_altLow = obj.constraint_altLow; - constraint_speedHi = obj.constraint_speedHi; - constraint_speedLow = obj.constraint_speedLow; - index = obj.index; - active_flag = obj.active_flag; - violation_flag = obj.violation_flag; - } - return *this; -} - -bool PrecalcConstraint::operator<(const PrecalcConstraint &obj) const { - return constraint_along_path_distance < obj.constraint_along_path_distance; -} - -bool PrecalcConstraint::operator==(const PrecalcConstraint &obj) const { - bool match = (constraint_along_path_distance == obj.constraint_along_path_distance); - - match = match && (constraint_altHi == obj.constraint_altHi); - match = match && (constraint_altLow == obj.constraint_altLow); - - match = match && (constraint_speedHi == obj.constraint_speedHi); - match = match && (constraint_speedLow == obj.constraint_speedLow); - - match = match && (index == obj.index); - - match = match && (active_flag == obj.active_flag); - match = match && (violation_flag == obj.violation_flag); - - return match; -} - -bool PrecalcConstraint::operator!=(const PrecalcConstraint &obj) const { return !operator==(obj); } diff --git a/Public/PrecalcWaypoint.cpp b/Public/PrecalcWaypoint.cpp deleted file mode 100644 index 7854048..0000000 --- a/Public/PrecalcWaypoint.cpp +++ /dev/null @@ -1,33 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include "public/PrecalcWaypoint.h" - -bool PrecalcWaypoint::operator==(const PrecalcWaypoint &obj) const { - bool match = (m_leg_length == obj.m_leg_length); - match = match && (m_course_angle == obj.m_course_angle); - match = match && (m_x_pos_meters == obj.m_x_pos_meters); - match = match && (m_y_pos_meters == obj.m_y_pos_meters); - match = match && (m_precalc_constraints == obj.m_precalc_constraints); - match = match && (m_rf_leg_center_x == obj.m_rf_leg_center_x); - match = match && (m_rf_leg_center_y == obj.m_rf_leg_center_y); - match = match && (m_radius_rf_leg == obj.m_radius_rf_leg); - - return match; -} diff --git a/Public/PredictionFileBase.cpp b/Public/PredictionFileBase.cpp deleted file mode 100644 index b8db4f4..0000000 --- a/Public/PredictionFileBase.cpp +++ /dev/null @@ -1,56 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// 2022 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include "public/PredictionFileBase.h" - -#include - -log4cplus::Logger PredictionFileBase::logger = log4cplus::Logger::getInstance(LOG4CPLUS_TEXT("PredictionFileBase")); - -PredictionFileBase::PredictionFileBase(const std::string &file_suffix) : OutputHandler("", file_suffix) {} - -std::vector PredictionFileBase::ExtractPredictionDataFromVerticalPath( - const unsigned int &iteration, const Units::Time simulation_time, const std::string &acid, - const VerticalPath &vertical_path, const PredictionData::DataSource &source) { - - std::vector prediction_data; - - for (auto m = 0; m < vertical_path.along_path_distance_m.size(); ++m) { - PredictionFileBase::PredictionData pdata; - - pdata.iteration_number = iteration; - pdata.source = source; - pdata.acid = acid; - pdata.simulation_time = simulation_time; - - pdata.altitude = Units::MetersLength(vertical_path.altitude_m[m]); - pdata.IAS = Units::MetersPerSecondSpeed(vertical_path.cas_mps[m]); - pdata.GS = Units::MetersPerSecondSpeed(vertical_path.gs_mps[m]); - pdata.TAS = Units::MetersPerSecondSpeed(vertical_path.true_airspeed[m]); - pdata.time_to_go = Units::SecondsTime(vertical_path.time_to_go_sec[m]); - pdata.distance_to_go = Units::MetersLength(vertical_path.along_path_distance_m[m]); - pdata.VwePred = vertical_path.wind_velocity_east[m]; - pdata.VwnPred = vertical_path.wind_velocity_north[m]; - pdata.algorithm = vertical_path.algorithm_type[m]; - pdata.flap_setting = vertical_path.flap_setting[m]; - - prediction_data.push_back(pdata); - } - return prediction_data; -} diff --git a/Public/RandomGenerator.cpp b/Public/RandomGenerator.cpp deleted file mode 100644 index baba4b6..0000000 --- a/Public/RandomGenerator.cpp +++ /dev/null @@ -1,114 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include "public/RandomGenerator.h" - -#include -#include -#include -#include - -#include "utility/UtilityConstants.h" - -const double RandomGenerator::m_IA = 16807; -const double RandomGenerator::m_IM = 2147483647; -const double RandomGenerator::m_AM = 1.0 / m_IM; -const double RandomGenerator::m_IQ = 127773.0; -const double RandomGenerator::m_IR = 2836.0; - -log4cplus::Logger RandomGenerator::m_logger = log4cplus::Logger::getInstance(LOG4CPLUS_TEXT("RandomGenerator")); - -RandomGenerator::RandomGenerator() { - double seed = time(NULL); - long k = seed / m_IM; - m_seed = seed - (k * m_IM); -} - -RandomGenerator::RandomGenerator(const double seed) { - assert(seed != 0.0); - long k = seed / m_IM; - m_seed = seed - (k * m_IM); -} - -const double RandomGenerator::UniformSample() { - long k = m_seed / m_IQ; - - m_seed = m_IA * (m_seed - k * m_IQ) - m_IR * k; - - if (m_seed < 0.0) { - m_seed += m_IM; - } - - double sample = m_AM * m_seed; - - LOG4CPLUS_TRACE(m_logger, m_seed << "," << sample); - - return sample; -} - -const double RandomGenerator::GaussianSample() { - double u1 = UniformSample(); - double u2 = UniformSample(); - - double eln = -2.0 * log(u1); - double ang = 2.0 * M_PI * u2; - - double v1 = sqrt(eln) * cos(ang); - - LOG4CPLUS_TRACE(m_logger, m_seed); - - return v1; -} - -const double RandomGenerator::TruncatedGaussianSample(const double max_standard_deviation) { - double val = max_standard_deviation + 1.0; - - while (val > (max_standard_deviation) || val < (-max_standard_deviation)) { - val = GaussianSample(); - } - LOG4CPLUS_TRACE(m_logger, m_seed); - - return val; -} - -const double RandomGenerator::RayleighSample() { - double u1 = UniformSample(); - double v1 = (sqrt(-2.0 * log(u1)) - 1.253) / sqrt(0.429); - - return v1; -} - -const double RandomGenerator::LaplaceSample() { - double uni = UniformSample(); - double err = -log(uni); - uni = UniformSample(); - - if (uni < 0.5) { - err = -err; - } - - return err; -} - -void RandomGenerator::SetSeed(const double seed) { - assert(seed != 0.0); - m_seed = seed; -} - -const double RandomGenerator::GetSeed(void) { return m_seed; } diff --git a/Public/RefReader.cpp b/Public/RefReader.cpp deleted file mode 100644 index 6ca2c21..0000000 --- a/Public/RefReader.cpp +++ /dev/null @@ -1,58 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// 2022 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -/* - * RefReader.cpp - * - * Created on: Mar 23, 2019 - * Author: klewis - */ - -#include "public/RefReader.h" - -namespace testvector { - -RefReader::RefReader(std::string file_name, int header_lines, size_t expected_columns) : - DataReader(file_name, header_lines, expected_columns) { -} - -RefReader::RefReader(std::shared_ptr input_stream, int header_lines, size_t expected_columns) : - DataReader(input_stream, header_lines, expected_columns) { -} - -RefReader::~RefReader() { -} - -bool RefReader::Advance() { - bool have_data = DataReader::Advance(); - if (have_data) { - m_time_to_fly = Units::SecondsTime(GetDouble(0)); - } - else { - m_time_to_fly = UNDEFINED_TIME; - } - - return have_data; -} - -const Units::SecondsTime RefReader::GetTimeToFly() const { - return m_time_to_fly; -} - -} // namespace testvector diff --git a/Public/RunFile.cpp b/Public/RunFile.cpp deleted file mode 100644 index 0a53ad2..0000000 --- a/Public/RunFile.cpp +++ /dev/null @@ -1,26 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// 2022 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include "public/RunFile.h" - -using namespace std; - -RunFile::RunFile(void) {} - -RunFile::~RunFile(void) { scenarios.clear(); } diff --git a/Public/Scenario.cpp b/Public/Scenario.cpp deleted file mode 100644 index e478d34..0000000 --- a/Public/Scenario.cpp +++ /dev/null @@ -1,60 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// 2022 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include "public/Scenario.h" - -using namespace std; - -const int Scenario::AIRCRAFT_ID_NOT_IN_MAP = -1; -log4cplus::Logger Scenario::m_logger = log4cplus::Logger::getInstance(LOG4CPLUS_TEXT("Scenario")); - -map Scenario::m_aircraft_string_int_map; -RandomGenerator Scenario::m_rand; -const Units::NauticalMilesLength Scenario::DEFAULT_ADS_B_RECEPTION_RANGE_THRESHOLD = Units::NauticalMilesLength(90.0); - -Scenario::Scenario() : m_scenario_name() {} - -Scenario::~Scenario() = default; - -bool Scenario::load(DecodedStream *input) { return true; } - -void Scenario::SetScenarioName(const string &in) { - m_scenario_name.assign(in); - - // remove the leading directory structure if present (search for last instance of "/" or "\\") - unsigned long index = m_scenario_name.find_last_of("/\\"); - if (index != string::npos) { - m_scenario_name = m_scenario_name.substr(index + 1); // sets the string to after the last "/" or "\\" - } - - index = m_scenario_name.find(".txt"); - if (index != string::npos) { - m_scenario_name.erase(index, 4); - } -} - -void Scenario::DuplicateAcidCheck(const size_t aircraft_count) { - size_t aircraft_id_count(m_aircraft_string_int_map.size()); - if (aircraft_count != aircraft_id_count) { - string msg = "Scenario has " + std::to_string(aircraft_count) + " aircraft but " + - std::to_string(aircraft_id_count) + " aircraft IDs."; - LOG4CPLUS_FATAL(m_logger, msg); - throw runtime_error(msg); - } -} diff --git a/Public/ScenarioUtils.cpp b/Public/ScenarioUtils.cpp deleted file mode 100644 index 806c24f..0000000 --- a/Public/ScenarioUtils.cpp +++ /dev/null @@ -1,27 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include "public/ScenarioUtils.h" - -#include -#include - -RandomGenerator aaesim::open_source::ScenarioUtils::RANDOM_NUMBER_GENERATOR; -const int aaesim::open_source::ScenarioUtils::AIRCRAFT_ID_NOT_IN_MAP = -1; -std::map aaesim::open_source::ScenarioUtils::m_aircraft_string_int_map; diff --git a/Public/SingleTangentPlaneSequence.cpp b/Public/SingleTangentPlaneSequence.cpp deleted file mode 100644 index 91ddbc7..0000000 --- a/Public/SingleTangentPlaneSequence.cpp +++ /dev/null @@ -1,42 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include "public/SingleTangentPlaneSequence.h" - -#include - -#include "public/CoreUtils.h" - -using namespace std; - -std::list SingleTangentPlaneSequence::m_master_waypoint_sequence = {}; -log4cplus::Logger SingleTangentPlaneSequence::m_logger = log4cplus::Logger::getInstance("SingleTangentPlaneSequence"); - -SingleTangentPlaneSequence::SingleTangentPlaneSequence(const list &waypoint_list) { - Initialize(waypoint_list); -} - -void SingleTangentPlaneSequence::Initialize(const std::list &waypoint_list) { - if (m_master_waypoint_sequence.empty()) { - m_master_waypoint_sequence = waypoint_list; - } - TangentPlaneSequence::Initialize(m_master_waypoint_sequence); -} - -void SingleTangentPlaneSequence::ClearStaticMembers() { m_master_waypoint_sequence.clear(); } diff --git a/Public/SpeedOnPitchControl.cpp b/Public/SpeedOnPitchControl.cpp deleted file mode 100644 index 3180caa..0000000 --- a/Public/SpeedOnPitchControl.cpp +++ /dev/null @@ -1,153 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include "public/SpeedOnPitchControl.h" - -#include -#include - -using namespace aaesim::open_source; - -void SpeedOnPitchControl::Initialize( - std::shared_ptr &aircraft_performance) { - AbstractDescentController::Initialize(aircraft_performance); - speed_on_thrust_controller_->Initialize(aircraft_performance_); -} - -void SpeedOnPitchControl::ComputeVerticalCommands( - const Guidance &guidance, const EquationsOfMotionState &equations_of_motion_state, - std::shared_ptr &sensed_weather, Units::Force &thrust_command, - Units::Angle &gamma_command, Units::Speed &true_airspeed_command, BoundedValue &speed_brake_command, - aaesim::open_source::bada_utils::FlapConfiguration &flap_configuration) { - Units::Force lift, drag; - ConfigureFlapsAndEstimateKineticForces(equations_of_motion_state, sensed_weather, aircraft_performance_, lift, drag, - flap_configuration); - - // Commands - SpeedValueType speed_type = guidance.GetSelectedSpeed().GetSpeedType(); - if (speed_type == SpeedValueType::MACH_SPEED) { - const auto mach_cmd = guidance.m_mach_command; - true_airspeed_command = - sensed_weather->GetTrueWeather()->MachToTAS(mach_cmd, equations_of_motion_state.altitude_msl); - } else { - const Units::Speed ias_com = guidance.m_ias_command; - true_airspeed_command = - sensed_weather->GetTrueWeather()->CAS2TAS(ias_com, equations_of_motion_state.altitude_msl); - } - const Units::Force max_thrust = Units::NewtonsForce(aircraft_performance_->GetMaxThrust( - equations_of_motion_state.altitude_msl, flap_configuration, - aaesim::open_source::bada_utils::EngineThrustMode::MAXIMUM_CRUISE, Units::ZERO_CELSIUS)); - const Units::Force min_thrust = Units::NewtonsForce(aircraft_performance_->GetMaxThrust( - equations_of_motion_state.altitude_msl, flap_configuration, - aaesim::open_source::bada_utils::EngineThrustMode::DESCENT, Units::ZERO_CELSIUS)); - const Units::Length alt_ref = Units::FeetLength(guidance.m_reference_altitude); - const Units::Length error_alt = equations_of_motion_state.altitude_msl - alt_ref; - const Units::Speed guidance_vertical_speed = guidance.m_vertical_speed; - const Units::Speed error_tas = true_airspeed_command - equations_of_motion_state.true_airspeed; - - if (is_level_flight_) { - speed_on_thrust_controller_->ComputeVerticalCommands(guidance, equations_of_motion_state, sensed_weather, - thrust_command, gamma_command, true_airspeed_command, - speed_brake_command, flap_configuration); - - // determine if staying in level flight - const static Units::Length tolerance = Units::FeetLength(200); - const bool altitude_above_constraint = - equations_of_motion_state.altitude_msl - guidance.m_active_precalc_constraints.constraint_altLow > - tolerance; - const bool altitude_rate_nonzero = guidance_vertical_speed != Units::zero(); - if (altitude_above_constraint && altitude_rate_nonzero) { - is_level_flight_ = false; - thrust_command = min_thrust; - } - - return; - } - - // manage altitude with thrust, speed with pitch - double esf = sensed_weather->GetTrueWeather()->ESFconstantCAS(equations_of_motion_state.true_airspeed, - equations_of_motion_state.altitude_msl); - - // adjust esf based on velocity error compared to the speed threshold - if (error_tas <= -speed_threshold_) { - esf = 0.3; - } else if (error_tas > -speed_threshold_ && error_tas <= Units::ZERO_SPEED) { - esf = (esf - 0.3) / speed_threshold_ * error_tas + esf; - } else if (error_tas > Units::ZERO_SPEED && error_tas <= speed_threshold_) { - esf = (1.7 - esf) / speed_threshold_ * error_tas + esf; - } else if (error_tas > speed_threshold_) { - esf = 1.7; - } - - // descent rate - const auto ac_mass = aircraft_performance_->GetAircraftMass(); - Units::Speed dh_dt = ((equations_of_motion_state.thrust - drag) * equations_of_motion_state.true_airspeed) / - (ac_mass * Units::ONE_G_ACCELERATION) * esf; - - gamma_command = Units::RadiansAngle(asin(-dh_dt / equations_of_motion_state.true_airspeed)); - - if (error_alt < -altitude_threshold_) { - thrust_command = 0.50 * max_thrust; - } else if (error_alt > altitude_threshold_) { - thrust_command = min_thrust; - } else { - thrust_command = (min_thrust - 0.50 * max_thrust) / (altitude_threshold_ * 2) * error_alt + - (0.50 * max_thrust + min_thrust) / 2.0; - } - - // Check if flight should level off - const bool altitude_near_constraint_low = - equations_of_motion_state.altitude_msl - guidance.m_active_precalc_constraints.constraint_altLow < - Units::FeetLength(100); - const bool altitude_rate_is_zero = guidance_vertical_speed == Units::zero(); - if (altitude_near_constraint_low || altitude_rate_is_zero) { - is_level_flight_ = true; - } - - // limit thrust to max and min limits - bool min_thrust_commanded = false; - if (thrust_command > max_thrust) { - thrust_command = max_thrust; - } else if (thrust_command < min_thrust) { - thrust_command = min_thrust; - min_thrust_commanded = true; - } - - // Determine if speed brake is needed - if (min_thrust_commanded) { - aaesim::open_source::bada_utils::FlapConfiguration updated_flap_configuration{ - aaesim::open_source::bada_utils::FlapConfiguration::UNDEFINED}; - if (error_alt > altitude_threshold_) { - Units::Speed v_cas = sensed_weather->GetTrueWeather()->TAS2CAS(equations_of_motion_state.true_airspeed, - equations_of_motion_state.altitude_msl); - aircraft_performance_->GetConfigurationForIncreasedDrag( - v_cas, Units::MetersLength(equations_of_motion_state.altitude_msl), updated_flap_configuration); - - if (updated_flap_configuration == flap_configuration && - updated_flap_configuration <= aaesim::open_source::bada_utils::FlapConfiguration::LANDING) { - speed_brake_controller_->Deploy(); - } - flap_configuration = updated_flap_configuration; - } - } - speed_brake_command = speed_brake_controller_->Update(min_thrust_commanded); - - DoLogging(logger_, equations_of_motion_state, error_alt, is_level_flight_, thrust_command, max_thrust, min_thrust, - error_tas, speed_brake_command, flap_configuration, gamma_command); -} diff --git a/Public/SpeedOnThrustControl.cpp b/Public/SpeedOnThrustControl.cpp deleted file mode 100644 index ff40f5f..0000000 --- a/Public/SpeedOnThrustControl.cpp +++ /dev/null @@ -1,113 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include "public/SpeedOnThrustControl.h" - -#include -#include - -#include "public/Environment.h" - -using namespace aaesim::open_source; - -void SpeedOnThrustControl::ComputeVerticalCommands( - const Guidance &guidance, const EquationsOfMotionState &equations_of_motion_state, - std::shared_ptr &sensed_weather, Units::Force &thrust_command, - Units::Angle &gamma_command, Units::Speed &tas_command, BoundedValue &speed_brake_command, - aaesim::open_source::bada_utils::FlapConfiguration &flap_configuration) { - const Units::Speed hdot_ref = guidance.m_vertical_speed; - const Units::Length alt_ref = guidance.m_reference_altitude; - const Units::Length error_alt = alt_ref - equations_of_motion_state.altitude_msl; - - double temp_gamma = -(hdot_ref + gain_altitude_ * error_alt) / equations_of_motion_state.true_airspeed; - if (temp_gamma > 1.0) { - temp_gamma = 1.0; - } else if (temp_gamma < -1.0) { - temp_gamma = -1.0; - } - gamma_command = Units::RadiansAngle(asin(temp_gamma)); - - SpeedValueType speed_type = guidance.GetSelectedSpeed().GetSpeedType(); - if (speed_type == SpeedValueType::MACH_SPEED) { - tas_command = sensed_weather->GetTrueWeather()->MachToTAS(guidance.m_mach_command, - equations_of_motion_state.altitude_msl); - } else { - tas_command = - sensed_weather->GetTrueWeather()->CAS2TAS(guidance.m_ias_command, equations_of_motion_state.altitude_msl); - } - - const Units::Speed error_tas = tas_command - equations_of_motion_state.true_airspeed; - const Units::Acceleration vel_dot_com = gain_true_airspeed_ * error_tas; - - Units::Force lift{}, drag{}; - ConfigureFlapsAndEstimateKineticForces(equations_of_motion_state, sensed_weather, aircraft_performance_, lift, drag, - flap_configuration); - - // Thrust to maintain speed - const auto ac_mass = aircraft_performance_->GetAircraftMass(); - const Units::Force thrust_equilibrium = - ac_mass * vel_dot_com + drag - ac_mass * Units::ONE_G_ACCELERATION * sin(equations_of_motion_state.gamma) - - ac_mass * equations_of_motion_state.true_airspeed * - (sensed_weather->GetWindSpeedVerticalDerivativeEast() * cos(equations_of_motion_state.psi_enu) + - sensed_weather->GetWindSpeedVerticalDerivativeNorth() * sin(equations_of_motion_state.psi_enu)) * - sin(equations_of_motion_state.gamma) * cos(equations_of_motion_state.gamma); - const Units::Force max_thrust = Units::NewtonsForce(aircraft_performance_->GetMaxThrust( - equations_of_motion_state.altitude_msl, flap_configuration, - aaesim::open_source::bada_utils::EngineThrustMode::MAXIMUM_CRUISE, Units::ZERO_CELSIUS)); - const Units::Force min_thrust = Units::NewtonsForce(aircraft_performance_->GetMaxThrust( - equations_of_motion_state.altitude_msl, flap_configuration, - aaesim::open_source::bada_utils::EngineThrustMode::DESCENT, Units::ZERO_CELSIUS)); - - // Check Configuration if min_thrust is commanded - thrust_command = thrust_equilibrium; - bool min_thrust_commanded{false}; - if (thrust_equilibrium < min_thrust) { - thrust_command = min_thrust; - min_thrust_commanded = true; - ++min_thrust_counter_; - - const Units::Speed calibrated_airspeed = sensed_weather->GetTrueWeather()->TAS2CAS( - equations_of_motion_state.true_airspeed, equations_of_motion_state.altitude_msl); - - aaesim::open_source::bada_utils::FlapConfiguration updated_flap_configuration = - aaesim::open_source::bada_utils::FlapConfiguration::UNDEFINED; - aircraft_performance_->GetConfigurationForIncreasedDrag( - calibrated_airspeed, equations_of_motion_state.altitude_msl, updated_flap_configuration); - flap_configuration = updated_flap_configuration; - } else { - min_thrust_counter_ = 0; - if (thrust_equilibrium > max_thrust) { - thrust_command = max_thrust; - } - } - - static const Units::KnotsSpeed tas_error_tolerance{-5}; - static const unsigned int minimum_thrust_duration{15}; - if (min_thrust_counter_ > minimum_thrust_duration and error_tas < tas_error_tolerance) { - if (flap_configuration <= aaesim::open_source::bada_utils::FlapConfiguration::LANDING) { - speed_brake_controller_->Deploy(); - } else { - speed_brake_controller_->Retract(); - } - } - speed_brake_command = speed_brake_controller_->Update(min_thrust_commanded); - - DoLogging(logger_, equations_of_motion_state, error_alt, true, thrust_command, max_thrust, min_thrust, error_tas, - speed_brake_command, flap_configuration, gamma_command); -} diff --git a/Public/StandardAtmosphere.cpp b/Public/StandardAtmosphere.cpp deleted file mode 100644 index cecc7db..0000000 --- a/Public/StandardAtmosphere.cpp +++ /dev/null @@ -1,103 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// 2023 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include "public/StandardAtmosphere.h" -#include "public/Logging.h" - -log4cplus::Logger StandardAtmosphere::m_logger = log4cplus::Logger::getInstance(LOG4CPLUS_TEXT("StandardAtmosphere")); - -// Standard Temperature at Sea Level -// BADA_37_USER_MANUAL eq. 3.2-3 -const Units::KelvinTemperature T0_ISA(288.15); - -// Temperature of the tropopause -// BADA_37_USER_MANUAL eq. 3.2-4 -const Units::KelvinTemperature T_TROP(216.65); - -const Units::KelvinPerMeter TEMPERATURE_GRADIENT_TROPOSPHERE(-.0065); - -StandardAtmosphere *StandardAtmosphere::MakeInstance(const Units::KelvinTemperature temperature, - const Units::Length altitude) { - if (temperature < T_TROP) { - LOG4CPLUS_DEBUG(m_logger, "Creating non-standard StandardAtmosphere with temperature " - << temperature << " at " << Units::FeetLength(altitude) - << ". Tropopause temperature is " << T_TROP); - } - if (altitude < Units::zero()) { - LOG4CPLUS_FATAL(m_logger, "Specified altitude is below sea level: " << Units::FeetLength(altitude)); - throw std::runtime_error("Invalid altitude"); - } - if (temperature == T_TROP) { - LOG4CPLUS_WARN(m_logger, "StandardAtmosphere created using tropopause temperature " - << T_TROP << " at " << Units::FeetLength(altitude) - << ". Assuming that represents the floor of the tropopause."); - } - - // sea_level_temperature = temperature + 6.5/1000 * altitude - Units::KelvinTemperature sea_level_temperature = temperature - TEMPERATURE_GRADIENT_TROPOSPHERE * altitude; - Units::KelvinTemperature offset(sea_level_temperature - T0_ISA); - return new StandardAtmosphere(offset); -} - -StandardAtmosphere *StandardAtmosphere::MakeInstanceFromTemperatureOffset( - Units::CelsiusTemperature temperature_offset) { - return new StandardAtmosphere(temperature_offset); -} - -StandardAtmosphere::StandardAtmosphere(const Units::Temperature temperatureOffset) { - SetTemperatureOffset(temperatureOffset); -} - -void StandardAtmosphere::SetTemperatureOffset(const Units::Temperature temperatureOffset) { - m_temperature_offset = temperatureOffset; - // BADA_37_USER_MANUAL eq. 3.2-1 - m_tropopause_height = - Units::MetersLength(11000) + Units::MetersLength(1000) * m_temperature_offset / Units::CelsiusTemperature(6.5); - m_sea_level_temperature = T0_ISA + m_temperature_offset; // BADA_37_USER_MANUAL eq. 3.2-2 - m_sea_level_density = RHO0_ISA * T0_ISA / m_sea_level_temperature; // BADA_37_USER_MANUAL eq. 3.2-7 - m_tropopause_density = m_sea_level_density * pow(T_TROP / m_sea_level_temperature, RHO_T_EXPONENT); - m_tropopause_pressure = P0_ISA * pow(T_TROP / m_sea_level_temperature, P_T_EXPONENT); -} - -StandardAtmosphere::~StandardAtmosphere() { - // nothing to do -} - -Units::Temperature StandardAtmosphere::GetTemperatureOffset() const { return m_temperature_offset; } - -Units::KelvinTemperature StandardAtmosphere::GetTemperature(const Units::Length altitude_msl) const { - Units::KelvinTemperature T; - if (altitude_msl < GetTropopauseHeight()) { - T = GetSeaLevelTemperature() - Units::KelvinPerMeter(6.5 / 1000) * altitude_msl; - } else { - T = T_TROP; - } - - return T; -} - -Units::KelvinTemperature StandardAtmosphere::GetSeaLevelTemperature() const { return m_sea_level_temperature; } - -Units::MetersLength StandardAtmosphere::GetTropopauseHeight() const { return m_tropopause_height; } - -Units::Density StandardAtmosphere::GetSeaLevelDensity() const { return m_sea_level_density; } - -Units::Density StandardAtmosphere::GetTropopauseDensity() const { return m_tropopause_density; } - -Units::Pressure StandardAtmosphere::GetTropopausePressure() const { return m_tropopause_pressure; } diff --git a/Public/StatisticalPilotDelay.cpp b/Public/StatisticalPilotDelay.cpp deleted file mode 100644 index 7446bd0..0000000 --- a/Public/StatisticalPilotDelay.cpp +++ /dev/null @@ -1,187 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include "public/StatisticalPilotDelay.h" - -#include - -#include - -#include "public/CustomMath.h" -#include "public/ScenarioUtils.h" - -using namespace aaesim::open_source; - -log4cplus::Logger StatisticalPilotDelay::m_logger = - log4cplus::Logger::getInstance(LOG4CPLUS_TEXT("StatisticalPilotDelay")); - -void StatisticalPilotDelay::IterationReset() { - m_time_to_next_speed_change = Units::SecondsTime(-1.0); - m_guidance_ias = Units::zero(); - m_guidance_mach = 0.0; -} - -/** - * Recomputes and updates mach as necessary according to the time delay - * and recomputes ias accordingly. Manages time to the next speed change. - * - * @param previous_im_speed_command_mach mach command from previous time. - * @param input_im_speed_command_mach mach computed from calling method. - * @param current_altitude altitude flight is at. - * @param altitude_at_end_of_route altitude at the FAF or last point. - * - * @return recomputed guidance ias speed. - */ -Units::Speed StatisticalPilotDelay::UpdateMach(double previous_speed_command_mach, double proposed_speed_command_mach, - Units::Length current_altitude, Units::Length altitude_at_end_of_route) { - if ((proposed_speed_command_mach != previous_speed_command_mach) && (m_time_to_next_speed_change < Units::zero())) { - // Reset time delay counter and set m_guidance_mach to previous speed. - m_time_to_next_speed_change = ComputeTimeToSpeedChange(current_altitude, altitude_at_end_of_route); - m_guidance_mach = previous_speed_command_mach; - } else if (m_guidance_ias == Units::zero()) { - // ias has not changed and guidance ias not set. Compute guidance ias - // from guidance mach. - m_guidance_mach = previous_speed_command_mach; - SetInitialIAS(current_altitude, m_atmosphere->MachToIAS(previous_speed_command_mach, current_altitude)); - - if (m_time_to_next_speed_change < Units::zero()) { - // past delay time-recompute new delay time and update guidance ias. - m_time_to_next_speed_change = ComputeTimeToSpeedChange(current_altitude, altitude_at_end_of_route); - } - } - - if (m_time_to_next_speed_change == Units::zero()) { - // At time to change speed-set to input mach. - m_guidance_mach = proposed_speed_command_mach; - } - - // Update time delay counter and return guidance speed in ias. - m_time_to_next_speed_change -= Units::SecondsTime(1.0); - - return m_atmosphere->MachToIAS(m_guidance_mach, current_altitude); -} - -/** - * Recomputes and updates ias as necessary. The pilot delay time is also updated. - * - * @param previous_speed_command_ias Computed ias from the last time. - * @param proposed_speed_command_ias computed ias from calling method. - * @param current_altitude aircraft altitude. - * @param altitude_at_end_of_route altitude at the FAF or last point. - * - * @return guidance ias speed. - */ -Units::Speed StatisticalPilotDelay::UpdateIAS(Units::Speed previous_speed_command_ias, - Units::Speed proposed_speed_command_ias, Units::Length current_altitude, - Units::Length altitude_at_end_of_route) { - if (proposed_speed_command_ias != previous_speed_command_ias) { - // ias has changed. - if (m_time_to_next_speed_change < Units::zero()) { - // past delay time-recompute new delay time and update guidance ias. - m_time_to_next_speed_change = ComputeTimeToSpeedChange(current_altitude, altitude_at_end_of_route); - - if (m_guidance_ias == Units::zero()) { - // compute first guidance ias - SetInitialIAS(current_altitude, previous_speed_command_ias); - } else { - // set guidance ias from previous ias. - m_guidance_ias = previous_speed_command_ias; - } - } else if ((m_time_to_next_speed_change > Units::zero()) && (m_guidance_ias == Units::zero())) { - // not at delay time yet and guidance ias not set-compute guidance ias - // from guidance mach. - SetInitialIAS(current_altitude, previous_speed_command_ias); - } - } else if (m_guidance_ias == Units::zero()) { - // ias has not changed and guidance ias not set-compute guidnace ias - // from guidance mach. - SetInitialIAS(current_altitude, previous_speed_command_ias); - } - - if (m_time_to_next_speed_change == Units::zero()) { - // time to process delay-set guidance ias from input ias. - m_guidance_ias = proposed_speed_command_ias; - } - - // update delay time and return guidance ias. - m_time_to_next_speed_change -= Units::SecondsTime(1.0); - - if (m_guidance_ias == Units::zero()) { - throw std::runtime_error("Zero guidance IAS computed."); - } - - return m_guidance_ias; -} - -/** - * Computes time to next speed change. - * - * @param current_altitude altitude flight is at. - * @param altitude_at_end_of_route altitude at FAF or last point. - * - * @return time to next speed change. - */ -Units::Time StatisticalPilotDelay::ComputeTimeToSpeedChange(Units::Length current_altitude, - Units::Length altitude_at_end_of_route) { - Units::SecondsTime tval; - - if ((current_altitude - altitude_at_end_of_route) > Units::FeetLength(9000.0)) { - tval = aaesim::open_source::ScenarioUtils::RANDOM_NUMBER_GENERATOR.TruncatedGaussianSample( - m_pilot_delay_mean, m_pilot_delay_standard_deviation, STANDARD_DEVIATION_LIMIT); - } else { - tval = aaesim::open_source::ScenarioUtils::RANDOM_NUMBER_GENERATOR.TruncatedGaussianSample( - m_pilot_delay_mean / 2, m_pilot_delay_standard_deviation / 2, STANDARD_DEVIATION_LIMIT); - } - - tval = abs(quantize(tval, Units::SecondsTime(1))); - - m_delay_count++; - double t = tval.value(); - m_delay_sum += t; - m_delay_square_sum += t * t; - m_delay_frequency[t]++; - - return tval; -} - -void StatisticalPilotDelay::SetPilotDelayParameters(const Units::Time mean, const Units::Time standard_deviation) { - m_pilot_delay_mean = mean; - m_pilot_delay_standard_deviation = standard_deviation; - if (m_pilot_delay_is_on && (m_pilot_delay_standard_deviation * STANDARD_DEVIATION_LIMIT > m_pilot_delay_mean)) { - Units::SecondsTime low = m_pilot_delay_mean - m_pilot_delay_standard_deviation * STANDARD_DEVIATION_LIMIT; - Units::SecondsTime high = m_pilot_delay_mean + m_pilot_delay_standard_deviation * STANDARD_DEVIATION_LIMIT; - LOG4CPLUS_WARN(m_logger, "Pilot delay can range from " - << low << " to " << high << " based on mean=" << m_pilot_delay_mean - << ", standard deviation=" << m_pilot_delay_standard_deviation - << ", and standard deviation cap=" << STANDARD_DEVIATION_LIMIT << "." << std::endl - << "Computed negative delays will be flipped to positive."); - } -} - -/** - * Sets the guidance IAS to the converted Mach if known; otherwise the provided fallback. - */ -void StatisticalPilotDelay::SetInitialIAS(Units::Length current_altitude, Units::Speed fallback_IAS) { - // Have we been using Mach? - if (m_guidance_mach != 0) { - m_guidance_ias = m_atmosphere->MachToIAS(m_guidance_mach, current_altitude); - } else { - m_guidance_ias = fallback_IAS; - } -} diff --git a/Public/StereographicProjection.cpp b/Public/StereographicProjection.cpp deleted file mode 100644 index 3e2a2ec..0000000 --- a/Public/StereographicProjection.cpp +++ /dev/null @@ -1,288 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include "public/StereographicProjection.h" - -#include "utility/UtilityConstants.h" - -Units::RadiansAngle StereographicProjection::latTPT(0); /* North latitude of tangency point (radians) */ -Units::RadiansAngle StereographicProjection::lonTPT(0); /* **WEST** longitude of tangency point (radians) */ - -Units::FeetLength StereographicProjection::eRadius(0); // earth radius at tangent point (lon1, lat1), feet -/* Convienience parameters calculated from raw parameters */ -double StereographicProjection::sin_latTPT = 0; /* sin of latitude of tangency point */ -double StereographicProjection::sin_clatTPT = 0; /* sin of conformal latitude of tangency point */ -double StereographicProjection::cos_clatTPT = 0; /* cos of conformal latitude of tangency point */ - -/* Convienience parameters used only for the reverse NAS projection */ -double StereographicProjection::cos_gamma = 0; -double StereographicProjection::sin_gamma = 0; - -double StereographicProjection::GEOD_CONST_A = 0.9932773; -double StereographicProjection::GEOD_CONST_B = 0.0066625; - -void StereographicProjection::init(Units::Angle lat, Units::Angle lon, Units::Length earthRadius) { - latTPT = lat; - lonTPT = -lon; - - eRadius = earthRadius; - - // beginning of init - double gamma; - - sin_latTPT = sin(latTPT); - - /* Calculate sin and cos of conformal latitude instead of geodetic */ - sin_clatTPT = toConformalSin(sin_latTPT); - if (sin_clatTPT > 1.0) { - sin_clatTPT = 1.0; - } else if (sin_clatTPT < -1.0) { - sin_clatTPT = -1.0; - } - - cos_clatTPT = 1 - sin_clatTPT * sin_clatTPT; - if (cos_clatTPT > 0.0) { - cos_clatTPT = sqrt(cos_clatTPT); - } else { - cos_clatTPT = 0.0; - } - /* ** ADDED FOR SOUTHERN HEMISPHERE */ - if (latTPT.value() < 0.0) { - cos_clatTPT = -cos_clatTPT; - } - - gamma = aaesim::open_source::constants::PI / 2.0 - asin(sin_clatTPT); - sin_gamma = sin(gamma); - cos_gamma = cos(gamma); -} - -// This function contains the logic for converting xy_vector (x, y) to lat2_lon2 with lat1_lon1 as the tangent point, -// where Input: Position lat_lon1 = (lon1, lat1), lon1 and lat1 in radian; It is the tangent point. Vector xy_vector = -// (x, y) in feet Output: Position lat2_lon2 = (*lon2, *lat2), *lon2, *lat2 in radian; Processing: Use lat1_lon1 as the -// tangent point and to convert xy_vector to lat2_lon2. A typical value of eRadius is 3440.1344*NM2FT (feet) - -void StereographicProjection::xy_to_ll(const Units::Length x, const Units::Length y, Units::Angle &lat2, - Units::Angle &lon2) - -/* Here is the real reverse conversion from NAS coordinates to lat,long. */ -/* Based on the routine CNV_XYLL in the AERA PL1 software, written by David */ -/* Chaloux (PSI). */ -/* This non-iterative approach was concieved by David Chaloux and is */ -/* documented in the memo F048-M-362 "Changes to the EnRoute Coordinate */ -/* Conversion Routines". The approach has the advantage over previous */ -/* iterative algorithms and the equations in NAS-MD-312 in that it works */ -/* over a much larger area of the globe. */ -{ - Units::RadiansAngle alpha; /* The angle between the point of tangency, the - XY position, and the center of the earth.*/ - Units::RadiansAngle beta; /* The angle between the point of tangency, the - XY position, and the Longitudinal line at the - point of tangency.*/ - Units::RadiansAngle delta; /* The latitude component measured from the - North Pole.*/ - Units::RadiansAngle epsilon; /* The longitude component measured from the - point of tangency.*/ - double sin_eps; /* Used in calculating Epsilon.*/ - double cos_eps; /* Used in calculating Epsilon.*/ - double sin_alpha; - double cos_alpha; - double sin_delta; - double cos_delta; - double dlatc; // it's an angle, but used in some other calculations too - - double sin_phi; - - /* Find alpha - the angle between the point of tangency, - the specified (X,Y) position, and the center of the earth.*/ - - /* First find the angle between point of tangency, - the specified position, and the point opposite the point - of tangency on the globe.*/ - - alpha = Units::RadiansAngle(asin(sqrt(x * x + y * y) / (sqrt(x * x + y * y + (4. * eRadius * eRadius))))); - - /* Now convert to what we originally wanted (angle with center - of earth) by multiplying by two.*/ - - alpha = alpha * 2.0; - - cos_alpha = cos(alpha); - sin_alpha = sin(alpha); - - /* Now find Beta, the angle between the point of tangency, - the XY position, and the Longitudinal line at the point - of tangency.*/ - - if (x == Units::FeetLength(0.0) && y == Units::FeetLength(0.0)) { - beta = Units::RadiansAngle(0.0); - } else { - beta = Units::arctan2(Units::FeetLength(x).value(), Units::FeetLength(y).value()); - } - - /* Find Delta, the latitude component measured from the - North Pole.*/ - - cos_delta = cos_alpha * cos_gamma + sin_alpha * sin_gamma * cos(beta); - - /* Be sure that roundoffs haven't gotten you slightly larger or - smaller than allowable limits.*/ - - if (cos_delta > 1.0) { - cos_delta = 1.0; - } - - if (cos_delta < -1.0) { - cos_delta = -1.0; - } - - delta = Units::RadiansAngle(acos(cos_delta)); - sin_delta = sin(delta); - - dlatc = aaesim::open_source::constants::PI / 2. - delta.value(); /* The conformal latitude of the X,Y point*/ - - /* Find Epsilon, The longitude component measured from the - point of tangency.*/ - - sin_eps = sin_delta; /* Catches both 0. and Pi case*/ - - if (sin_delta != 0.0) { - sin_eps = sin_alpha * sin(beta) / sin_delta; - } - - /*Again make sure that we are within allowable limits. Must do - this because of roundoff errors*/ - - if (sin_eps > 1.0) { - sin_eps = 1.0; - } else if (sin_eps < -1.0) { - sin_eps = -1.0; - } - - /* Note: The following can fail in a couple of cases. If the - point of tangency is at one of the poles or if the XY position - to be converted is at one of the poles it will fail. The case - where the XY location is at one of the poles is tested here. - Point of tangency at the pole should be avoided.*/ - - if (sin_delta > 0.0) { - cos_eps = (cos_alpha - cos_gamma * cos_delta) / (sin_gamma * sin_delta); - } else { - cos_eps = 0.0; - } - - /*Again make sure that roundoff doesn't bite you.*/ - - if (cos_eps < -1.0) { - cos_eps = -1.0; - } else if (cos_eps > 1.0) { - cos_eps = 1.0; - } - - epsilon = Units::arctan2(sin_eps, cos_eps); - - /*Now find the actual longitude*/ - - lon2 = lonTPT - epsilon; - - /* Now, convert the latitude which is in conformal coordinates - to geodetic coordinates. This method of doing that is not the - method used by Chaloux, but uses a technique outlined by - NAS-MD-312. However, the calculation is performed twice. - First to derive an initial estimate, then second to refine the - estimate. This technique was found to have the lowest worst - case errors. */ - - sin_phi = sin(dlatc); - - dlatc = sin_phi / (GEOD_CONST_A + GEOD_CONST_B * sin_phi * sin_phi); - - /*Perform the calculation 1 more time for more accuracy*/ - - dlatc = sin_phi / (GEOD_CONST_A + GEOD_CONST_B * dlatc * dlatc); - - if (dlatc > 1.0) { - dlatc = 1.0; - } else if (dlatc < -1.0) { - dlatc = -1.0; - } - - lat2 = Units::RadiansAngle(asin(dlatc)); /* WILL NOT WORK FOR THE SOUTHERN HEMISPHERE!!! */ - lon2 = -lon2; /* Convert back to east longitude */ -} - -//------------------------------------------------------------------------------- -// This function is mainly for the purpose of testing the above function. -// This function contains the logic for the converting lat2_lon2 to xy_vector with lat1_lon1 as the tangent point, where -// Input: -// Position lat1_lon1 = (lon1, lat1), lon1 and lat1 in radian; It is the tangent point. -// Position position2 = (lon2, lat2), lon2, lat2 in radian. -// -// Output: -// Vector Vector = (*x, *y) in feet -// Processing: -// Use position1 as the tangent point and convert position2 to Vector. -// A typical value of eRadius is 3440.1344*NM2FT (feet) -void StereographicProjection::ll_to_xy(const Units::Angle lat2, Units::Angle lon2, Units::Length &x, Units::Length &y) - -/* Here is the real conversion to NAS coordinates.*/ -/* Based on the routine CNV_LLXY from the AERA PL1 software and*/ -/* NAS-MD-312 Appendix D.*/ -{ - double sin_lat; - Units::RadiansAngle dlong; - double cos_dlong, sin_dlong; - double sin_PHI, cos_PHI; - double denom; - - lon2 = -lon2; /* Convert from east longitude to west longitude*/ - - sin_lat = sin(lat2); - dlong = lonTPT - lon2; /* delta longitude from point of tangency.*/ - cos_dlong = cos(dlong); - sin_dlong = sin(dlong); - - /* Convert to conformal latitude instead of geodetic*/ - sin_PHI = toConformalSin(sin_lat); - - if (sin_PHI > 1.0) { - sin_PHI = 1.0; - } else if (sin_PHI < -1.) { - sin_PHI = -1.0; - } - - /* Determine the cos_PHI. This is a faster and more accurate*/ - /* method than cos_PHI = cos(Asin(sin_PHI)).*/ - cos_PHI = 1 - sin_PHI * sin_PHI; - if (cos_PHI > 0.0) { - cos_PHI = sqrt(cos_PHI); - } else { - cos_PHI = 0.0; - } - /* ADDED FOR THE SOUTHERN HEMISPHERE*/ - if (lat2 < Units::RadiansAngle(0.0)) { - cos_PHI = -cos_PHI; - } - - denom = 1 + sin_PHI * sin_clatTPT + cos_PHI * cos_clatTPT * cos_dlong; - - x = 2.0 * eRadius * sin_dlong * cos_PHI / denom; - y = 2.0 * eRadius * (sin_PHI * cos_clatTPT - cos_PHI * sin_clatTPT * cos_dlong) / denom; -} - -double StereographicProjection::toConformalSin(double x) { return x * (GEOD_CONST_A + GEOD_CONST_B * (x) * (x)); } diff --git a/Public/TangentPlaneSequence.cpp b/Public/TangentPlaneSequence.cpp deleted file mode 100644 index 8671d73..0000000 --- a/Public/TangentPlaneSequence.cpp +++ /dev/null @@ -1,144 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include "public/TangentPlaneSequence.h" - -#include -#include -#include -#include - -#include "public/Environment.h" - -using namespace std; - -TangentPlaneSequence::TangentPlaneSequence() {} - -TangentPlaneSequence::TangentPlaneSequence(list &waypoint_list) { Initialize(waypoint_list); } - -TangentPlaneSequence::TangentPlaneSequence(const TangentPlaneSequence &in) { Copy(in); } - -void TangentPlaneSequence::Copy(const TangentPlaneSequence &in) { - this->local_positions_from_initialization_ = in.local_positions_from_initialization_; - this->tangent_planes_from_initialization_ = in.tangent_planes_from_initialization_; - this->waypoints_from_initialization_ = in.waypoints_from_initialization_; -} - -void TangentPlaneSequence::Initialize(const std::list &waypoint_list) { - shared_ptr tangent_plane = shared_ptr((LocalTangentPlane *)NULL); - EarthModel::LocalPositionEnu enu; - enu.x = enu.y = enu.z = Units::zero(); - - auto vector_idx = waypoint_list.size() - 1; - waypoints_from_initialization_.resize(vector_idx + 1); - tangent_planes_from_initialization_.resize(vector_idx + 1); - local_positions_from_initialization_.resize(vector_idx + 1); - auto build_tangent_planes = [this, &tangent_plane, &vector_idx](const Waypoint &waypoint) { - EarthModel::GeodeticPosition geo; - geo.altitude = Units::zero(); - geo.latitude = waypoint.GetLatitude(); - geo.longitude = waypoint.GetLongitude(); - - EarthModel::LocalPositionEnu enu; - if (tangent_plane == NULL) { - enu.x = enu.y = enu.z = Units::zero(); - } else { - // convert using previous tangent_plane - tangent_plane->ConvertGeodeticToLocal(geo, enu); - } - - // make the new tangent plane - tangent_plane = Environment::GetInstance()->GetEarthModel()->MakeEnuConverter(geo, enu); - - // update vectors - tangent_planes_from_initialization_[vector_idx] = tangent_plane; - local_positions_from_initialization_[vector_idx] = enu; - waypoints_from_initialization_[vector_idx] = waypoint; - - // decrement index - vector_idx--; - }; - // use reverse iterator so that last will be processed first - std::for_each(waypoint_list.rbegin(), waypoint_list.rend(), build_tangent_planes); -} - -void TangentPlaneSequence::ConvertLocalToGeodetic(EarthModel::LocalPositionEnu local_position, - EarthModel::GeodeticPosition &geo_position) const { - std::vector areas; - auto compute_metric = [&areas, local_position](const EarthModel::LocalPositionEnu &point_of_tangency) { - Units::Length x = local_position.x - point_of_tangency.x; - Units::Length y = local_position.y - point_of_tangency.y; - Units::Area d2 = x * x + y * y; - areas.push_back(d2); - }; - auto get_point_of_tangency = [&compute_metric](const std::shared_ptr &tangent_plane) { - compute_metric(tangent_plane->getPointOfTangencyEnu()); - }; - std::for_each(tangent_planes_from_initialization_.begin(), tangent_planes_from_initialization_.end(), - get_point_of_tangency); - auto minimum = std::min_element(areas.begin(), areas.end()); - if (minimum == areas.end()) { - LOG4CPLUS_FATAL(logger_, - "size of tangent_planes_from_initialization_: " << tangent_planes_from_initialization_.size()); - throw logic_error("Unable to determine closest point (empty?)"); - } - auto closest_tangent_plane_itr = std::distance(areas.begin(), minimum); - tangent_planes_from_initialization_[closest_tangent_plane_itr]->ConvertLocalToGeodetic(local_position, geo_position); -} - -void TangentPlaneSequence::ConvertGeodeticToLocal(EarthModel::GeodeticPosition geo_position, - EarthModel::LocalPositionEnu &local_position) const { - EarthModel::AbsolutePositionEcef ecef_position; - Environment::GetInstance()->GetEarthModel()->ConvertGeodeticToAbsolute(geo_position, ecef_position); - std::vector areas; - auto compute_metric = [&areas, ecef_position](const EarthModel::AbsolutePositionEcef &point_of_tangency) { - Units::Length x = ecef_position.x - point_of_tangency.x; - Units::Length y = ecef_position.y - point_of_tangency.y; - Units::Length z = ecef_position.z - point_of_tangency.z; - Units::Area d2 = x * x + y * y + z * z; - areas.push_back(d2); - }; - auto get_point_of_tangency = [&compute_metric](const std::shared_ptr &tangent_plane) { - compute_metric(tangent_plane->getPointOfTangencyEcef()); - }; - std::for_each(tangent_planes_from_initialization_.begin(), tangent_planes_from_initialization_.end(), - get_point_of_tangency); - auto minimum = std::min_element(areas.begin(), areas.end()); - if (minimum == areas.end()) { - LOG4CPLUS_FATAL(logger_, - "size of tangent_planes_from_initialization_: " << tangent_planes_from_initialization_.size()); - throw logic_error("Unable to determine closest point (empty?)"); - } - auto closest_tangent_plane_itr = std::distance(areas.begin(), minimum); - tangent_planes_from_initialization_[closest_tangent_plane_itr]->ConvertAbsoluteToLocal(ecef_position, - local_position); -} - -const std::vector &TangentPlaneSequence::GetLocalPositionsFromInitialization() const { - return local_positions_from_initialization_; -} - -const std::vector &TangentPlaneSequence::GetWaypointsFromInitialization() const { - return waypoints_from_initialization_; -} - -const std::vector > &TangentPlaneSequence::GetTangentPlanesFromInitialization() - const { - return tangent_planes_from_initialization_; -} diff --git a/Public/ThreeDOFDynamics.cpp b/Public/ThreeDOFDynamics.cpp deleted file mode 100644 index 2e71114..0000000 --- a/Public/ThreeDOFDynamics.cpp +++ /dev/null @@ -1,422 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include "public/ThreeDOFDynamics.h" - -#include -#include - -#include -#include -#include - -#include "nlohmann/json.hpp" -#include "public/CoreUtils.h" - -using json = nlohmann::json; -using namespace std; -using namespace aaesim::open_source; - -AircraftState ThreeDOFDynamics::Update(const int unique_acid, const aaesim::open_source::SimulationTime &simtime, - const Guidance &guidance, const shared_ptr &aircraft_control) { - auto dynamics_state = Integrate(guidance, aircraft_control); - m_dynamics_history.insert(std::make_pair(simtime, dynamics_state)); - - LatLonDerivative position_rate; - m_position_estimator->ComputePosition(simtime, m_equations_of_motion_state, m_equations_of_motion_state_derivative, - m_last_resolved_position, position_rate); - Units::Angle trk = Units::RadiansAngle(dynamics_state.psi); - Units::Speed Vw_para = m_wind_velocity_east * cos(trk) + m_wind_velocity_north * sin(trk); - Units::Speed Vw_perp = -m_wind_velocity_east * sin(trk) + m_wind_velocity_north * cos(trk); - Units::Temperature outside_air_temperature = m_true_weather_operator->GetTemperature(); - return AircraftState::Builder(unique_acid, simtime.GetCurrentSimulationTime()) - .Position(m_equations_of_motion_state.enu_x, m_equations_of_motion_state.enu_y) - ->AltitudeMsl(dynamics_state.h) - ->Psi(dynamics_state.psi) - ->GroundSpeed(dynamics_state.xd, dynamics_state.yd) - ->AltitudeRate(-dynamics_state.v_true_airspeed * Units::sin(dynamics_state.gamma)) - ->FlightPathAngle(dynamics_state.gamma) - ->SensedWindComponents(m_wind_velocity_east, m_wind_velocity_north) - ->SensedWindsParallel(Vw_para) - ->SensedWindsPerpendicular(Vw_perp) - ->VerticalWindDerivatives(m_true_weather_operator->GetWindSpeedVerticalDerivativeEast(), - m_true_weather_operator->GetWindSpeedVerticalDerivativeNorth()) - ->SensedTemperature(outside_air_temperature) - ->SensedDensity(m_true_weather_operator->GetDensity()) - ->SensedPressure(m_true_weather_operator->GetPressure()) - ->DynamicsState(dynamics_state) - ->Latitude(m_last_resolved_position.latitude) - ->Longitude(m_last_resolved_position.longitude) - ->LatitudeRate(position_rate.latitude_time_derivative) - ->LongitudeRate(position_rate.longitude_time_derivative) - ->Build(); -} - -DynamicsState ThreeDOFDynamics::Integrate(const Guidance &guidance, - const shared_ptr &aircraft_control) { - const Units::SecondsTime dt = SimulationTime::GetSimulationTimeStep(); - UpdateTrueWeatherConditions(); - const auto controller_response = - aircraft_control->CalculateControlCommands(guidance, m_equations_of_motion_state, m_true_weather_operator); - bool perform_takeoff_roll_logic = controller_response.first.flap_configuration == bada_utils::TAKEOFF && - controller_response.first.true_airspeed_command < - m_bada_calculator->GetAerodynamicsInformation().take_off.V_stall; - if (perform_takeoff_roll_logic) { - m_equations_of_motion_state_derivative = StatePropagationOnRunway(controller_response.first, guidance); - } else { - // First-order derivative of the state calculated by the EOM - m_equations_of_motion_state_derivative = StatePropagation( - m_true_weather_operator->GetWindSpeedVerticalDerivativeEast(), - m_true_weather_operator->GetWindSpeedVerticalDerivativeNorth(), - controller_response.second.k_flight_path_angle, controller_response.second.k_thrust, - controller_response.second.k_roll, controller_response.second.k_speed_brake, controller_response.first); - } - - // Integrate the state - m_equations_of_motion_state.enu_x += m_equations_of_motion_state_derivative.enu_velocity_x * dt; - m_equations_of_motion_state.enu_y += m_equations_of_motion_state_derivative.enu_velocity_y * dt; - m_equations_of_motion_state.altitude_msl += m_equations_of_motion_state_derivative.enu_velocity_z * dt; - m_equations_of_motion_state.true_airspeed += m_equations_of_motion_state_derivative.true_airspeed_deriv * dt; - m_equations_of_motion_state.gamma += m_equations_of_motion_state_derivative.gamma_deriv * dt; - m_equations_of_motion_state.psi_enu += m_equations_of_motion_state_derivative.heading_deriv * dt; - m_equations_of_motion_state.thrust += m_equations_of_motion_state_derivative.thrust_deriv * dt; - m_equations_of_motion_state.phi += m_equations_of_motion_state_derivative.roll_rate * dt; - m_equations_of_motion_state.speed_brake_percentage += - m_equations_of_motion_state_derivative.speed_brake_deriv * dt.value(); - m_equations_of_motion_state.flap_configuration = m_equations_of_motion_state_derivative.flap_configuration; - - m_equations_of_motion_state.speed_brake_percentage = m_equations_of_motion_state.speed_brake_percentage < 1e-10 - ? 0.0 - : m_equations_of_motion_state.speed_brake_percentage; - return ComputeDynamicsState(m_equations_of_motion_state, m_equations_of_motion_state_derivative); -} - -EquationsOfMotionStateDeriv ThreeDOFDynamics::StatePropagation(Units::Frequency dVwx_dh, Units::Frequency dVwy_dh, - Units::Frequency k_gamma, Units::Frequency k_t, - Units::Frequency k_phi, double k_speedBrake, - ControlCommands commands) { - // Aircraft Configuration - const Units::Mass ac_mass = m_bada_calculator->GetAircraftMass(); - - // States - const Units::Speed true_airspeed = m_equations_of_motion_state.true_airspeed; - const Units::Angle gamma = m_equations_of_motion_state.gamma; - const Units::Angle psi = m_equations_of_motion_state.psi_enu; - const Units::Force thrust = m_equations_of_motion_state.thrust; - const Units::Angle phi = m_equations_of_motion_state.phi; - const double speed_brake_percentage = m_equations_of_motion_state.speed_brake_percentage; - - Units::Force drag, lift; - CalculateKineticForces(lift, drag); - - // calculate the first-order derivative of the state vector - EquationsOfMotionStateDeriv dX; - dX.enu_velocity_x = Units::MetersPerSecondSpeed(true_airspeed * cos(gamma) * cos(psi) + m_wind_velocity_east); - dX.enu_velocity_y = Units::MetersPerSecondSpeed(true_airspeed * cos(gamma) * sin(psi) + m_wind_velocity_north); - dX.enu_velocity_z = Units::MetersPerSecondSpeed(-true_airspeed * sin(gamma)); - dX.true_airspeed_deriv = (thrust - drag) / ac_mass + Units::ONE_G_ACCELERATION * sin(gamma) + - true_airspeed * (dVwx_dh * cos(psi) + dVwy_dh * sin(psi)) * sin(gamma) * cos(gamma); - dX.gamma_deriv = k_gamma * (commands.flight_path_angle_command - gamma) - - (dVwx_dh * cos(psi) + dVwy_dh * sin(psi)) * pow(sin(gamma), 2) * Units::ONE_RADIAN_ANGLE; - dX.heading_deriv = (-lift * sin(phi) / (ac_mass * true_airspeed * cos(gamma)) - - (dVwx_dh * sin(psi) - dVwy_dh * cos(psi)) * tan(gamma)) * - Units::ONE_RADIAN_ANGLE; - dX.thrust_deriv = k_t * (commands.thrust_command - thrust); - dX.roll_rate = k_phi * (commands.roll_angle_command - phi); - dX.speed_brake_deriv = k_speedBrake * (commands.speed_brake_command - speed_brake_percentage); - dX.flap_configuration = commands.flap_configuration; - - return dX; -} - -void ThreeDOFDynamics::CalculateKineticForces(Units::Force &lift, Units::Force &drag) { - // Aircraft Configuration - const Units::Mass ac_mass = m_bada_calculator->GetAircraftMass(); - const Units::Area wing_area = m_bada_calculator->GetAerodynamicsInformation().S; - - // States important for this method - const Units::Length altitude_msl = m_equations_of_motion_state.altitude_msl; - const Units::Speed true_airspeed = m_equations_of_motion_state.true_airspeed; - const Units::Angle phi = m_equations_of_motion_state.phi; // roll angle - const double speed_brake_setting = - m_equations_of_motion_state.speed_brake_percentage; // speed brake (% of deployment) - - // Get temp, density, and pressure - Units::KilogramsMeterDensity rho(m_true_weather_operator->GetDensity()); - Units::PascalsPressure pressure(m_true_weather_operator->GetPressure()); - - // Get aerodynamic configuration - double cd0, cd2, gear; - m_bada_calculator->GetCurrentDragCoefficients(cd0, cd2, gear); - - // Lift and Drag Coefficients - const double cL = - (2. * ac_mass * Units::ONE_G_ACCELERATION) / (rho * Units::sqr(true_airspeed) * wing_area * cos(phi)); - double cD = cd0 + gear + cd2 * pow(cL, 2); - if (speed_brake_setting != 0.0) { - cD = (1.0 + 0.6 * speed_brake_setting) * cD; - } - - // Drag & Lift - drag = 1. / 2. * rho * cD * Units::sqr(true_airspeed) * wing_area; - lift = 1. / 2. * rho * cL * Units::sqr(true_airspeed) * wing_area; - - if (m_logger.getLogLevel() == log4cplus::TRACE_LOG_LEVEL) { - json j; - j["mass_kg"] = Units::KilogramsMass(ac_mass).value(); - j["altitude_msl_ft"] = Units::FeetLength(altitude_msl).value(); - j["true_airspeed_kts"] = Units::KnotsSpeed(true_airspeed).value(); - j["rho_kgm3"] = Units::KilogramsMeterDensity(rho).value(); - j["gear"] = gear; - j["cd0"] = cd0; - j["cd2"] = cd2; - j["cD"] = cD; - j["drag_newtons"] = Units::NewtonsForce(drag).value(); - j["cL"] = cL; - j["lift_newtons"] = Units::NewtonsForce(lift).value(); - j["speed_brake_setting"] = speed_brake_setting; - j["updated_flap_setting"] = aaesim::open_source::bada_utils::GetFlapConfigurationAsString( - m_bada_calculator->GetCurrentFlapConfiguration()); - LOG4CPLUS_TRACE(m_logger, j.dump()); - } -} - -Units::SignedRadiansAngle ThreeDOFDynamics::CalculateTrimmedPsiForWind(Units::SignedAngle ground_track_enu) { - UpdateTrueWeatherConditions(); - const double gamma = Units::RadiansAngle(m_equations_of_motion_state.gamma).value(); - const double trkRad = Units::RadiansAngle(Units::ToUnsigned(ground_track_enu)).value(); - Units::MetersPerSecondSpeed vwpara = - Units::MetersPerSecondSpeed(Units::MetersPerSecondSpeed(m_wind_velocity_east).value() * cos(trkRad) + - Units::MetersPerSecondSpeed(m_wind_velocity_north).value() * sin(trkRad)); - Units::MetersPerSecondSpeed vwperp = - Units::MetersPerSecondSpeed(-Units::MetersPerSecondSpeed(m_wind_velocity_east).value() * sin(trkRad) + - Units::MetersPerSecondSpeed(m_wind_velocity_north).value() * cos(trkRad)); - - Units::MetersPerSecondSpeed w = sqrt(Units::sqr(Units::MetersPerSecondSpeed(m_wind_velocity_east)) + - Units::sqr(Units::MetersPerSecondSpeed(m_wind_velocity_north))); - - const Units::MetersPerSecondSpeed v = m_equations_of_motion_state.true_airspeed; - Units::MetersPerSecondSpeed gs = - Units::MetersPerSecondSpeed(sqrt(pow(v.value() * cos(gamma), 2) - pow(vwperp.value(), 2)) + vwpara.value()); - - double numerator = (pow(v.value() * cos(gamma), 2) + pow(gs.value(), 2) - pow(w.value(), 2)); - - double denominator = (2.0 * v.value() * cos(gamma) * gs.value()); - - double temp = numerator / denominator; - if (temp > 1.0) { // Limit temp so acos function doesn't give undefined value. - temp = 1.0; - } else if (temp < -1.0) { - temp = -1.0; - } - - // Wind correction angle - Units::Angle beta = Units::RadiansAngle(acos(temp) * -1.0 * CoreUtils::SignOfValue(vwperp.value())); - - return ground_track_enu + beta; -} - -void ThreeDOFDynamics::Initialize( - const aaesim::open_source::SimulationTime &simulation_time, - std::shared_ptr aircraft_performance, - const EarthModel::GeodeticPosition &initial_position, const EarthModel::LocalPositionEnu &initial_position_enu, - Units::Length initial_altitude_msl, Units::Speed initial_true_airspeed, Units::Angle initial_ground_course_enu, - double initial_mass_fraction, - std::shared_ptr position_estimator, - std::shared_ptr true_weather_operator) { - m_bada_calculator = aircraft_performance; - m_position_estimator = position_estimator; - m_true_weather_operator = true_weather_operator; - - m_last_resolved_position.latitude = initial_position.latitude; - m_last_resolved_position.longitude = initial_position.longitude; - m_last_resolved_position.altitude = initial_altitude_msl; - - DynamicsState initial_dynamics_state{}; - initial_dynamics_state.h = initial_altitude_msl; - initial_dynamics_state.gamma = Units::RadiansAngle(0); - initial_dynamics_state.phi = Units::RadiansAngle(0); - initial_dynamics_state.v_true_airspeed = initial_true_airspeed; - initial_dynamics_state.flap_configuration = m_bada_calculator->GetCurrentFlapConfiguration(); - initial_dynamics_state.mach = - m_true_weather_operator->GetTrueWeather()->TAS2Mach(initial_true_airspeed, initial_altitude_msl); - initial_dynamics_state.v_indicated_airspeed = - m_true_weather_operator->GetTrueWeather()->TAS2CAS(initial_true_airspeed, initial_altitude_msl); - - // initialize the state - m_equations_of_motion_state.enu_x = initial_position_enu.x; - m_equations_of_motion_state.enu_y = initial_position_enu.y; - m_equations_of_motion_state.altitude_msl = initial_dynamics_state.h; - m_equations_of_motion_state.true_airspeed = initial_dynamics_state.v_true_airspeed; - m_equations_of_motion_state.gamma = initial_dynamics_state.gamma; - m_equations_of_motion_state.thrust = Units::ZERO_FORCE; - m_equations_of_motion_state.phi = initial_dynamics_state.phi; - m_equations_of_motion_state.speed_brake_percentage = 0.0; - m_equations_of_motion_state.flap_configuration = m_bada_calculator->GetCurrentFlapConfiguration(); - - if (initial_dynamics_state.flap_configuration == bada_utils::FlapConfiguration::TAKEOFF) { - // On the ground. Don't trim aircraft. - initial_dynamics_state.psi = initial_ground_course_enu; - m_equations_of_motion_state.psi_enu = initial_dynamics_state.psi; - initial_dynamics_state.xd = initial_dynamics_state.v_true_airspeed * cos(initial_dynamics_state.psi); - initial_dynamics_state.yd = initial_dynamics_state.v_true_airspeed * sin(initial_dynamics_state.psi); - - Units::Force takeoff_max_thrust = m_bada_calculator->GetMaxThrust( - Units::MetersLength(initial_dynamics_state.h), aaesim::open_source::bada_utils::FlapConfiguration::TAKEOFF, - aaesim::open_source::bada_utils::EngineThrustMode::MAXIMUM_CLIMB, Units::ZERO_CELSIUS); - m_equations_of_motion_state.thrust = takeoff_max_thrust; - } else { - // Now that the initial state has been determined, still need to trim laterally for wind - m_equations_of_motion_state.psi_enu = CalculateTrimmedPsiForWind(initial_ground_course_enu); - initial_dynamics_state.psi = m_equations_of_motion_state.psi_enu; - initial_dynamics_state.xd = initial_dynamics_state.v_true_airspeed * cos(initial_dynamics_state.psi) * - cos(initial_dynamics_state.gamma) + - m_wind_velocity_east; - initial_dynamics_state.yd = initial_dynamics_state.v_true_airspeed * sin(initial_dynamics_state.psi) * - cos(initial_dynamics_state.gamma) + - m_wind_velocity_north; - - // Calculate initial Aircraft Thrust - Units::Mass ac_mass = m_bada_calculator->GetAircraftMass(); - Units::Force drag, lift; - CalculateKineticForces(lift, drag); - Units::Force equilibrium_thrust_required = drag - ac_mass * Units::ONE_G_ACCELERATION * sin(asin(0.0)); - const Units::Force max_thrust = m_bada_calculator->GetMaxThrust( - Units::MetersLength(initial_dynamics_state.h), aaesim::open_source::bada_utils::FlapConfiguration::CRUISE, - aaesim::open_source::bada_utils::EngineThrustMode::MAXIMUM_CRUISE, Units::ZERO_CELSIUS); - const Units::Force min_thrust = m_bada_calculator->GetMaxThrust( - Units::MetersLength(initial_dynamics_state.h), initial_dynamics_state.flap_configuration, - aaesim::open_source::bada_utils::EngineThrustMode::DESCENT, Units::ZERO_CELSIUS); - if (equilibrium_thrust_required > max_thrust * m_max_thrust_percent) { - equilibrium_thrust_required = max_thrust * m_max_thrust_percent; - } else if (equilibrium_thrust_required < min_thrust * m_min_thrust_percent) { - equilibrium_thrust_required = min_thrust * m_min_thrust_percent; - } - m_equations_of_motion_state.thrust = equilibrium_thrust_required; - } - m_dynamics_history.insert(std::make_pair(simulation_time, initial_dynamics_state)); -} - -EquationsOfMotionStateDeriv ThreeDOFDynamics::StatePropagationOnRunway(ControlCommands commands, - const Guidance &guidance) { - const double wind_factor = 1.25; - const Units::SignedAngle takeoff_roll_psi_enu = guidance.m_enu_track_angle; - const Units::Mass ac_mass = m_bada_calculator->GetAircraftMass(); - const Units::Speed true_airspeed = m_equations_of_motion_state.true_airspeed; - const Units::Force thrust = - m_bada_calculator->GetMaxThrust(m_equations_of_motion_state.altitude_msl, commands.flap_configuration, - bada_utils::EngineThrustMode::MAXIMUM_CLIMB, Units::ZERO_CELSIUS); - const Units::Speed wind_magnitude = - Units::sqrt(Units::sqr(m_wind_velocity_east) + Units::sqr(m_wind_velocity_north)); - Units::Speed wind_east_parallel_to_track = m_wind_velocity_east * cos(takeoff_roll_psi_enu); - Units::Speed wind_north_parallel_to_track = m_wind_velocity_north * sin(takeoff_roll_psi_enu); - if (true_airspeed < wind_magnitude * wind_factor) { - // ignore wind; don't allow aircraft to be blown around on runway - wind_east_parallel_to_track = Units::zero(); - wind_north_parallel_to_track = Units::zero(); - } - - EquationsOfMotionStateDeriv dX; - dX.enu_velocity_x = true_airspeed * cos(takeoff_roll_psi_enu) + wind_east_parallel_to_track; - dX.enu_velocity_y = true_airspeed * sin(takeoff_roll_psi_enu) + wind_north_parallel_to_track; - dX.enu_velocity_z = Units::zero(); - dX.true_airspeed_deriv = thrust / ac_mass; - dX.gamma_deriv = Units::zero(); - auto derived_psi_enu = Units::arctan2(Units::MetersPerSecondSpeed(dX.enu_velocity_y).value(), - Units::MetersPerSecondSpeed(dX.enu_velocity_x).value()); - auto delta_psi_enu = derived_psi_enu - m_equations_of_motion_state.psi_enu; - dX.heading_deriv = delta_psi_enu / SimulationTime::GetSimulationTimeStep(); - dX.thrust_deriv = Units::zero(); - dX.roll_rate = Units::zero(); - dX.speed_brake_deriv = 0; - dX.flap_configuration = commands.flap_configuration; - return dX; -} - -DynamicsState ThreeDOFDynamics::ComputeDynamicsState( - const EquationsOfMotionState &equations_of_motion_state, - const EquationsOfMotionStateDeriv &equations_of_motion_state_derivative) const { - DynamicsState dynamics_state; - - dynamics_state.h = m_equations_of_motion_state.altitude_msl; - dynamics_state.v_true_airspeed = m_equations_of_motion_state.true_airspeed; - dynamics_state.v_indicated_airspeed = - m_true_weather_operator->GetTrueWeather()->TAS2CAS(dynamics_state.v_true_airspeed, dynamics_state.h); - dynamics_state.mach = m_true_weather_operator->GetTrueWeather()->TAS2Mach(m_equations_of_motion_state.true_airspeed, - m_equations_of_motion_state.altitude_msl); - dynamics_state.gamma = m_equations_of_motion_state.gamma; - dynamics_state.psi = m_equations_of_motion_state.psi_enu; - dynamics_state.thrust = m_equations_of_motion_state.thrust; - dynamics_state.phi = m_equations_of_motion_state.phi; - dynamics_state.speed_brake = m_equations_of_motion_state.speed_brake_percentage; - dynamics_state.flap_configuration = m_equations_of_motion_state.flap_configuration; - dynamics_state.current_mass = m_bada_calculator->GetAircraftMass(); - dynamics_state.xd = equations_of_motion_state_derivative.enu_velocity_x; - dynamics_state.yd = equations_of_motion_state_derivative.enu_velocity_y; - - aaesim::open_source::bada_utils::FlapConfiguration mode = m_bada_calculator->GetCurrentFlapConfiguration(); - Units::Force max_thrust, min_thrust; - switch (mode) { - case aaesim::open_source::bada_utils::FlapConfiguration::CRUISE: - case aaesim::open_source::bada_utils::FlapConfiguration::APPROACH: - case aaesim::open_source::bada_utils::FlapConfiguration::LANDING: - case aaesim::open_source::bada_utils::FlapConfiguration::GEAR_DOWN: - max_thrust = Units::NewtonsForce(m_bada_calculator->GetMaxThrust( - Units::MetersLength(dynamics_state.h), mode, - aaesim::open_source::bada_utils::EngineThrustMode::MAXIMUM_CRUISE, Units::ZERO_CELSIUS)); - break; - - case aaesim::open_source::bada_utils::FlapConfiguration::TAKEOFF: - case aaesim::open_source::bada_utils::FlapConfiguration::INITIAL_CLIMB: - max_thrust = Units::NewtonsForce(m_bada_calculator->GetMaxThrust( - Units::MetersLength(dynamics_state.h), mode, - aaesim::open_source::bada_utils::EngineThrustMode::MAXIMUM_CLIMB, Units::ZERO_CELSIUS)); - break; - - case aaesim::open_source::bada_utils::FlapConfiguration::UNDEFINED: - default: - throw std::logic_error("Design Error: should never get here"); - } - min_thrust = Units::NewtonsForce(m_bada_calculator->GetMaxThrust( - Units::MetersLength(dynamics_state.h), mode, aaesim::open_source::bada_utils::EngineThrustMode::DESCENT, - Units::ZERO_CELSIUS)); - - if (dynamics_state.thrust > max_thrust) { - dynamics_state.thrust = max_thrust; - } else if (dynamics_state.thrust < min_thrust) { - dynamics_state.thrust = min_thrust; - } - - if (dynamics_state.speed_brake > 0.5) { - dynamics_state.speed_brake = 0.5; - } else if (dynamics_state.speed_brake < 0.0) { - dynamics_state.speed_brake = 0.0; - } - - dynamics_state.true_temperature = - Units::AbsCelsiusTemperature(Units::AbsKelvinTemperature(m_true_weather_operator->GetTemperature().value())); - return dynamics_state; -} - -void ThreeDOFDynamics::UpdateTrueWeatherConditions() { - m_true_weather_operator->CalculateEnvironmentalWind(m_last_resolved_position, - m_equations_of_motion_state.altitude_msl); - m_wind_velocity_east = m_true_weather_operator->GetWindSpeedEast(); - m_wind_velocity_north = m_true_weather_operator->GetWindSpeedNorth(); -} diff --git a/Public/TvReader.cpp b/Public/TvReader.cpp deleted file mode 100644 index f9a113f..0000000 --- a/Public/TvReader.cpp +++ /dev/null @@ -1,122 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -/* - * TvReader.cpp - * - * Created on: Mar 23, 2019 - * Author: klewis - */ - -#include "public/TvReader.h" - -#include -#include -#include - -using namespace std; - -namespace aaesim { -namespace open_source { - -const size_t TvReader::EXPECTED_TV_COLUMN_COUNT(17); - -TvReader::TvReader(const std::string &file_name, int header_lines) : DataReader(file_name, 0, 0) { - SetColumnIndexesFromHeader(header_lines); -} - -TvReader::TvReader(std::shared_ptr input_stream, int header_lines) : DataReader(input_stream, 0, 0) { - SetColumnIndexesFromHeader(header_lines); -} - -bool TvReader::Advance() { - bool result = DataReader::Advance(); - if (result) { - m_time_of_receipt = Units::SecondsTime(GetDouble(0)); - } else { - m_time_of_receipt = DataReader::UNDEFINED_TIME; - } - return result; -} - -const Units::SecondsTime TvReader::GetTimeOfReceipt() const { return m_time_of_receipt; } - -const int TvReader::GetAcid() const { return GetDouble(m_aircraft_id_column); } - -const Units::SecondsTime TvReader::GetToap() const { - return Units::SecondsTime(GetDouble(m_time_of_applicability_position_column)); -} - -const Units::DegreesAngle TvReader::GetLat() const { return Units::DegreesAngle(GetDouble(m_latitude_column)); } - -const Units::DegreesAngle TvReader::GetLon() const { return Units::DegreesAngle(GetDouble(m_longitude_column)); } - -const Units::FeetLength TvReader::GetAlt() const { return Units::FeetLength(GetDouble(m_altitude_column)); } - -const Units::KnotsSpeed TvReader::GetEwvel() const { return Units::KnotsSpeed(GetDouble(m_east_velocity_column)); } - -const Units::KnotsSpeed TvReader::GetNsvel() const { return Units::KnotsSpeed(GetDouble(m_north_velocity_column)); } - -const Units::SecondsTime TvReader::GetToav() const { - return Units::SecondsTime(GetDouble(m_time_of_applicability_velocity_column)); -} - -const int TvReader::GetNacp() const { return GetDouble(m_nacp_column); } - -const int TvReader::GetNic() const { return GetDouble(m_nic_column); } - -const int TvReader::GetNacv() const { return GetDouble(m_nacv_column); } - -const Units::FeetPerMinuteSpeed TvReader::GetVertRate() const { - return Units::FeetPerMinuteSpeed(GetDouble(m_vert_rate_column)); -} - -void TvReader::SetColumnIndexesFromHeader(const int header_lines) { - if (header_lines < 1) { - throw logic_error("Headers are required for TvReader."); - } - Advance(); // first header line - BuildColumnIndex(); - - // Record required column indexes. - // This will insert an entry if index is missing. - // tRec[sec] -- hardcoded to column 0 - m_aircraft_id_column = GetColumnNumber("ACID"); - // TargetType -- not used - m_time_of_applicability_position_column = GetColumnNumber("TOAp[sec]"); - m_latitude_column = GetColumnNumber("Lat[degrees]"); - m_longitude_column = GetColumnNumber("Lon[degrees]"); - m_altitude_column = GetColumnNumber("Alt[feet]"); - m_east_velocity_column = GetColumnNumber("EWVel[knots]"); - m_north_velocity_column = GetColumnNumber("NSVel[knots]"); - m_time_of_applicability_velocity_column = GetColumnNumber("TOAv[sec]"); - m_nacp_column = GetColumnNumber("NACp"); - m_nic_column = GetColumnNumber("NIC"); - m_nacv_column = GetColumnNumber("NACv"); - // SIL -- Source Integrity Level, not used - // SDA -- System Design Assurance, not used - m_vert_rate_column = GetColumnNumber("VertRate[fpm]"); - - SetExpectedColumnCount(GetColumnCount()); - - SkipLines(header_lines - 1); -} - -} // namespace open_source -} // namespace aaesim diff --git a/Public/USStandardAtmosphere1976.cpp b/Public/USStandardAtmosphere1976.cpp deleted file mode 100644 index 5a8a7bc..0000000 --- a/Public/USStandardAtmosphere1976.cpp +++ /dev/null @@ -1,186 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include "public/USStandardAtmosphere1976.h" - -#include -#include - -#include - -const Units::KelvinTemperature TEMPERATURE_TOLERANCE(0.1); - -/** Lowest altitude supported in US Standard Atmosphere */ -const Units::MetersLength MINIMUM_ALTITUDE(-5000); - -/** Sea-level constants, from US Standard Atmosphere table */ -const Units::KelvinTemperature T0(288.15); - -/** Bottom-of-tropopause constants, from US Standard Atmosphere table */ -const Units::MetersLength H_TROP(11000); -const Units::KelvinTemperature T_TROP(216.65); -const Units::PascalsPressure P_TROP(22632); -const Units::KilogramsMeterDensity RHO_TROP(0.36392); - -log4cplus::Logger USStandardAtmosphere1976::m_logger = - log4cplus::Logger::getInstance(LOG4CPLUS_TEXT("USStandardAtmosphere1976")); - -const double USStandardAtmosphere1976::P_T_EXPONENT(std::log(P_TROP / P0_ISA) / std::log(T_TROP / T0)); -const double USStandardAtmosphere1976::RHO_T_EXPONENT(std::log(RHO_TROP / RHO0_ISA) / std::log(T_TROP / T0)); - -USStandardAtmosphere1976::USStandardAtmosphere1976() { Atmosphere::SetTemperatureOffset(Units::CelsiusTemperature(0)); } -USStandardAtmosphere1976::USStandardAtmosphere1976(const Units::Temperature temperature_offset) { - Atmosphere::SetTemperatureOffset(temperature_offset); -} - -Atmosphere *USStandardAtmosphere1976::Clone() const { return new USStandardAtmosphere1976(); } - -void USStandardAtmosphere1976::SetTemperatureOffset(const Units::Temperature temperature_offset) { - if (Units::abs(temperature_offset) < TEMPERATURE_TOLERANCE) { - LOG4CPLUS_WARN(m_logger, - "Temperature offset is not supported in USStandardAtmosphere1976, but the specified value of " - << Units::KelvinTemperature(temperature_offset) << " is within tolerance."); - } else { - std::string error("Temperature offset is not supported in USStandardAtmosphere1976."); - LOG4CPLUS_FATAL(m_logger, error); - throw std::runtime_error(error); - } -} - -void USStandardAtmosphere1976::CalibrateTemperatureAtAltitude(const Units::KelvinTemperature temperature, - const Units::Length altitude) { - Units::KelvinTemperature difference = GetTemperature(altitude) - temperature; - if (Units::abs(difference) <= TEMPERATURE_TOLERANCE) { - LOG4CPLUS_WARN(m_logger, - "Calibration is not supported in USStandardAtmosphere1976, but the specified difference of " - << difference << " is within tolerance."); - } else { - std::string error("Calibration is not supported in USStandardAtmosphere1976."); - LOG4CPLUS_FATAL(m_logger, error); - throw std::runtime_error(error); - } -} - -void USStandardAtmosphere1976::AirDensity(const Units::Length h, Units::Density &rho, Units::Pressure &P) const { - Units::KelvinTemperature T = GetTemperature(h); - if (h < H_TROP) { - // troposphere - rho = RHO0_ISA * std::pow(T / T0, RHO_T_EXPONENT); - P = P0_ISA * std::pow(T / T0, P_T_EXPONENT); - } else { - // tropopause - const double factor(exp(-Units::ONE_G_ACCELERATION / (R * T_TROP) * (h - H_TROP))); - P = GetTropopausePressure() * factor; - rho = GetTropopauseDensity() * factor; - } - AirDensity_Log(h, T, P, rho); -} - -Units::KelvinTemperature USStandardAtmosphere1976::GetTemperature(const Units::Length altitude_msl) const { - if (altitude_msl < MINIMUM_ALTITUDE) { - std::ostringstream s; - s << "Lowest supported altitude is " << MINIMUM_ALTITUDE; - std::string error(s.str()); - LOG4CPLUS_FATAL(m_logger, error); - throw std::runtime_error(error); - } - - Units::KelvinTemperature result; - - if (altitude_msl < H_TROP) { - result = T0 + altitude_msl * K_T; - } else { - result = T_TROP; - } - return result; -} - -Units::KelvinTemperature USStandardAtmosphere1976::GetSeaLevelTemperature() const { return T0; } - -Units::Density USStandardAtmosphere1976::GetSeaLevelDensity() const { return RHO0_ISA; } - -Units::MetersLength USStandardAtmosphere1976::GetTropopauseHeight() const { return H_TROP; } - -Units::Density USStandardAtmosphere1976::GetTropopauseDensity() const { return RHO_TROP; } - -Units::Pressure USStandardAtmosphere1976::GetTropopausePressure() const { return P_TROP; } - -Units::Speed USStandardAtmosphere1976::CAS2TAS(const Units::Speed vcas, const Units::Pressure p, - const Units::Density rho) const { - // https://ppla.education/navcomp/Calculation_of_TAS_from_CAS-Correction_of_Density_Error/ - // CAS = TAS √relative density - double relative_density(rho / GetSeaLevelDensity()); - Units::Speed vtas = vcas / sqrt(relative_density); - return vtas; -} - -Units::Speed USStandardAtmosphere1976::TAS2CAS(const Units::Speed vtas, const Units::Pressure p, - const Units::Density rho) const { - // https://ppla.education/navcomp/Calculation_of_TAS_from_CAS-Correction_of_Density_Error/ - // CAS = TAS √relative density - double relative_density(rho / GetSeaLevelDensity()); - Units::Speed vcas = vtas * sqrt(relative_density); - return vcas; -} - -Units::Speed USStandardAtmosphere1976::SpeedOfSound(Units::KelvinTemperature temperature) const { - // from https://en.wikipedia.org/wiki/Speed_of_sound#Speed_of_sound_in_ideal_gases_and_air - Units::KnotsSpeed speed_of_sound = sqrt(kGamma * R * temperature); - return speed_of_sound; -} - -double USStandardAtmosphere1976::ESFconstantCAS(const Units::Speed true_airspeed, const Units::Length altitude_msl, - const Units::KelvinTemperature temperature) const { - std::string error("USStandardAtmosphere1976::ESFconstantCAS is not implemented."); - LOG4CPLUS_FATAL(m_logger, error); - throw std::runtime_error(error); -} - -Units::Length USStandardAtmosphere1976::GetMachIASTransition(const Units::Speed ias, const double mach) const { - // Find the altitude at which CAS2TAS matches Mach2TAS (mach * SpeedOfSound) - Units::MetersLength h(H_TROP); // Only altitude at which the derivative is discontinuous, start there - - // Use Newton's Method to find the zero for - // f(h) = mach * SpeedOfSound(h) - CAS2TAS(h) - Units::KnotsSpeed f = mach * Atmosphere::SpeedOfSound(h) - Atmosphere::CAS2TAS(ias, h); - const bool below_trop(f < Units::zero()); - - int max_iterations(100); - for (int i = 1; i <= max_iterations; ++i) { - Units::HertzFrequency f_prime(0); - if (below_trop) { - f_prime = mach * K_T * sqrt(kGamma * R / (T0 + h * K_T)); - double temp = 1 + h * K_T / T0; - f_prime -= ias * K_T / T0 * (-RHO_T_EXPONENT / 2) * std::pow(temp, -RHO_T_EXPONENT / 2 - 1); - } else { - f_prime = Units::HertzFrequency(0); - const Units::MetersSecondAcceleration G(Units::ONE_G_ACCELERATION); - const double density_ratio(RHO0_ISA / RHO_TROP); - f_prime -= ias * sqrt(density_ratio) * G / 2 / (R * T_TROP) * std::exp(G / 2 / (R * T_TROP) * (h - H_TROP)); - } - - h -= f / f_prime; - f = mach * Atmosphere::SpeedOfSound(h) - Atmosphere::CAS2TAS(ias, h); - - static const auto CONVERGENCE_TOLERANCE{Units::KnotsSpeed(1e-4)}; - if (Units::abs(f) < CONVERGENCE_TOLERANCE) break; - } - - return h; -} diff --git a/Public/VectorDifferenceWindEvaluator.cpp b/Public/VectorDifferenceWindEvaluator.cpp deleted file mode 100644 index a294d8a..0000000 --- a/Public/VectorDifferenceWindEvaluator.cpp +++ /dev/null @@ -1,63 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include "public/VectorDifferenceWindEvaluator.h" - -#include -#include - -#include "public/Environment.h" - -using namespace aaesim::open_source; - -std::map > VectorDifferenceWindEvaluator::m_instances; - -const std::shared_ptr VectorDifferenceWindEvaluator::GetInstance( - const Units::Speed maxSpeedDiff) { - std::weak_ptr cached = m_instances[maxSpeedDiff]; - std::shared_ptr result = cached.lock(); - if (!result) { - result = std::unique_ptr(new VectorDifferenceWindEvaluator(maxSpeedDiff)); - cached = result; - m_instances[maxSpeedDiff] = cached; - } - return result; -} - -VectorDifferenceWindEvaluator::VectorDifferenceWindEvaluator(const Units::Speed &max_allowed_difference) - : m_max_allowed_difference(max_allowed_difference) {} - -VectorDifferenceWindEvaluator::~VectorDifferenceWindEvaluator() {} - -bool VectorDifferenceWindEvaluator::ArePredictedWindsAccurate( - const aaesim::open_source::AircraftState &state, const aaesim::open_source::WeatherPrediction &weather_prediction, - const Units::Speed reference_cas, const Units::Length reference_altitude, - const std::shared_ptr &sensed_atmosphere) const { - Units::MetersPerSecondSpeed wind_east, wind_north; - Units::Frequency dtmp; - weather_prediction.east_west().CalculateWindGradientAtAltitude(Units::FeetLength(state.GetAltitudeMsl()), wind_east, - dtmp); - weather_prediction.north_south().CalculateWindGradientAtAltitude(Units::FeetLength(state.GetAltitudeMsl()), - wind_north, dtmp); - - Units::Speed x_diff = state.GetSensedWindEast() - wind_east; - Units::Speed y_diff = state.GetSensedWindNorth() - wind_north; - - return Units::sqr(x_diff) + Units::sqr(y_diff) <= Units::sqr(m_max_allowed_difference); -} diff --git a/Public/VerticalPath.cpp b/Public/VerticalPath.cpp deleted file mode 100644 index 0468842..0000000 --- a/Public/VerticalPath.cpp +++ /dev/null @@ -1,124 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include "public/VerticalPath.h" - -#include "utility/UtilityConstants.h" - -using namespace std; - -VerticalPath::VerticalPath() = default; - -VerticalPath::~VerticalPath() = default; - -void VerticalPath::Append(const VerticalPath &in) { - for (int index = 0; index < in.along_path_distance_m.size(); index++) { - along_path_distance_m.push_back(in.along_path_distance_m[index]); - altitude_m.push_back(in.altitude_m[index]); - cas_mps.push_back(in.cas_mps[index]); - mach.push_back(in.mach[index]); - altitude_rate_mps.push_back(in.altitude_rate_mps[index]); - true_airspeed.push_back(in.true_airspeed[index]); - tas_rate_mps.push_back(in.tas_rate_mps[index]); - theta_radians.push_back(in.theta_radians[index]); - gs_mps.push_back(in.gs_mps[index]); - time_to_go_sec.push_back(in.time_to_go_sec[index]); - mass_kg.push_back(in.mass_kg[index]); - wind_velocity_east.push_back(in.wind_velocity_east[index]); - wind_velocity_north.push_back(in.wind_velocity_north[index]); - algorithm_type.push_back(in.algorithm_type[index]); - } -} - -void VerticalPath::operator+=(const VerticalPath &in) { Append(in); } - -bool VerticalPath::operator==(const VerticalPath &obj) const { - bool match = (along_path_distance_m.size() == obj.along_path_distance_m.size()); - match = match && (altitude_m.size() == obj.altitude_m.size()); - match = match && (cas_mps.size() == obj.cas_mps.size()); - match = match && (mach.size() == obj.mach.size()); - match = match && (altitude_rate_mps.size() == obj.altitude_rate_mps.size()); - match = match && (true_airspeed.size() == obj.true_airspeed.size()); - match = match && (tas_rate_mps.size() == obj.tas_rate_mps.size()); - match = match && (theta_radians.size() == obj.theta_radians.size()); - match = match && (gs_mps.size() == obj.gs_mps.size()); - match = match && (time_to_go_sec.size() == obj.time_to_go_sec.size()); - match = match && (mass_kg.size() == obj.mass_kg.size()); - match = match && (wind_velocity_east.size() == obj.wind_velocity_east.size()); - match = match && (wind_velocity_north.size() == obj.wind_velocity_north.size()); - match = match && (algorithm_type.size() == obj.algorithm_type.size()); - - for (auto ix = 0; match && (ix < along_path_distance_m.size()); ix++) { - match = match && (along_path_distance_m[ix] == obj.along_path_distance_m[ix]); - } - - for (auto ix = 0; match && (ix < altitude_m.size()); ix++) { - match = match && (altitude_m[ix] == obj.altitude_m[ix]); - } - - for (auto ix = 0; match && (ix < cas_mps.size()); ix++) { - match = match && (cas_mps[ix] == obj.cas_mps[ix]); - } - - for (auto ix = 0; match && (ix < mach.size()); ix++) { - match = match && (mach[ix] == obj.mach[ix]); - } - - for (auto ix = 0; match && (ix < altitude_rate_mps.size()); ix++) { - match = match && (altitude_rate_mps[ix] == obj.altitude_rate_mps[ix]); - } - - for (auto ix = 0; match && (ix < tas_rate_mps.size()); ix++) { - match = match && (tas_rate_mps[ix] == obj.tas_rate_mps[ix]); - } - - for (auto ix = 0; match && (ix < theta_radians.size()); ix++) { - match = match && (theta_radians[ix] == obj.theta_radians[ix]); - } - - for (auto ix = 0; match && (ix < gs_mps.size()); ix++) { - match = match && (gs_mps[ix] == obj.gs_mps[ix]); - } - - for (auto ix = 0; match && (ix < true_airspeed.size()); ix++) { - match = match && (true_airspeed[ix] == obj.true_airspeed[ix]); - } - - for (auto ix = 0; match && (ix < time_to_go_sec.size()); ix++) { - match = match && (time_to_go_sec[ix] == obj.time_to_go_sec[ix]); - } - - for (auto ix = 0; match && (ix < mass_kg.size()); ix++) { - match = match && (mass_kg[ix] == obj.mass_kg[ix]); - } - - for (auto ix = 0; match && (ix < wind_velocity_east.size()); ix++) { - match = match && (wind_velocity_east[ix] == obj.wind_velocity_east[ix]); - } - - for (auto ix = 0; match && (ix < wind_velocity_north.size()); ix++) { - match = match && (wind_velocity_north[ix] == obj.wind_velocity_north[ix]); - } - - for (auto ix = 0; match && (ix < algorithm_type.size()); ix++) { - match = match && (algorithm_type[ix] == obj.algorithm_type[ix]); - } - - return match; -} diff --git a/Public/VerticalPathObserver.cpp b/Public/VerticalPathObserver.cpp deleted file mode 100644 index d3ceeef..0000000 --- a/Public/VerticalPathObserver.cpp +++ /dev/null @@ -1,135 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// 2022 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include - -#include - -// **************************************************************************** -// NOTICE -// -// This is the copyright work of The MITRE Corporation, and was produced -// for the U. S. Government under Contract Number DTFAWA-10-C-00080, and -// is subject to Federal Aviation Administration Acquisition Management -// System Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV -// (Oct. 1996). No other use other than that granted to the U. S. -// Government, or to those acting on behalf of the U. S. Government, -// under that Clause is authorized without the express written -// permission of The MITRE Corporation. For further information, please -// contact The MITRE Corporation, Contracts Office, 7515 Colshire Drive, -// McLean, VA 22102-7539, (703) 983-6000. -// -// Copyright 2019 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include "public/VerticalPathObserver.h" -#include "utility/constants.h" -#include -#include - -using std::string; -using std::cout; -using std::endl; - -using namespace aaesim::constants; -log4cplus::Logger VerticalPathObserver::m_logger = log4cplus::Logger::getInstance("VerticalPathObserver"); - -VerticalPathObserver::VerticalPathObserver() - : m_scenario_name(), - m_file_name(), - m_column_header() { - m_iteration = -1; - m_is_target_aircraft_data = false; -} - - -VerticalPathObserver::VerticalPathObserver(string scenario_name, - string file_name, - bool is_target_aircraft_data) - : m_scenario_name(std::move(scenario_name)), - m_file_name(std::move(file_name)) { - - - m_iteration = -1; - m_is_target_aircraft_data = is_target_aircraft_data; - - m_column_header = GetHeader(); - - Initialize(); -} - -VerticalPathObserver::~VerticalPathObserver() = default; - -void VerticalPathObserver::Initialize() { - string fullFileName = CreateFullFileName(m_scenario_name, m_file_name); - - out_stream.open(fullFileName.c_str()); - - if (out_stream.is_open()) { - out_stream << m_column_header << endl; - } else { - LOG4CPLUS_ERROR(m_logger, "Cannot open file for output: " + fullFileName); - } -} - -void VerticalPathObserver::AddTrajectory(int id, - const VerticalPath& vertical_path) { - if (out_stream.is_open()) { - for (unsigned int i = 0; i < vertical_path.along_path_distance_m.size(); i++) { - out_stream << m_iteration << ","; - out_stream << id << ","; - out_stream << vertical_path.time_to_go_sec[i] << ","; - out_stream << vertical_path.along_path_distance_m[i] / FEET_TO_METERS << ","; - out_stream << vertical_path.altitude_m[i] / FEET_TO_METERS << ","; - out_stream << vertical_path.cas_mps[i] / KNOTS_TO_METERS_PER_SECOND << ","; - out_stream << vertical_path.altitude_rate_mps[i] << ","; - out_stream << vertical_path.tas_rate_mps[i] << ","; - out_stream << vertical_path.theta_radians[i] << ","; - out_stream << Units::KnotsSpeed(Units::MetersPerSecondSpeed(vertical_path.gs_mps[i])).value() << ","; - out_stream << vertical_path.mass_kg[i] << ","; - out_stream << vertical_path.algorithm_type[i] << endl; - } - } -} - -string VerticalPathObserver::CreateFullFileName(const string& scenario_name, - const string& file_name) { - return scenario_name + "_" + file_name + ".csv"; -} - -string VerticalPathObserver::GetHeader() { - string hdr = "Iteration,"; - - if (m_is_target_aircraft_data) { - hdr += "target AC_ID,"; - } else { - hdr += "AC_ID,"; - } - - hdr += "Time,Distance(feet),Altitude(Feet),IAS_Speed(Knots),Altitude_Change,Velocity_Change,Theta,GroundSpeed(Knots)," - "Mass,Algorithm"; - - return hdr; -} - -void VerticalPathObserver::WriteData() { - if (out_stream.is_open()) { - out_stream.close(); - } -} diff --git a/Public/VerticalPredictor.cpp b/Public/VerticalPredictor.cpp deleted file mode 100644 index 1b2a898..0000000 --- a/Public/VerticalPredictor.cpp +++ /dev/null @@ -1,347 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include "public/VerticalPredictor.h" - -#include - -#include "public/CoreUtils.h" - -using namespace std; -using namespace aaesim::open_source; -using namespace aaesim::open_source::constants; - -VerticalPredictor::VerticalPredictor() - : LOW_GROUNDSPEED_WARNING(50), - LOW_GROUNDSPEED_FATAL(0.1), - DESCENT_ANGLE_MAX(6.0), - DESCENT_ANGLE_WARNING(4.0), - m_current_trajectory_index(0), - m_cruise_altitude_msl(Units::FeetLength(37000)), - m_descent_start_time(Units::SecondsTime(0.0)), - m_transition_ias(Units::KnotsSpeed(310)), - m_transition_altitude_msl(Units::FeetLength(0.0)), - m_cruise_mach(0.8), - m_transition_mach(0.8) {} - -void VerticalPredictor::SetMembers(const VerticalPredictor &vertical_predictor) { - m_descent_start_time = vertical_predictor.m_descent_start_time; - m_transition_ias = vertical_predictor.m_transition_ias; - m_transition_mach = vertical_predictor.m_transition_mach; - m_cruise_altitude_msl = vertical_predictor.m_cruise_altitude_msl; - m_transition_altitude_msl = vertical_predictor.m_transition_altitude_msl; - m_current_trajectory_index = vertical_predictor.m_current_trajectory_index; -} - -Guidance VerticalPredictor::Update(const AircraftState ¤t_state, const Guidance ¤t_guidance, - const Units::Length distance_to_go) { - Guidance guidanceout = CalculateGuidanceCommands(current_state, distance_to_go, current_guidance); - - return guidanceout; -} - -PrecalcConstraint VerticalPredictor::CheckActiveConstraint(double along_path_distance_to_go_meter, - double altitude_msl_meter, double calibrated_airspeed_mps, - const PrecalcConstraint &constraints, - double transition_altitude_meter) { - PrecalcConstraint result = constraints; - - // if distance is greater than the constraint distance process the constraint values - if (along_path_distance_to_go_meter > Units::MetersLength(constraints.constraint_along_path_distance).value()) { - if (altitude_msl_meter >= Units::MetersLength(constraints.constraint_altLow).value() && - altitude_msl_meter <= Units::MetersLength(constraints.constraint_altHi).value()) { - // constraints are not violated - result.violation_flag = false; - result.active_flag = ActiveFlagType::SEG_END_MID_ALT; - } else if (altitude_msl_meter <= Units::MetersLength(constraints.constraint_altLow).value()) { - // Minimum Altitude Constraint is violated - result.violation_flag = true; - result.active_flag = ActiveFlagType::SEG_END_LOW_ALT; - } else if (altitude_msl_meter >= Units::MetersLength(constraints.constraint_altHi).value()) { - // constraints not violated - result.violation_flag = true; - result.active_flag = ActiveFlagType::SEG_END_AT_ALT; - } - } - // else if altitude is greater than the altitude constraint - else if (altitude_msl_meter >= Units::MetersLength(constraints.constraint_altHi).value()) { - // Maximum Altitude Constraint is violated - result.violation_flag = true; - - if ((calibrated_airspeed_mps - Units::MetersPerSecondSpeed(constraints.constraint_speedHi).value()) < - Units::MetersPerSecondSpeed(SPEED_HIGH_CONSTRAINT_TOLERANCE).value() && - Units::MetersPerSecondSpeed(constraints.constraint_speedHi).value() < - Units::MetersPerSecondSpeed(SPEED_HIGH_MAXIMUM).value() && - (Units::MetersLength(constraints.constraint_altHi).value() - altitude_msl_meter) <= - Units::MetersLength(ALT_HIGH_CONSTRAINT_TOLERANCE).value() && - altitude_msl_meter < transition_altitude_meter) { - result.active_flag = ActiveFlagType::AT_ALT_SLOW; - } else { - result.active_flag = ActiveFlagType::AT_ALT_ON_SPEED; - } - } - // else accelerate to upper speed constraint - else if ((calibrated_airspeed_mps - Units::MetersPerSecondSpeed(constraints.constraint_speedHi).value()) < - Units::MetersPerSecondSpeed(SPEED_HIGH_CONSTRAINT_TOLERANCE).value() && - // Numerical precision tolerance - calibrated_airspeed_mps < Units::MetersPerSecondSpeed(constraints.constraint_speedHi).value() && - Units::MetersPerSecondSpeed(constraints.constraint_speedHi).value() < - Units::MetersPerSecondSpeed(SPEED_HIGH_MAXIMUM).value() && - (Units::MetersLength(constraints.constraint_altHi).value() - altitude_msl_meter) > - Units::MetersLength(ALT_HIGH_CONSTRAINT_TOLERANCE).value()) { - result.violation_flag = true; - result.active_flag = ActiveFlagType::BELOW_ALT_SLOW; - } else if ((calibrated_airspeed_mps - Units::MetersPerSecondSpeed(constraints.constraint_speedHi).value()) < - Units::MetersPerSecondSpeed(SPEED_HIGH_CONSTRAINT_TOLERANCE).value() && - // Numerical precision tolerance - calibrated_airspeed_mps < Units::MetersPerSecondSpeed(constraints.constraint_speedHi).value() && - Units::MetersPerSecondSpeed(constraints.constraint_speedHi).value() < - Units::MetersPerSecondSpeed(SPEED_HIGH_MAXIMUM).value() && - (Units::MetersLength(constraints.constraint_altHi).value() - altitude_msl_meter) <= - Units::MetersLength(ALT_HIGH_CONSTRAINT_TOLERANCE).value() && - altitude_msl_meter < transition_altitude_meter) { - result.violation_flag = true; - result.active_flag = ActiveFlagType::AT_ALT_SLOW; - } else { - // constraints not violated - result.violation_flag = false; - } - - return result; -} - -PrecalcConstraint VerticalPredictor::FindActiveConstraint(const double &along_path_distance_to_go_meters, - const vector &precalculated_waypoints) { - PrecalcConstraint result; - unsigned int index = 0; - - // loop to find the current constraint being used - bool found = false; - for (unsigned int loop = 0; loop < precalculated_waypoints.size() && found == false; loop++) { - if (along_path_distance_to_go_meters < - Units::MetersLength(precalculated_waypoints[loop].m_precalc_constraints.constraint_along_path_distance) - .value()) { - found = true; - index = loop + 1; - } - } - - // find the desired constraint - if (index < precalculated_waypoints.size() && found == true) { - result.active_flag = ActiveFlagType::BELOW_ALT_ON_SPEED; - result.constraint_along_path_distance = - precalculated_waypoints[index - 1].m_precalc_constraints.constraint_along_path_distance; - result.constraint_altLow = precalculated_waypoints[index - 1].m_precalc_constraints.constraint_altLow; - result.constraint_altHi = precalculated_waypoints[index - 1].m_precalc_constraints.constraint_altHi; - result.constraint_speedHi = precalculated_waypoints[index - 1].m_precalc_constraints.constraint_speedHi; - result.constraint_speedLow = precalculated_waypoints[index - 1].m_precalc_constraints.constraint_speedLow; - result.index = (int)index; - } - - return result; -} - -Guidance VerticalPredictor::CalculateGuidanceCommands(const AircraftState &state, const Units::Length distance_to_go, - const Guidance ¤t_guidance) { - Guidance result = current_guidance; - - if (result.GetSelectedSpeed().GetSpeedType() == UNSPECIFIED_SPEED) { - // no selected speed, so set it to Mach or IAS depending on altitude - if (Units::FeetLength(state.GetAltitudeMsl()) > m_transition_altitude_msl) { - // Mach - result.SetSelectedSpeed(AircraftSpeed::OfMach(BoundedValue(m_transition_mach))); - } else { - // IAS - result.SetSelectedSpeed(AircraftSpeed::OfIndicatedAirspeed( - Units::MetersPerSecondSpeed(m_vertical_path.cas_mps[m_current_trajectory_index]))); - } - } - - const Units::MetersLength distance_remaining(distance_to_go); - double h_next; - double v_next; - double h_dot_next; - double gs_next; - - // if the check if the distance left is <= the start of the precalculated descent distance - if (distance_remaining.value() <= fabs(m_vertical_path.along_path_distance_m.back())) { - // Get index. - m_current_trajectory_index = - CoreUtils::FindNearestIndex(distance_remaining.value(), m_vertical_path.along_path_distance_m); - - // Set _next values. - if (m_current_trajectory_index == 0) { - // Below lowest distance-take values at end of route. - - h_next = m_vertical_path.altitude_m[m_current_trajectory_index]; - v_next = m_vertical_path.cas_mps[m_current_trajectory_index]; - h_dot_next = m_vertical_path.altitude_rate_mps[m_current_trajectory_index]; - gs_next = m_vertical_path.gs_mps[m_current_trajectory_index]; - - } else { - // Interpolate values using distance. - h_next = CoreUtils::LinearlyInterpolate(m_current_trajectory_index, distance_remaining.value(), - m_vertical_path.along_path_distance_m, m_vertical_path.altitude_m); - - v_next = CoreUtils::LinearlyInterpolate(m_current_trajectory_index, distance_remaining.value(), - m_vertical_path.along_path_distance_m, m_vertical_path.cas_mps); - - h_dot_next = - CoreUtils::LinearlyInterpolate(m_current_trajectory_index, distance_remaining.value(), - m_vertical_path.along_path_distance_m, m_vertical_path.altitude_rate_mps); - gs_next = CoreUtils::LinearlyInterpolate(m_current_trajectory_index, distance_remaining.value(), - m_vertical_path.along_path_distance_m, m_vertical_path.gs_mps); - } - - // Set result - result.m_reference_altitude = Units::MetersLength(h_next); - result.m_vertical_speed = Units::MetersPerSecondSpeed(h_dot_next); - result.m_ias_command = Units::MetersPerSecondSpeed(v_next); - result.m_ground_speed = Units::MetersPerSecondSpeed(gs_next); - } - - return result; -} - -double VerticalPredictor::CalculateEsfUsingConstantCAS(const double true_airspeed_mps, const double altitude_msl_meter, - const Units::Temperature temperature) { - double esf; - const Units::KelvinTemperature temperature_kelvin(temperature); - double mach; - double temp1, temp2, temp3; - - mach = true_airspeed_mps / sqrt(kGamma * R.value() * temperature_kelvin.value()); - - temp1 = 1.0 + (kGamma - 1.0) / 2 * pow(mach, 2); - temp2 = (pow(temp1, (-1.0 / (kGamma - 1)))) * (pow(temp1, (kGamma / (kGamma - 1))) - 1.0); - - if (altitude_msl_meter <= GetAtmosphere()->GetTropopauseHeight().value()) { - temp3 = 1.0 + (kGamma * R.value() * K_T.value()) / (2 * GRAVITY_METERS_PER_SECOND) * pow(mach, 2) + temp2; - } else { - temp3 = 1.0 + temp2; - } - - esf = 1.0 / temp3; - return esf; -} - -double VerticalPredictor::CalculateEsfUsingConstantMach(const double true_airspeed_mps, const double altitude_msl_meter, - const Units::Temperature temperature) { - double esf = 1.0; - const Units::KelvinTemperature temperature_kelvin(temperature); - double mach; - - mach = true_airspeed_mps / sqrt(kGamma * R.value() * temperature_kelvin.value()); - - if (altitude_msl_meter <= GetAtmosphere()->GetTropopauseHeight().value()) { - esf = 1.0 / (1.0 + (kGamma * R.value() * K_T.value()) / (2 * GRAVITY_METERS_PER_SECOND) * pow(mach, 2)); - } - - return esf; -} - -void VerticalPredictor::TrimDuplicatesFromVerticalPath() { - // Trims duplicate records from the trajectory. - vector::iterator time_it = m_vertical_path.time_to_go_sec.begin(); - vector::iterator x_it = m_vertical_path.along_path_distance_m.begin(); - vector::iterator alt_it = m_vertical_path.altitude_m.begin(); - vector::iterator speed_it = m_vertical_path.cas_mps.begin(); - vector::iterator alt_delta_it = m_vertical_path.altitude_rate_mps.begin(); - vector::iterator speed_delta_it = m_vertical_path.tas_rate_mps.begin(); - vector::iterator tas_it = m_vertical_path.true_airspeed.begin(); - vector::iterator theta_it = m_vertical_path.theta_radians.begin(); - vector::iterator gs_it = m_vertical_path.gs_mps.begin(); - vector::iterator mass_it = m_vertical_path.mass_kg.begin(); - - // double to store previous time stamp - double prev_time = -1; - - while (time_it != m_vertical_path.time_to_go_sec.end()) { - // if time doesn't match previous time iterate position - if (prev_time != (*time_it)) { - prev_time = (*time_it); // set new previous time value - - // iterate position in lists - - ++time_it; - ++x_it; - ++alt_it; - ++speed_it; - ++alt_delta_it; - ++speed_delta_it; - ++theta_it; - ++gs_it; - ++mass_it; - ++tas_it; - } - // else remove duplicate value - else { - time_it = m_vertical_path.time_to_go_sec.erase(time_it); - x_it = m_vertical_path.along_path_distance_m.erase(x_it); - alt_it = m_vertical_path.altitude_m.erase(alt_it); - speed_it = m_vertical_path.cas_mps.erase(speed_it); - alt_delta_it = m_vertical_path.altitude_rate_mps.erase(alt_delta_it); - tas_it = m_vertical_path.true_airspeed.erase(tas_it); - speed_delta_it = m_vertical_path.tas_rate_mps.erase(speed_delta_it); - theta_it = m_vertical_path.theta_radians.erase(theta_it); - gs_it = m_vertical_path.gs_mps.erase(gs_it); - mass_it = m_vertical_path.mass_kg.erase(mass_it); - } - } -} - -VerticalPredictor &VerticalPredictor::operator=(const VerticalPredictor &obj) { - if (this != &obj) { - this->m_wind_calculator = obj.m_wind_calculator; - this->m_transition_altitude_msl = obj.m_transition_altitude_msl; - this->m_cruise_altitude_msl = obj.m_cruise_altitude_msl; - this->m_transition_ias = obj.m_transition_ias; - this->m_transition_mach = obj.m_transition_mach; - this->m_cruise_mach = obj.m_cruise_mach; - this->m_descent_start_time = obj.m_descent_start_time; - this->m_precalculated_constraints = obj.m_precalculated_constraints; - this->m_vertical_path = obj.m_vertical_path; - this->m_current_trajectory_index = obj.m_current_trajectory_index; - this->m_course_calculator = obj.m_course_calculator; - this->m_atmosphere = obj.m_atmosphere; - } - - return *this; -} - -bool VerticalPredictor::operator==(const VerticalPredictor &obj) const { - // Note: not including mCalcWind comparison here because differences - // between this->mCalcWind and obj.mCalcWind should not affect - // processing. - - bool match = this->m_transition_altitude_msl == obj.m_transition_altitude_msl; - match = match && (this->m_cruise_altitude_msl == obj.m_cruise_altitude_msl); - match = match && (this->m_transition_ias == obj.m_transition_ias); - match = match && (this->m_transition_mach == obj.m_transition_mach); - match = match && (this->m_cruise_mach == obj.m_cruise_mach); - match = match && (this->m_descent_start_time == obj.m_descent_start_time); - match = match && (this->m_precalculated_constraints == obj.m_precalculated_constraints); - match = match && (this->m_vertical_path == obj.m_vertical_path); - match = match && (this->m_current_trajectory_index == obj.m_current_trajectory_index); - match = match && (this->m_atmosphere == obj.m_atmosphere); - - return match; -} - -bool VerticalPredictor::operator!=(const VerticalPredictor &obj) const { return !this->operator==(obj); } diff --git a/Public/Waypoint.cpp b/Public/Waypoint.cpp deleted file mode 100644 index 9a6c27c..0000000 --- a/Public/Waypoint.cpp +++ /dev/null @@ -1,72 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include "public/Waypoint.h" - -#include -#include -#include - -const Units::FeetLength Waypoint::MAX_ALTITUDE_CONSTRAINT(50000); -const Units::FeetLength Waypoint::MIN_ALTITUDE_CONSTRAINT(0); -const Units::KnotsSpeed Waypoint::MAX_SPEED_CONSTRAINT(1000); -const Units::KnotsSpeed Waypoint::MIN_SPEED_CONSTRAINT(0); - -Waypoint::Waypoint(const std::string &name, Units::Angle latitude, Units::Angle longitude, - Units::Length altitude_constraint_upper, Units::Length altitude_constraint_lower, - Units::Speed speed_constraint, Units::Length nominal_altitude, Units::Speed nominal_ias, - const std::string &arinc424_leg_type) - : m_name(name), - m_latitude(latitude), - m_longitude(longitude), - m_altitude(nominal_altitude), - m_nominal_ias(nominal_ias), - m_altitude_constraint_high(altitude_constraint_upper), - m_altitude_constraint_low(altitude_constraint_lower), - m_speed_constraint_high(speed_constraint), - m_speed_constraint_low(MIN_SPEED_CONSTRAINT), - m_rf_turn_center_latitude(Units::ZERO_ANGLE), - m_rf_turn_center_longitude(Units::ZERO_ANGLE), - m_rf_turn_arc_radius(Units::ZERO_LENGTH), - m_arinc424_leg_type(arinc424_leg_type) {} - -std::ostream &operator<<(std::ostream &out, const Waypoint &waypoint) { - out << waypoint.GetName() << " "; - out << Units::DegreesAngle(waypoint.GetLatitude()).value() << " "; - out << Units::DegreesAngle(waypoint.GetLongitude()).value() << " "; - out << Units::FeetLength(waypoint.GetAltitude()).value() << " "; - out << Units::KnotsSpeed(waypoint.GetNominalIas()).value() << " "; - out << Units::FeetLength(waypoint.GetAltitudeConstraintHigh()).value() << " "; - out << Units::FeetLength(waypoint.GetAltitudeConstraintLow()).value() << " "; - out << Units::KnotsSpeed(waypoint.GetSpeedConstraintHigh()).value() << " "; - out << Units::KnotsSpeed(waypoint.GetSpeedConstraintLow()).value() << " "; - out << Units::NauticalMilesLength(waypoint.GetRfTurnArcRadius()).value() << " "; - out << Units::DegreesAngle(waypoint.GetRfTurnCenterLatitude()).value() << " "; - out << Units::DegreesAngle(waypoint.GetRfTurnCenterLongitude()).value() << " "; - out << waypoint.GetArinc424LegType() << " "; - out << std::endl; - return out; -} - -std::ostream &operator<<(std::ostream &out, const std::list &waypoints) { - for (std::list::const_iterator i = waypoints.begin(); i != waypoints.end(); ++i) { - out << *i; - } - return out; -} diff --git a/Public/WaypointLoader.cpp b/Public/WaypointLoader.cpp deleted file mode 100644 index 53f7e59..0000000 --- a/Public/WaypointLoader.cpp +++ /dev/null @@ -1,121 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include "WaypointLoader.h" - -#include -#include -#include - -namespace aaesim { -namespace loaders { - -bool WaypointLoader::load(DecodedStream *input) { - set_stream(input); - - constexpr int kColumnCountV4Schema = 13; - constexpr int kColumnCountNoRfLegsPreV4Schema = 11; - constexpr int kColumnCountCompletePreV4Schema = 14; - - std::string name; - Units::Angle latitude{Units::zero()}; - Units::Angle longitude{Units::zero()}; - Units::Length altitude{Units::zero()}; - - bool load_successful = load_datum(name); - if (!load_successful) { - LoggingLoadable::report_error("could not load waypoint_name"); - } - - load_successful = loadAngleDegrees(latitude); - if (!load_successful) { - LoggingLoadable::report_error("could not load waypoint_Latitude"); - } - - load_successful = loadAngleDegrees(longitude); - if (!load_successful) { - LoggingLoadable::report_error("could not load waypoint_Longitude"); - } - - load_successful = loadLengthFeet(altitude); - if (!load_successful) { - LoggingLoadable::report_error("could not load waypoint_altitude"); - } - - std::array uninterpreted_loaded_values{}; - for (int i = 4; i < kColumnCountV4Schema - 1; ++i) { - load_successful = load_datum(uninterpreted_loaded_values[i]); - if (!load_successful) { - input->push_back(); - if (i == kColumnCountNoRfLegsPreV4Schema) { - Waypoint loaded_waypoint(name, latitude, longitude, Units::FeetLength(uninterpreted_loaded_values[7]), - Units::FeetLength(uninterpreted_loaded_values[8]), - Units::KnotsSpeed(uninterpreted_loaded_values[9]), altitude, - Units::KnotsSpeed(uninterpreted_loaded_values[5])); - loaded_waypoint.SetSpeedConstraintLow(Units::KnotsSpeed(uninterpreted_loaded_values[10])); - waypoint_ = loaded_waypoint; - return true; - } - LoggingLoadable::report_error("could not load a waypoint parameter"); - return false; - } - } - - std::string uninterpreted_next_value; - load_successful = load_datum(uninterpreted_next_value); - if (!load_successful) { - LoggingLoadable::report_error("could not load a waypoint parameter...reason unknown"); - return false; - } - - const auto loaded_character_length = uninterpreted_next_value.size(); - constexpr std::string::size_type kLegTypeIdentifierSize = 2; - if (loaded_character_length == kLegTypeIdentifierSize) { - Waypoint loaded_waypoint(name, latitude, longitude, Units::FeetLength(uninterpreted_loaded_values[5]), - Units::FeetLength(uninterpreted_loaded_values[6]), - Units::KnotsSpeed(uninterpreted_loaded_values[7]), altitude, - Units::KnotsSpeed(uninterpreted_loaded_values[4]), uninterpreted_next_value); - loaded_waypoint.SetSpeedConstraintLow(Units::KnotsSpeed(uninterpreted_loaded_values[8])); - loaded_waypoint.SetRfTurnArcRadius(Units::NauticalMilesLength(uninterpreted_loaded_values[9])); - loaded_waypoint.SetRfTurnCenterLatitude(Units::DegreesAngle(uninterpreted_loaded_values[10])); - loaded_waypoint.SetRfTurnCenterLongitude(Units::DegreesAngle(uninterpreted_loaded_values[11])); - waypoint_ = loaded_waypoint; - return true; - } - - Waypoint loaded_waypoint(name, latitude, longitude, Units::FeetLength(uninterpreted_loaded_values[7]), - Units::FeetLength(uninterpreted_loaded_values[8]), - Units::KnotsSpeed(uninterpreted_loaded_values[9]), altitude, - Units::KnotsSpeed(uninterpreted_loaded_values[5]), "UNSET"); - loaded_waypoint.SetSpeedConstraintLow(Units::KnotsSpeed(uninterpreted_loaded_values[10])); - loaded_waypoint.SetRfTurnArcRadius(Units::NauticalMilesLength(uninterpreted_loaded_values[11])); - loaded_waypoint.SetRfTurnCenterLatitude(Units::DegreesAngle(std::strtod(uninterpreted_next_value.c_str(), nullptr))); - - load_successful = load_datum(uninterpreted_loaded_values[13]); - if (!load_successful) { - LoggingLoadable::report_error("could not load a waypoint parameter"); - return false; - } - loaded_waypoint.SetRfTurnCenterLongitude(Units::DegreesAngle(uninterpreted_loaded_values[13])); - waypoint_ = loaded_waypoint; - return true; -} - -} // namespace loaders -} // namespace aaesim diff --git a/Public/WaypointLoader.h b/Public/WaypointLoader.h deleted file mode 100644 index 89412be..0000000 --- a/Public/WaypointLoader.h +++ /dev/null @@ -1,45 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include - -#include "public/Waypoint.h" - -namespace aaesim { -namespace loaders { - -class WaypointLoader final : public LoggingLoadable { - public: - WaypointLoader() = default; - ~WaypointLoader() override = default; - - bool load(DecodedStream *input) override; - - const Waypoint &BuildWaypoint() const; - - private: - Waypoint waypoint_{}; -}; - -inline const Waypoint &WaypointLoader::BuildWaypoint() const { return waypoint_; } - -} // namespace loaders -} // namespace aaesim diff --git a/Public/WeatherEstimate.cpp b/Public/WeatherEstimate.cpp deleted file mode 100644 index 8c95e49..0000000 --- a/Public/WeatherEstimate.cpp +++ /dev/null @@ -1,172 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include "public/WeatherEstimate.h" - -#include - -#include "public/CoreUtils.h" -#include "public/Wind.h" -#include "public/WindZero.h" - -using namespace aaesim::open_source; - -WeatherEstimate::WeatherEstimate() - : m_shared_members(std::make_shared()), - m_wind(nullptr), - m_temperature_checked(true), - m_temperature_available(false) {} - -WeatherEstimate::WeatherEstimate(std::shared_ptr wind, std::shared_ptr atmosphere) - : m_shared_members(std::make_shared(atmosphere)), - m_wind(wind), - m_temperature_checked(false), - m_temperature_available(false) { - if (CoreUtils::InstanceOf(wind.get())) { - m_temperature_checked = true; - m_temperature_available = false; - } -} - -std::shared_ptr WeatherEstimate::getWind() const { return m_wind; } - -std::shared_ptr WeatherEstimate::getAtmosphere() const { return m_shared_members->m_atmosphere; } - -WeatherEstimate::~WeatherEstimate() {} - -void WeatherEstimate::LoadConditionsAt(const Units::Angle latitude, const Units::Angle longitude, - const Units::Length altitude) { - SetLocation(latitude, longitude, altitude); - m_wind->InterpolateTrueWind(latitude, longitude, altitude, east_west(), north_south()); - - if (IsTemperatureAvailable(latitude, longitude, altitude)) { - m_temperature = m_wind->InterpolateTemperature(latitude, longitude, altitude); - m_pressure = m_wind->InterpolatePressure(latitude, longitude, altitude); - m_density = m_pressure / (m_temperature * R); - } else { - m_temperature = m_shared_members->m_atmosphere->GetTemperature(altitude); - m_shared_members->m_atmosphere->AirDensity(altitude, m_density, m_pressure); - } -} - -Units::Density WeatherEstimate::GetDensity() const { return m_density; } - -Units::Pressure WeatherEstimate::GetPressure() const { return m_pressure; } - -Units::KelvinTemperature WeatherEstimate::GetTemperature() const { return m_temperature; } - -Units::Speed WeatherEstimate::MachToTAS(const double mach, const Units::Length altitude) const { - Units::MetersPerSecondSpeed speed_of_sound; - - if (m_temperature_available) { - // assume conditions have been loaded - speed_of_sound = getAtmosphere()->SpeedOfSound(m_temperature); - } else { - speed_of_sound = getAtmosphere()->SpeedOfSound(altitude); - } - - Units::Speed true_airspeed = mach * speed_of_sound; - return true_airspeed; -} - -Units::Speed WeatherEstimate::MachToCAS(const double mach, const Units::Length altitude) const { - Units::Speed true_airspeed = MachToTAS(mach, altitude); - Units::Speed calibrated_airspeed = TAS2CAS(true_airspeed, altitude); - - return calibrated_airspeed; -} - -double WeatherEstimate::ESFconstantCAS(const Units::Speed true_airspeed, const Units::Length altitude) const { - Units::KelvinTemperature temperature; - if (!m_temperature_available) { - /* assume LoadConditionsAt has been called with the current location to set m_temperature */ - temperature = m_temperature; - } else { - temperature = getAtmosphere()->GetTemperature(altitude); - } - - double esf = getAtmosphere()->ESFconstantCAS(true_airspeed, altitude, temperature); - return esf; -} - -bool WeatherEstimate::IsTemperatureAvailable(const Units::Angle latitude, const Units::Angle longitude, - const Units::Length altitude) const { - if (!m_temperature_checked) { - /* - * This check changes values of boolean fields, but we consider it - * a lazy init and therefore const. Results may depend on the - * coordinates used the first time through. - */ - - Units::KelvinTemperature t = m_wind->InterpolateTemperature(latitude, longitude, altitude); - m_temperature_available = (t.value() >= 0); - m_temperature_checked = true; - if (m_temperature_available) { - LOG4CPLUS_INFO(m_logger, "Temperature is available from Wind object."); - } else { - LOG4CPLUS_INFO(m_logger, "Temperature is unavailable from wind object, using Atmosphere"); - } - } - - return m_temperature_available; -} - -Units::Speed WeatherEstimate::TAS2CAS(const Units::Speed true_airspeed, const Units::Length altitude) const { - Units::Speed calibrated_airspeed; - - if (m_temperature_available) { - // Assume current conditions have been loaded - calibrated_airspeed = getAtmosphere()->TAS2CAS(true_airspeed, m_pressure, m_density); - } else { - calibrated_airspeed = getAtmosphere()->TAS2CAS(true_airspeed, altitude); - } - - return calibrated_airspeed; -} - -double WeatherEstimate::TAS2Mach(const Units::Speed true_airspeed, const Units::Length altitude) const { - Units::MetersPerSecondSpeed speed_of_sound; - - if (m_temperature_available) { - // Assume correct conditions have been loaded - speed_of_sound = getAtmosphere()->SpeedOfSound(m_temperature); - } else { - speed_of_sound = getAtmosphere()->SpeedOfSound(altitude); - } - - double mach = true_airspeed / speed_of_sound; - return mach; -} - -Units::Speed WeatherEstimate::CAS2TAS(const Units::Speed calibrated_airspeed, const Units::Length altitude) const { - Units::Speed true_airspeed; - - if (m_temperature_available) { - // assume conditions have been loaded - true_airspeed = getAtmosphere()->CAS2TAS(calibrated_airspeed, m_pressure, m_density); - } else { - true_airspeed = getAtmosphere()->CAS2TAS(calibrated_airspeed, altitude); - } - - return true_airspeed; -} - -double WeatherEstimate::CAS2Mach(const Units::Speed calibrated_airspeed, const Units::Length altitude) const { - return TAS2Mach(CAS2TAS(calibrated_airspeed, altitude), altitude); -} diff --git a/Public/WeatherPrediction.cpp b/Public/WeatherPrediction.cpp deleted file mode 100644 index a842d03..0000000 --- a/Public/WeatherPrediction.cpp +++ /dev/null @@ -1,48 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include "public/WeatherPrediction.h" - -#include -#include - -#include "public/Atmosphere.h" - -using namespace aaesim::open_source; - -WeatherPrediction::WeatherPrediction(std::shared_ptr wind, std::shared_ptr atmosphere) - : WeatherEstimate(std::move(wind), std::move(atmosphere)), update_count_(0) { - // inhibit 3-D predicted temperature for now. - m_temperature_checked = true; - m_temperature_available = false; -} - -WeatherPrediction WeatherPrediction::CreateZeroWindPrediction(std::shared_ptr atmosphere) { - aaesim::open_source::WindStack zeroWinds(1, 5); - zeroWinds.Insert(1, Units::FeetLength(0.), Units::KnotsSpeed(0.)); - zeroWinds.Insert(2, Units::FeetLength(10000.), Units::KnotsSpeed(0.)); - zeroWinds.Insert(3, Units::FeetLength(20000.), Units::KnotsSpeed(0.)); - zeroWinds.Insert(4, Units::FeetLength(30000.), Units::KnotsSpeed(0.)); - zeroWinds.Insert(5, Units::FeetLength(50000.), Units::KnotsSpeed(0.)); - WeatherPrediction zeroWeather; - zeroWeather.east_west() = zeroWinds; - zeroWeather.north_south() = zeroWinds; - zeroWeather.SetAtmosphere(atmosphere); - return zeroWeather; -} diff --git a/Public/WeatherTruth.cpp b/Public/WeatherTruth.cpp deleted file mode 100644 index e07c944..0000000 --- a/Public/WeatherTruth.cpp +++ /dev/null @@ -1,40 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include "public/WeatherTruth.h" - -#include - -#include "public/Atmosphere.h" - -using namespace aaesim::open_source; - -WeatherTruth::WeatherTruth() : WeatherEstimate() {} - -WeatherTruth::WeatherTruth(std::shared_ptr wind, std::shared_ptr atmosphere, - bool inhibit_weather_temperature) - : WeatherEstimate(wind, atmosphere) { - if (inhibit_weather_temperature) { - // inhibit 3-D temperature - m_temperature_checked = true; - m_temperature_available = false; - } -} - -WeatherTruth::~WeatherTruth() {} diff --git a/Public/Wgs84PrecalcWaypoint.cpp b/Public/Wgs84PrecalcWaypoint.cpp deleted file mode 100644 index 33d58ac..0000000 --- a/Public/Wgs84PrecalcWaypoint.cpp +++ /dev/null @@ -1,39 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include "public/Wgs84PrecalcWaypoint.h" - -using namespace aaesim::open_source; - -bool Wgs84PrecalcWaypoint::operator==(const Wgs84PrecalcWaypoint &obj) const { - bool match = (m_leg_length == obj.m_leg_length); - match = match && (m_name == obj.m_name); - match = match && (m_bank_angle == obj.m_bank_angle); - match = match && (m_enu_course_in_angle == obj.m_enu_course_in_angle); - match = match && (m_enu_course_out_angle == obj.m_enu_course_out_angle); - match = match && (m_ground_speed == obj.m_ground_speed); - match = match && (m_precalc_constraints == obj.m_precalc_constraints); - match = match && (m_radius_rf_leg == obj.m_radius_rf_leg); - match = match && (m_rf_leg_center == obj.m_rf_leg_center); - match = match && (m_position == obj.m_position); - match = match && (m_position == obj.m_position); - match = match && (m_leg_type == obj.m_leg_type); - - return match; -} diff --git a/Public/Wind.cpp b/Public/Wind.cpp deleted file mode 100644 index 5ccccb9..0000000 --- a/Public/Wind.cpp +++ /dev/null @@ -1,54 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include "public/Wind.h" - -#include -#include -#include -#include -#include - -#include "public/AircraftCalculations.h" - -using std::shared_ptr; -using std::string; -using namespace aaesim::open_source; - -void Wind::InterpolateTrueWind(const Units::Angle lat_in, const Units::Angle lon_in, const Units::Length altitude, - aaesim::open_source::WindStack &east_west, aaesim::open_source::WindStack &north_south) { - InterpolateWindMatrix(lat_in, lon_in, altitude, east_west, north_south); -} - -void Wind::InterpolateForecastWind(const shared_ptr &tangentPlaneSequence, - const Units::Length x_in, const Units::Length y_in, const Units::Length altitude, - Units::Speed &east_west, Units::Speed &north_south) { - Units::RadiansAngle lat(0.), lon(0.); - - EarthModel::LocalPositionEnu localPosition; - localPosition.x = x_in; - localPosition.y = y_in; - localPosition.z = altitude; - EarthModel::GeodeticPosition wpnt; - tangentPlaneSequence->ConvertLocalToGeodetic(localPosition, wpnt); - lat = wpnt.latitude; - lon = wpnt.longitude; - - InterpolateWindScalar(lat, lon, altitude, east_west, north_south); -} diff --git a/Public/WindStack.cpp b/Public/WindStack.cpp deleted file mode 100644 index 373d1dd..0000000 --- a/Public/WindStack.cpp +++ /dev/null @@ -1,278 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include "public/WindStack.h" - -#include -#include -#include -#include - -#include "public/CustomMath.h" -#include "utility/CustomUnits.h" - -using namespace aaesim::open_source; -using namespace std; - -WindStack::WindStack() : m_altitude(), m_speed(), m_minimum_data_index(0), m_maximum_data_index(0) { - SetBounds(m_minimum_data_index, m_maximum_data_index); -} - -WindStack::WindStack(const int min, const int max) - : m_altitude(), m_speed(), m_minimum_data_index(min), m_maximum_data_index(max) { - SetBounds(min, max); -} - -WindStack::WindStack(const WindStack &in) { Copy(in); } - -WindStack &WindStack::operator=(const WindStack &in) { - Copy(in); - return *this; -} - -void WindStack::Copy(const WindStack &in) { - m_minimum_data_index = in.m_minimum_data_index; - m_maximum_data_index = in.m_maximum_data_index; - m_altitude = in.m_altitude; - m_speed = in.m_speed; -} - -bool WindStack::operator==(const WindStack &obj) const { - bool match = ((GetMinRow() == obj.GetMinRow()) && (GetMaxRow() == obj.GetMaxRow())); - if (match && m_minimum_data_index != m_maximum_data_index) { - for (auto ix = GetMinRow(); (match && (ix <= GetMaxRow())); ++ix) { - match = match && (GetAltitude(ix) == obj.GetAltitude(ix)) && (GetSpeed(ix) == obj.GetSpeed(ix)); - } - } - return match; -} - -bool WindStack::operator!=(const WindStack &obj) const { return !operator==(obj); } - -Units::FeetLength WindStack::GetAltitude(const int index) const { return m_altitude.at(index); } - -Units::KnotsSpeed WindStack::GetSpeed(const int index) const { return m_speed.at(index); } - -void WindStack::SetBounds(int min, int max) { - m_minimum_data_index = min; - m_maximum_data_index = max; - - m_altitude.clear(); - m_speed.clear(); - - for (int i = 0; i <= max; ++i) { - m_altitude.push_back(Units::Infinity()); - m_speed.push_back(Units::Infinity()); - } -} - -void WindStack::Insert(const int index, const Units::Length altitude, const Units::Speed speed) { - m_altitude.at(index) = altitude; - m_speed.at(index) = speed; -} - -void WindStack::SortAltitudesAscending() { - if (m_minimum_data_index == m_maximum_data_index) return; - - std::vector> zipped_data; - for (auto idx = m_minimum_data_index; idx <= m_maximum_data_index; ++idx) { - zipped_data.push_back(std::make_pair(m_altitude.at(idx), m_speed.at(idx))); - } - std::sort(zipped_data.begin(), zipped_data.end(), AltitudeComparator); - - auto insert_index = m_minimum_data_index; - auto vector_inserter = [this, &insert_index](const std::pair item) { - m_altitude.at(insert_index) = item.first; - m_speed.at(insert_index) = item.second; - ++insert_index; - }; - std::for_each(zipped_data.cbegin(), zipped_data.cend(), vector_inserter); -} - -WindStack WindStack::CreateZeroSpeedStack() { - WindStack wind_stack(0, 4); - Units::FeetLength altitude(1000), altitude_step(10000); - for (auto i = wind_stack.GetMinRow(); i <= wind_stack.GetMaxRow(); ++i) { - wind_stack.Insert(i, altitude, Units::zero()); - altitude += altitude_step; - } - return wind_stack; -} - -void WindStack::CalculateWindGradientAtAltitude(const Units::Length altitude_in, Units::Speed &wind_speed, - Units::Frequency &wind_gradient) const { - const WindStack &wind_stack(*this); // was a parameter to a static function - Units::FeetLength altitude = altitude_in; - - Units::FeetLength maximum_altitude = wind_stack.GetAltitude(wind_stack.GetMaxRow()); - Units::FeetLength minimum_altitude = wind_stack.GetAltitude(wind_stack.GetMinRow()); - if (altitude > maximum_altitude) { - altitude = maximum_altitude; - } else if (altitude < minimum_altitude) { - altitude = minimum_altitude; - } - - // Find appropriate altitudes to sample for wind gradient. The wind data used will be from the low index through - // low index + 4. - int low_index; - int number_of_rows_of_wind_matrix = wind_stack.GetMaxRow() - wind_stack.GetMinRow() + 1; - if (number_of_rows_of_wind_matrix == 5) { - low_index = wind_stack.GetMinRow(); - } else if (altitude <= wind_stack.GetAltitude(wind_stack.GetMinRow() + 2)) { - // Altitude at low end-take bottom 5 winds. - low_index = wind_stack.GetMinRow(); - } else if (altitude >= wind_stack.GetAltitude(wind_stack.GetMaxRow() - 2)) { - // Altitude at high end-take top 5 winds. - low_index = wind_stack.GetMaxRow() - 4; - } else { - // Altitude somewhere in the middle-compute index to take. - low_index = wind_stack.GetMinRow(); - while ((low_index < wind_stack.GetMaxRow()) && (wind_stack.GetAltitude(low_index) < altitude)) { - low_index++; - } - - Units::FeetLength upper_bounding_altitude = wind_stack.GetAltitude(low_index); - Units::FeetLength lower_bounding_altitude = wind_stack.GetAltitude(low_index - 1); - - if ((altitude - lower_bounding_altitude) < (upper_bounding_altitude - altitude)) { - low_index = low_index - 1; - } - low_index = low_index - 2; - } - - DVector wind_altitudes_ft(1, 5); - DVector wind_velocities_knots(1, 5); - - int row_index = 1; - for (int i = low_index; i <= (low_index + 4); i++) { - wind_altitudes_ft.Set(row_index, wind_stack.GetAltitude(i) / Units::FeetLength(1)); - wind_velocities_knots.Set(row_index, wind_stack.GetSpeed(i) / Units::KnotsSpeed(1)); - row_index++; - } - - double A_original[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - DMatrix A((double **)&A_original, 1, 3, 1, 3); - - DVector B(1, 3); - DVector M_(1, 3); - - double h2 = wind_altitudes_ft[2] - wind_altitudes_ft[1]; - double h3 = wind_altitudes_ft[3] - wind_altitudes_ft[2]; - double h4 = wind_altitudes_ft[4] - wind_altitudes_ft[3]; - double h5 = wind_altitudes_ft[5] - wind_altitudes_ft[4]; - - // 1st Row of A Matrix - A.Set(1, 1, -1.0 / 2.0 * h2 - 1.0 / 3.0 * h3); - A.Set(1, 2, -1.0 / 6.0 * h3); - A.Set(1, 3, 0); - B.Set(1, (wind_velocities_knots[2] - wind_velocities_knots[1]) / h2 - - (wind_velocities_knots[3] - wind_velocities_knots[2]) / h3); - - // 2nd Row of A Matrix - A.Set(2, 1, -1.0 / 6.0 * h3); - A.Set(2, 2, -1.0 / 3.0 * h3 - 1.0 / 3.0 * h4); - A.Set(2, 3, -1.0 / 6.0 * h4); - B.Set(2, (wind_velocities_knots[3] - wind_velocities_knots[2]) / h3 - - (wind_velocities_knots[4] - wind_velocities_knots[3]) / h4); - - // 3rd Row of A Matrix - A.Set(3, 1, 0); - A.Set(3, 2, -1.0 / 6.0 * h4); - A.Set(3, 3, -1.0 / 3.0 * h4 - 1.0 / 2.0 * h5); - B.Set(3, (wind_velocities_knots[4] - wind_velocities_knots[3]) / h4 - - (wind_velocities_knots[5] - wind_velocities_knots[4]) / h5); - - DMatrix A_inverse(1, 3, 1, 3); - inverse(A, 3, A_inverse); - - DVector tmp(1, 3); - - matrix_times_vector(A_inverse, B, 3, M_); - - DVector M(1, 5); - M.Set(1, M_[1]); - M.Set(2, M_[1]); - M.Set(3, M_[2]); - M.Set(4, M_[3]); - M.Set(5, M_[3]); - - DVector a(1, 5); - DVector b(1, 5); - DVector c(1, 5); - DVector d(1, 5); - DVector x(1, 5); - for (int ind1 = 1; ind1 < 5; ind1++) { - double h = wind_altitudes_ft[ind1 + 1] - wind_altitudes_ft[ind1]; - a.Set(ind1, (M[ind1 + 1] - M[ind1]) / (6 * h)); - b.Set(ind1, M[ind1] / 2); - c.Set(ind1, - (wind_velocities_knots[ind1 + 1] - wind_velocities_knots[ind1]) / h - (M[ind1 + 1] + 2 * M[ind1]) / 6 * h); - d.Set(ind1, wind_velocities_knots[ind1]); - x.Set(ind1, wind_altitudes_ft[ind1]); - } - - bool found = false; - DVector new_altitudes(wind_altitudes_ft.GetMin(), wind_altitudes_ft.GetMax() + 1); - for (int loop = wind_altitudes_ft.GetMin(); loop <= wind_altitudes_ft.GetMax(); loop++) { - if (wind_altitudes_ft[loop] <= altitude.value()) { - new_altitudes.Set(loop, wind_altitudes_ft[loop]); - } else if (wind_altitudes_ft[loop] > altitude.value() && found) { - new_altitudes.Set(loop, wind_altitudes_ft[loop - 1]); - } else { - found = true; - new_altitudes.Set(loop, altitude.value()); - } - } - - if (!found) { - new_altitudes.Set(new_altitudes.GetMax(), altitude.value()); - } else { - new_altitudes.Set(new_altitudes.GetMax(), wind_altitudes_ft[wind_altitudes_ft.GetMax()]); - } - - int index = new_altitudes.GetMin(); - list ind; - for (int loop = new_altitudes.GetMin(); loop < new_altitudes.GetMax(); loop++) { - if (new_altitudes[loop] == altitude.value()) { - ind.push_back(index); - } - index++; - } - - if (ind.size() == 1 && (*ind.begin()) > 1) { - wind_speed = Units::KnotsSpeed(a[(*ind.begin()) - 1] * pow((altitude.value() - x[(*ind.begin()) - 1]), 3) + - b[(*ind.begin()) - 1] * pow(altitude.value() - x[(*ind.begin()) - 1], 2) + - c[(*ind.begin()) - 1] * (altitude.value() - x[(*ind.begin()) - 1]) + - d[(*ind.begin()) - 1]); - wind_gradient = Units::KnotsPerFootFrequency( - 3 * a[(*ind.begin()) - 1] * pow(altitude.value() - x[(*ind.begin()) - 1], 2) + - 2 * b[(*ind.begin()) - 1] * (altitude.value() - x[(*ind.begin()) - 1]) + c[(*ind.begin()) - 1]); - } else if (!ind.empty()) { - wind_speed = Units::KnotsSpeed(a[(*ind.begin())] * pow((altitude.value() - x[(*ind.begin())]), 3) + - b[(*ind.begin())] * pow(altitude.value() - x[(*ind.begin())], 2) + - c[(*ind.begin())] * (altitude.value() - x[(*ind.begin())]) + d[(*ind.begin())]); - wind_gradient = Units::KnotsPerFootFrequency( - 3 * a[(*ind.begin())] * pow(altitude.value() - x[(*ind.begin())], 2) + - 2 * b[(*ind.begin())] * (altitude.value() - x[(*ind.begin())]) + c[(*ind.begin())]); - } - - if (wind_gradient != Units::zero()) { - wind_gradient = -wind_gradient; - } -} diff --git a/Public/WindZero.cpp b/Public/WindZero.cpp deleted file mode 100644 index 8aba22a..0000000 --- a/Public/WindZero.cpp +++ /dev/null @@ -1,63 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include "public/WindZero.h" - -using namespace aaesim::open_source; - -WindZero::WindZero(std::shared_ptr atmosphere) : m_atmosphere(atmosphere) {} - -WindZero::~WindZero() {} - -void WindZero::InterpolateWind(Units::Angle latitude_in, Units::Angle longitude_in, Units::Length alt, Units::Speed &u, - Units::Speed &v) { - u = Units::KnotsSpeed(0); - v = Units::KnotsSpeed(0); -} - -void WindZero::InterpolateWindScalar(Units::Angle lat_in, Units::Angle lon_in, Units::Length altitude, - Units::Speed &east_west, Units::Speed &north_south) { - east_west = Units::KnotsSpeed(0); - north_south = Units::KnotsSpeed(0); -} - -void WindZero::InterpolateWindMatrix(Units::Angle lat_in, Units::Angle lon_in, Units::Length alt_in, - aaesim::open_source::WindStack &east_west, - aaesim::open_source::WindStack &north_south) { - for (int i = east_west.GetMinRow(); i <= east_west.GetMaxRow(); i++) { - east_west.Insert(i, Units::FeetLength((i - 1) * 1000), Units::KnotsSpeed(0)); - } - - for (int i = north_south.GetMinRow(); i <= north_south.GetMaxRow(); i++) { - north_south.Insert(i, Units::FeetLength((i - 1) * 1000), Units::KnotsSpeed(0)); - } -} - -Units::KelvinTemperature WindZero::InterpolateTemperature(Units::Angle latitude_in, Units::Angle longitude_in, - Units::Length alt) { - // use the standard atmosphere, ignoring lat/lon - return m_atmosphere->GetTemperature(alt); -} - -Units::Pressure WindZero::InterpolatePressure(Units::Angle latitude_in, Units::Angle longitude_in, Units::Length alt) { - Units::Pressure pressure; - Units::Density density; - m_atmosphere->AirDensity(alt, density, pressure); - return pressure; -} diff --git a/Public/ZeroWindTrueWeatherOperator.cpp b/Public/ZeroWindTrueWeatherOperator.cpp deleted file mode 100644 index 3be9ab2..0000000 --- a/Public/ZeroWindTrueWeatherOperator.cpp +++ /dev/null @@ -1,34 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include "public/ZeroWindTrueWeatherOperator.h" - -#include - -aaesim::open_source::ZeroWindTrueWeatherOperator::ZeroWindTrueWeatherOperator( - std::shared_ptr true_weather) - : AbstractTrueWeatherOperator(true_weather) { - m_true_weather->east_west() = ZERO_STACK; - m_true_weather->north_south() = ZERO_STACK; -} - -void aaesim::open_source::ZeroWindTrueWeatherOperator::CalculateEnvironmentalWind( - const EarthModel::GeodeticPosition &position, const Units::Length &altitude_msl) { - m_true_weather->LoadConditionsAt(position.latitude, position.longitude, altitude_msl); -} diff --git a/include/public/ADSBReceiver.h b/include/public/ADSBReceiver.h deleted file mode 100644 index 1f24c20..0000000 --- a/include/public/ADSBReceiver.h +++ /dev/null @@ -1,44 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include - -#include -#include - -#include "public/ADSBSVReport.h" -#include "public/AircraftState.h" -#include "public/SimulationTime.h" - -namespace aaesim { -namespace open_source { -struct ADSBReceiver { - virtual ADSBSVReport GetCurrentADSBReport(int id) const = 0; - virtual ADSBSVReport GetADSBReportBefore(int id, Units::Time time) const = 0; - virtual const std::vector &GetReportsReceivedByTime(const SimulationTime &time) const = 0; - virtual const std::map > &GetAllReportsReceived() const = 0; - virtual std::map const GetCurrentADSBReport() const = 0; - virtual void Initialize(Units::Length adsb_reception_range_threshold) = 0; - virtual std::map Receive(const aaesim::open_source::SimulationTime &time, - const aaesim::open_source::AircraftState &state) = 0; -}; -} // namespace open_source -} // namespace aaesim diff --git a/include/public/ADSBSVReport.h b/include/public/ADSBSVReport.h deleted file mode 100644 index 9f99c9f..0000000 --- a/include/public/ADSBSVReport.h +++ /dev/null @@ -1,135 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include -#include -#include -#include - -namespace aaesim::open_source { -class ADSBSVReport final { - public: - ADSBSVReport() = default; - ~ADSBSVReport() = default; - static const ADSBSVReport EMPTY_REPORT; - - bool operator==(const ADSBSVReport &in); - - bool HasPosition() const; - bool HasVelocity() const; - int GetId() const; - int GetNacp() const; - int GetNacv() const; - int GetNicp() const; - int GetNicv() const; - Units::SecondsTime GetTime() const; - Units::FeetLength GetX() const; - Units::FeetPerSecondSpeed GetXd() const; - Units::FeetLength GetY() const; - Units::FeetPerSecondSpeed GetYd() const; - Units::FeetLength GetAltitudeMsl() const; - Units::FeetPerSecondSpeed GetVerticalSpeed() const; - Units::Angle GetLatitude() const; - Units::Angle GetLongitude() const; - - class Builder { - private: - int id_{-1}; - Units::Time timestamp_{Units::SecondsTime(-1)}; - int nacp_{0}, nacv_{0}, nicp_{0}, nicv_{0}; - bool has_position_{false}, has_velocity_{false}; - Units::FeetLength position_x_{Units::zero()}, position_y_{Units::zero()}, altitude_msl_{Units::zero()}; - Units::FeetPerSecondSpeed xd_{Units::zero()}, yd_{Units::zero()}, altitude_rate_{Units::zero()}; - Units::Length horizontal_position_quantum_{Units::zero()}, vertical_position_quantum_{Units::zero()}; - Units::Speed horizontal_velocity_quantum_{Units::zero()}, vertical_velocity_quantum_{Units::zero()}; - Units::Angle latitude_{Units::zero()}, longitude_{Units::zero()}; - - public: - Builder(int unique_acid, Units::Time timestamp); - ~Builder() = default; - ADSBSVReport Build(); - Builder *NACp(int nacp); - Builder *NACv(int nacv); - Builder *NICp(int nicp); - Builder *NICv(int nicv); - Builder *Position(Units::FeetLength enu_x, Units::FeetLength enu_y); - Builder *GeodeticPosition(Units::Angle latitude, Units::Angle longitude); - Builder *AltitudeMsl(Units::FeetLength altitude_msl); - Builder *GroundSpeed(Units::FeetPerSecondSpeed enu_xd, Units::FeetPerSecondSpeed enu_yd); - Builder *AltitudeRate(Units::FeetPerSecondSpeed altitude_rate); - Builder *HorizontalPositionQuantum(Units::Length quantum); - Builder *VerticalPositionQuantum(Units::Length quantum); - Builder *HorizontalVelocityQuantum(Units::Speed quantum); - Builder *VerticalVelocityQuantum(Units::Speed quantum); - int GetNACp() const { return nacp_; } - int GetNACv() const { return nacv_; } - int GetNICp() const { return nicp_; } - int GetNICv() const { return nicv_; } - int GetUniqueId() const { return id_; } - Units::Time GetTimestamp() const { return timestamp_; } - Units::Length GetPositionEnuX() const { return position_x_; } - Units::Length GetPositionEnuY() const { return position_y_; } - Units::Length GetAltitudeMsl() const { return altitude_msl_; } - Units::Speed GetGroundSpeedEnuXd() const { return xd_; } - Units::Speed GetGroundSpeedEnuYd() const { return yd_; } - Units::Speed GetAltitudeRate() const { return altitude_rate_; } - Units::Length GetHorizontalPositionQuantum() const { return horizontal_position_quantum_; } - Units::Length GetVerticalPositionQuantum() const { return vertical_position_quantum_; } - Units::Speed GetHorizontalVelocityQuantum() const { return horizontal_velocity_quantum_; } - Units::Speed GetVerticalVelocityQuantum() const { return vertical_velocity_quantum_; } - bool HasPosition() const { return has_position_; } - bool HasVelocity() const { return has_velocity_; } - Units::Angle GetLatitude() const { return latitude_; } - Units::Angle GetLongitude() const { return longitude_; } - }; - - private: - ADSBSVReport(const Builder &builder); - - int m_id{-1}; - Units::SecondsTime m_time{Units::SecondsTime(-1)}; - int m_nacp{0}, m_nacv{0}, m_nicp{0}, m_nicv{0}; - bool m_has_position{false}, m_has_velocity{false}; - Units::FeetLength m_enu_x{Units::zero()}, m_enu_y{Units::zero()}, m_altitude_msl{Units::zero()}; - Units::Angle m_latitude{Units::zero()}, m_longitude{Units::zero()}; - Units::FeetPerSecondSpeed m_enu_xd{Units::zero()}, m_enu_yd{Units::zero()}, m_altitude_rate{Units::zero()}; - Units::FeetLength m_horizontal_position_quantum{Units::zero()}, m_vertical_position_quantum{Units::zero()}; - Units::FeetPerSecondSpeed m_horizontal_velocity_quantum{Units::zero()}, m_vertical_velocity_quantum{Units::zero()}; -}; - -inline bool ADSBSVReport::HasPosition() const { return m_has_position; } -inline bool ADSBSVReport::HasVelocity() const { return m_has_velocity; } -inline int ADSBSVReport::GetId() const { return m_id; } -inline int ADSBSVReport::GetNacp() const { return m_nacp; } -inline int ADSBSVReport::GetNacv() const { return m_nacv; } -inline int ADSBSVReport::GetNicp() const { return m_nicp; } -inline int ADSBSVReport::GetNicv() const { return m_nicv; } -inline Units::SecondsTime ADSBSVReport::GetTime() const { return m_time; } -inline Units::FeetLength ADSBSVReport::GetX() const { return m_enu_x; } -inline Units::FeetPerSecondSpeed ADSBSVReport::GetXd() const { return m_enu_xd; } -inline Units::FeetLength ADSBSVReport::GetY() const { return m_enu_y; } -inline Units::FeetPerSecondSpeed ADSBSVReport::GetYd() const { return m_enu_yd; } -inline Units::FeetLength ADSBSVReport::GetAltitudeMsl() const { return m_altitude_msl; } -inline Units::FeetPerSecondSpeed ADSBSVReport::GetVerticalSpeed() const { return m_altitude_rate; } -inline Units::Angle ADSBSVReport::GetLatitude() const { return m_latitude; } -inline Units::Angle ADSBSVReport::GetLongitude() const { return m_longitude; } - -} // namespace aaesim::open_source diff --git a/include/public/ADSBTransmitter.h b/include/public/ADSBTransmitter.h deleted file mode 100644 index 4afdc01..0000000 --- a/include/public/ADSBTransmitter.h +++ /dev/null @@ -1,50 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include -#include - -#include "public/ADSBSVReport.h" -#include "public/AircraftState.h" -#include "public/SimulationTime.h" -#include "public/Waypoint.h" - -namespace aaesim { -namespace open_source { - -static const Units::FeetLength ADS_B_HOR_POS_QUANT(7.83); -static const Units::FeetPerSecondSpeed ADS_B_HOR_VEL_QUANT(1.68); -static const Units::FeetLength ADS_B_VER_POS_QUANT(25.); -static const Units::FeetPerSecondSpeed ADS_B_VER_VEL_QUANT(3.5); -static const Units::FeetPerSecondSpeed CPR_SPD_QUANT(1.68); -static const Units::FeetLength CPR_LONG_QUANT(16.69); -static const Units::FeetLength CPR_LAT_QUANT(16.69); -static const Units::FeetPerSecondSpeed CPR_Z_RATE_QUANT(0.490); -static const Units::FeetLength CPR_ALT_QUANT(25.0); - -struct ADSBTransmitter { - virtual void Initialize(const std::list &waypoints_along_route) = 0; - virtual void Transmit(const aaesim::open_source::SimulationTime &simulation_time, - const aaesim::open_source::AircraftState &nav_measurement) = 0; - virtual const std::vector &GetAllTransmissions() const = 0; -}; -} // namespace open_source -} // namespace aaesim diff --git a/include/public/ASSAP.h b/include/public/ASSAP.h deleted file mode 100644 index a558840..0000000 --- a/include/public/ASSAP.h +++ /dev/null @@ -1,45 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include -#include - -#include "public/ADSBReceiver.h" -#include "public/ADSBSVReport.h" -#include "public/AircraftState.h" - -namespace aaesim { -namespace open_source { -struct ASSAP { // Airborne Surveillance & Separation Assurance Processing - virtual ~ASSAP() = default; - - virtual aaesim::open_source::AircraftState Update( - const aaesim::open_source::AircraftState &state_to_sync_with, - const aaesim::open_source::ADSBSVReport &most_recent_ads_b) const = 0; - - virtual std::shared_ptr GetAdsbReceiver() const = 0; - - virtual void Initialize(std::shared_ptr adsb_receiver) = 0; - - virtual const Units::SecondsTime GetMaxCoastTime() const = 0; -}; -} // namespace open_source -} // namespace aaesim diff --git a/include/public/AbstractAscentController.h b/include/public/AbstractAscentController.h deleted file mode 100644 index 4815000..0000000 --- a/include/public/AbstractAscentController.h +++ /dev/null @@ -1,65 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include - -#include "public/AscentController.h" - -namespace aaesim::open_source { -class AbstractAscentController : public AscentController, public VerticalController { - public: - AbstractAscentController() = default; - ~AbstractAscentController() = default; - void ComputeVerticalCommands(const Guidance &guidance, const EquationsOfMotionState &equations_of_motion_state, - std::shared_ptr &sensed_weather, - Units::Force &thrust_command, Units::Angle &gamma_command, Units::Speed &tas_command, - BoundedValue &speed_brake_command, - aaesim::open_source::bada_utils::FlapConfiguration &flap_command) override { - speed_brake_command = 0.0; - ComputeAscentCommands(guidance, equations_of_motion_state, sensed_weather, thrust_command, gamma_command, - tas_command, flap_command); - } - void Initialize( - std::shared_ptr &performance_calculator) override { - aircraft_performance_ = performance_calculator; - } - double GetSpeedBrakeGain() const override { return 0.0; }; - - protected: - std::shared_ptr aircraft_performance_{}; -}; - -class NullAscentController final : public AbstractAscentController { - public: - NullAscentController() = default; - ~NullAscentController() = default; - void ComputeAscentCommands(const Guidance &guidance, const EquationsOfMotionState &equations_of_motion_state, - std::shared_ptr &sensed_weather, - Units::Force &thrust_command, Units::Angle &gamma_command, Units::Speed &tas_command, - aaesim::open_source::bada_utils::FlapConfiguration &flap_command) override { - thrust_command = equations_of_motion_state.thrust; - gamma_command = equations_of_motion_state.gamma; - tas_command = equations_of_motion_state.true_airspeed; - flap_command = equations_of_motion_state.flap_configuration; - } -}; - -} // namespace aaesim::open_source diff --git a/include/public/AbstractDescentController.h b/include/public/AbstractDescentController.h deleted file mode 100644 index cbe65bc..0000000 --- a/include/public/AbstractDescentController.h +++ /dev/null @@ -1,66 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include -#include - -#include "public/FixedMassAircraftPerformance.h" -#include "public/SpeedBrakeController.h" -#include "public/TrueWeatherOperator.h" -#include "public/VerticalController.h" - -namespace aaesim::open_source { -class AbstractDescentController : public VerticalController { - public: - AbstractDescentController() = default; - ~AbstractDescentController() = default; - void Initialize( - std::shared_ptr &performance_calculator) override { - aircraft_performance_ = performance_calculator; - } - double GetSpeedBrakeGain() const override { return speed_brake_controller_->GetSpeedBrakeGain(); }; - - protected: - inline static void DoLogging(log4cplus::Logger &logger, const EquationsOfMotionState &state, Units::Length error_alt, - bool is_level_flight, Units::Force thrust_command, Units::Force max_thrust, - Units::Force min_thrust, Units::Speed error_tas, double speed_brake_command, - bada_utils::FlapConfiguration flap_configuration, Units::Angle gamma_command) { - if (logger.getLogLevel() == log4cplus::TRACE_LOG_LEVEL) { - using json = nlohmann::json; - json j; - j["altitude_error_ft"] = Units::FeetLength(error_alt).value(); - j["is_level_flight_bool"] = is_level_flight; - j["thrust_command_newtons"] = Units::NewtonsForce(thrust_command).value(); - j["dynamics_thrust_newtons"] = Units::NewtonsForce(state.thrust).value(); - j["max_thrust_newtons"] = Units::NewtonsForce(max_thrust).value(); - j["min_thrust_newtons"] = Units::NewtonsForce(min_thrust).value(); - j["true_airspeed_error_knots"] = Units::KnotsSpeed(error_tas).value(); - j["speed_brake_command"] = speed_brake_command; - j["flap_configuration"] = bada_utils::GetFlapConfigurationAsString(flap_configuration); - j["gamma_command_deg"] = Units::DegreesAngle(gamma_command).value(); - LOG4CPLUS_TRACE(logger, j.dump()); - } - }; - std::shared_ptr aircraft_performance_{}; - std::shared_ptr speed_brake_controller_{std::make_shared()}; -}; - -} // namespace aaesim::open_source diff --git a/include/public/AbstractTrueWeatherOperator.h b/include/public/AbstractTrueWeatherOperator.h deleted file mode 100644 index 220e919..0000000 --- a/include/public/AbstractTrueWeatherOperator.h +++ /dev/null @@ -1,44 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include - -#include "public/TrueWeatherOperator.h" -#include "public/WeatherTruth.h" - -namespace aaesim::open_source { -class AbstractTrueWeatherOperator : public TrueWeatherOperator { - public: - explicit AbstractTrueWeatherOperator(std::shared_ptr true_weather) - : m_true_weather{true_weather} {} - ~AbstractTrueWeatherOperator() = default; - Units::KelvinTemperature GetTemperature() const override { return m_true_weather->GetTemperature(); } - Units::Density GetDensity() const override { return m_true_weather->GetDensity(); } - Units::Pressure GetPressure() const override { return m_true_weather->GetPressure(); } - std::shared_ptr GetAtmosphere() const override { - throw std::runtime_error("AAES-1545: Do not call this method. Design error that needs to be fixed!"); - } - std::shared_ptr GetTrueWeather() const override { return m_true_weather; } - - protected: - std::shared_ptr m_true_weather{}; -}; -} // namespace aaesim::open_source diff --git a/include/public/AchieveObserver.h b/include/public/AchieveObserver.h deleted file mode 100644 index c95a26c..0000000 --- a/include/public/AchieveObserver.h +++ /dev/null @@ -1,55 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// 2022 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include -#include -#include - -// Holds data used to produce time to go metrics for the achieve by algorithms. - -class AchieveObserver -{ -public: - AchieveObserver(); - - AchieveObserver(const int iter, - const int aircraft_id, - const double tm, - const double target_ttg_to_ach, - const double own_ttg_to_ach, - const double curr_distance, - const double reference_distance); - - ~AchieveObserver(); - - const std::string Hdr(); - - std::string ToString(); - -private: - int m_iteration; - int m_id; // aircraft id - Units::SecondsTime m_time; - Units::Time m_targ_ttg_to_ach; - Units::Time m_own_ttg_to_ach; - Units::Length m_curr_dist; - Units::Length m_ref_dist; -}; diff --git a/include/public/Aircraft.h b/include/public/Aircraft.h deleted file mode 100644 index cf2eb82..0000000 --- a/include/public/Aircraft.h +++ /dev/null @@ -1,39 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// 2022 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include "public/WeatherTruth.h" -#include "public/LoggingLoadable.h" -#include "public/SimulationTime.h" - -namespace aaesim { -namespace open_source { -struct Aircraft : public LoggingLoadable { - Aircraft(void) = default; - - virtual ~Aircraft(void) = default; - - virtual void Initialize(const Units::Length adsb_reception_range_threshold, const WeatherTruth &weather_truth) = 0; - - virtual bool Update(const SimulationTime &simulation_time) = 0; -}; -} // namespace open_source - -} // namespace aaesim diff --git a/include/public/AircraftCalculations.h b/include/public/AircraftCalculations.h deleted file mode 100644 index 5ca29e6..0000000 --- a/include/public/AircraftCalculations.h +++ /dev/null @@ -1,149 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include -#include -#include -#include -#include - -#include - -#include "public/AircraftState.h" -#include "public/Atmosphere.h" -#include "public/HorizontalPath.h" - -namespace aaesim::open_source { - -class AircraftCalculations { - public: - /** - * Get the position and course of an Aircraft based on current distance and precalculated Horizontal Trajectory - * - * @deprecated Do not write new code that calls this. - * @see PositionCalculator - * @param distance_to_go for the position desired - * @param horizontal_trajectory on which distance_to_go is calculated - * @param x_position, an output - * @param y_position, an output - * @param course, an output - * @param traj_index, an output - * @return true if calculation succeeded, false otherwise - */ - static bool LegacyGetPositionFromPathLength(const Units::Length &distance_to_go, - const std::vector &horizontal_trajectory, - Units::Length &x_position, Units::Length &y_position, - Units::UnsignedAngle &course, int &traj_index); - - /** - * Get the distance and course of the Aircraft based on the current position and precalculated Horizontal Trajectory - * - * @deprecated Do not write new code that calls this. - * @see AlongPathDistanceCalculator - * @param x position for distance calculation - * @param y position for distance calculation - * @param horizontal_trajectory on which position exists and distance is to be calculated - * @param distance_along_path, an output - * @param course, an output - */ - static void LegacyGetPathLengthFromPosition(const Units::Length x, const Units::Length y, - const std::vector &horizontal_trajectory, - Units::Length &distance_along_path, Units::Angle &course); - - /** - * Get the distance and course of the Aircraft based on the current position and precalculated Horizontal Trajectory - * - * @param position_x for distance calculation - * @param position_y for distance calculation - * @param horizontal_trajectory that the position must be on and that will be used for distance calculation - * @param starting_trajectory_index, allows the caller to keep track of indices and make calls more efficient - * @param distance_along_path, an output - * @param course, an output - * @param resolved_trajectory_index, an output that indexes into horizontal_trajectory - * @return true if calculation succeeded, false otherwise - */ - static bool CalculateDistanceAlongPathFromPosition( - const Units::Length position_x, const Units::Length position_y, - const std::vector &horizontal_trajectory, - const std::vector::size_type starting_trajectory_index, Units::Length &distance_along_path, - Units::Angle &course, std::vector::size_type &resolved_trajectory_index); - - /** - * Intentionally shadows the same-named method. But this one exposes finer control over the cross track error - * tolerance used in the algorithm. If you don't have a specific reason to control the tolerance, plesae use - * the other method that sets the tolerance for you. - * - * @see AircraftCalculations::CalculateDistanceAlongPathFromPosition - * @param cross_track_tolerance - * @param position_x - * @param position_y - * @param horizontal_trajectory - * @param starting_trajectory_index - * @param distance_along_path - * @param course - * @param resolved_trajectory_index - * @return - */ - static bool CalculateDistanceAlongPathFromPosition( - const Units::Length cross_track_tolerance, const Units::Length position_x, const Units::Length position_y, - const std::vector &horizontal_trajectory, - const std::vector::size_type starting_trajectory_index, Units::Length &distance_along_path, - Units::Angle &course, std::vector::size_type &resolved_trajectory_index); - - /** - * @deprecated - * @see CoreUtils::CalculateEuclideanDistance() - */ - static Units::NauticalMilesLength PtToPtDist(Units::Length x0, Units::Length y0, Units::Length x1, Units::Length y1); - - /** - * Use the dot product rule to calculate the angle between vectors. The angle will be on the - * interval [-pi, pi]. - */ - static Units::SignedRadiansAngle ComputeAngleBetweenVectors(const Units::Length &xvertex, - const Units::Length &yvertex, const Units::Length &x1, - const Units::Length &y1, const Units::Length &x2, - const Units::Length &y2); - - /** - * Cross-product calculation of (p1-vertex) X (p2-vertex) - */ - static Units::Area ComputeCrossProduct(const Units::Length &xvertex, const Units::Length &yvertex, - const Units::Length &x1, const Units::Length &y1, const Units::Length &x2, - const Units::Length &y2); - - private: - static log4cplus::Logger logger; - - struct PathDistance { - std::vector::size_type m_horizontal_path_index; - Units::Length m_distance_to_path_node; - }; - - static std::vector ComputePathDistances(const Units::Length x, const Units::Length y, - const std::vector::size_type &starting_index, - const std::vector &hTraj); - - static void CrossTrackError(const Units::Length position_enu_x, const Units::Length position_enu_y, - int current_trajectory_index, const std::vector &horizontal_trajectory, - int &next_trajectory_index, Units::Length &cross_track_error); -}; -} // namespace aaesim::open_source diff --git a/include/public/AircraftControl.h b/include/public/AircraftControl.h deleted file mode 100644 index fc38510..0000000 --- a/include/public/AircraftControl.h +++ /dev/null @@ -1,136 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include -#include -#include - -#include "public/DefaultLateralController.h" -#include "public/EquationsOfMotionState.h" -#include "public/FixedMassAircraftPerformance.h" -#include "public/Guidance.h" -#include "public/LateralController.h" -#include "public/TrueWeatherOperator.h" -#include "public/VerticalController.h" - -namespace aaesim::open_source { -struct ControlCommands { - Units::Angle roll_angle_command{Units::zero()}; - Units::Force thrust_command{Units::zero()}; - Units::Angle flight_path_angle_command{Units::zero()}; - Units::Speed true_airspeed_command{Units::zero()}; - double speed_brake_command{0.0}; - aaesim::open_source::bada_utils::FlapConfiguration flap_configuration{ - aaesim::open_source::bada_utils::FlapConfiguration::UNDEFINED}; -}; - -struct ControlGains { - Units::Frequency k_flight_path_angle{Units::zero()}; - Units::Frequency k_thrust{Units::zero()}; - Units::Frequency k_roll{Units::zero()}; - double k_speed_brake{0.0}; -}; - -class AircraftControl final { - public: - AircraftControl( - const std::map, - std::shared_ptr>> &controller_pairs); - - virtual ~AircraftControl() = default; - - void Initialize(std::shared_ptr aircraft_performance); - - std::pair CalculateControlCommands( - const Guidance &guidance, const EquationsOfMotionState &equations_of_motion_state, - std::shared_ptr sensed_weather); - - class Builder { - public: - Builder &WithCruiseDescentVerticalController(std::shared_ptr ctrl) { - descent_vertical_controller_ = std::move(ctrl); - return *this; - } - Builder &WithTakeoffVerticalController(std::shared_ptr ctrl) { - takeoff_vertical_controller_ = std::move(ctrl); - return *this; - } - Builder &WithClimbVerticalController(std::shared_ptr ctrl) { - climb_vertical_controller_ = std::move(ctrl); - return *this; - } - Builder &WithTakeoffLateralController(std::shared_ptr ctrl) { - takeoff_lateral_controller_ = std::move(ctrl); - return *this; - } - Builder &WithClimbLateralController(std::shared_ptr ctrl) { - climb_lateral_controller_ = std::move(ctrl); - return *this; - } - Builder &WithCruiseDescentLateralController(std::shared_ptr ctrl) { - cruise_descent_lateral_controller_ = std::move(ctrl); - return *this; - } - std::shared_ptr Build() const { - std::map, - std::shared_ptr>> - controller_map; - - if (takeoff_lateral_controller_ && takeoff_vertical_controller_) { - controller_map.emplace(aaesim::open_source::GuidanceFlightPhase::TAKEOFF_ROLL, - std::make_pair(takeoff_lateral_controller_, takeoff_vertical_controller_)); - } - - if (climb_lateral_controller_ && climb_vertical_controller_) { - controller_map.emplace(aaesim::open_source::GuidanceFlightPhase::CLIMB, - std::make_pair(climb_lateral_controller_, climb_vertical_controller_)); - } - - if (cruise_descent_lateral_controller_ && descent_vertical_controller_) { - controller_map.emplace(aaesim::open_source::GuidanceFlightPhase::CRUISE_DESCENT, - std::make_pair(cruise_descent_lateral_controller_, descent_vertical_controller_)); - } - - if (controller_map.size() == 0) { - throw std::runtime_error( - "Configuration Error: AircraftControl cannot be built because there are no controllers provided"); - } - return std::make_shared(controller_map); - } - - private: - std::shared_ptr descent_vertical_controller_{}; - std::shared_ptr takeoff_vertical_controller_{}; - std::shared_ptr climb_vertical_controller_{}; - std::shared_ptr takeoff_lateral_controller_{}; - std::shared_ptr climb_lateral_controller_{}; - std::shared_ptr cruise_descent_lateral_controller_{}; - }; - - private: - std::map, - std::shared_ptr>> - controller_map_{}; -}; -} // namespace aaesim::open_source diff --git a/include/public/AircraftControllerFactory.h b/include/public/AircraftControllerFactory.h deleted file mode 100644 index b882c1b..0000000 --- a/include/public/AircraftControllerFactory.h +++ /dev/null @@ -1,98 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include - -#include "public/AircraftControl.h" -#include "public/AircraftIntent.h" -#include "public/BadaUtils.h" -#include "public/ClimbPhaseVerticalController.h" -#include "public/DefaultLateralController.h" -#include "public/SpeedOnPitchControl.h" -#include "public/SpeedOnThrustControl.h" -#include "public/TakeOffVerticalController.h" - -namespace aaesim::open_source { -class AircraftControllerFactory final { - public: - enum DescentSpeedControlStrategy { THRUST, PITCH, NONE }; - struct DescentSpeedControlConfig { - DescentSpeedControlStrategy type{DescentSpeedControlStrategy::NONE}; - Units::Speed speed_threshold{Units::KnotsSpeed{20.0}}; - Units::Length altitude_threshold{Units::FeetLength{500.0}}; - }; - struct AircraftControllerConfig { - DescentSpeedControlConfig descent_controller_config; - Units::Angle maximum_allowable_roll_angle{Units::DegreesAngle{30.0}}; - }; - static std::shared_ptr BuildController(const AircraftControllerConfig &config, - const AircraftIntent &aircraft_intent) { - auto lateral_controller = std::make_shared(config.maximum_allowable_roll_angle); - AircraftControl::Builder builder{}; - if (config.descent_controller_config.type == DescentSpeedControlStrategy::NONE and - aircraft_intent.ContainsDescentWaypoints()) { - throw std::runtime_error( - "Invalid vertical controller configuration for descent. Must specify the descent strategy"); - } - - if (aircraft_intent.ContainsDescentWaypoints() || aircraft_intent.ContainsCruiseWaypoints()) { - builder.WithCruiseDescentLateralController(lateral_controller); - if (config.descent_controller_config.type == THRUST) { - auto descent_controller = std::make_shared(); - builder.WithCruiseDescentVerticalController(descent_controller); - } else if (config.descent_controller_config.type == PITCH) { - auto descent_controller = std::make_shared( - config.descent_controller_config.speed_threshold, - config.descent_controller_config.altitude_threshold); - builder.WithCruiseDescentVerticalController(descent_controller); - } - } - - if (aircraft_intent.ContainsAscentWaypoints()) { - auto ascent_controller = std::make_shared(); - builder.WithClimbVerticalController(ascent_controller); - builder.WithClimbLateralController(lateral_controller); - builder.WithTakeoffLateralController(std::make_shared()); - builder.WithTakeoffVerticalController(std::make_shared()); - } - - return builder.Build(); - }; - static std::shared_ptr BuildForCruiseDescentOnly(const AircraftControllerConfig &config) { - if (config.descent_controller_config.type == DescentSpeedControlStrategy::NONE) { - throw std::runtime_error( - "Invalid vertical controller configuration for descent. Must specify the descent strategy"); - } - AircraftControl::Builder builder{}; - auto lateral_controller = std::make_shared(config.maximum_allowable_roll_angle); - builder.WithCruiseDescentLateralController(lateral_controller); - if (config.descent_controller_config.type == THRUST) { - auto descent_controller = std::make_shared(); - builder.WithCruiseDescentVerticalController(descent_controller); - } else if (config.descent_controller_config.type == PITCH) { - auto descent_controller = std::make_shared( - config.descent_controller_config.speed_threshold, config.descent_controller_config.altitude_threshold); - builder.WithCruiseDescentVerticalController(descent_controller); - } - return builder.Build(); - }; -}; -}; // namespace aaesim::open_source diff --git a/include/public/AircraftIntent.h b/include/public/AircraftIntent.h deleted file mode 100644 index 97cb9b3..0000000 --- a/include/public/AircraftIntent.h +++ /dev/null @@ -1,328 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -#include "nlohmann/json.hpp" -#include "public/TangentPlaneSequence.h" -#include "public/Waypoint.h" -#include "utility/BoundedValue.h" -#include "utility/UtilityConstants.h" - -namespace aaesim { -namespace loaders { -class AircraftIntentLoader; -} // namespace loaders -} // namespace aaesim - -class AircraftIntent { - public: - enum WaypointPhaseOfFlight { ASCENT, CRUISE, DESCENT }; - - enum Arinc424LegType { UNSET = -1, IF, VI, TF, RF, CF, VA, CA }; - - struct RouteData { - std::vector m_name{}; - std::vector m_waypoint_phase_of_flight{}; - std::vector m_x{}; - std::vector m_y{}; - std::vector m_z{}; - std::vector m_nominal_altitude{}; - std::vector m_latitude{}; - std::vector m_longitude{}; - std::vector m_nominal_ias{}; - std::vector m_high_altitude_constraint{}; - std::vector m_low_altitude_constraint{}; - std::vector m_high_speed_constraint{}; - std::vector m_low_speed_constraint{}; - std::vector m_leg_type{}; - std::vector m_x_rf_center{}; - std::vector m_y_rf_center{}; - std::vector m_rf_radius{}; - std::vector m_rf_latitude{}; - std::vector m_rf_longitude{}; - }; - - AircraftIntent(); - - virtual ~AircraftIntent() = default; - - AircraftIntent(const AircraftIntent &in); - - AircraftIntent &operator=(const AircraftIntent &in); - - bool operator==(const AircraftIntent &obj) const; - - void Copy(const AircraftIntent &in); - - void Initialize(); - - virtual void LoadWaypointsFromList(const std::list &ascent_waypoints, - const std::list &cruise_waypoints, - const std::list &descent_waypoints); - - void UpdateXYZFromLatLonWgs84(); - - std::list GetWaypointList() const; - - void GetLatLonFromXYZ(const Units::Length &xMeters, const Units::Length &yMeters, const Units::Length &zMeters, - Units::Angle &lat, Units::Angle &lon) const; - - void SetNumberOfWaypoints(unsigned int n); - - const Waypoint &GetWaypoint(unsigned int i) const; - - const std::string &GetWaypointName(unsigned int i) const; - - Units::MetersLength GetWaypointX(unsigned int i) const; - - Units::MetersLength GetWaypointY(unsigned int i) const; - - Units::MetersLength GetPlannedCruiseAltitude() const; - - void SetPlannedCruiseAltitude(Units::Length altitude); - - const RouteData &GetRouteData() const; - - int GetWaypointIndexByName(const std::string &waypoint_name) const; - - std::pair FindCommonWaypoint(const AircraftIntent &intent) const; - - void InsertPairAtIndex(const std::string &wpname, const Units::Length &x, const Units::Length &y, const int index); - - void InsertWaypointAtIndex(const Waypoint &waypoint, const int index); - - virtual void UpdateWaypoint(const Waypoint &waypoint); - - virtual void ClearWaypoints(); - - void SetId(int id_in); - - int GetId() const; - - unsigned int GetNumberOfWaypoints() const; - - bool IsLoaded() const; - - void Dump(std::ostream &fileOut) const; - - void DumpParms(const std::string &str) const; - - bool ContainsAscentWaypoints() const; - - const std::vector &GetAscentWaypoints() const; - - bool ContainsCruiseWaypoints() const; - - const std::vector &GetCruiseWaypoints() const; - - bool ContainsDescentWaypoints() const; - - const std::vector &GetDescentWaypoints() const; - - double GetPlannedCruiseMach() const; - - void SetPlannedCruiseMach(BoundedValue mach_number); - - bool ContainsWaypointName(const std::string &waypoint_name) const; - - /** - * @brief trim all waypoints after the named waypoint, returning a functional updated object - * - * @param aircraft_intent - * @param waypoint_name - * @return AircraftIntent - */ - static AircraftIntent CopyAndTrimAfterNamedWaypoint(const AircraftIntent &aircraft_intent, - const std::string &waypoint_name); - - protected: - static inline std::map m_arinc424_dictionary{ - {"IF", AircraftIntent::Arinc424LegType::IF}, {"UNSET", AircraftIntent::Arinc424LegType::UNSET}, - {"RF", AircraftIntent::Arinc424LegType::RF}, {"TF", AircraftIntent::Arinc424LegType::TF}, - {"VI", AircraftIntent::Arinc424LegType::VI}, {"CF", AircraftIntent::Arinc424LegType::CF}, - {"VA", AircraftIntent::Arinc424LegType::VA}, {"CA", AircraftIntent::Arinc424LegType::CA}, - }; - std::list AddConnectingLeg(const std::list &first_waypoint_vector, - const std::list &second_waypoint_vector) const; - - static std::list RemoveZeroLengthLegs(const std::list &waypoints); - - static std::vector ConvertListToVector(const std::list &waypoint_list); - - static std::list ConvertVectorToList(const std::vector &waypoint_vector); - - void ClearAndResetRouteDataContent(const std::vector &ascent_waypoints, - const std::vector &cruise_waypoints, - const std::vector &descent_waypoints); - - void DoRouteDataLogging() const; - - struct RouteData route_data_{}; - std::shared_ptr m_tangent_plane_sequence{}; - double planned_cruise_mach_{0}; - std::vector m_all_waypoints{}; - bool m_is_loaded{false}; - std::vector m_ascent_waypoints{}, m_cruise_waypoints{}, m_descent_waypoints{}; - Units::MetersLength m_planned_cruise_altitude{Units::ZERO_LENGTH}; - - private: - static inline log4cplus::Logger m_logger{log4cplus::Logger::getInstance(LOG4CPLUS_TEXT("AircraftIntent"))}; - - void DeleteRouteDataContent(); - void AddWaypointsToRouteDataVectors(const std::vector &waypoints, enum WaypointPhaseOfFlight add_as_phase); - friend class aaesim::loaders::AircraftIntentLoader; - friend std::ostream &operator<<(std::ostream &out, const AircraftIntent &intent); - int m_id{-1}; -}; - -inline const AircraftIntent::RouteData &AircraftIntent::GetRouteData() const { return route_data_; } - -inline int AircraftIntent::GetId() const { return m_id; } - -inline void AircraftIntent::SetId(int id_in) { m_id = id_in; } - -inline unsigned int AircraftIntent::GetNumberOfWaypoints() const { return route_data_.m_name.size(); } - -inline Units::MetersLength AircraftIntent::GetPlannedCruiseAltitude() const { return m_planned_cruise_altitude; } - -inline bool AircraftIntent::IsLoaded() const { return m_is_loaded; } - -inline std::list AircraftIntent::GetWaypointList() const { - return std::list(m_all_waypoints.begin(), m_all_waypoints.end()); -} - -inline std::vector AircraftIntent::ConvertListToVector(const std::list &waypoint_list) { - return std::vector(waypoint_list.begin(), waypoint_list.end()); -} - -inline std::list AircraftIntent::ConvertVectorToList(const std::vector &waypoint_vector) { - return std::list(waypoint_vector.begin(), waypoint_vector.end()); -} - -inline void AircraftIntent::AddWaypointsToRouteDataVectors(const std::vector &waypoints, - enum WaypointPhaseOfFlight add_as_phase) { - auto waypoint_itr = waypoints.begin(); - while (waypoint_itr != waypoints.end()) { - m_all_waypoints.push_back(*waypoint_itr); - route_data_.m_name.push_back(waypoint_itr->GetName()); - route_data_.m_waypoint_phase_of_flight.push_back(add_as_phase); - route_data_.m_nominal_altitude.emplace_back(waypoint_itr->GetAltitude()); - route_data_.m_latitude.emplace_back(waypoint_itr->GetLatitude()); - route_data_.m_longitude.emplace_back(waypoint_itr->GetLongitude()); - route_data_.m_nominal_ias.emplace_back(waypoint_itr->GetNominalIas()); - route_data_.m_leg_type.push_back(m_arinc424_dictionary[waypoint_itr->GetArinc424LegType()]); - route_data_.m_high_altitude_constraint.emplace_back(waypoint_itr->GetAltitudeConstraintHigh()); - route_data_.m_low_altitude_constraint.emplace_back(waypoint_itr->GetAltitudeConstraintLow()); - route_data_.m_high_speed_constraint.emplace_back(waypoint_itr->GetSpeedConstraintHigh()); - route_data_.m_low_speed_constraint.emplace_back(waypoint_itr->GetSpeedConstraintLow()); - route_data_.m_rf_latitude.emplace_back(waypoint_itr->GetRfTurnCenterLatitude()); - route_data_.m_rf_longitude.emplace_back(waypoint_itr->GetRfTurnCenterLongitude()); - route_data_.m_rf_radius.emplace_back(waypoint_itr->GetRfTurnArcRadius()); - - ++waypoint_itr; - } -} - -inline std::list AircraftIntent::AddConnectingLeg(const std::list &first_waypoint_vector, - const std::list &second_waypoint_vector) const { - if (first_waypoint_vector.empty()) - // nothing to do - return second_waypoint_vector; - if (!second_waypoint_vector.empty() && - (first_waypoint_vector.back().GetName() == second_waypoint_vector.front().GetName())) { - // nothing to do - return second_waypoint_vector; - } - - std::list updated_waypoints; - Waypoint to_copy(first_waypoint_vector.back()); - Waypoint new_tf_leg(to_copy.GetName() + "_copy_as_IF", to_copy.GetLatitude(), to_copy.GetLongitude(), - to_copy.GetAltitudeConstraintHigh(), to_copy.GetAltitudeConstraintLow(), - to_copy.GetSpeedConstraintHigh(), to_copy.GetAltitude(), to_copy.GetNominalIas(), "IF"); - updated_waypoints.push_back(new_tf_leg); - std::copy(second_waypoint_vector.begin(), second_waypoint_vector.end(), std::back_inserter(updated_waypoints)); - return updated_waypoints; -} - -std::ostream &operator<<(std::ostream &out, const AircraftIntent &intent); - -inline bool AircraftIntent::ContainsAscentWaypoints() const { return !m_ascent_waypoints.empty(); } - -inline bool AircraftIntent::ContainsCruiseWaypoints() const { return !m_cruise_waypoints.empty(); } - -inline bool AircraftIntent::ContainsDescentWaypoints() const { return !m_descent_waypoints.empty(); } - -inline double AircraftIntent::GetPlannedCruiseMach() const { return planned_cruise_mach_; } - -inline void AircraftIntent::DoRouteDataLogging() const { - using json = nlohmann::json; - if (m_logger.getLogLevel() == log4cplus::TRACE_LOG_LEVEL) { - for (auto idx = 0; idx < route_data_.m_name.size(); ++idx) { - json j; - j["segment_index"] = idx; - j["name"] = route_data_.m_name[idx]; - j["phase_of_flight_int"] = route_data_.m_waypoint_phase_of_flight[idx]; - j["x_position_m"] = Units::MetersLength(route_data_.m_x[idx]).value(); - j["y_position_m"] = Units::MetersLength(route_data_.m_y[idx]).value(); - j["z_position_m"] = Units::MetersLength(route_data_.m_z[idx]).value(); - j["nominal_alitude_ft"] = Units::FeetLength(route_data_.m_nominal_altitude[idx]).value(); - j["latitude_deg"] = Units::DegreesAngle(route_data_.m_latitude[idx]).value(); - j["longitude_deg"] = Units::DegreesAngle(route_data_.m_longitude[idx]).value(); - j["nominal_ias_kts"] = Units::KnotsSpeed(route_data_.m_nominal_ias[idx]).value(); - j["alt_high_ft"] = Units::FeetLength(route_data_.m_high_altitude_constraint[idx]).value(); - j["alt_low_ft"] = Units::FeetLength(route_data_.m_low_altitude_constraint[idx]).value(); - j["speed_high_knots"] = Units::KnotsSpeed(route_data_.m_high_speed_constraint[idx]).value(); - j["speed_low_knots"] = Units::KnotsSpeed(route_data_.m_low_speed_constraint[idx]).value(); - j["leg_type_int"] = route_data_.m_leg_type[idx]; - j["rf_turn_x_position_m"] = Units::MetersLength(route_data_.m_x_rf_center[idx]).value(); - j["rf_turn_y_position_m"] = Units::MetersLength(route_data_.m_y_rf_center[idx]).value(); - j["rf_turn_radius_nm"] = Units::NauticalMilesLength(route_data_.m_rf_radius[idx]).value(); - j["rf_turn_lat_deg"] = Units::SignedDegreesAngle(route_data_.m_rf_latitude[idx]).value(); - j["rf_turn_lon_deg"] = Units::SignedDegreesAngle(route_data_.m_rf_longitude[idx]).value(); - LOG4CPLUS_TRACE(m_logger, j.dump()); - } - } -} - -inline bool AircraftIntent::ContainsWaypointName(const std::string &waypoint_name) const { - auto name_comparator = [&waypoint_name](const Waypoint &waypoint_to_test) { - return waypoint_to_test.GetName().compare(waypoint_name) == 0; - }; - return std::any_of(m_all_waypoints.rbegin(), m_all_waypoints.rend(), name_comparator); -} - -inline const std::vector &AircraftIntent::GetAscentWaypoints() const { return m_ascent_waypoints; } - -inline const std::vector &AircraftIntent::GetDescentWaypoints() const { return m_descent_waypoints; } - -inline const std::vector &AircraftIntent::GetCruiseWaypoints() const { return m_cruise_waypoints; } diff --git a/include/public/AircraftIntentLoader.h b/include/public/AircraftIntentLoader.h deleted file mode 100644 index 0043efe..0000000 --- a/include/public/AircraftIntentLoader.h +++ /dev/null @@ -1,58 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include -#include -#include - -#include "public/AircraftIntent.h" - -namespace aaesim { -namespace loaders { - -class AircraftIntentLoader final : public LoggingLoadable { - public: - AircraftIntentLoader() = default; - ~AircraftIntentLoader() override = default; - - bool load(DecodedStream *input) override; - - const AircraftIntent &BuildAircraftIntent() const; - AircraftIntent &GetAircraftIntent(); - const AircraftIntent &GetAircraftIntent() const; - bool IsLoaded() const; - - private: - static inline log4cplus::Logger logger_{log4cplus::Logger::getInstance(LOG4CPLUS_TEXT("AircraftIntentLoader"))}; - - AircraftIntent aircraft_intent_{}; -}; - -inline const AircraftIntent &AircraftIntentLoader::BuildAircraftIntent() const { return aircraft_intent_; } - -inline AircraftIntent &AircraftIntentLoader::GetAircraftIntent() { return aircraft_intent_; } - -inline const AircraftIntent &AircraftIntentLoader::GetAircraftIntent() const { return aircraft_intent_; } - -inline bool AircraftIntentLoader::IsLoaded() const { return aircraft_intent_.IsLoaded(); } - -} // namespace loaders -} // namespace aaesim diff --git a/include/public/AircraftSpeed.h b/include/public/AircraftSpeed.h deleted file mode 100644 index 33d4908..0000000 --- a/include/public/AircraftSpeed.h +++ /dev/null @@ -1,60 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include - -#include "utility/BoundedValue.h" - -enum SpeedValueType { - UNSPECIFIED_SPEED, - INDICATED_AIR_SPEED, - MACH_SPEED, -}; - -namespace aaesim { -namespace open_source { -class AircraftSpeed final { - public: - static AircraftSpeed OfMach(const BoundedValue mach_value); - static AircraftSpeed OfIndicatedAirspeed(const Units::Speed ias); - - AircraftSpeed(); - virtual ~AircraftSpeed(); - SpeedValueType GetSpeedType() const; - double GetValue() const; - - private: - AircraftSpeed(const SpeedValueType type, const double value); - AircraftSpeed(const SpeedValueType type, const Units::Speed value); - void SetSpeed(const SpeedValueType type, const double value); - SpeedValueType m_speed_type; - double m_value; -}; - -inline AircraftSpeed AircraftSpeed::OfMach(const BoundedValue mach_value) { - return AircraftSpeed(MACH_SPEED, mach_value); -} - -inline AircraftSpeed AircraftSpeed::OfIndicatedAirspeed(const Units::Speed ias) { - return AircraftSpeed(INDICATED_AIR_SPEED, ias); -} -} // namespace open_source -} // namespace aaesim diff --git a/include/public/AircraftState.h b/include/public/AircraftState.h deleted file mode 100644 index e1df641..0000000 --- a/include/public/AircraftState.h +++ /dev/null @@ -1,209 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include "public/ADSBSVReport.h" -#include "public/BadaUtils.h" -#include "public/DynamicsState.h" - -namespace aaesim { -namespace open_source { - -class AircraftState final { - public: - static AircraftState FromAdsbReport(const ADSBSVReport &adsb_report); - - AircraftState() = default; - virtual ~AircraftState() = default; - - AircraftState &Interpolate(const AircraftState &a, const AircraftState &b, const double time); - AircraftState &Extrapolate(const AircraftState &in, const Units::SecondsTime &time); - - int GetUniqueId() const; - Units::Length GetPositionEnuX() const; - Units::Length GetPositionEnuY() const; - Units::Length GetAltitudeMsl() const; - Units::Speed GetSpeedEnuX() const; - Units::Speed GetSpeedEnuY() const; - Units::Speed GetVerticalSpeed() const; - Units::Speed GetTrueAirspeed() const; - Units::SignedAngle GetLatitude() const; - Units::SignedAngle GetLongitude() const; - Units::AngularSpeed GetLatitudeRate() const; - Units::AngularSpeed GetLongitudeRate() const; - Units::Angle GetFlightPathAngle() const; - Units::SecondsTime GetTime() const; - const aaesim::open_source::DynamicsState &GetDynamicsState() const; - Units::Angle GetPsi() const; - Units::UnsignedRadiansAngle GetHeadingCcwFromEastRadians() const; - Units::Speed GetGroundSpeed() const; - Units::Acceleration GetAccelerationEnuX() const; - Units::Acceleration GetAccelerationEnuY() const; - Units::Acceleration GetVerticalAcceleration() const; - Units::Temperature GetSensedTemperature() const; - Units::Speed GetSensedWindEast() const; - Units::Speed GetSensedWindNorth() const; - Units::Speed GetSensedWindParallel() const; - Units::Speed GetSensedWindPerpendicular() const; - Units::Frequency GetVerticalWindDerivativeEastComponent() const; - Units::Frequency GetVerticalWindDerivativeNorthComponent() const; - - class Builder { - private: - int id_{-1}; - Units::Time timestamp_{Units::SecondsTime(-1)}; - Units::FeetLength enu_x_{Units::zero()}, enu_y_{Units::zero()}, altitude_msl_{Units::zero()}; - Units::FeetPerSecondSpeed enu_east_{Units::zero()}, enu_north_{Units::zero()}, altitude_rate_{Units::zero()}; - Units::FeetSecondAcceleration enu_accel_east_{Units::zero()}, enu_accel_north_{Units::zero()}, - altitude_accel_{Units::zero()}; - Units::RadiansAngle flight_path_angle_{Units::zero()}; - Units::FeetPerSecondSpeed sensed_wind_east_{Units::zero()}, sensed_wind_north_{Units::zero()}; - Units::FeetPerSecondSpeed sensed_wind_parallel_{Units::zero()}, sensed_wind_perpendicular_{Units::zero()}; - Units::HertzFrequency sensed_wind_vertical_derivative_east_{Units::zero()}, - sensed_wind_vertical_derivative_north_{Units::zero()}; - Units::Temperature sensed_temperature_{Units::zero()}; - Units::KilogramsMeterDensity sensed_density_{Units::zero()}; - Units::AtmospheresPressure sensed_pressure_{Units::zero()}; - Units::SignedAngle latitude_{Units::zero()}, longitude_{Units::zero()}; - Units::AngularSpeed latitude_rate_{Units::zero()}, longitude_rate_{Units::zero()}; - Units::RadiansAngle psi_{Units::zero()}; - aaesim::open_source::DynamicsState dynamics_state_{}; - - public: - Builder(int unique_acid, int time_since_epoch_seconds); - Builder(int unique_acid, Units::Time timestamp); - explicit Builder(const AircraftState &state_to_copy); - ~Builder() = default; - AircraftState Build(); - Builder *Position(Units::FeetLength enu_x, Units::FeetLength enu_y); - Builder *AltitudeMsl(Units::FeetLength altitude_msl); - Builder *GroundSpeed(Units::FeetPerSecondSpeed enu_east, Units::FeetPerSecondSpeed enu_north); - Builder *AltitudeRate(Units::FeetPerSecondSpeed altitude_rate); - Builder *GroundAcceleration(Units::FeetSecondAcceleration enu_east, Units::FeetSecondAcceleration enu_north); - Builder *AltitudeAcceleration(Units::FeetSecondAcceleration altitude_acceleration); - Builder *FlightPathAngle(Units::SignedAngle fpa); - Builder *SensedWindComponents(Units::Speed east, Units::Speed north); - Builder *VerticalWindDerivatives(Units::Frequency east, Units::Frequency north); - Builder *SensedTemperature(Units::Temperature temperature); - Builder *SensedDensity(Units::Density density); - Builder *SensedPressure(Units::Pressure pressure); - Builder *Latitude(Units::SignedAngle latitude); - Builder *Longitude(Units::SignedAngle longitude); - Builder *LatitudeRate(Units::AngularSpeed latitude_rate); - Builder *LongitudeRate(Units::AngularSpeed longitude_rate); - Builder *DynamicsState(const aaesim::open_source::DynamicsState &dynamics_state); - Builder *Psi(Units::Angle psi); - Builder *SensedWindsPerpendicular(Units::Speed wind_perpendicular_component); - Builder *SensedWindsParallel(Units::Speed wind_parallel_component); - - int GetUniqueId() const { return id_; } - Units::SecondsTime GetTimestamp() const { return timestamp_; } - Units::FeetLength GetPositionEnuX() const { return enu_x_; } - Units::FeetLength GetPositionEnuY() const { return enu_y_; } - Units::FeetLength GetAltitudeMsl() const { return altitude_msl_; } - Units::FeetPerSecondSpeed GetGroundSpeedEast() const { return enu_east_; } - Units::FeetPerSecondSpeed GetGroundSpeedNorth() const { return enu_north_; } - Units::FeetPerSecondSpeed GetAltitudeRate() const { return altitude_rate_; } - Units::FeetSecondAcceleration GetGroundAccelerationEastComponent() const { return enu_accel_east_; }; - Units::FeetSecondAcceleration GetGroundAccelerationNorthComponent() const { return enu_accel_north_; }; - Units::FeetSecondAcceleration GetAltitudeAcceleration() const { return altitude_accel_; }; - Units::SignedRadiansAngle GetFlightPathAngle() const { return flight_path_angle_; }; - Units::MetersPerSecondSpeed GetSensedWindEastComponent() const { return sensed_wind_east_; }; - Units::MetersPerSecondSpeed GetSensedWindNorthComponent() const { return sensed_wind_north_; }; - Units::MetersPerSecondSpeed GetSensedWindParallelComponent() const { return sensed_wind_parallel_; }; - Units::MetersPerSecondSpeed GetSensedWindPerpendicularComponent() const { return sensed_wind_perpendicular_; }; - Units::Frequency GetVerticalWindDerivativeEastComponent() const { return sensed_wind_vertical_derivative_east_; }; - Units::Frequency GetVerticalWindDerivativeNorthComponent() const { - return sensed_wind_vertical_derivative_north_; - }; - Units::Temperature GetSensedTemperature() const { return sensed_temperature_; }; - Units::Density GetSensedDensity() const { return sensed_density_; }; - Units::Pressure GetSensedPressure() const { return sensed_pressure_; }; - Units::SignedAngle GetLatitude() const { return latitude_; }; - Units::SignedAngle GetLongitude() const { return longitude_; }; - Units::AngularSpeed GetLatitudeRate() const { return latitude_rate_; }; - Units::AngularSpeed GetLongitudeRate() const { return longitude_rate_; }; - const aaesim::open_source::DynamicsState &GetDynamicsState() const { return dynamics_state_; }; - Units::Angle GetPsi() const { return psi_; }; - }; - - private: - inline static log4cplus::Logger logger{log4cplus::Logger::getInstance(LOG4CPLUS_TEXT("AircraftState"))}; - - AircraftState(const Builder &builder); - - int m_id{-1}; - Units::SecondsTime m_time{-1}; - Units::FeetLength m_x{0}, m_y{0}, m_z{0}; - Units::FeetPerSecondSpeed m_xd{0}, m_yd{0}, m_zd{0}; - Units::FeetSecondAcceleration m_xdd{0}, m_ydd{0}, m_zdd{0}; - Units::MetersPerSecondSpeed m_sensed_wind_east{0}, m_sensed_wind_north{0}; - Units::RadiansAngle m_psi{Units::zero()}; - Units::RadiansAngle m_gamma{0}; - Units::MetersPerSecondSpeed m_sensed_wind_parallel{0}, m_sensed_wind_perpendicular{0}; - Units::Frequency m_Vwx_dh{Units::zero()}, m_Vwy_dh{Units::zero()}; - Units::Temperature m_sensed_temperature{Units::zero()}; - Units::Density m_sensed_density{Units::zero()}; - Units::Pressure m_sensed_pressure{Units::zero()}; - Units::SignedAngle m_latitude{Units::zero()}, m_longitude{Units::zero()}; - Units::AngularSpeed m_latitude_rate{Units::zero()}, m_longitude_rate{Units::zero()}; - aaesim::open_source::DynamicsState m_dynamics_state{}; -}; - -inline Units::Length AircraftState::GetPositionEnuX() const { return m_x; } -inline Units::Length AircraftState::GetPositionEnuY() const { return m_y; } -inline Units::Length AircraftState::GetAltitudeMsl() const { return m_z; } -inline Units::Speed AircraftState::GetSpeedEnuX() const { return m_xd; } -inline Units::Speed AircraftState::GetSpeedEnuY() const { return m_yd; } -inline Units::Speed AircraftState::GetVerticalSpeed() const { return m_zd; } -inline Units::SignedAngle AircraftState::GetLatitude() const { return m_latitude; } -inline Units::SignedAngle AircraftState::GetLongitude() const { return m_longitude; } -inline Units::AngularSpeed AircraftState::GetLatitudeRate() const { return m_latitude_rate; } -inline Units::AngularSpeed AircraftState::GetLongitudeRate() const { return m_longitude_rate; } -inline const aaesim::open_source::DynamicsState &AircraftState::GetDynamicsState() const { return m_dynamics_state; } -inline Units::SecondsTime AircraftState::GetTime() const { return m_time; } -inline int AircraftState::GetUniqueId() const { return m_id; } -inline Units::Angle AircraftState::GetPsi() const { return m_psi; } -inline Units::Angle AircraftState::GetFlightPathAngle() const { return m_gamma; } -inline Units::Acceleration AircraftState::GetAccelerationEnuX() const { return m_xdd; } -inline Units::Acceleration AircraftState::GetAccelerationEnuY() const { return m_ydd; } -inline Units::Acceleration AircraftState::GetVerticalAcceleration() const { return m_zdd; } -inline Units::Temperature AircraftState::GetSensedTemperature() const { return m_sensed_temperature; } -inline Units::Speed AircraftState::GetSensedWindEast() const { return m_sensed_wind_east; } -inline Units::Speed AircraftState::GetSensedWindNorth() const { return m_sensed_wind_north; } -inline Units::Speed AircraftState::GetSensedWindParallel() const { return m_sensed_wind_parallel; } -inline Units::Speed AircraftState::GetSensedWindPerpendicular() const { return m_sensed_wind_perpendicular; } -inline Units::Frequency AircraftState::GetVerticalWindDerivativeEastComponent() const { return m_Vwx_dh; } -inline Units::Frequency AircraftState::GetVerticalWindDerivativeNorthComponent() const { return m_Vwy_dh; } - -} // namespace open_source -} // namespace aaesim diff --git a/include/public/AlongPathDistanceCalculator.h b/include/public/AlongPathDistanceCalculator.h deleted file mode 100644 index d42e309..0000000 --- a/include/public/AlongPathDistanceCalculator.h +++ /dev/null @@ -1,80 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include - -#include - -#include "HorizontalPath.h" -#include "HorizontalPathTracker.h" - -namespace aaesim::open_source { - -/** - * Calculates distance along a horizontal path and ensures that the progression of - */ -class AlongPathDistanceCalculator : public HorizontalPathTracker { - public: - static AlongPathDistanceCalculator CreateForCaptureClearance(const std::vector &horizontal_path); - AlongPathDistanceCalculator(); - AlongPathDistanceCalculator(const std::vector &horizontal_path, - TrajectoryIndexProgressionDirection expected_index_progression); - - AlongPathDistanceCalculator(const std::vector &horizontal_path, - TrajectoryIndexProgressionDirection expected_index_progression, - bool use_large_cross_track_tolerance); - - virtual ~AlongPathDistanceCalculator(); - - /** - * Calculate a distance along path of the horizontal trajectory for a given position. - */ - bool CalculateAlongPathDistanceFromPosition(const Units::Length position_x, const Units::Length position_y, - Units::Length &distance_along_path, Units::UnsignedAngle &course); - - /** - * @brief Same as other public method, but this one does not return course. This is here for convenience of callers - * that do not want the course returned. - */ - bool CalculateAlongPathDistanceFromPosition(const Units::Length position_x, const Units::Length position_y, - Units::Length &distance_along_path); - - /** - * @brief Same as previous but includes the course into the next waypoint if in a turn. Used for Capture - * IM-clearance. - */ - bool CalculateAlongPathDistanceFromPosition(const Units::Length position_x, const Units::Length position_y, - Units::Length &distance_along_path, Units::UnsignedAngle &course, - Units::UnsignedAngle &pt_to_pt_course); - - void UpdateHorizontalTrajectory(const std::vector &horizontal_trajectory) override; - - private: - static log4cplus::Logger m_logger; - static Units::Length CROSS_TRACK_TOLERANCE, EXTENDED_CROSS_TRACK_TOLERANCE, CAPTURE_CROSS_TRACK_TOLERANCE; - bool m_is_first_call; - Units::NauticalMilesLength m_cross_track_tolerance; - // private constructor used to construct an AlongPathDistanceCalculator when clearance is CAPTURE - AlongPathDistanceCalculator(const std::vector &horizontal_path, - TrajectoryIndexProgressionDirection expected_index_progression, - Units::Length specified_cross_track_tolerance); -}; -} // namespace aaesim::open_source diff --git a/include/public/ArcOnEllipsoid.h b/include/public/ArcOnEllipsoid.h deleted file mode 100644 index cd276c4..0000000 --- a/include/public/ArcOnEllipsoid.h +++ /dev/null @@ -1,103 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include - -#include "public/LatitudeLongitudePoint.h" -#include "public/ShapeOnEllipsoid.h" - -namespace aaesim { -class ArcOnEllipsoid final : public ShapeOnEllipsoid { - public: - ArcOnEllipsoid() = default; - - ArcOnEllipsoid(const geolib_idealab::Arc &arc); - - kShapeType GetShapeType() const override; - - Units::Length GetShapeLength() const override; - - Units::SignedAngle GetArcAngularExtent() const; - - Units::SignedAngle GetCourseEnuTangentToStartPoint() const; - - Units::SignedAngle GetForwardCourseEnuAtStartPoint() const override; - - Units::SignedAngle GetCourseEnuTangentToEndPoint() const; - - Units::SignedAngle GetForwardCourseEnuAtEndPoint() const override; - - LatitudeLongitudePoint GetCenterPoint() const; - - LatitudeLongitudePoint GetStartPoint() const override; - - LatitudeLongitudePoint GetEndPoint() const override; - - Units::Length GetRadius() const; - - /** - * @return the angle, in ENU coords, that points from the center point to the start point of the arc. - */ - Units::SignedAngle GetStartAzimuthEnu() const; - - /** - * @return the angle, in ENU coords, that points from the center point to the end point of the arc. - */ - Units::SignedAngle GetEndAzimuthEnu() const; - - geolib_idealab::ArcDirection GetArcDirection() const; - - bool IsPointOnShape(const LatitudeLongitudePoint &test_point) const override; - - bool IsPointInsideArc(const LatitudeLongitudePoint &test_point) const; - - kDirectionRelativeToShape GetRelativeDirection( - const LatitudeLongitudePoint &latitude_longitude_point) const override; - - Units::Length GetDistanceToEndPoint(const LatitudeLongitudePoint &latitude_longitude_point) const override; - - LatitudeLongitudePoint GetNearestPointOnShape(const LatitudeLongitudePoint &latitude_longitude_point) const override; - - LatitudeLongitudePoint CalculatePointAtDistanceFromStartPoint( - const Units::Length &distance_along_shape_from_start_point) const override; - - std::pair CalculateCourseAtDistanceFromStartPoint( - const Units::Length &distance_along_shape_from_start_point) const override; - - protected: - Units::Length CalculateDistanceFromPointOnShapeToEnd(const LatitudeLongitudePoint &point_on_shape) const override; - - private: - static log4cplus::Logger m_logger; - geolib_idealab::Arc m_arc_primitive{}; -}; - -inline Units::SignedAngle ArcOnEllipsoid::GetForwardCourseEnuAtStartPoint() const { - return GetCourseEnuTangentToStartPoint(); -} - -inline Units::SignedAngle ArcOnEllipsoid::GetForwardCourseEnuAtEndPoint() const { - return GetCourseEnuTangentToEndPoint(); -} - -inline ShapeOnEllipsoid::kShapeType ArcOnEllipsoid::GetShapeType() const { return ARC; } - -} // namespace aaesim diff --git a/include/public/AscentController.h b/include/public/AscentController.h deleted file mode 100644 index 987f460..0000000 --- a/include/public/AscentController.h +++ /dev/null @@ -1,34 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include - -#include "public/VerticalController.h" - -namespace aaesim::open_source { -struct AscentController { - virtual void ComputeAscentCommands(const Guidance &guidance, const EquationsOfMotionState &equations_of_motion_state, - std::shared_ptr &sensed_weather, - Units::Force &thrust_command, Units::Angle &gamma_command, - Units::Speed &tas_command, - aaesim::open_source::bada_utils::FlapConfiguration &flap_command) = 0; -}; -} // namespace aaesim::open_source diff --git a/include/public/Atmosphere.h b/include/public/Atmosphere.h deleted file mode 100644 index ae33130..0000000 --- a/include/public/Atmosphere.h +++ /dev/null @@ -1,147 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include -#include -#include -#include -#include -#include - -#include "public/WindStack.h" -#include "utility/CustomUnits.h" - -// Standard air pressure at sea level -const Units::PascalsPressure P0_ISA(101325.); - -// Standard Density at Sea Level -// BADA_37_USER_MANUAL eq. 3.2-8 -const Units::KilogramsMeterDensity RHO0_ISA(1.225); - -// Speed of Sound at sea level -const Units::MetersPerSecondSpeed A0(340.29); - -// Isentropic expansion coefficient for air -constexpr double kGamma = 1.4; - -// mu -constexpr double MU = ((kGamma - 1) / (kGamma)); - -// Real gas constant (m^2/K-s^2) -const Units::MetersSecondsKelvinGasConstant R(287.05287); - -// Temperature gradient (K/m) -const Units::KelvinPerMeter K_T(-0.0065); - -class Atmosphere { - public: - enum AtmosphereType { UNKNOWN = 0, BADA37, BADA312, BADA316 }; - - Atmosphere() {} - - virtual ~Atmosphere() = default; - - virtual Atmosphere *Clone() const = 0; - - virtual void SetTemperatureOffset(const Units::Temperature temperature_offset) { - m_temperature_offset = temperature_offset; - } - - Units::CelsiusTemperature GetTemperatureOffset() const { return m_temperature_offset; } - - /** - * Calculate the temperature offset which will yield the specified temperature - * at the specified altitude and then call SetTemperatureOffset. - */ - virtual void CalibrateTemperatureAtAltitude(const Units::KelvinTemperature temperature, - const Units::Length altitude) = 0; - - virtual Units::KelvinTemperature GetTemperature(const Units::Length altitude_msl) const = 0; - - virtual void AirDensity(const Units::Length h, Units::Density &rho, Units::Pressure &P) const = 0; - - virtual Units::Speed CAS2TAS(const Units::Speed vcas, const Units::Pressure p, const Units::Density rho) const = 0; - - Units::Speed CAS2TAS(const Units::Speed vcas, const Units::Length alt) const { - // Get the air density - Units::KilogramsMeterDensity rho; - Units::Pressure p; - - AirDensity(alt, rho, p); - - Units::Speed vtas = CAS2TAS(vcas, p, rho); - return vtas; - } - - virtual Units::Speed TAS2CAS(const Units::Speed vtas, const Units::Pressure p, const Units::Density rho) const = 0; - - Units::Speed TAS2CAS(const Units::Speed vtas, const Units::Length alt) const { - // Get the air density - Units::KilogramsMeterDensity rho; - Units::Pressure p; - - AirDensity(alt, rho, p); - - Units::Speed vcas = TAS2CAS(vtas, p, rho); - return vcas; - } - - virtual Units::Length GetMachIASTransition(const Units::Speed ias, const double mach) const = 0; - - Units::Speed MachToIAS(const double mach, const Units::Length alt) const { - Units::Speed tas = mach * SpeedOfSound(alt); - - return TAS2CAS(tas, alt); - } - - double IASToMach(const Units::Speed ias, const Units::Length alt) const { - Units::Speed tas = CAS2TAS(ias, alt); - - return tas / SpeedOfSound(alt); - } - - virtual Units::Speed SpeedOfSound(Units::KelvinTemperature temperature) const = 0; - - Units::Speed SpeedOfSound(Units::Length altitude) const { - Units::KelvinTemperature temperature = GetTemperature(altitude); - return SpeedOfSound(temperature); - } - - virtual Units::KelvinTemperature GetSeaLevelTemperature() const = 0; - - virtual Units::MetersLength GetTropopauseHeight() const = 0; - virtual Units::Pressure GetTropopausePressure() const = 0; - - /** - * Calculate the Calibrated Airspeed BADA Energy Share Factor - */ - virtual double ESFconstantCAS(const Units::Speed true_airspeed, const Units::Length altitude_msl, - const Units::KelvinTemperature temperature) const = 0; - - protected: - Units::Temperature m_temperature_offset; - - void AirDensity_Log(const Units::MetersLength h, const Units::KelvinTemperature t, const Units::PascalsPressure p, - const Units::KilogramsMeterDensity rho) const; - - private: - static log4cplus::Logger m_logger; -}; diff --git a/include/public/BadaUtils.h b/include/public/BadaUtils.h deleted file mode 100644 index bd1dab6..0000000 --- a/include/public/BadaUtils.h +++ /dev/null @@ -1,261 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include - -#include -#include - -#include "utility/CustomUnits.h" - -namespace aaesim { -namespace open_source { -namespace bada_utils { - -enum ENGINE_TYPE { JET, TURBOPROP, PISTON }; - -enum WAKE_CATEGORY { HEAVY_, MEDIUM_, LIGHT_ }; - -const int FL_NMAX(40); - -enum FlapConfiguration { - UNDEFINED = -1, - TAKEOFF = 0, - INITIAL_CLIMB = 1, - CRUISE = 2, - APPROACH = 3, - LANDING = 4, - GEAR_DOWN = 5 -}; - -static std::string GetFlapConfigurationAsString(FlapConfiguration flap_configuration) { - switch (flap_configuration) { - case UNDEFINED: - return "UNDEFINED"; - case TAKEOFF: - return "TAKEOFF"; - case INITIAL_CLIMB: - return "INITIAL_CLIMB"; - case CRUISE: - return "CRUISE"; - case APPROACH: - return "APPROACH"; - case LANDING: - return "LANDING"; - case GEAR_DOWN: - return "GEAR_DOWN"; - default: - throw std::runtime_error("Invalid flap_configuration encountered: " + std::to_string(flap_configuration)); - } -} - -struct climb_struct { - int FL{0}; - Units::Speed TAS{}; - struct { - Units::Speed lo{}; - Units::Speed nom{}; - Units::Speed hi{}; - } ROCD{}; - Units::MassFlowRate fuel{}; -}; - -struct cruise_struct { - int FL{0}; - Units::Speed TAS{}; - struct { - Units::MassFlowRate lo{}; - Units::MassFlowRate nom{}; - Units::MassFlowRate hi{}; - } fuel{}; -}; - -struct descent_struct { - int FL{0}; - Units::Speed TAS{}; - Units::Speed ROCD{}; - Units::MassFlowRate fuel{}; -}; - -struct perf_speed_struct { - struct { - int LO{0}; - int HI{0}; - } CAS{}; - double Mach{0}; -}; - -enum EngineThrustMode { MAXIMUM_CLIMB = 0, MAXIMUM_CRUISE, DESCENT }; - -struct FlightEnvelope { - Units::Speed V_mo{}; // maximum operating speed (CAS). - double M_mo{0}; // maximum operating Mach number - Units::Length h_mo{}; // maximum operating altitude - Units::Length h_max{}; // maximum altitude at MTOW and ISA - Units::LengthToMassGradient G_w{}; // weight gradient on max. altitude - double G_t{0}; // temperature gradient on max. altitude (feet/C) -}; - -struct FlapSpeeds { - Units::Speed cas_approach_minimum{}; - Units::Speed cas_approach_maximum{}; - Units::Speed cas_landing_minimum{}; - Units::Speed cas_landing_maximum{}; - Units::Speed cas_gear_out_minimum{}; - Units::Speed cas_gear_out_maximum{}; - Units::Speed cas_takeoff_minimum{}; - Units::Speed cas_climb_minimum{}; - Units::Speed cas_cruise_minimum{}; -}; - -struct Mass { - Units::Mass m_ref{}; // reference mass - Units::Mass m_min{}; // minimum mass - Units::Mass m_max{}; // maximum mass - Units::Mass m_pyld{}; // maximum payload mass -}; - -struct Aerodynamics { - Units::Area S{}; // Reference wing surface area. - double C_Lbo{0}; // Buffet onset lift coeff. (jet only) - double K{0}; // Buffeting gradient (jet only) - double C_M16{0}; // No idea, but it's in the OPF - - struct { - Units::Speed V_stall{}; // CAS - double cd0{0}; // parasitic drag coeff. - double cd2{0}; // induced drag coeff. - } cruise{}; - - struct { - Units::Speed V_stall{}; // CAS - double cd0{0}; // parasitic drag coeff. - double cd2{0}; // induced drag coeff. - } initial_climb{}; - - struct { - Units::Speed V_stall{}; // CAS - double cd0{0}; // parasitic drag coeff. - double cd2{0}; // induced drag coeff. - } take_off{}; - - struct { - Units::Speed V_stall{}; // CAS - double cd0{0}; // parasitic drag coeff. - double cd2{0}; // induced drag coeff. - } approach{}; - - struct { - Units::Speed V_stall{}; // CAS - double cd0{0}; // parasitic drag coeff. - double cd2{0}; // induced drag coeff. - } landing{}; - - struct { - double cd0{0}; // parasitic drag coeff. - } landing_gear{}; -}; - -struct EngineThrust { - struct { - double CT_c1{0}; // 1st max. climb thrust coeff. (N, jet/piston) - // (kt-N, turboprop) - Units::Length CT_c2{}; // 2nd max. climb thrust coeff. - double CT_c3{0}; // 3rd max. climb thrust coeff. (1/feet^2, jet) - // (N, turboprop) - // (kt-N, piston) - Units::AbsCelsiusTemperature CT_c4{}; // 1st thrust temperature coeff. - double CT_c5{0}; // 2nd thrust temperature coeff. (1/deg. C) - } max_climb{}; - struct { - double CT_low{0}; // low altitude descent thrust coeff. - double CT_high{0}; // high altitude descent thrust coeff. - Units::Length h{}; // Transition altitude (feet) - double CT_app{0}; // approach thrust coeff. - double CT_ld{0}; // landing thrust coeff. - Units::Speed V_ref{}; // reference descent speed (kt) - double M_ref{0}; // reference descent Mach number - } descent{}; -}; - -struct AircraftType { - int n_eng; // number of engines - ENGINE_TYPE engine_type; - WAKE_CATEGORY wake_category; -}; - -struct FuelFlow { - double C_f1{0}; // 1st Thrust specific fuel consumption coeff. - // (kg/min*kN) (jet) - // (kg/min*kN*knot) (turboprop) - // (kg/min) (piston) - Units::Speed C_f2{}; // 2nd Thrust specific fuel consumption coeff. - Units::MassFlowRate C_f3{}; // 1st descent thrust fuel flow coeff. - Units::Length C_f4{}; // 2nd descent thrust fuel flow coeff. - double C_fcr{0}; // Cruise fuel flow coeff. (dimensionless) -}; - -struct GroundMovement { - Units::Length TOL{}; // Take-off length (m) - Units::Length LDL{}; // Landing length (m) - Units::Length span{}; // Wingspan (m) - Units::Length length{}; // Length (m) -}; - -struct AircraftPerformance { - struct { - perf_speed_struct climb{}; - perf_speed_struct cruise{}; - perf_speed_struct descent{}; - } speed{}; - - struct { - Units::Mass low{}; - Units::Mass nominal{}; - Units::Mass high{}; - } mass{}; - - cruise_struct cruise[FL_NMAX]{}; - climb_struct climb[FL_NMAX]{}; - descent_struct descent[FL_NMAX]{}; -}; - -struct Procedure { - struct { - int V1; - int V2; - int M; - } climb; - struct { - int V1; - int V2; - int M; - } cruise; - struct { - int V1; - int V2; - int M; - } descent; -}; - -} // namespace bada_utils -} // namespace open_source -} // namespace aaesim diff --git a/include/public/BlendWindsVerticallyByAltitude.h b/include/public/BlendWindsVerticallyByAltitude.h deleted file mode 100644 index 4b6f3a4..0000000 --- a/include/public/BlendWindsVerticallyByAltitude.h +++ /dev/null @@ -1,40 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include - -#include "public/WindBlendingAlgorithm.h" - -namespace aaesim::open_source { -class BlendWindsVerticallyByAltitude final : public WindBlendingAlgorithm { - private: - inline static const Units::FeetLength MAXIMUM_ALTITUDE_LIMIT{45000}; - inline static const Units::FeetLength MINIMUM_ALTITUDE_LIMIT{0}; - inline static const Units::FeetLength BLEND_HEIGHT{5000}; - - public: - BlendWindsVerticallyByAltitude() = default; - ~BlendWindsVerticallyByAltitude() = default; - void BlendSensedWithPredicted(const aaesim::open_source::AircraftState ¤t_state, - aaesim::open_source::WeatherPrediction &weather_prediction) override; -}; - -} // namespace aaesim::open_source diff --git a/include/public/CalcWindGradControl.h b/include/public/CalcWindGradControl.h deleted file mode 100644 index 8654558..0000000 --- a/include/public/CalcWindGradControl.h +++ /dev/null @@ -1,67 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include -#include -#include - -#include - -#include "public/WeatherPrediction.h" - -namespace aaesim { -namespace open_source { -class CalcWindGradControl { - public: - CalcWindGradControl(); - - virtual ~CalcWindGradControl(); - - /** - * Returns xy wind direction and gradient. To cut down unnecessary calls - * to Atmosphere::calcWindGrad, the stored previous calculation is checked - * for match. If inputs match, stored outputs returned. Else - * Atmosphere::calcWindGrad is called to get new wind direction and gradient - * and the new calculation values are stored. - * - * @param msl_altitude - * @param weather_prediction - * @param wind_speed_x output - * @param wind_speed_y output - * @param wind_gradient_x output - * @param wind_gradient_y output - */ - void ComputeWindGradients(const Units::Length &msl_altitude, const WeatherPrediction &weather_prediction, - Units::Speed &wind_speed_x, Units::Speed &wind_speed_y, Units::Frequency &wind_gradient_x, - Units::Frequency &wind_gradient_y); - - private: - aaesim::open_source::WindStack m_wind_x; - aaesim::open_source::WindStack m_wind_y; - - Units::Length m_altitude; - Units::Speed m_wind_speed_x; - Units::Speed m_wind_speed_y; - Units::Frequency m_wind_gradient_x; - Units::Frequency m_wind_gradient_y; -}; -} // namespace open_source -} // namespace aaesim diff --git a/include/public/ClimbPhaseVerticalController.h b/include/public/ClimbPhaseVerticalController.h deleted file mode 100644 index 282c835..0000000 --- a/include/public/ClimbPhaseVerticalController.h +++ /dev/null @@ -1,63 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include -#include - -#include -#include - -#include "public/AbstractAscentController.h" -#include "public/FixedMassAircraftPerformance.h" - -namespace aaesim::open_source { -class ClimbPhaseVerticalController final : public AbstractAscentController { - public: - ClimbPhaseVerticalController() = default; - ~ClimbPhaseVerticalController() = default; - void ComputeAscentCommands(const Guidance &guidance, const EquationsOfMotionState &equations_of_motion_state, - std::shared_ptr &sensed_weather, - Units::Force &thrust_command, Units::Angle &gamma_command, Units::Speed &tas_command, - aaesim::open_source::bada_utils::FlapConfiguration &flap_command) override; - - private: - inline static log4cplus::Logger logger_{log4cplus::Logger::getInstance("ClimbPhaseVerticalController")}; - static void DoLogging(const Units::Length &error_alt, const Units::Force &thrust_command, const Units::Force &thrust, - const Units::Force &min_thrust, const Units::Force &max_thrust, - const bada_utils::FlapConfiguration &flap_configuration, const Units::Speed &error_tas, - const Units::Speed &tas_command, const Units::Angle &gamma_command) { - if (logger_.getLogLevel() == log4cplus::TRACE_LOG_LEVEL) { - using json = nlohmann::json; - json j; - j["altitude_error"] = Units::FeetLength(error_alt).value(); - j["thrust_command"] = Units::NewtonsForce(thrust_command).value(); - j["dynamics_thrust"] = Units::NewtonsForce(thrust).value(); - j["max_thrust"] = Units::NewtonsForce(max_thrust).value(); - j["min_thrust"] = Units::NewtonsForce(min_thrust).value(); - j["new_flap_configuration"] = bada_utils::GetFlapConfigurationAsString(flap_configuration); - j["true_airspeed_error"] = Units::KnotsSpeed(error_tas).value(); - j["true_airspeed_command"] = Units::KnotsSpeed(tas_command).value(); - j["gamma_command"] = Units::DegreesAngle(gamma_command).value(); - LOG4CPLUS_TRACE(logger_, j.dump()); - } - } -}; -} // namespace aaesim::open_source diff --git a/include/public/ClosestPointMetric.h b/include/public/ClosestPointMetric.h deleted file mode 100644 index e12998f..0000000 --- a/include/public/ClosestPointMetric.h +++ /dev/null @@ -1,57 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// 2022 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include - - -// Class used to compute the closest point metric, which is used to determine -// closest point between the IM and target aircrafts. - -class ClosestPointMetric -{ -public: - ClosestPointMetric(void); - - ~ClosestPointMetric(void); - - // Computes distance for input position and updates minimum - // position if less than minimum distance. - void update(double imx, - double imy, - double targx, - double targy); - - Units::Length getMinDist(); - - void SetAcIds(int im_ac_id, int target_ac_id); - int GetImAcId() const; - bool IsReportMetrics() const; - int GetTargetAcId() const; - -private: - int m_im_ac_id; - int m_target_ac_id; - bool m_report_metrics; - - // Minimum distance between IM and target aircraft. - Units::Length mMinDist; - -}; diff --git a/include/public/ConfigurationFileReader.h b/include/public/ConfigurationFileReader.h deleted file mode 100644 index c3445db..0000000 --- a/include/public/ConfigurationFileReader.h +++ /dev/null @@ -1,38 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include - -#include -#include -#include - -namespace aaesim::open_source { -class ConfigurationFileReader { - public: - ~ConfigurationFileReader() = default; - static const std::vector LoadConfigurationFile(const std::string &suggested_filename); - - private: - ConfigurationFileReader() = default; - static log4cplus::Logger m_logger; -}; -} // namespace aaesim::open_source diff --git a/include/public/ControlCommands.h b/include/public/ControlCommands.h deleted file mode 100644 index 3199989..0000000 --- a/include/public/ControlCommands.h +++ /dev/null @@ -1,58 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// 2023 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include -#include -#include -#include "BadaUtils.h" - -class ControlCommands { - public: - ControlCommands(const Units::Angle &phi, const Units::Force &thrust, const Units::Angle &gamma, - const Units::Speed &trueAirspeed, const double speedBrake, - const aaesim::open_source::bada_utils::FlapConfiguration flapMode) - : phi(phi), - thrust(thrust), - gamma(gamma), - trueAirspeed(trueAirspeed), - speedBrake(speedBrake), - flapMode(flapMode) {} - - const Units::Angle &getPhi() const { return phi; } - - const Units::Force &getThrust() const { return thrust; } - - const Units::Angle &getGamma() const { return gamma; } - - const Units::Speed &getTrueAirspeed() const { return trueAirspeed; } - - double getSpeedBrake() const { return speedBrake; } - - aaesim::open_source::bada_utils::FlapConfiguration getFlapMode() const { return flapMode; } - - private: - Units::Angle phi; - Units::Force thrust; - Units::Angle gamma; - Units::Speed trueAirspeed; - double speedBrake; - aaesim::open_source::bada_utils::FlapConfiguration flapMode; -}; diff --git a/include/public/CoreUtils.h b/include/public/CoreUtils.h deleted file mode 100644 index 8b4ee54..0000000 --- a/include/public/CoreUtils.h +++ /dev/null @@ -1,152 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -#include "public/AircraftIntent.h" -#include "public/AircraftState.h" -#include "public/HorizontalPath.h" -#include "public/LineOnEllipsoid.h" - -class CoreUtils { - public: - inline static const std::string INTERMEDIATE_WAYPOINT_ROOT_NAME{"intermediate"}; - - /** - * Find the index of a value in a vector. Uses STL upper_bound(), but with one modified return. - * - * @param value_to_find: value to search for - * @param vector_to_search: vector to be searched - * @return upper index that bounds. Will never grow larger than size()-1. - */ - static int FindNearestIndex(const double &value_to_find, const std::vector &vector_to_search); - - /** - * Linearly interpolate between items in vector. Caller must have already calculated where the interpolation - * should start from. - * - * @param upper_index upper index for start of interpolation, see CoreUtils::FindNearestIndex() - * @param x_interpolation_value value to interpolate to - * @param x_values - * @param y_values - * @throws if upper_index is not within the range of value_vector - * @return a y-value the linearly corresponds to x_interpolation_value - */ - static double LinearlyInterpolate(int upper_index, double x_interpolation_value, const std::vector &x_values, - const std::vector &y_values); - - /** - * Linear interpolator for speed-typed y_values. - * - * @see LinearlyInterpolate - */ - static Units::Speed LinearlyInterpolate(int upper_index, Units::Length x_interpolation_value, - const std::vector &x_values, - const std::vector &y_values); - - /** - * @param xyLoc1: first x,y pair - * @param xyLoc2 second x,y pair - * @return The Euclidean straight line distance between the two points. - */ - static const Units::Length CalculateEuclideanDistance(const std::pair &xyLoc1, - const std::pair &xyLoc2); - - /** - * Limit value on the exclusive range of (low_limit, high_limit). - * - * @param value - * @param low_limit default is double min - * @param high_limit default is double max - * @return the limited value - */ - static const double LimitOnInterval(double value, double low_limit = std::numeric_limits::min(), - double high_limit = std::numeric_limits::max()); - - /** - * @param value - * @return 0 for zero input value, -1 for negative values, 1 for positive values - */ - static const int SignOfValue(double value); - - /** - * - * @param ordered_waypoints - * @param maximum_allowable_length, default is CoreUtils::MAXIMUM_ALLOWABLE_SINGLE_LEG_LENGTH - * @see CoreUtils::MAXIMUM_ALLOWABLE_SINGLE_LEG_LENGTH - * @return - */ - static std::list ShortenLongLegs( - const std::list &ordered_waypoints, - Units::Length maximum_allowable_length = MAXIMUM_ALLOWABLE_SINGLE_LEG_LENGTH); - - /** - * Visible for testing. - * - * Update the static parameter value. - * - * @param new_value - */ - static void UpdateMaximumAllowableSingleLegLength(Units::Length new_value); - - /** - * Visible for testing. - */ - static void ResetMaximumAllowableSingleLegLength(); - - /** - * @brief Find out if ptr is of type typename. - * - * @tparam Base - * @tparam T - * @param ptr - * @return true - * @return false - */ - template - inline static bool InstanceOf(const T *ptr) { - return dynamic_cast(ptr) != nullptr; - } - - private: - inline static log4cplus::Logger m_logger{log4cplus::Logger::getInstance(LOG4CPLUS_TEXT("CoreUtils"))}; - inline static Units::NauticalMilesLength MAXIMUM_ALLOWABLE_SINGLE_LEG_LENGTH{Units::infinity()}; - - static std::list GetIntermediateWaypointsForLongLeg(const aaesim::LineOnEllipsoid &line_on_ellipsoid, - Units::Length maximum_allowable_single_leg_distance); -}; - -inline void CoreUtils::UpdateMaximumAllowableSingleLegLength(Units::Length new_value) { - MAXIMUM_ALLOWABLE_SINGLE_LEG_LENGTH = new_value; -} - -inline void CoreUtils::ResetMaximumAllowableSingleLegLength() { - MAXIMUM_ALLOWABLE_SINGLE_LEG_LENGTH = Units::NauticalMilesLength(Units::infinity()); -} diff --git a/include/public/CrossTrackObserver.h b/include/public/CrossTrackObserver.h deleted file mode 100644 index 9d41985..0000000 --- a/include/public/CrossTrackObserver.h +++ /dev/null @@ -1,40 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// 2022 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -class CrossTrackObserver -{ -public: - CrossTrackObserver(void); - - ~CrossTrackObserver(void); - - double time; - double x; - double y; - double dynamic_cross; - double commanded_cross; - double unmodified_cross; - double psi_command; - double phi; - double limited_phi; - double reported_distance; -}; - diff --git a/include/public/CustomMath.h b/include/public/CustomMath.h deleted file mode 100644 index 6da3f37..0000000 --- a/include/public/CustomMath.h +++ /dev/null @@ -1,47 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include -#include -#include -#include - -#include "public/DMatrix.h" -#include "public/DVector.h" - -double atan3(double x, - double y); // arc tangent from 0 - 2pi -double quantize(double value, - double lsb); // quantizes value to lsb - -Units::Length quantize(Units::Length value, Units::Length lsb); - -Units::Speed quantize(Units::Speed value, Units::Speed lsb); - -Units::Time quantize(Units::Time value, Units::Time lsb); - -double subtract_headings(double hd1, double hd2); - -bool inverse(DMatrix &in, int n, DMatrix &inverse); - -void matrix_times_vector(DMatrix &matrix_in, DVector &vector_in, int n, DVector &vector_out); - -DMatrix &CreateRotationMatrix(double l, double m, double n, const Units::Angle theta); diff --git a/include/public/DMatrix.h b/include/public/DMatrix.h deleted file mode 100644 index a448005..0000000 --- a/include/public/DMatrix.h +++ /dev/null @@ -1,83 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include - -#include "public/DVector.h" - -class DMatrix { - public: - class IncompatibleDimensionsException : public std::exception { - public: - explicit IncompatibleDimensionsException(char *explanation); - - virtual ~IncompatibleDimensionsException() throw(); - - virtual const char *what() const throw(); - - private: - const char *m_explanation; - }; - - DMatrix(); - - virtual ~DMatrix(); - - DMatrix(const DMatrix &in); - - DMatrix(double **array_in, int row_min, int row_max, int column_min, int column_max); - - DMatrix(int row_min, int row_max, int column_min, int column_max); - - double Get(const int row, const int column) const; - - void Set(const int row, const int column, const double value); - - void AscendSort(); - - void SetBounds(int row_min, int row_max, int column_min, int column_max); - - bool InRange(const int row, const int colomn) const; - - bool InRange(const int row) const; - - DVector &operator[](const int); - - const DVector &operator[](const int) const; - - DMatrix &operator=(const DMatrix &in); - - DMatrix &operator*(const DMatrix &that) const; - - int GetMinRow() const; - - int GetMaxRow() const; - - int GetMinColumn() const; - - int GetMaxColumn() const; - - private: - static char *MULTIPLICATION_DIMENSIONS_MESSAGE; - DVector *m_rows; - int m_min_row; - int m_max_row; -}; diff --git a/include/public/DVector.h b/include/public/DVector.h deleted file mode 100644 index 8d78edb..0000000 --- a/include/public/DVector.h +++ /dev/null @@ -1,60 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#ifndef DVECTOR_H -#define DVECTOR_H - -class DVector { - public: - DVector(); - - DVector(int min, int max); - - DVector(const DVector &in); - - virtual ~DVector(); - - double Get(int index); - - void Set(int index, double value); - - void SetBounds(int min, int max); - - bool IsIndexInRange(int index) const; - - int GetMin(); - - int GetMax(); - - double &operator[](int); - - const double &operator[](int) const; - - DVector &operator=(const DVector &in); - - bool operator<(const DVector &other) const; - - private: - int m_min_index; - int m_max_index; - double *m_vector; -}; -#endif diff --git a/include/public/DataReader.h b/include/public/DataReader.h deleted file mode 100644 index fb23b2d..0000000 --- a/include/public/DataReader.h +++ /dev/null @@ -1,73 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -/* - * DataReader.h - * - * Reads any .csv file - * - * Created on: Mar 23, 2019 - * Author: klewis - */ - -#pragma once - -#include -#include - -#include -#include -#include -#include - -#include "utility/CsvParser.h" - -namespace aaesim { -namespace open_source { - -class DataReader { - public: - static const Units::SecondsTime UNDEFINED_TIME; - DataReader() = default; - DataReader(const std::string &file_name, int header_lines, size_t expected_columns); - DataReader(std::shared_ptr input_stream, int header_lines, size_t expected_columns); - virtual ~DataReader(); - void OpenFile(std::string file_name, int header_lines); - void OpenStream(std::shared_ptr input_stream, int header_lines); - virtual bool Advance(); - double GetDouble(int column) const; - std::string GetString(int column) const; - size_t GetColumnCount() const; - - protected: - void BuildColumnIndex(); - int GetColumnNumber(const std::string &column_name); - void SetExpectedColumnCount(size_t expected_column_count); - void SkipLines(int header_lines); - - private: - static log4cplus::Logger m_logger; - std::shared_ptr m_input_stream; - size_t m_expected_column_count{0}; - CsvParser::CsvRow m_csv_row; - std::map m_column_index; -}; - -} // namespace open_source -} // namespace aaesim diff --git a/include/public/DefaultLateralController.h b/include/public/DefaultLateralController.h deleted file mode 100644 index ac38980..0000000 --- a/include/public/DefaultLateralController.h +++ /dev/null @@ -1,56 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include -#include - -#include -#include - -#include "public/LateralController.h" - -namespace aaesim::open_source { -class DefaultLateralController final : public LateralController { - public: - DefaultLateralController(const Units::Angle &max_bank_angle) : max_bank_angle_{max_bank_angle} {}; - virtual ~DefaultLateralController() = default; - Units::Angle ComputeRollCommand( - const Guidance &guidance, const EquationsOfMotionState &equations_of_motion_state, - std::shared_ptr &sensed_weather) override; - Units::Frequency GetRollGain() const override { return gain_phi_; } - - private: - inline static const log4cplus::Logger logger_{log4cplus::Logger::getInstance("DefaultLateralController")}; - inline static const Units::Frequency gain_phi_{Units::HertzFrequency(0.40)}; - static void DoLogging(const Units::Length &cross_track_error, const Units::Angle &track_angle_error, - const Units::Angle roll_command) { - if (logger_.getLogLevel() == log4cplus::TRACE_LOG_LEVEL) { - using json = nlohmann::json; - json j; - j["cross_track_error_ft"] = Units::FeetLength(cross_track_error).value(); - j["track_angle_error_deg"] = Units::DegreesAngle(track_angle_error).value(); - j["roll_command_deg"] = Units::DegreesAngle(roll_command).value(); - LOG4CPLUS_TRACE(logger_, j.dump()); - } - } - Units::Angle max_bank_angle_{}; -}; -} // namespace aaesim::open_source diff --git a/include/public/DirectionOfFlightCourseCalculator.h b/include/public/DirectionOfFlightCourseCalculator.h deleted file mode 100644 index eb6a592..0000000 --- a/include/public/DirectionOfFlightCourseCalculator.h +++ /dev/null @@ -1,72 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include - -#include - -namespace aaesim::open_source { - -/** - * @brief Calculates the forward course along a horizontal path. "Forward" is defined as the direction of flight. - */ -class DirectionOfFlightCourseCalculator : public HorizontalPathTracker { - public: - DirectionOfFlightCourseCalculator(); - DirectionOfFlightCourseCalculator(const std::vector &horizontal_path, - TrajectoryIndexProgressionDirection expected_index_progression); - virtual ~DirectionOfFlightCourseCalculator(); - - /** - * @brief Calculate the course on the horizontal path at a specific distance along the path. - */ - bool CalculateCourseAtAlongPathDistance(const Units::Length &distance_along_path, - Units::UnsignedAngle &forward_course); - - /** - * @brief The course at index zero of the horizontal path. - */ - Units::UnsignedAngle GetCourseAtPathEnd() const; - - /** - * @brief The course at index (size-1) of the horizontal path. - */ - Units::UnsignedAngle GetCourseAtPathStart() const; - - private: - static log4cplus::Logger m_logger; - Units::UnsignedAngle m_end_course; - Units::UnsignedAngle m_start_course; - - protected: - bool CalculateForwardCourse(const Units::Length &distance_along_path, - const std::vector &horizontal_trajectory, - const std::vector::size_type starting_trajectory_index, - Units::UnsignedAngle &forward_course, Units::Angle &turn_theta, - Units::Length &turn_radius, - std::vector::size_type &resolved_trajectory_index); -}; - -inline Units::UnsignedAngle DirectionOfFlightCourseCalculator::GetCourseAtPathEnd() const { return m_end_course; } - -inline Units::UnsignedAngle DirectionOfFlightCourseCalculator::GetCourseAtPathStart() const { return m_start_course; } - -} // namespace aaesim::open_source diff --git a/include/public/DynamicsObserver.h b/include/public/DynamicsObserver.h deleted file mode 100644 index b65bb4c..0000000 --- a/include/public/DynamicsObserver.h +++ /dev/null @@ -1,37 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// 2022 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -class DynamicsObserver -{ -public: - DynamicsObserver(void); - - ~DynamicsObserver(void); - - bool operator<(const DynamicsObserver &dyn_in) const; - - int iter; - int id; - double time; - double achieved_groundspeed; - double speed_command; - double IAS_command; -}; diff --git a/include/public/DynamicsState.h b/include/public/DynamicsState.h deleted file mode 100644 index 7cef5af..0000000 --- a/include/public/DynamicsState.h +++ /dev/null @@ -1,57 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include -#include -#include -#include -#include - -#include "BadaUtils.h" -#include "utility/CustomUnits.h" - -namespace aaesim { -namespace open_source { - -struct DynamicsState { - // This structure is not used at all within the EOM function. It only serves to - // represent the state outside the EOM function. Within the EOM function, the state is - // represented with EquationsOfMotionState. - int id{0}; - Units::MetersLength h{}; - Units::MetersPerSecondSpeed v_true_airspeed{}; - Units::KnotsSpeed v_indicated_airspeed{}; - double mach{0}; - Units::SignedAngle psi{}; - Units::RadiansAngle phi{}; - Units::RadiansAngle gamma; // aircraft flight-path angle (rad) NOTE: for gamma, heading down is positive; heading - // up is negative - Units::NewtonsForce thrust{}; - Units::MetersPerSecondSpeed xd{}; - Units::MetersPerSecondSpeed yd{}; - double speed_brake{0}; // % of deployment - aaesim::open_source::bada_utils::FlapConfiguration flap_configuration{ - aaesim::open_source::bada_utils::FlapConfiguration::UNDEFINED}; - Units::Mass current_mass{}; - Units::AbsCelsiusTemperature true_temperature{}; -}; -} // namespace open_source -} // namespace aaesim diff --git a/include/public/EarthModel.h b/include/public/EarthModel.h deleted file mode 100644 index 17ce614..0000000 --- a/include/public/EarthModel.h +++ /dev/null @@ -1,150 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -/* - * EarthModel.h - * - * Created on: Jun 25, 2015 - * Author: klewis - */ - -#pragma once - -#include -#include - -#include - -#include "public/AircraftState.h" -#include "public/Waypoint.h" -#include "utility/CustomUnits.h" - -// can't include LocalTangentPlane.h here because of mutual dependency -class LocalTangentPlane; - -/** - * EarthModel is a base class for a singleton that converts between - * geodetic and ECEF coordinate systems. It also acts as a factory - * for EnuConverter, which converts between ECEF and ENU coordinates. - */ -class EarthModel { - public: - class GeodeticPosition { - public: - Units::SignedAngle latitude, longitude; - Units::Length altitude; - static EarthModel::GeodeticPosition CreateFromWaypoint(const Waypoint &waypoint) { - EarthModel::GeodeticPosition return_this; - return_this.altitude = Units::zero(); - return_this.latitude = waypoint.GetLatitude(); - return_this.longitude = waypoint.GetLongitude(); - return return_this; - } - static EarthModel::GeodeticPosition Of(Units::SignedAngle latitude, Units::SignedAngle longitude) { - EarthModel::GeodeticPosition return_this; - return_this.altitude = Units::zero(); - return_this.latitude = latitude; - return_this.longitude = longitude; - return return_this; - } - }; - - class AbsolutePositionEcef { - public: - Units::MetersLength x, y, z; - AbsolutePositionEcef ToUnitVector() const { - AbsolutePositionEcef unit_vector; - const Units::MetersLength vector_mag = Units::sqrt(Units::sqr(x) + Units::sqr(y) + Units::sqr(z)); - unit_vector.x = Units::MetersLength(x / vector_mag); - unit_vector.y = Units::MetersLength(y / vector_mag); - unit_vector.z = Units::MetersLength(z / vector_mag); - return unit_vector; - } - }; - - class LocalPositionEnu { - public: - static LocalPositionEnu Of(Units::Length x, Units::Length y, Units::Length z); - static LocalPositionEnu Of(const aaesim::open_source::AircraftState &state); - static LocalPositionEnu OfZeros(); - Units::Length x{}, y{}, z{}; - }; - - virtual ~EarthModel(); - - virtual void ConvertGeodeticToAbsolute(const EarthModel::GeodeticPosition &geo, - EarthModel::AbsolutePositionEcef &ecef) const = 0; - - virtual void ConvertAbsoluteToGeodetic(const EarthModel::AbsolutePositionEcef &ecef, - EarthModel::GeodeticPosition &geo) const = 0; - - virtual std::shared_ptr MakeEnuConverter(const GeodeticPosition &pointOfTangencyGeo, - const LocalPositionEnu &pointOfTangencyEnu) const = 0; - - protected: - EarthModel(); -}; - -std::ostream &operator<<(std::ostream &out, const EarthModel::GeodeticPosition &geo); -std::ostream &operator<<(std::ostream &out, const EarthModel::LocalPositionEnu &local); - -inline Units::Length VectorDotProduct(const EarthModel::AbsolutePositionEcef &a, - const EarthModel::AbsolutePositionEcef &b) { - Units::MetersLength dp_result = a.x * b.x.value() + a.y * b.y.value() + a.z * b.z.value(); - return dp_result; -} - -inline EarthModel::AbsolutePositionEcef VectorCrossProduct(const EarthModel::AbsolutePositionEcef &a, - const EarthModel::AbsolutePositionEcef &b) { - EarthModel::AbsolutePositionEcef cp_result; - cp_result.x = a.y * b.z.value() - a.z * b.y.value(); - cp_result.y = a.z * b.x.value() - a.x * b.z.value(); - cp_result.z = a.x * b.y.value() - a.y * b.x.value(); - return cp_result; -} - -inline EarthModel::AbsolutePositionEcef VectorDifference(const EarthModel::AbsolutePositionEcef &a, - const EarthModel::AbsolutePositionEcef &b) { - EarthModel::AbsolutePositionEcef result; - result.x = a.x - b.x; - result.y = a.y - b.y; - result.z = a.z - b.z; - return result; -} - -inline EarthModel::LocalPositionEnu EarthModel::LocalPositionEnu::Of(Units::Length x, Units::Length y, - Units::Length z) { - LocalPositionEnu lpe; - lpe.x = x; - lpe.y = y; - lpe.z = z; - return lpe; -} - -inline EarthModel::LocalPositionEnu EarthModel::LocalPositionEnu::Of(const aaesim::open_source::AircraftState &state) { - return Of(state.GetPositionEnuX(), state.GetPositionEnuY(), state.GetAltitudeMsl()); -} - -inline EarthModel::LocalPositionEnu EarthModel::LocalPositionEnu::OfZeros() { - LocalPositionEnu lpe; - lpe.x = Units::zero(); - lpe.y = Units::zero(); - lpe.z = Units::zero(); - return lpe; -} diff --git a/include/public/EllipsoidalEarthModel.h b/include/public/EllipsoidalEarthModel.h deleted file mode 100644 index f0e23f3..0000000 --- a/include/public/EllipsoidalEarthModel.h +++ /dev/null @@ -1,55 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include -#include -#include - -#include - -#include "public/EarthModel.h" -#include "public/LocalTangentPlane.h" -#include "public/WGS84EarthModelConstants.h" - -class EllipsoidalEarthModel final : public EarthModel { - public: - EllipsoidalEarthModel() - : m_semi_major_radius_squared(aaesim::open_source::kWgs84SemiMajorAxis * - aaesim::open_source::kWgs84SemiMajorAxis), - m_eccentricity_4(aaesim::open_source::kWgs84EccentricitySquared * - aaesim::open_source::kWgs84EccentricitySquared) {} - - ~EllipsoidalEarthModel() = default; - - void ConvertGeodeticToAbsolute(const EarthModel::GeodeticPosition &geo, - EarthModel::AbsolutePositionEcef &ecef) const override; - - void ConvertAbsoluteToGeodetic(const EarthModel::AbsolutePositionEcef &ecef, - EarthModel::GeodeticPosition &geo) const override; - - std::shared_ptr MakeEnuConverter(const GeodeticPosition &pointOfTangencyGeo, - const LocalPositionEnu &pointOfTangencyEnu) const override; - - private: - inline static log4cplus::Logger m_logger{log4cplus::Logger::getInstance(LOG4CPLUS_TEXT("EllipsoidalEarthModel"))}; - const Units::Area m_semi_major_radius_squared; - const double m_eccentricity_4; -}; diff --git a/include/public/EllipsoidalPositionEstimator.h b/include/public/EllipsoidalPositionEstimator.h deleted file mode 100644 index 5679bf8..0000000 --- a/include/public/EllipsoidalPositionEstimator.h +++ /dev/null @@ -1,40 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include - -#include "public/EarthModel.h" -#include "public/EquationsOfMotionState.h" -#include "public/EquationsOfMotionStateDeriv.h" -#include "public/SimulationTime.h" - -namespace aaesim::open_source { -struct LatLonDerivative { - Units::AngularSpeed latitude_time_derivative; - Units::AngularSpeed longitude_time_derivative; -}; - -struct EllipsoidalPositionEstimator { - virtual void ComputePosition(const SimulationTime &simtime, const EquationsOfMotionState &eqm_state, - const EquationsOfMotionStateDeriv &eqm_state_derivative, - EarthModel::GeodeticPosition &position, LatLonDerivative &position_rate) = 0; -}; -} // namespace aaesim::open_source diff --git a/include/public/EnvReader.h b/include/public/EnvReader.h deleted file mode 100644 index c873ffd..0000000 --- a/include/public/EnvReader.h +++ /dev/null @@ -1,51 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// 2022 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -/* - * EnvReader.h - * - * Reads an ENV.csv file containing a sequence of local weather observations - * - * Created on: Mar 23, 2019 - * Author: klewis - */ - -#pragma once - -#include "public/DataReader.h" - -namespace testvector { - -class EnvReader : public DataReader { -public: - static const size_t EXPECTED_ENV_COLUMN_COUNT; - EnvReader(std::string file_name, int header_lines); - EnvReader(std::shared_ptr input_stream, int header_lines); - EnvReader(); - virtual ~EnvReader(); - virtual bool Advance(); - const Units::SecondsTime GetTime() const; - -private: - Units::SecondsTime m_time; // column 1 - -}; - -} // namespace testvector - diff --git a/include/public/Environment.h b/include/public/Environment.h deleted file mode 100644 index d89a890..0000000 --- a/include/public/Environment.h +++ /dev/null @@ -1,42 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -class Wind; - -#include - -#include "public/EarthModel.h" - -class Environment final { - public: - static Environment *GetInstance(); - - EarthModel *GetEarthModel() const; - - virtual ~Environment() = default; - - private: - static std::unique_ptr m_instance; - - std::unique_ptr m_earth_model; - - Environment(); -}; diff --git a/include/public/EquationsOfMotionState.h b/include/public/EquationsOfMotionState.h deleted file mode 100644 index 3728c0d..0000000 --- a/include/public/EquationsOfMotionState.h +++ /dev/null @@ -1,40 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include -#include -#include -#include - -#include "public/BadaUtils.h" - -class EquationsOfMotionState { - public: - Units::Length enu_x{}, enu_y{}, altitude_msl{}; - Units::Speed true_airspeed{}; - Units::Angle gamma{}; - Units::SignedAngle psi_enu{}; - Units::Force thrust{}; - Units::Angle phi{}; - double speed_brake_percentage{0}; - aaesim::open_source::bada_utils::FlapConfiguration flap_configuration{ - aaesim::open_source::bada_utils::FlapConfiguration::UNDEFINED}; -}; diff --git a/include/public/EquationsOfMotionStateDeriv.h b/include/public/EquationsOfMotionStateDeriv.h deleted file mode 100644 index 773b11e..0000000 --- a/include/public/EquationsOfMotionStateDeriv.h +++ /dev/null @@ -1,36 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include - -class EquationsOfMotionStateDeriv { - public: - Units::Speed enu_velocity_x{}, enu_velocity_y{}, enu_velocity_z{}; // east, north, altitude change - Units::Acceleration true_airspeed_deriv{}; // true airspeed change - Units::AngularSpeed gamma_deriv{}; // flight-path angle change NOTE: for flight-path angle (gamma), heading down is - // positive; heading up is negative - Units::AngularSpeed heading_deriv{}; // heading change measured from east counter-clockwise - Units::ForceChange thrust_deriv{}; // thrust change - Units::AngularSpeed roll_rate{}; // roll angle change - double speed_brake_deriv{0}; // speed brake (% of deployment) change rate - aaesim::open_source::bada_utils::FlapConfiguration flap_configuration{ - aaesim::open_source::bada_utils::FlapConfiguration::UNDEFINED}; // flap configuration -}; diff --git a/include/public/EuclideanThreeDofDynamics.h b/include/public/EuclideanThreeDofDynamics.h deleted file mode 100644 index ed244f5..0000000 --- a/include/public/EuclideanThreeDofDynamics.h +++ /dev/null @@ -1,48 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// 2023 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include "public/ThreeDOFDynamics.h" - -namespace aaesim { -namespace open_source { -class EuclideanThreeDofDynamics : public ThreeDOFDynamics { - public: - EuclideanThreeDofDynamics() = default; - - ~EuclideanThreeDofDynamics() = default; - - void Initialize(std::shared_ptr aircraft_performance, - const Waypoint &initial_position, std::shared_ptr tangent_plane_sequence, - Units::Length initial_altitude_msl, Units::Speed initial_true_airspeed, - Units::Angle initial_ground_course_enu, double initial_mass_fraction, - std::shared_ptr true_weather); - - protected: - void CalculateEnvironmentalWind(WindStack &wind_east, WindStack &wind_north, Units::Frequency &dVwx_dh, - Units::Frequency &dVwy_dh) override; - - private: - static log4cplus::Logger m_logger; - std::shared_ptr m_tangent_plane_sequence; -}; - -} // namespace open_source -} // namespace aaesim \ No newline at end of file diff --git a/include/public/EuclideanTightTurnResolver.h b/include/public/EuclideanTightTurnResolver.h deleted file mode 100644 index ca680dd..0000000 --- a/include/public/EuclideanTightTurnResolver.h +++ /dev/null @@ -1,28 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -namespace aaesim::open_source { -struct EuclideanTightTurnResolver { - // TODO use units here - virtual void ResolveTightTurnGeometry(const double courseChange1, const double courseChange2, const double legLength, - double &radius, double &turnDist) = 0; -}; -} // namespace aaesim::open_source diff --git a/include/public/EuclideanTrajectoryPredictor.h b/include/public/EuclideanTrajectoryPredictor.h deleted file mode 100644 index 9c27965..0000000 --- a/include/public/EuclideanTrajectoryPredictor.h +++ /dev/null @@ -1,199 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include - -#include -#include -#include - -#include "nlohmann/json.hpp" -#include "public/AircraftIntent.h" -#include "public/AlongPathDistanceCalculator.h" -#include "public/EuclideanTightTurnResolver.h" -#include "public/HorizontalPath.h" -#include "public/PositionCalculator.h" -#include "public/PrecalcWaypoint.h" -#include "public/TurnAnticipation.h" -#include "public/VerticalPredictor.h" -#include "public/WeatherPrediction.h" - -namespace aaesim { -namespace open_source { - -class EuclideanTrajectoryPredictor { - public: - EuclideanTrajectoryPredictor(); - - virtual ~EuclideanTrajectoryPredictor() = default; - - virtual void CalculateWaypoints(const AircraftIntent &aircraft_intent, - const aaesim::open_source::WeatherPrediction &weather_prediction); - - const std::vector EstimateHorizontalTrajectory( - const aaesim::open_source::WeatherPrediction &weather_prediction); - - virtual void BuildTrajectoryPrediction(aaesim::open_source::WeatherPrediction &weather, - const std::shared_ptr &position_converter, - Units::Length start_altitude); - - virtual void BuildTrajectoryPrediction(aaesim::open_source::WeatherPrediction &weather, - const std::shared_ptr &position_converter, - Units::Length start_altitude, Units::Length aircraft_distance_to_go); - - aaesim::open_source::Guidance Update(const aaesim::open_source::AircraftState &state, - const aaesim::open_source::Guidance ¤t_guidance); - - const AircraftIntent &GetAircraftIntent() const; - - const std::vector &GetHorizontalPath() const; - - const std::shared_ptr GetAtmosphere() const; - - const std::vector &GetPrecalcWaypoints() const; - - const std::shared_ptr &GetVerticalPredictor() const; - - void SetBankAngle(Units::Angle bank_angle); - - Units::Angle GetBankAngle() const; - - Units::Length GetAltitudeAtFinalWaypoint() const; - - static double CounterClockwise(const double ax, const double ay, const double bx, const double by, const double cx, - const double cy); - - static double CounterClockwise(const double ax, const double ay, const double bx, const double by); - - static bool SamePoint(const PrecalcWaypoint &wp, const HorizontalPath &hp); - - static double FindCenterPoint(double fromX, double fromY, double toX, double toY, double gcpX, double gcpY, - double radius, double &cpX, double &cpY); - - static double HalfTurn(HorizontalTurnPath turn); - - protected: - void UpdateWeatherPrediction(aaesim::open_source::WeatherPrediction &weather, - const std::shared_ptr &position_converter) const; - - /** - * Forces altitude constraints to form a monotonic space. - */ - virtual void AdjustConstraints(const Units::Speed start_speed); - - std::vector m_waypoint_vector{}; - std::vector m_horizontal_path{}; - std::shared_ptr m_vertical_predictor{}; - Units::Angle m_bank_angle{Units::DUMMY_DEGREES_ANGLE}; - Units::Length m_altitude_at_final_waypoint{Units::FeetLength(-50.0)}; - Units::Length m_aircraft_distance_to_go{Units::infinity()}; - AircraftIntent m_aircraft_intent{}; - AlongPathDistanceCalculator m_distance_calculator{}; - PositionCalculator m_position_calculator{}; - std::shared_ptr m_atmosphere{}; - std::shared_ptr m_tight_turn_resolver{}; - - private: - static log4cplus::Logger m_logger; - enum HorizontalTrajOption { FIRST_PASS, SECOND_PASS }; - - std::string GetTrajectoryOptionAsString(HorizontalTrajOption option) const { - std::string option_as_string = "FIRST_PASS"; - if (option == HorizontalTrajOption::SECOND_PASS) { - option_as_string = "SECOND_PASS"; - } - return option_as_string; - } - - void DoHorizontalPathLogging(log4cplus::Logger &logger, HorizontalTrajOption current_option) const { - using json = nlohmann::json; - if (logger.getLogLevel() == log4cplus::TRACE_LOG_LEVEL) { - auto idx = 0; - for (HorizontalPath segment : GetHorizontalPath()) { - json j; - j["segment_index"] = idx; - j["trajectory_option"] = GetTrajectoryOptionAsString(current_option); - j["segment_type"] = segment.GetSegmentTypeAsString(); - j["cumulative_path_length_nm"] = - Units::NauticalMilesLength(Units::MetersLength(segment.m_path_length_cumulative_meters)).value(); - j["path_course_deg"] = Units::DegreesAngle(Units::RadiansAngle(segment.m_path_course)).value(); - j["x_position_m"] = segment.GetXPositionMeters(); - j["y_position_m"] = segment.GetYPositionMeters(); - LOG4CPLUS_TRACE(logger, j.dump()); - ++idx; - } - } - } - - void DoVerticalPathLogging(log4cplus::Logger &logger, HorizontalTrajOption current_option) const { - using json = nlohmann::json; - if (logger.getLogLevel() == log4cplus::TRACE_LOG_LEVEL) { - VerticalPath vertical_path = GetVerticalPredictor()->GetVerticalPath(); - json j; - j["trajectory_option"] = GetTrajectoryOptionAsString(current_option); - j["time_to_go_sec"] = vertical_path.time_to_go_sec; - j["along_path_distance_m"] = vertical_path.along_path_distance_m; - j["altitude_m"] = vertical_path.altitude_m; - j["cas_mps"] = vertical_path.cas_mps; - j["altitude_rate_mps"] = vertical_path.altitude_rate_mps; - j["tas_rate_mps"] = vertical_path.tas_rate_mps; - j["theta_radians"] = vertical_path.theta_radians; - j["gs_mps"] = vertical_path.gs_mps; - j["algorithm_enum"] = vertical_path.algorithm_type; - LOG4CPLUS_TRACE(logger, j.dump()); - } - } - - void SetAtmosphere(std::shared_ptr atmosphere); - - void DefineRoute(); - - void CalculateHorizontalTrajectory(const HorizontalTrajOption option); - - std::vector CalculateTurnAnticipation(const HorizontalTrajOption option); -}; - -} // namespace open_source -} // namespace aaesim - -inline const std::shared_ptr aaesim::open_source::EuclideanTrajectoryPredictor::GetAtmosphere() const { - return m_atmosphere; -} - -inline const std::vector &aaesim::open_source::EuclideanTrajectoryPredictor::GetPrecalcWaypoints() - const { - return m_waypoint_vector; -} - -inline const std::shared_ptr & - aaesim::open_source::EuclideanTrajectoryPredictor::GetVerticalPredictor() const { - return m_vertical_predictor; -} - -inline void aaesim::open_source::EuclideanTrajectoryPredictor::SetBankAngle(Units::Angle bank_angle) { - m_bank_angle = bank_angle; -} - -inline Units::Angle aaesim::open_source::EuclideanTrajectoryPredictor::GetBankAngle() const { return m_bank_angle; } - -inline Units::Length aaesim::open_source::EuclideanTrajectoryPredictor::GetAltitudeAtFinalWaypoint() const { - return Units::FeetLength(m_altitude_at_final_waypoint); -} diff --git a/include/public/EuclideanWaypointMonitor.h b/include/public/EuclideanWaypointMonitor.h deleted file mode 100644 index c8378db..0000000 --- a/include/public/EuclideanWaypointMonitor.h +++ /dev/null @@ -1,56 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include - -#include "public/LatitudeLongitudePoint.h" -#include "public/WaypointPassingMonitor.h" -#include "public/Wgs84PrecalcWaypoint.h" - -namespace aaesim { -namespace open_source { -class EuclideanWaypointMonitor final : public WaypointPassingMonitor { - public: - ~EuclideanWaypointMonitor() = default; - - void Update(const aaesim::LatitudeLongitudePoint &position, const Units::SignedAngle &ground_course_enu) override; - - bool IsPassedWaypoint() const override { return m_is_passed_waypoint; } - - static std::shared_ptr OfWgs84PrecalcWaypoint( - const aaesim::open_source::Wgs84PrecalcWaypoint &waypoint); - - static std::shared_ptr OfEllipsoidalPoint( - const aaesim::LatitudeLongitudePoint &ellipsoidal_point); - - static std::shared_ptr OfGeodeticPoint(const EarthModel::GeodeticPosition &geodetic_point); - - private: - EuclideanWaypointMonitor(const aaesim::LatitudeLongitudePoint &lat_lon_point); - - void PerformFakeTranslationToEuclidean(const aaesim::LatitudeLongitudePoint &lat_lon_point, Units::Length &x, - Units::Length &y); - - bool m_is_passed_waypoint{false}; - aaesim::LatitudeLongitudePoint m_point_to_monitor{}; -}; -} // namespace open_source -} // namespace aaesim diff --git a/include/public/FixedMassAircraftPerformance.h b/include/public/FixedMassAircraftPerformance.h deleted file mode 100644 index 2ae2674..0000000 --- a/include/public/FixedMassAircraftPerformance.h +++ /dev/null @@ -1,122 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include -#include -#include - -#include - -#include "public/BadaUtils.h" -#include "utility/BoundedValue.h" - -namespace aaesim { -namespace open_source { -struct FixedMassAircraftPerformance { - /** - * Calculate expected drag coefficients and flap settings for the incoming state values. - */ - virtual void GetDragCoefficients( - const Units::Speed &calibrated_airspeed, const Units::Length &altitude_msl, - const aaesim::open_source::bada_utils::FlapConfiguration ¤t_flap_configuration, - double &cd0, /** [out] parasitic drag coefficient */ - double &cd2, /** [out] induced drag coefficient */ - double &gear, /** [out] landing gear drag coefficient */ - aaesim::open_source::bada_utils::FlapConfiguration &flap_configuration /** [out] flap configuration */ - ) const = 0; - - /** - * Calculate drag coefficients and increments flap configuration when appropriate. - */ - virtual void GetDragCoefficientsAndIncrementFlapConfiguration( - const Units::Speed &calibrated_airspeed, const Units::Length &altitude_msl, - double &cd0, /** [out] parasitic drag coefficient */ - double &cd2, /** [out] induced drag coefficient */ - double &gear, /** [out] landing gear drag coefficient */ - aaesim::open_source::bada_utils::FlapConfiguration &updated_flap_setting /** [out] flap configuration */ - ) = 0; - - /** - * Calculate drag coefficients based on existing state. - */ - virtual void GetCurrentDragCoefficients(double &cd0, /** [out] parasitic drag coefficient */ - double &cd2, /** [out] induced drag coefficient */ - double &gear /** [out] landing gear drag coefficient */ - ) const = 0; - - /** - * Allow early configuration changes; use when extra drag is needed and speed is less than max configuration speed. - */ - virtual void GetConfigurationForIncreasedDrag( - const Units::Speed &calibrated_airspeed, const Units::Length &altitude_msl, - aaesim::open_source::bada_utils::FlapConfiguration &updated_flap_setting /** [out] flap configuration */ - ) = 0; - - /** - * Calculate the maximum available thrust in Newtons. - */ - virtual Units::NewtonsForce GetMaxThrust(const Units::Length &altitude_msl, - aaesim::open_source::bada_utils::FlapConfiguration flap_configuration, - aaesim::open_source::bada_utils::EngineThrustMode engine_thrust_mode, - Units::AbsCelsiusTemperature temperature_offset) const = 0; - - virtual void GetCoefficientsForFlapConfiguration(open_source::bada_utils::FlapConfiguration flap_configuration, - double &cd0, /** [out] parasitic drag coefficient */ - double &cd2, /** [out] induced drag coefficient */ - double &gear /** [out] landing gear drag coefficient */ - ) const = 0; - - virtual aaesim::open_source::bada_utils::FlapConfiguration GetFlapConfigurationForState( - const Units::Speed &calibrated_airspeed, const Units::Length &altitude_msl, - const aaesim::open_source::bada_utils::FlapConfiguration ¤t_flap_configuration) const = 0; - - virtual Units::Mass GetAircraftMass() const = 0; - - virtual double GetAircraftMassPercentile() const = 0; - - virtual open_source::bada_utils::FlapSpeeds GetFlapSpeeds() const = 0; - - virtual open_source::bada_utils::FlapConfiguration GetCurrentFlapConfiguration() const = 0; - - virtual void UpdateMassFraction(BoundedValue mass_fraction) = 0; - - virtual aaesim::open_source::bada_utils::AircraftType GetAircraftTypeInformation() const = 0; - - virtual aaesim::open_source::bada_utils::Mass GetAircraftMassInformation() const = 0; - - virtual aaesim::open_source::bada_utils::FlightEnvelope GetFlightEnvelopeInformation() const = 0; - - virtual aaesim::open_source::bada_utils::Aerodynamics GetAerodynamicsInformation() const = 0; - - virtual aaesim::open_source::bada_utils::EngineThrust GetEngineThrustInformation() const = 0; - - virtual aaesim::open_source::bada_utils::FuelFlow GetFuelFlowInformation() const = 0; - - virtual aaesim::open_source::bada_utils::GroundMovement GetGroundMovementInformation() const = 0; - - virtual aaesim::open_source::bada_utils::Procedure GetProcedureInformation(unsigned int index) const = 0; - - virtual aaesim::open_source::bada_utils::AircraftPerformance GetAircraftPerformanceInformation() const = 0; - - virtual std::string GetAircraftTypeIdentifier() const = 0; -}; -} // namespace open_source -} // namespace aaesim diff --git a/include/public/FlightDeckApplication.h b/include/public/FlightDeckApplication.h deleted file mode 100644 index bfb95f6..0000000 --- a/include/public/FlightDeckApplication.h +++ /dev/null @@ -1,70 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include - -#include "public/ASSAP.h" -#include "public/AircraftIntent.h" -#include "public/AircraftState.h" -#include "public/BadaUtils.h" -#include "public/DynamicsState.h" -#include "public/Guidance.h" -#include "public/TangentPlaneSequence.h" -#include "public/WeatherPrediction.h" - -namespace aaesim { -namespace open_source { -struct OwnshipPerformanceParameters { - aaesim::open_source::bada_utils::FlapSpeeds flap_speeds{}; - aaesim::open_source::bada_utils::FlightEnvelope flight_envelope{}; - aaesim::open_source::bada_utils::Mass mass_data{}; - aaesim::open_source::bada_utils::Aerodynamics aerodynamics{}; -}; - -struct OwnshipFmsPredictionParameters { - Units::Angle maximum_allowable_bank_angle{}; - Units::Speed transition_ias{}; - double transition_mach{}; - Units::Length transition_altitude{}; - Units::Length expected_cruise_altitude{}; - WeatherPrediction weather_prediction{}; - AircraftIntent fms_intent{}; -}; - -struct FlightDeckApplicationInitializer { - virtual ~FlightDeckApplicationInitializer() = default; - OwnshipFmsPredictionParameters fms_prediction_parameters{}; - OwnshipPerformanceParameters performance_parameters{}; - std::shared_ptr surveillance_processor{}; - std::shared_ptr position_converter{}; -}; - -struct FlightDeckApplication { - virtual ~FlightDeckApplication() = default; - virtual void Initialize(FlightDeckApplicationInitializer &initializer_visitor) = 0; - virtual aaesim::open_source::Guidance Update(const aaesim::open_source::SimulationTime &simtime, - const aaesim::open_source::Guidance ¤t_guidance, - const aaesim::open_source::DynamicsState &dynamics_state, - const aaesim::open_source::AircraftState &own_state) = 0; - virtual bool IsActive() const = 0; -}; -} // namespace open_source -} // namespace aaesim diff --git a/include/public/FlightDeckApplicationLibrary.h b/include/public/FlightDeckApplicationLibrary.h deleted file mode 100644 index dbacb41..0000000 --- a/include/public/FlightDeckApplicationLibrary.h +++ /dev/null @@ -1,60 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include -#include -#include - -#include "public/FlightDeckApplicationLoader.h" -#include "public/OutputHandler.h" - -// FIXME should I replace OutputHandler with special FlightDeckWriter? Probably. -// See also AAES-1329 -namespace aaesim { -namespace open_source { - -struct aaesim::open_source::FlightDeckApplicationLibrary { - /* - Allows the library to provide a mapping of keyword:factory for each flightdeck application it can construct. - - The key is a unique string that will be used internally to identify which application has been loaded at run-time. - */ - std::map RegisterFlightDeckApplications() = 0; - - /* - Allows the library to declare output handlers that can be used with their flightdeck applications. - - The map key is a string name that will appear in the scenario file. The simulation will use this - string to enable/disable the use of a file writer. - */ - std::map RegisterApplicationDataWriters() = 0; - - /* - The map key must be a string match for each application declared in RegisterFlightDeckApplications(); - - FIXME Consider if this Register should be combined with the return of RegisterFlightDeckApplications() like this: - - */ - std::map> - RegisterDataWritersPerFlightDeckApplication() = 0; -}; -} // namespace open_source -} // namespace aaesim diff --git a/include/public/FlightDeckApplicationLoader.h b/include/public/FlightDeckApplicationLoader.h deleted file mode 100644 index 3a377c3..0000000 --- a/include/public/FlightDeckApplicationLoader.h +++ /dev/null @@ -1,39 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include -#include - -#include "loader/LoggingLoadable.h" -#include "public/FlightDeckApplication.h" -#include "public/StatisticalPilotDelay.h" - -namespace aaesim { -namespace open_source { -struct FlightDeckApplicationLoader : public LoggingLoadable { - virtual std::string GetTopLevelTag() const = 0; - virtual void RegisterLoadableVariables() = 0; - virtual bool VariablesAreLoaded() const = 0; - virtual std::shared_ptr ConstructLoadedAlgorithm( - aaesim::open_source::StatisticalPilotDelay &pilot_delay) = 0; -}; -} // namespace open_source -} // namespace aaesim diff --git a/include/public/FlightDeckDataWriter.h b/include/public/FlightDeckDataWriter.h deleted file mode 100644 index 330b3f7..0000000 --- a/include/public/FlightDeckDataWriter.h +++ /dev/null @@ -1,34 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include - -#include "public/FlightDeckApplication.h" -#include "public/OutputHandler.h" - -namespace aaesim { -namespace open_source { -struct FlightDeckApplicationDataWriter : public aaesim::open_source::OutputHandler { - virtual ~FlightDeckApplicationDataWriter() = default; - virtual void CollectData(std::shared_ptr application) = 0; -}; -} // namespace open_source -} // namespace aaesim diff --git a/include/public/FlightEnvelopeSpeedLimiter.h b/include/public/FlightEnvelopeSpeedLimiter.h deleted file mode 100644 index 292df04..0000000 --- a/include/public/FlightEnvelopeSpeedLimiter.h +++ /dev/null @@ -1,52 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include "public/SpeedCommandLimiter.h" - -namespace aaesim { -namespace open_source { -class FlightEnvelopeSpeedLimiter : public SpeedCommandLimiter { - public: - FlightEnvelopeSpeedLimiter(const aaesim::open_source::bada_utils::FlapSpeeds &flap_speeds, - const aaesim::open_source::bada_utils::FlightEnvelope &flight_envelope); - - Units::Speed LimitSpeedCommand(const Units::Speed previous_ias_speed_command, - const Units::Speed current_ias_speed_command, - const Units::Speed reference_velocity_mps, - const Units::Length speed_quantization_distance, - const Units::Length distance_to_end_of_route, const Units::Length current_altitude, - const aaesim::open_source::bada_utils::FlapConfiguration flap_configuration) override; - - BoundedValue LimitMachCommand(const BoundedValue &previous_reference_speed_command_mach, - const BoundedValue ¤t_mach_command, - const BoundedValue &nominal_mach, - const Units::Mass ¤t_mass, const Units::Length ¤t_altitude, - const WeatherPrediction &weather_prediction) override; - - static const Units::Speed MINIMUM_IAS_LIMIT; - static const BoundedValue MINIMUM_MACH_LIMIT; - - private: - aaesim::open_source::bada_utils::FlapSpeeds m_flap_speeds; - aaesim::open_source::bada_utils::FlightEnvelope m_flight_envelope; -}; -} // namespace open_source -} // namespace aaesim diff --git a/include/public/FmsWaypointSequenceFile.h b/include/public/FmsWaypointSequenceFile.h deleted file mode 100644 index c68a0a6..0000000 --- a/include/public/FmsWaypointSequenceFile.h +++ /dev/null @@ -1,50 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// 2022 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include "public/OutputHandler.h" -#include "aaesim/SimpleAircraft.h" - -namespace aaesim { -namespace open_source { -class FmsWaypointSequenceFile : public OutputHandler { - - public: - FmsWaypointSequenceFile(); - - void Gather(const int iteration_number, const SimpleAircraft &aircraft); - - virtual void Finish(); - - private: - struct FmsWaypointData { - FmsWaypointData() : iteration_number(-1), simulation_time(Units::SecondsTime(-1.0)), acid(), waypoint_name(){}; - - int iteration_number; - Units::Time simulation_time; - std::string acid; - std::string waypoint_name; - }; - std::vector m_fms_waypoint_data; - - static log4cplus::Logger logger; -}; -} // namespace open_source -} // namespace aaesim diff --git a/include/public/ForeWindReader.h b/include/public/ForeWindReader.h deleted file mode 100644 index 7e5e714..0000000 --- a/include/public/ForeWindReader.h +++ /dev/null @@ -1,47 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// 2022 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -/* - * ForeWindReader.h - * - * Reads a ForeWind.csv file containing a list of altitudes and wind velocities - * - * Created on: Mar 23, 2019 - * Author: klewis - */ - -#pragma once - -#include "public/DataReader.h" -#include "public/WeatherPrediction.h" - -namespace testvector { - -class ForeWindReader : DataReader { -public: - ForeWindReader(std::string file_name, int header_lines); - ForeWindReader(std::shared_ptr input_stream, int header_lines); - virtual ~ForeWindReader(); - bool ReadWind(WeatherPrediction &weather_prediction); - -private: -}; - -} // namespace testvector - diff --git a/include/public/FullWindTrueWeatherOperator.h b/include/public/FullWindTrueWeatherOperator.h deleted file mode 100644 index ae82b35..0000000 --- a/include/public/FullWindTrueWeatherOperator.h +++ /dev/null @@ -1,43 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include "public/AbstractTrueWeatherOperator.h" - -namespace aaesim::open_source { -class FullWindTrueWeatherOperator final : public AbstractTrueWeatherOperator { - public: - FullWindTrueWeatherOperator(std::shared_ptr true_weather) - : AbstractTrueWeatherOperator(true_weather) {} - ~FullWindTrueWeatherOperator() = default; - void CalculateEnvironmentalWind(const EarthModel::GeodeticPosition &position, - const Units::Length &altitude_msl) override; - Units::Speed GetWindSpeedEast() const override; - Units::Speed GetWindSpeedNorth() const override; - Units::Frequency GetWindSpeedVerticalDerivativeEast() const override; - Units::Frequency GetWindSpeedVerticalDerivativeNorth() const override; - - private: - Units::Speed m_wind_speed_east{Units::zero()}; - Units::Speed m_wind_speed_north{Units::zero()}; - Units::Frequency m_vertical_derivative_east{Units::zero()}; - Units::Frequency m_vertical_derivative_north{Units::zero()}; -}; -} // namespace aaesim::open_source diff --git a/include/public/GeolibUtils.h b/include/public/GeolibUtils.h deleted file mode 100644 index c40c21b..0000000 --- a/include/public/GeolibUtils.h +++ /dev/null @@ -1,307 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include -#include -#include -#include -#include - -#include "public/ArcOnEllipsoid.h" -#include "public/LatitudeLongitudePoint.h" -#include "public/LineOnEllipsoid.h" - -namespace aaesim { -//--------------------------------------------- -/* Developers: - * These (epsilon and tolerance) values can be played with (tuned), but the values directly impact the precision of - * geolib's WGS84 calculations. And, the two values are related. So changing one may require changing both. If you don't - * understand the low-level details of the geolib algorithms, you probably should -not- be messing with this. - */ -static const double GEOLIB_EPSILON = 1e-20; -static const double GEOLIB_TOLERANCE = 1.37e-9; -static const Units::Length GEOLIB_TOLERANCE_UNITZED = Units::NauticalMilesLength(GEOLIB_TOLERANCE); -//--------------------------------------------- - -class GeolibUtils { - public: - inline static std::string m_basic_error_message{ - "Arg! Something very bad occurred inside a geolib library operation!"}; - - inline static bool IsSuccess(const geolib_idealab::ErrorSet &error_set) { return std::bitset<32>(error_set).none(); } - - inline static bool HasErrorBitSet(const geolib_idealab::ErrorSet &error_set, geolib_idealab::ErrorCodes code) { - if (IsSuccess(error_set)) { - if (!IsSuccess(code)) return false; - return true; - } - return error_set & code; - } - - /** - * Create a line of zero length that has directionality correctly defined. - * - * @param location - * @param course_enu - * @return - */ - static LineOnEllipsoid CreateLineOfZeroLength(const LatitudeLongitudePoint &location, - const Units::SignedAngle &course_enu); - - /** - * - * @param point1 - * @param point2 - * @return true if point1 and point2 are mathematically equal on the ellipsoid - */ - static bool ArePointsMathematicallyEqual(const LatitudeLongitudePoint &point1, const LatitudeLongitudePoint &point2); - - /** - * From a start_point, project a distance along a course and return that location. - * - * @param start_point, the starting location of the projection - * @param distance, the distance to project - * @param course_enu, the ENU course to project along - * @return the new point - */ - static LatitudeLongitudePoint CalculateNewPoint(const LatitudeLongitudePoint &start_point, - const Units::Length &distance, const Units::SignedAngle &course_enu); - - /** - * Define the mathematical relationship between two points on the WGS84 ellipsoid. - * - * @param start_point - * @param end_point - * @return a pair that contains: - * - the wgs84 distance between the points; - * - the course from start_point to end_point in the ENU coordinate frame. - */ - static std::pair CalculateRelationshipBetweenPoints( - const LatitudeLongitudePoint &start_point, const LatitudeLongitudePoint &end_point); - - /** - * Define the mathematical relationship between two points on the WGS84 ellipsoid. - * - * @param start_point - * @param end_point - * @return a tuple that contains: - * - the wgs84 distance between the points; - * - the course from start_point to end_point in the ENU coordinate frame; - * - the course from end_point to start_point in the ENU coordinate frame. - */ - static std::tuple CalculateRelationshipBetweenPointsExpanded( - const LatitudeLongitudePoint &start_point, const LatitudeLongitudePoint &end_point); - - /** - * AAESim uses the East-North-Up coordinate frame and the angle is -usually- on the interval [-180, 180]. This means - * that 0 is east, 90 is north, 180 is west and -90 is south. - * By constrast, geolib uses an NED coordinate frame and the course angle is on the interval [0, 360]. This means - * that 0 is north, 90 is east, 180 is south and 270 is west. - * - * This method converts course from ENU to NED. - * - * @param course_enu - * @return course_ned - */ - static const Units::UnsignedAngle ConvertCourseFromEnuToNed(const Units::SignedAngle &course_enu); - - /** - * AAESim uses the East-North-Up coordinate frame and the angle is -usually- on the interval [-180, 180]. This means - * that 0 is east, 90 is north, 180 is west and -90 is south. - * By constrast, geolib uses an NED coordinate frame and the course angle is on the interval [0, 360]. This means - * that 0 is north, 90 is east, 180 is south and 270 is west. - * - * This method converts course from NED to ENU. - * - * @param course_ned, an unsigned angle on the interval [0, 360] - * @return course_enu, a signed angle on the interval [-180, 180] - */ - static const Units::SignedAngle ConvertCourseFromNedToEnu(const Units::UnsignedAngle &course_ned); - - /** - * - * @param start_point - * @param end_point - * @return LineOnEllipsoid, fully populated - */ - static const LineOnEllipsoid CreateLineOnEllipsoid(const LatitudeLongitudePoint &start_point, - const LatitudeLongitudePoint &end_point); - - /** - * Create an ArcOnEllipsoid object. - * - * NOTE: This is mathematically sensitive! The Latitude/Longitude must be on-the-arc in a double precision sense. - * - * @param start_point - * @param end_point - * @param center_point - * @param arc_direction geolib_idealab::ArcDirection which is CLOCKWISE or COUNTERCLOCKWISE - * @return ArcOnEllipsoid, fully populated - */ - static const ArcOnEllipsoid CreateArcOnEllipsoid(const LatitudeLongitudePoint &start_point, - const LatitudeLongitudePoint &end_point, - const LatitudeLongitudePoint ¢er_point, - const geolib_idealab::ArcDirection &arc_direction); - - /** - * Create a full-circle ArcOnEllipsoid object. - * - * @param center_point - * @param arc_radius - * @param arc_direction - * @return - */ - static const ArcOnEllipsoid CreateFullCircleOnEllipsoid(const LatitudeLongitudePoint ¢er_point, - const Units::Length &arc_radius, - const geolib_idealab::ArcDirection &arc_direction); - - /** - * Calculate an intersection point of two lines. The returned point is guaranteed to be between the start and end - * point on both lines. - * - * @param line1 - * @param line2 - * @return a tuple that contains: - * - index 0: bool for valid intersection; - * - index 1: intersection point as LatitudeLongitudePoint; - * - index 2: vector of distances between the start point of each line and the intersection point - */ - static const std::tuple > - CalculateLineLineIntersectionPoint(const LineOnEllipsoid &line1, const LineOnEllipsoid &line2); - - /** - * Check if the provided point is on the line. - * - * @param line - * @param test_point - * @return true or false - */ - static const bool IsPointOnLine(const LineOnEllipsoid &line, const LatitudeLongitudePoint &test_point); - - /** - * - * @param arc - * @param test_point - * @return true or false - */ - static const bool IsPointOnArc(const ArcOnEllipsoid &arc, const LatitudeLongitudePoint &test_point); - - /** - * Project from point_not_on_line to the line, making a perpendicular line. Return that point that has been - * calculated as the projection point. - * - * This returned point also defines the nearest location on the line to point_not_on_line. - * - * @param line - * @param point_not_on_line - * @return a tuple that contains: - * - the point on the line that creates a perpendicular angle to the line when connected to point_not_on_line - * - an ENU course from point_not_on_line to the point this is returned - * - a distance from the point_not_on_line to the point that is on the line - */ - static std::tuple - FindNearestPointOnLineUsingPerpendicularProjection(const LineOnEllipsoid &line, - const LatitudeLongitudePoint &point_not_on_line); - - /** - * Checks the test point to see if it lies in the boundary of the arc segment, taking the start and end points of - * the arc into account. - * - * @param finite_arc - * @param test_point - * @return true if inside the arc segment, false otherwise - */ - static const bool IsPointInsideArcSegment(const ArcOnEllipsoid &finite_arc, - const LatitudeLongitudePoint &test_point); - - /** - * Calculates a point on the arc that is the nearest to the point_not_on_arc. The point returned is constrained to - * be on the finite arc, taking into account its start and end points. - * - * @param arc - * @param point_not_on_arc - * @return a pair of boolean and LatitudeLongitudePoint - */ - static std::pair FindNearestPointOnArcUsingPerpendiculorProjection( - const ArcOnEllipsoid &arc, const LatitudeLongitudePoint &point_not_on_arc); - - /** - * For two given lines, calculate an arc that is tangent to both lines and that has a specific radius. - * - * @param line1 - * @param line2 - * @param required_radius - * @return a pair that contains a boolean and an ArcOnEllipsoid. The boolean will be false if no arc could be found, - * true otherwise. - */ - static std::pair CreateArcTangentToTwoLines(const LineOnEllipsoid &line1, - const LineOnEllipsoid &line2, - const Units::Length &required_radius); - - /** - * For a shape that is inbound to an arc, and that has an end point where the arc must begin, and also given a - * location where the arc must end, find an arc that fits. - * - * @param inbound_line - * @param end_point - * @return a fully formed ArcOnEllipsoid object - * @throws std::runtime_error - */ - static ArcOnEllipsoid CreateArcFromInboundShapeAndEndPoint(const ShapeOnEllipsoid *inbound_shape, - const LatitudeLongitudePoint &end_point); - - /** - * For a given line and arc, calculate any points of intersection. - * - * @param line - * @param arc - * @return a vector of pair. Each pair contains a boolean and a point. The boolean is true if the point lies on both - * the arc and the line and false otherwise. - */ - static std::vector > CalculateLineArcIntersectionPoints( - const LineOnEllipsoid &line, const ArcOnEllipsoid &arc); - - /** - * Build a reverse line. - */ - static const LineOnEllipsoid ReverseLine(const LineOnEllipsoid &line) { - return GeolibUtils::CreateLineOnEllipsoid(line.GetEndPoint(), line.GetStartPoint()); - } - - /** - * Build a reverse arc. - */ - static const ArcOnEllipsoid ReverseArc(const ArcOnEllipsoid &arc) { - geolib_idealab::ArcDirection reversed_direction; - if (arc.GetArcDirection() == geolib_idealab::ArcDirection::CLOCKWISE) { - reversed_direction = geolib_idealab::ArcDirection::COUNTERCLOCKWISE; - } else { - reversed_direction = geolib_idealab::ArcDirection::CLOCKWISE; - } - return GeolibUtils::CreateArcOnEllipsoid(arc.GetEndPoint(), arc.GetStartPoint(), arc.GetCenterPoint(), - reversed_direction); - } - - private: - inline static log4cplus::Logger m_logger{log4cplus::Logger::getInstance(LOG4CPLUS_TEXT("GeolibUtils"))}; -}; - -} // namespace aaesim diff --git a/include/public/Guidance.h b/include/public/Guidance.h deleted file mode 100644 index 3d86a9d..0000000 --- a/include/public/Guidance.h +++ /dev/null @@ -1,103 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include -#include - -#include "public/AircraftSpeed.h" -#include "public/PrecalcWaypoint.h" - -namespace aaesim { -namespace open_source { -enum GuidanceFlightPhase { TAKEOFF_ROLL, CLIMB, CRUISE_DESCENT }; - -static std::string GuidanceFlightPhaseAsString(GuidanceFlightPhase guidance_flight_phase) { - switch (guidance_flight_phase) { - case aaesim::open_source::GuidanceFlightPhase::TAKEOFF_ROLL: - return "TAKEOFF_ROLL"; - case aaesim::open_source::GuidanceFlightPhase::CLIMB: - return "CLIMB"; - case aaesim::open_source::GuidanceFlightPhase::CRUISE_DESCENT: - return "CRUISE_DESCENT"; - default: - throw std::logic_error("Invalid guidance flight phase encountered: " + std::to_string(guidance_flight_phase)); - } -} - -class Guidance { - public: - Guidance() = default; - - virtual ~Guidance() = default; - - void SetValid(bool value); - - const bool IsValid() const; - - const AircraftSpeed &GetSelectedSpeed() const; - - void SetSelectedSpeed(const AircraftSpeed &selected_speed); - - int GetIasCommandIntegerKnots() const; // FIXME Stuart is this needed anymore? - - double GetMachCommand() const; - - void SetMachCommand(double mach_value); - - PrecalcConstraint m_active_precalc_constraints; - - Units::Speed m_ias_command{Units::ZERO_SPEED}; - double m_mach_command{0}; - Units::Speed m_ground_speed{Units::ZERO_SPEED}; - Units::Speed m_vertical_speed{Units::ZERO_SPEED}; - Units::Length m_reference_altitude{Units::ZERO_LENGTH}; - Units::Length m_cross_track_error{Units::ZERO_LENGTH}; - Units::Angle m_reference_bank_angle{Units::ZERO_ANGLE}; - Units::SignedAngle m_enu_track_angle{Units::ZERO_ANGLE}; - GuidanceFlightPhase m_active_guidance_phase{GuidanceFlightPhase::TAKEOFF_ROLL}; - - bool m_use_cross_track{false}; - - private: - bool m_valid{false}; - AircraftSpeed m_selected_speed{}; // FIXME Stuart this is super dangerous...get rid of and only keep SpeedValueType - // in here -}; - -inline void Guidance::SetValid(bool value) { m_valid = value; } - -inline const bool Guidance::IsValid() const { return m_valid; } - -inline const AircraftSpeed &Guidance::GetSelectedSpeed() const { return m_selected_speed; } - -inline void Guidance::SetSelectedSpeed(const AircraftSpeed &selected_speed) { m_selected_speed = selected_speed; } - -inline double Guidance::GetMachCommand() const { return m_mach_command; } - -inline void Guidance::SetMachCommand(double mach_value) { m_mach_command = mach_value; } - -inline int Guidance::GetIasCommandIntegerKnots() const { - double result = round(Units::KnotsSpeed(m_ias_command).value()); - return (int)result; -} - -} // namespace open_source -} // namespace aaesim diff --git a/include/public/GuidanceCalculator.h b/include/public/GuidanceCalculator.h deleted file mode 100644 index 5141f59..0000000 --- a/include/public/GuidanceCalculator.h +++ /dev/null @@ -1,61 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include "public/AircraftState.h" -#include "public/Guidance.h" -#include "public/ShapeOnEllipsoid.h" - -namespace aaesim { -namespace open_source { -struct GuidanceCalculator { - virtual open_source::Guidance Update(const open_source::AircraftState ¤t_state) = 0; - static aaesim::open_source::Guidance CombineGuidance(const aaesim::open_source::Guidance &horizontal_guidance, - const aaesim::open_source::Guidance &vertical_guidance) { - aaesim::open_source::Guidance full_guidance; - - full_guidance.m_cross_track_error = horizontal_guidance.m_cross_track_error; - full_guidance.m_reference_bank_angle = horizontal_guidance.m_reference_bank_angle; - full_guidance.m_use_cross_track = horizontal_guidance.m_use_cross_track; - full_guidance.m_enu_track_angle = horizontal_guidance.m_enu_track_angle; - - full_guidance.m_reference_altitude = vertical_guidance.m_reference_altitude; - full_guidance.m_vertical_speed = vertical_guidance.m_vertical_speed; - full_guidance.m_ias_command = vertical_guidance.m_ias_command; - full_guidance.m_ground_speed = vertical_guidance.m_ground_speed; - full_guidance.m_active_guidance_phase = vertical_guidance.m_active_guidance_phase; - full_guidance.SetSelectedSpeed(vertical_guidance.GetSelectedSpeed()); - full_guidance.SetMachCommand(vertical_guidance.GetMachCommand()); - - full_guidance.SetValid(true); - return full_guidance; - } - - static Units::Length AddSignToCrossTrack(Units::Length cross_track_measurement, - aaesim::ShapeOnEllipsoid::kDirectionRelativeToShape side_of_shape) { - assert(side_of_shape != ShapeOnEllipsoid::UNSET); - if (side_of_shape == ShapeOnEllipsoid::LEFT_OF_SHAPE) { - return -Units::abs(cross_track_measurement); - } - return Units::abs(cross_track_measurement); - }; -}; -} // namespace open_source -} // namespace aaesim diff --git a/include/public/HfpReader.h b/include/public/HfpReader.h deleted file mode 100644 index 4bb95c9..0000000 --- a/include/public/HfpReader.h +++ /dev/null @@ -1,76 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This is the copyright work of The MITRE Corporation, and was produced -// for the U. S. Government under Contract Number DTFAWA-10-C-00080, and -// is subject to Federal Aviation Administration Acquisition Management -// System Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV -// (Oct. 1996). No other use other than that granted to the U. S. -// Government, or to those acting on behalf of the U. S. Government, -// under that Clause is authorized without the express written -// permission of The MITRE Corporation. For further information, please -// contact The MITRE Corporation, Contracts Office, 7515 Colshire Drive, -// McLean, VA 22102-7539, (703) 983-6000. -// -// Copyright 2020 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -/* - * HfpReader.h - * - * Reads a TV.csv file containing a sequence of aircraft states. - * - * Created on: Sep 5, 2019 - * Author: klewis - */ - -#pragma once - -#include "public/DataReader.h" -#include -#include -#include - -namespace testvector { - -class HfpReader : public DataReader { -public: - static const size_t EXPECTED_TV_COLUMN_COUNT; - HfpReader(std::string file_name, int header_lines); - HfpReader(std::shared_ptr input_stream, int header_lines); - HfpReader(); - virtual ~HfpReader(); - Units::Length GetX(); - Units::Length GetY(); - Units::Length GetDTG(); - std::string GetSegmentType(); - Units::Angle GetCourse(); - Units::Length GetTurnCenterX(); - Units::Length GetTurnCenterY(); - Units::Angle GetAngleStartOfTurn(); - Units::Angle GetAngleEndOfTurn(); - Units::Length GetTurnRadius(); - Units::Speed GetGroundSpeed(); - Units::Angle GetBankAngle(); - Units::Angle GetLatitude(); - Units::Angle GetLongitude(); - Units::Angle GetTurnCenterLatitude(); - Units::Angle GetTurnCenterLongitude(); - -private: - void SetColumnIndexesFromHeader(const int header_lines); - int m_x_column, m_y_column; - int m_dtg_column; - int m_segment_type_column; - int m_course_column; - int m_turn_center_x_column, m_turn_center_y_column; - int m_angle_start_of_turn_column, m_angle_end_of_turn_column; - int m_turn_radius_column; - int m_ground_speed_column; - int m_bank_angle_column; - int m_latitude_column, m_longitude_column; - int m_turn_center_latitude_column, m_turn_center_longitude_column; -}; - -} // namespace testvector - diff --git a/include/public/HfpReader2020.h b/include/public/HfpReader2020.h deleted file mode 100644 index 0646612..0000000 --- a/include/public/HfpReader2020.h +++ /dev/null @@ -1,101 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// 2022 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -/* - * HfpReader2020.h - * - * Reads a TV.csv file containing a sequence of aircraft states. - */ - -#pragma once - -#include "public/DataReader.h" -#include "HorizontalPath.h" -#include -#include -#include - -namespace testvector { - - class HfpReader2020 : public DataReader - { - public: - - HfpReader2020(std::string file_name, - int header_lines); - - HfpReader2020(std::shared_ptr input_stream, - int header_lines); - - virtual ~HfpReader2020(); - - Units::Length GetX(); - - Units::Length GetY(); - - Units::Length GetDTG(); - - HorizontalPath::SegmentType GetSegmentType(); - - Units::Angle GetCourse(); - - Units::Length GetTurnCenterX(); - - Units::Length GetTurnCenterY(); - - Units::Angle GetAngleStartOfTurn(); - - Units::Angle GetAngleEndOfTurn(); - - Units::Length GetTurnRadius(); - - Units::Speed GetGroundSpeed(); - - Units::Angle GetBankAngle(); - - Units::Angle GetLatitude(); - - Units::Angle GetLongitude(); - - Units::Angle GetTurnCenterLatitude(); - - Units::Angle GetTurnCenterLongitude(); - - private: - void SetColumnIndexesFromHeader(const int header_lines); - - int m_x_column; - int m_y_column; - int m_dtg_column; - int m_segment_type_column; - int m_course_column; - int m_turn_center_x_column; - int m_turn_center_y_column; - int m_angle_start_of_turn_column; - int m_angle_end_of_turn_column; - int m_turn_radius_column; - int m_ground_speed_column; - int m_bank_angle_column; - int m_latitude_column; - int m_longitude_column; - int m_turn_center_latitude_column; - int m_turn_center_longitude_column; - }; -} - diff --git a/include/public/HfpReaderPre2020.h b/include/public/HfpReaderPre2020.h deleted file mode 100644 index ac87048..0000000 --- a/include/public/HfpReaderPre2020.h +++ /dev/null @@ -1,78 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// 2022 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -/* - * HfpReaderPre2020.h - * - * Reads a TV.csv file containing a sequence of aircraft states. - * - * Created on: Sep 5, 2019 - * Author: klewis - */ - -#pragma once - -#include "public/DataReader.h" -#include -#include -#include - -namespace testvector { - -class HfpReaderPre2020 : public DataReader { -public: - static const size_t EXPECTED_TV_COLUMN_COUNT; - HfpReaderPre2020(std::string file_name, int header_lines); - HfpReaderPre2020(std::shared_ptr input_stream, int header_lines); - HfpReaderPre2020(); - virtual ~HfpReaderPre2020(); - Units::Length GetX(); - Units::Length GetY(); - Units::Length GetDTG(); - std::string GetSegmentType(); - Units::Angle GetCourse(); - Units::Length GetTurnCenterX(); - Units::Length GetTurnCenterY(); - Units::Angle GetAngleStartOfTurn(); - Units::Angle GetAngleEndOfTurn(); - Units::Length GetTurnRadius(); - Units::Speed GetGroundSpeed(); - Units::Angle GetBankAngle(); - Units::Angle GetLatitude(); - Units::Angle GetLongitude(); - Units::Angle GetTurnCenterLatitude(); - Units::Angle GetTurnCenterLongitude(); - -private: - void SetColumnIndexesFromHeader(const int header_lines); - int m_x_column, m_y_column; - int m_dtg_column; - int m_segment_type_column; - int m_course_column; - int m_turn_center_x_column, m_turn_center_y_column; - int m_angle_start_of_turn_column, m_angle_end_of_turn_column; - int m_turn_radius_column; - int m_ground_speed_column; - int m_bank_angle_column; - int m_latitude_column, m_longitude_column; - int m_turn_center_latitude_column, m_turn_center_longitude_column; -}; - -} // namespace testvector - diff --git a/include/public/HorizontalPath.h b/include/public/HorizontalPath.h deleted file mode 100644 index fadd5a6..0000000 --- a/include/public/HorizontalPath.h +++ /dev/null @@ -1,64 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include - -#include "public/HorizontalTurnPath.h" - -namespace aaesim::open_source { -class HorizontalPath final { - public: - enum SegmentType { STRAIGHT, TURN, UNSET }; - - HorizontalPath() = default; - virtual ~HorizontalPath() = default; - double GetXPositionMeters() const; - double GetYPositionMeters() const; - void SetXYPositionMeters(double x_position_meters, double y_position_meters); - bool operator==(const HorizontalPath &that) const; - - SegmentType m_segment_type{HorizontalPath::SegmentType::UNSET}; - double m_path_length_cumulative_meters{0}; - double m_path_course{0}; - HorizontalTurnPath m_turn_info{}; - - std::string GetSegmentTypeAsString() const { - std::string return_this = "UNSET"; - switch (m_segment_type) { - case STRAIGHT: - return_this = "STRAIGHT"; - break; - - case TURN: - return_this = "TURN"; - break; - - default: - break; - } - return return_this; - } - - private: - double m_x_position_meters{0}; - double m_y_position_meters{0}; -}; -} // namespace aaesim::open_source diff --git a/include/public/HorizontalPathTracker.h b/include/public/HorizontalPathTracker.h deleted file mode 100644 index a509d0c..0000000 --- a/include/public/HorizontalPathTracker.h +++ /dev/null @@ -1,149 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include -#include - -#include - -namespace aaesim::open_source { - -/** - * Use to define the direction of index travel along the horizontal path. - */ -enum TrajectoryIndexProgressionDirection { - /** - * No directionality known or expected. - */ - UNDEFINED, - /** - * The trajectory index is expected to start from zero and increment during subsequent calls. Trajectroy prediction - * operations tend to progress in this direction. - */ - INCREMENTING, - /** - * The trajectory index is expected to start from the size of the vector and decrement during subsequent calls. This - * direction is consistent with the aircraft direction of flight. - */ - DECREMENTING -}; - -class HorizontalPathTracker { - public: - HorizontalPathTracker() = default; - HorizontalPathTracker(const std::vector &horizontal_trajectory, - TrajectoryIndexProgressionDirection expected_index_progression); - virtual ~HorizontalPathTracker() = default; - - TrajectoryIndexProgressionDirection GetExpectedProgressionDirection() const; - - /** - * Provides a boolean to indicate if passed end of route. Logic for passed end of route is implemented in child - * classes. - * - * At end of route is a numerical definition with ambiguity. Therefore, this method will likely return false if _at_ - * the end of the route. But this must be decided by child class implementations. - * - * @return - */ - bool IsPassedEndOfRoute() const; - - const std::vector::size_type GetCurrentTrajectoryIndex() const; - - /** - * @brief Update the horizontal trajectory without resetting the tracking behavior. To be used when the trajectory - * has experienced a slight change and the tracking behavior should remain consistent. For large changes in the - * horizontal trajectory, build a new object instead of calling this. - */ - virtual void UpdateHorizontalTrajectory(const std::vector &horizontal_trajectory); - - const std::vector &GetHorizontalPath() const; - - void UpdateCurrentIndex(std::vector::size_type new_index); - - /** - * Logic to initialize the starting index. - */ - void InitializeStartingIndex(); - - const HorizontalPath GetActivePathSegment() const; - - protected: - inline static const Units::Length EXTENSION_LENGTH{Units::NauticalMilesLength(1.0)}; - std::vector::size_type m_current_index{0}; - std::vector m_extended_horizontal_trajectory{}, m_unmodified_horizontal_trajectory{}; - bool m_is_passed_end_of_route{false}; - TrajectoryIndexProgressionDirection m_index_progression_direction{TrajectoryIndexProgressionDirection::UNDEFINED}; - - /** - * @brief Subclasses can call this to verify that the index is progressing appropriately. - */ - bool ValidateIndexProgression(std::vector::size_type index_to_check); - - /** - * @brief Extends a horizontal path vector in both directions so that the edges of the original path are more - * conveniently handled mathematically. - * - * @param horizontal_trajectory the original vector that will be extended - * @return a copy of the original vector, but extended with straight segments at both ends - */ - std::vector ExtendHorizontalTrajectory(const std::vector &horizontal_trajectory); - - /** - * @brief Checks the incoming location to determine if on a local horizontal path node. - */ - bool IsPositionOnNode(const Units::Length position_x, const Units::Length position_y, - std::vector::size_type &node_index); - - /** - * @brief Checks the incoming distance to see if it is "close" to a horizontal path node. - */ - bool IsDistanceAlongPathOnNode(const Units::Length distance_along_path, - std::vector::size_type &node_index); - - private: - inline static log4cplus::Logger m_logger{log4cplus::Logger::getInstance("HorizontalPathTracker")}; - inline static const Units::MetersLength ON_NODE_TOLERANCE{Units::MetersLength(1e-10)}; -}; - -inline bool HorizontalPathTracker::IsPassedEndOfRoute() const { return m_is_passed_end_of_route; } - -inline const std::vector &HorizontalPathTracker::GetHorizontalPath() const { - return m_unmodified_horizontal_trajectory; -} - -inline const std::vector::size_type HorizontalPathTracker::GetCurrentTrajectoryIndex() const { - return m_current_index - 1; // subtract one because caller doesn't know about m_extended_horizontal_trajectory -} - -inline TrajectoryIndexProgressionDirection HorizontalPathTracker::GetExpectedProgressionDirection() const { - return m_index_progression_direction; -} - -inline void HorizontalPathTracker::UpdateCurrentIndex(std::vector::size_type new_index) { - m_current_index = new_index; -} - -inline const HorizontalPath HorizontalPathTracker::GetActivePathSegment() const { - return m_extended_horizontal_trajectory[m_current_index]; -} - -} // namespace aaesim::open_source diff --git a/include/public/HorizontalTurnPath.h b/include/public/HorizontalTurnPath.h deleted file mode 100644 index c227e5e..0000000 --- a/include/public/HorizontalTurnPath.h +++ /dev/null @@ -1,64 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include -#include -#include -#include - -#include - -namespace aaesim::open_source { - -class HorizontalPath; // avoid recursive class defs - -class HorizontalTurnPath final { - public: - enum TURN_TYPE { UNKNOWN, PERFORMANCE, RADIUS_FIXED }; - static std::string GetTurnTypeAsString(TURN_TYPE tt) { - switch (tt) { - case PERFORMANCE: - return "PERFORMANCE"; - case RADIUS_FIXED: - return "RADIUS_FIXED"; - case UNKNOWN: - default: - return "UNKNOWN"; - } - }; - enum TURN_DIRECTION { NO_TURN, LEFT_TURN, RIGHT_TURN }; - - HorizontalTurnPath() = default; - - ~HorizontalTurnPath() = default; - - TURN_DIRECTION GetTurnDirection(const HorizontalPath &p0, const HorizontalPath &p1) const; - - double x_position_meters{0}; - double y_position_meters{0}; - Units::UnsignedRadiansAngle q_start{0}; - Units::UnsignedRadiansAngle q_end{0}; - Units::MetersLength radius{0}; - Units::UnsignedRadiansAngle bankAngle{0}; - Units::MetersPerSecondSpeed groundspeed{0}; - TURN_TYPE turn_type{UNKNOWN}; -}; -} // namespace aaesim::open_source diff --git a/include/public/IMCommandObserver.h b/include/public/IMCommandObserver.h deleted file mode 100644 index f888244..0000000 --- a/include/public/IMCommandObserver.h +++ /dev/null @@ -1,47 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// 2022 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -class IMCommandObserver -{ -public: - IMCommandObserver(void); - - ~IMCommandObserver(void); - - // operator < for sort algorithm - bool operator<(const IMCommandObserver &im_in) const; - - int iteration; - double id; - double time; - double distance_to_go; - double state_altitude; - double state_TAS; - double state_groundspeed; - double IAS_command; - double unmodified_IAS; - double TAS_command; - double reference_velocity; - double reference_distance; - double predictedDistance; - double distance_difference; - double trueDistance; -}; diff --git a/include/public/InternalObserver.h b/include/public/InternalObserver.h deleted file mode 100644 index 47ed539..0000000 --- a/include/public/InternalObserver.h +++ /dev/null @@ -1,237 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// 2022 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include -#include -#include -#include "public/AircraftState.h" -#include "public/Guidance.h" -#include "public/IMCommandObserver.h" -#include "public/DynamicsObserver.h" -#include "public/NMObserver.h" -#include "public/CrossTrackObserver.h" -#include "public/AchieveObserver.h" -#include "public/VerticalPathObserver.h" -#include "public/MaintainMetric.h" -#include "public/MergePointMetric.h" -#include "public/ClosestPointMetric.h" -#include "public/WeatherPrediction.h" - -class InternalObserver -{ -public: - static InternalObserver *getInstance(); - - static void clearInstance(); - - void reset(void); - - void process(void); - - // stores and output the state-model report. - - void storeStateModel(aaesim::open_source::AircraftState asv, - int flapsConfig, - float speed_brake, - double ias); - - void outputStateModel(); - - //gwang 2014-05-12: write ADS-B reports into files - void collect_ptis_b_report(Sensor::ADSB::ADSBSVReport adsb_sv_report); - - void process_ptis_b_reports(); - - // output the IM commands (UNITS ARE IN METERS) - void IM_command_output(int id_in, - double time_in, - double state_alt, - double state_TAS, - double state_groundspeed_in, - double ias_command_in, - double unmod_ias, - double tas_command_in, - double ref_vel_in, - double ref_dist_in, - double curr_dist_in, - double true_dist_in); // add an entry to the command list - void process_IM_command(); // process the IM report - - void process_NM_aircraft(); - - void process_NM_stats(); - - // output number of speed commands per aircraft - void speed_command_count_output(vector speed_change_list); - - void process_speed_command_count(); - - // output maintain metrics - void outputMaintainMetrics(); - - void processMaintainMetrics(); - - // output final groundspeed - void updateFinalGS(int id, - double gs); - - void outputFinalGS(); - - void processFinalGS(); - - // output merge point metric - void outputMergePointMetric(); - - void processMergePointMetric(); - - // output closest point metric - void outputClosestPointMetric(); - - void processClosestPointMetric(); - - // predicted wind matrix metric - void addPredictedWind(int id, const WeatherPrediction &weatherPrediction); - - void dumpPredictedWind(); - - std::string predWindsHeading(int lastIx); - - std::string predWindsData(int id, - int row, - std::string field, - const WindStack &mat); - - std::string predTempData(int id, - std::string field, - const WeatherPrediction &weatherPrediction); - - - // time to go metric - void addAchieveRcd(size_t aircraftId, - double tm, - double target_ttg_to_ach, - double own_ttg_to_ach, - double curr_distance, - double reference_distance); - - void dumpAchieveList(); - - static void FatalError(const char *str) - { - LOG4CPLUS_FATAL(logger, str); - throw std::logic_error(str); - } // FatalError - - NMObserver &GetNMObserver(int id); - - MaintainMetric &GetMaintainMetric(int id); - - MergePointMetric &GetMergePointMetric(int id); - - ClosestPointMetric &GetClosestPointMetric(int id); - - void set_scenario_name(std::string in); - - // Initializes metrics where necessary. - void initializeIteration(int number_of_aircraft_in_scenario); - - // Sets NM file output flag. - void setNMOutput(bool NMflag); - - // Determines whether to output NM data or not. - bool outputNM(void); - - void SetRecordMaintainMetrics(bool new_value); - - const bool GetRecordMaintainMetrics() const; - int GetScenarioIter() const; - void SetScenarioIter(int scenario_iter); - CrossTrackObserver& GetCrossEntry(); - -private: - class AircraftIterationStats { - public: - MergePointMetric m_merge_point_metric; - MaintainMetric m_maintain_metric; - ClosestPointMetric m_closest_point_metric; - double finalGS; - AircraftIterationStats(); - }; - - class AircraftScenarioStats { - public: - NMObserver m_nm_observer; - std::vector m_achieve_list; - }; - - - static InternalObserver *mInstance; - static log4cplus::Logger logger; - - InternalObserver(void); - - ~InternalObserver(void); - - // Formats state model report data. - std::string stateModelString(aaesim::open_source::AircraftState asv, - int flapsConfig, - float speed_brake, - double ias); - - // Returns header for state model report. - std::string stateModelHdr(); - - // output flags - bool outputNMFiles; - bool m_save_maintain_metrics; - - //Data for aggregate - std::string scenario_name; - - int m_scenario_iter; // variable to store the current scenario iteration - - CrossTrackObserver m_cross_entry; - - std::vector predWinds; - - //Data for individual aircraft - std::map m_aircraft_iteration_stats; // cleared between iterations - std::map m_aircraft_scenario_stats; // never cleared - - // output data vectors - std::vector > > stateModelOutput; - std::vector im_commands; - std::vector > aircraft_speed_count_list; - std::vector ptis_b_report_list; - - // string vectors for file output - std::vector maintainOutput; - std::vector finalGSOutput; - std::vector mergePointOutput; - std::vector closestPointOutput; - - // Kinematic trajectory output objects. Each dumps kinematic trajectories - // over a whole scenario, for all iterations for all aircraft into a single - // file. - VerticalPathObserver *mOwnKinVertPathObs; // Outputs own kinematic predicted vertical paths. - VerticalPathObserver *mTargKinVertPathObs; // Outputs target kinematic predicted vertical paths. - -}; diff --git a/include/public/InvalidIndexException.h b/include/public/InvalidIndexException.h deleted file mode 100644 index af1a64c..0000000 --- a/include/public/InvalidIndexException.h +++ /dev/null @@ -1,32 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#ifndef INVALIDINDEXEXCEPTION_H -#define INVALIDINDEXEXCEPTION_H - -#include - -class InvalidIndexException : public std::exception { - public: - InvalidIndexException(const int value, const int low_limit, const int high_limit); - - explicit InvalidIndexException(char *); -}; - -#endif diff --git a/include/public/KinematicDescent4DPredictor.h b/include/public/KinematicDescent4DPredictor.h deleted file mode 100644 index 6b54e96..0000000 --- a/include/public/KinematicDescent4DPredictor.h +++ /dev/null @@ -1,161 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include - -#include "public/VerticalPredictor.h" - -namespace aaesim { -namespace open_source { -class KinematicDescent4DPredictor : public VerticalPredictor { - public: - enum KinematicDescentType { CONSTRAINED }; - - KinematicDescent4DPredictor(); - - virtual ~KinematicDescent4DPredictor(); - - void BuildVerticalPrediction(std::vector &horizontal_path, - std::vector &precalc_waypoints, - const WeatherPrediction &weather_prediction, const Units::Length &start_altitude, - const Units::Length &aircraft_distance_to_go); - - void SetMembers(const double &mach_descent, const Units::Speed ias_descent, const Units::Length cruise_altitude, - const Units::Length transition_altitude); - - void SetConditionsAtEndOfRoute(const Units::Length altitude_at_end_of_route, const Units::Speed ias_at_end_of_route); - - KinematicDescentType GetDescentType() const; - - virtual const Units::Length GetAltitudeAtEndOfRoute() const; - - const double GetDecelerationRateFPA() const; - - private: - void ConstrainedVerticalPath(std::vector &horizontal_path, - std::vector &precalc_waypoints, double deceleration, - double const_gamma_cas_term, double const_gamma_cas_er, double const_gamma_mach, - const WeatherPrediction &weather_prediction, - const Units::Length &aircraft_distance_to_go); - - VerticalPath ConstantCasVerticalPath(const VerticalPath &vertical_path, double altitude_at_end, - std::vector &horizontal_path, - std::vector &precalc_waypoints, double gamma, - const WeatherPrediction &weather_prediction, - const Units::Length &aircraft_distance_to_go); - - VerticalPath ConstantMachVerticalPath(const VerticalPath &vertical_path, double altitude_at_end, - std::vector &horizontal_path, - std::vector &precalc_waypoints, double gamma, - const WeatherPrediction &weather_prediction, - const Units::Length &aircraft_distance_to_go); - - VerticalPath ConstantGeometricFpaVerticalPath(const VerticalPath &vertical_path, double altitude_at_end, - double flight_path_angle, std::vector &horizontal_path, - std::vector &precalc_waypoints, - const WeatherPrediction &weather_prediction, - const Units::Length &aircraft_distance_to_go); - - VerticalPath ConstantFpaDecelerationVerticalPath(const VerticalPath &vertical_path, double altitude_at_end, - double deceleration, double velocity_cas_end, - double flight_path_angle, - std::vector &horizontal_path, - std::vector &precalc_waypoints, - const WeatherPrediction &weather_prediction, - const Units::Length &aircraft_distance_to_go); - - VerticalPath LevelVerticalPath(const VerticalPath &vertical_path, double x_end, - std::vector &horizontal_path, - const WeatherPrediction &weather_prediction, - const Units::Length &aircraft_distance_to_go); - - VerticalPath ConstantDecelerationVerticalPath(const VerticalPath &vertical_path, Units::Length distance_to_go, - Units::Length altitude_high, double deceleration, - double velocity_cas_end, std::vector &horizontal_path, - const WeatherPrediction &weather_prediction, - const Units::Length &aircraft_distance_to_go); - - VerticalPath LevelDecelerationVerticalPath(const VerticalPath &vertical_path, double deceleration, - double velocity_cas_end, std::vector &horizontal_path, - const WeatherPrediction &weather_prediction, - const Units::Length &aircraft_distance_to_go); - - VerticalPath LevelDecelerationVerticalPath(const VerticalPath &vertical_path, Units::Length distance_to_go, - double deceleration, double velocity_cas_end, - std::vector &horizontal_path, - const WeatherPrediction &weather_prediction, - const Units::Length &aircraft_distance_to_go); - - VerticalPath ConstantFpaToCurrentPositionVerticalPath(const VerticalPath &vertical_path, - std::vector &horizontal_path, - std::vector &precalc_waypoints, - double const_gamma_mach, - const WeatherPrediction &weather_prediction, - const Units::Length &aircraft_distance_to_go); - - void ComputeWindCoefficients(Units::Length altitude, Units::Angle course, - const WeatherPrediction &weather_prediction, Units::Speed ¶llel_wind_velocity, - Units::Speed &perpendicular_wind_velocity, Units::Speed &wind_velocity_x, - Units::Speed &wind_velocity_y); - - void TrimVerticalPath(VerticalPath &vertical_path, int path_index); - - KinematicDescentType m_kinematic_descent_type; - - Units::Length m_altitude_at_end_of_route; - - double m_deceleration_mps; - double m_deceleration_level_mps; - double m_deceleration_fpa_mps; - - double m_const_gamma_cas_term_rad; - double m_const_gamma_cas_er_rad; - double m_const_gamma_mach_rad; - - std::vector m_vertical_path_waypoint_index; - static const Units::Length m_vertical_tolerance_distance; - - bool m_prediction_too_low; - bool m_prediction_too_high; - - static log4cplus::Logger m_logger; -}; -} // namespace open_source -} // namespace aaesim - -inline aaesim::open_source::KinematicDescent4DPredictor::KinematicDescentType - aaesim::open_source::KinematicDescent4DPredictor::GetDescentType() const { - return m_kinematic_descent_type; -} - -inline const Units::Length aaesim::open_source::KinematicDescent4DPredictor::GetAltitudeAtEndOfRoute() const { - return m_altitude_at_end_of_route; -} - -inline void aaesim::open_source::KinematicDescent4DPredictor::SetConditionsAtEndOfRoute( - const Units::Length altitude_at_end_of_route, const Units::Speed ias_at_end_of_route) { - m_altitude_at_end_of_route = altitude_at_end_of_route; - m_ias_at_end_of_route = ias_at_end_of_route; -} - -inline const double aaesim::open_source::KinematicDescent4DPredictor::GetDecelerationRateFPA() const { - return m_deceleration_fpa_mps; -} diff --git a/include/public/KinematicTrajectoryPredictor.h b/include/public/KinematicTrajectoryPredictor.h deleted file mode 100644 index ea3f6a2..0000000 --- a/include/public/KinematicTrajectoryPredictor.h +++ /dev/null @@ -1,122 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include - -#include -#include - -#include "public/AircraftIntent.h" -#include "public/EuclideanTrajectoryPredictor.h" -#include "public/Guidance.h" -#include "public/HorizontalPath.h" -#include "public/KinematicDescent4DPredictor.h" -#include "public/PrecalcWaypoint.h" - -namespace aaesim { -namespace open_source { -class KinematicTrajectoryPredictor : public aaesim::open_source::EuclideanTrajectoryPredictor { - public: - KinematicTrajectoryPredictor(); - - KinematicTrajectoryPredictor(Units::Angle maximum_bank_angle, Units::Speed transition_ias, double transition_mach, - Units::Length transition_altitude_msl, Units::Length cruise_altitude_msl); - - KinematicTrajectoryPredictor(const KinematicTrajectoryPredictor &obj); - - virtual ~KinematicTrajectoryPredictor() = default; - - void CalculateWaypoints(const AircraftIntent &aircraft_intent, - const WeatherPrediction &weather_prediction) override final; - - KinematicTrajectoryPredictor &operator=(const KinematicTrajectoryPredictor &obj); - - const std::vector &GetVerticalPathDistances() const; - - const double GetVerticalPathDistanceByIndex(int index) const; - - const std::vector &GetVerticalPathTimes() const; - - const double GetVerticalPathTimeByIndex(int index) const; - - const std::vector &GetVerticalPathGroundspeeds() const; - - const std::vector &GetVerticalPathVelocities() const; - - // Deprecated - const double GetVerticalPathVelocityByIndex(int index) const; - - const Units::Speed GetVerticalPathCasByIndex(int index) const; - - const std::vector &GetVerticalPathAltitudes() const; - - const double GetVerticalPathAltitudeByIndex(const int index) const; - - std::shared_ptr GetKinematicDescent4dPredictor() const; - - private: - static log4cplus::Logger m_logger; -}; -} // namespace open_source -} // namespace aaesim - -inline const std::vector &aaesim::open_source::KinematicTrajectoryPredictor::GetVerticalPathDistances() const { - return m_vertical_predictor->GetVerticalPath().along_path_distance_m; -} - -inline const double aaesim::open_source::KinematicTrajectoryPredictor::GetVerticalPathDistanceByIndex(int index) const { - return m_vertical_predictor->GetVerticalPath().along_path_distance_m[index]; -} - -inline const std::vector &aaesim::open_source::KinematicTrajectoryPredictor::GetVerticalPathTimes() const { - return m_vertical_predictor->GetVerticalPath().time_to_go_sec; -} - -inline const double aaesim::open_source::KinematicTrajectoryPredictor::GetVerticalPathTimeByIndex(int index) const { - return m_vertical_predictor->GetVerticalPath().time_to_go_sec[index]; -} - -inline const std::vector &aaesim::open_source::KinematicTrajectoryPredictor::GetVerticalPathGroundspeeds() - const { - return m_vertical_predictor->GetVerticalPath().gs_mps; -} - -inline const std::vector &aaesim::open_source::KinematicTrajectoryPredictor::GetVerticalPathVelocities() const { - return m_vertical_predictor->GetVerticalPath().cas_mps; -} - -inline const double aaesim::open_source::KinematicTrajectoryPredictor::GetVerticalPathVelocityByIndex(int index) const { - return m_vertical_predictor->GetVerticalPath().cas_mps[index]; -} - -inline const Units::Speed aaesim::open_source::KinematicTrajectoryPredictor::GetVerticalPathCasByIndex( - int index) const { - return Units::MetersPerSecondSpeed(m_vertical_predictor->GetVerticalPath().cas_mps[index]); -} - -inline const std::vector &aaesim::open_source::KinematicTrajectoryPredictor::GetVerticalPathAltitudes() const { - return m_vertical_predictor->GetVerticalPath().altitude_m; -} - -inline const double aaesim::open_source::KinematicTrajectoryPredictor::GetVerticalPathAltitudeByIndex( - const int index) const { - return m_vertical_predictor->GetVerticalPath().altitude_m[index]; -} diff --git a/include/public/KiteTightTurnResolver.h b/include/public/KiteTightTurnResolver.h deleted file mode 100644 index c68051a..0000000 --- a/include/public/KiteTightTurnResolver.h +++ /dev/null @@ -1,111 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include "public/EuclideanTightTurnResolver.h" - -namespace aaesim::open_source { -class KiteTightTurnResolver final : public EuclideanTightTurnResolver { - public: - KiteTightTurnResolver() = default; - ~KiteTightTurnResolver() = default; - void ResolveTightTurnGeometry(const double courseChange1, const double courseChange2, const double legLength, - double &radius, double &turnDist) override { - CalculateUsingKite(courseChange1, courseChange2, legLength, radius, turnDist); - } - - private: - static void CalculateUsingKite(const double courseChange1, const double courseChange2, const double legLength, - double &radius, double &turnDist) { - // Calculates the trajectory for a turn when the leg length is less than the required turn distance. - // courseChange1 is first turn (change in m_path_course at the firat waypoint) in degrees. - // courseChange2 is second turn (change in m_path_course at the second waypoint)in degrees. - // legLength is the distance between the first waypoint and the second waypoint. - // Calculates the inscribed turn that is tangent to the m_path_course into the firat waypoint, - // tangent to the line between waypoint 1 and 2, and tangent to the m_path_course out of the - // second waypoint. - - // varaibles for Kite algorithm - double c, d; // tangent segments for Kite algorithm - - /* - // A - // / \ - // / \ - // B O C - // \ E / - // \ /F - // D - // E is center of inscribed circle - // F is tangent point of inscribed circle and segment_type CD - */ - - Units::Angle A, C, D; // Kite angles - double AC, OC, CD; // Kite side distances - Units::Angle ECD, ACE, ACO, OCE; // Angles for inscribed circle points - double BC, OE, OA, AE, AD, ED, EF; // distances for inscribed circle calculations - // radius; // equal to EF - - A = Units::ToUnsigned(Units::DegreesAngle(180) - Units::RadiansAngle(fabs(courseChange1))); - C = Units::ToUnsigned(Units::DegreesAngle(180) - Units::RadiansAngle(fabs(courseChange2))); - // B = C; // unused - D = Units::DegreesAngle(360) - (A + C + C); - // test for D positive - if (Units::DegreesAngle(D).value() < 0.0) // turns do not form a kite - { - LOG4CPLUS_INFO(m_logger, "Kite called for tight turn that cannot form a kite, Using half leg-length instead."); - radius = -1; - turnDist = 0; - return; - } - - // find kite side distances in meters - AC = legLength; - // AB = AC; // unused - OC = AC * sin(A / 2); - CD = OC / sin(D / 2); - // BD = CD; // unused - - // find radius of inscribed circle - ECD = C / 2; - ACE = ECD; - ACO = Units::DegreesAngle(90) - A / 2; - OCE = ACE - ACO; - BC = OC * 2; - OE = BC * tan(OCE) / 2; - OA = AC * cos(A / 2); - AE = OA + OE; - AD = AC * cos(A / 2) + CD * cos(D / 2); - ED = AD - AE; - EF = ED * sin(D / 2); - radius = EF; - - // find tangent segments - // segment_type a is the turn anticipation at angle A. - // segment_type b = c is the turn anticipation at angle C - // segment_type d is the turn anticipation at Angle D, which does not exist as a way point - d = ED * cos(D / 2); - c = CD - d; - // b= c; - // a = AC - c; - turnDist = AC - c; - } -}; -} // namespace aaesim::open_source diff --git a/include/public/LateralController.h b/include/public/LateralController.h deleted file mode 100644 index 9c91cd9..0000000 --- a/include/public/LateralController.h +++ /dev/null @@ -1,49 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include - -#include - -#include "public/EquationsOfMotionState.h" -#include "public/Guidance.h" -#include "public/TrueWeatherOperator.h" - -namespace aaesim::open_source { -struct LateralController { - virtual Units::Angle ComputeRollCommand( - const Guidance &guidance, const EquationsOfMotionState &equations_of_motion_state, - std::shared_ptr &sensed_weather) = 0; - virtual Units::Frequency GetRollGain() const = 0; -}; - -class NoTurnLateralController final : public LateralController { - public: - NoTurnLateralController() = default; - ~NoTurnLateralController() = default; - Units::Angle ComputeRollCommand( - const Guidance &guidance, const EquationsOfMotionState &equations_of_motion_state, - std::shared_ptr &sensed_weather) override { - return Units::zero(); - }; - Units::Frequency GetRollGain() const override { return Units::zero(); } -}; -} // namespace aaesim::open_source diff --git a/include/public/LatitudeLongitudePoint.h b/include/public/LatitudeLongitudePoint.h deleted file mode 100644 index e0d9778..0000000 --- a/include/public/LatitudeLongitudePoint.h +++ /dev/null @@ -1,87 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include -#include -#include - -#include - -#include "geolib/Geolib.h" -#include "public/EllipsoidalEarthModel.h" -#include "public/Waypoint.h" - -/* - * - */ -namespace aaesim { - -class LatitudeLongitudePoint { - public: - LatitudeLongitudePoint() = default; - - LatitudeLongitudePoint(const Units::SignedAngle &wgs84_latitude, const Units::SignedAngle &wgs84_longitude); - - ~LatitudeLongitudePoint() = default; - - bool operator==(const LatitudeLongitudePoint &rhs) const; - bool operator!=(const LatitudeLongitudePoint &rhs) const; - - Units::SignedAngle GetLatitude() const; - - Units::SignedAngle GetLongitude() const; - - const geolib_idealab::LLPoint &GetGeolibPrimitiveLLPoint() const; - - bool ArePointsEqual(const LatitudeLongitudePoint &test_point) const; - - static LatitudeLongitudePoint CreateFromGeolibPrimitive(geolib_idealab::LLPoint ll_point); - - static LatitudeLongitudePoint CreateFromWaypoint(const Waypoint &wgs84_waypoint); - - static LatitudeLongitudePoint CreateFromGeodeticPosition( - const EllipsoidalEarthModel::GeodeticPosition &geodetic_position); - - /** - * - * @param projection_distance - * @param course_enu this is the angle in the ENU convention used by aaesim - * @see GeolibUtils::CalculateNewPoint - * @return - */ - LatitudeLongitudePoint ProjectDistanceAlongCourse(Units::Length projection_distance, - Units::SignedAngle course_enu) const; - - /** - * Get the defined relationship (distance and course) between "this" and "other". - * - * @param other_point - * @return defined relationship pair - * @see GeolibUtils::CalculateRelationshipBetweenPoints - */ - std::pair CalculateRelationshipBetweenPoints( - const LatitudeLongitudePoint &other_point) const; - - private: - static log4cplus::Logger m_logger; - geolib_idealab::LLPoint m_llpoint{}; -}; -} // namespace aaesim diff --git a/include/public/LawOfSinesResolver.h b/include/public/LawOfSinesResolver.h deleted file mode 100644 index 60e73ba..0000000 --- a/include/public/LawOfSinesResolver.h +++ /dev/null @@ -1,57 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include "public/EuclideanTightTurnResolver.h" - -namespace aaesim::open_source { -class LawOfSinesResolver final : public EuclideanTightTurnResolver { - public: - LawOfSinesResolver() = default; - ~LawOfSinesResolver() = default; - void ResolveTightTurnGeometry(const double courseChange1, const double courseChange2, const double legLength, - double &radius, double &turnDist) override { - CalculateUsingLawOfSines(courseChange1, courseChange2, legLength, radius, turnDist); - } - - private: - static void CalculateUsingLawOfSines(const double courseChange1, const double courseChange2, const double legLength, - double &radius, double &turnDist) { - // Calculates the trajectory for a turn when the leg length is less than the required turn distance. - // courseChange1 is first turn (change in m_path_course at the first waypoint). - // courseChange2 is second turn (change in m_path_course at the second waypoint). - // legLength is the distance between the first waypoint and the second waypoint. - // Calculates the inscribed turn that is tangent to the m_path_course into the first waypoint, - // tangent to the line between waypoint 1 and 2, and tangent to the m_path_course out of the - // second waypoint. - // output parameters: - // radius - the radius of the inscribed circle - // turnDist - the turn anticipation distance for the first turn - const auto A = Units::ToUnsigned(Units::DegreesAngle(180) - Units::RadiansAngle(fabs(courseChange1))); - const auto B = Units::ToUnsigned(Units::DegreesAngle(180) - Units::RadiansAngle(fabs(courseChange2))); - const auto AOB = Units::ToUnsigned(Units::DegreesAngle(180) - A / 2 - B / 2); - const auto AO = legLength * sin(B / 2) / sin(AOB); - const auto AOC = Units::ToUnsigned(Units::DegreesAngle(90) - A / 2); - radius = AO * sin(A / 2); - const auto AC = AO * sin(AOC); - turnDist = AC; - } -}; -} // namespace aaesim::open_source diff --git a/include/public/LegacyPositionEstimator.h b/include/public/LegacyPositionEstimator.h deleted file mode 100644 index 6658958..0000000 --- a/include/public/LegacyPositionEstimator.h +++ /dev/null @@ -1,43 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include - -#include "public/EllipsoidalPositionEstimator.h" -#include "public/TangentPlaneSequence.h" - -namespace aaesim::open_source { -class LegacyPositionEstimator final : public EllipsoidalPositionEstimator { - public: - LegacyPositionEstimator(const std::shared_ptr &position_converter, - const EarthModel::GeodeticPosition &initial_position) - : m_tangent_plane_sequence(position_converter), m_last_resolved_position(initial_position) {} - ~LegacyPositionEstimator() = default; - void ComputePosition(const SimulationTime &simtime, const EquationsOfMotionState &eqm_state, - const EquationsOfMotionStateDeriv &eqm_state_derivative, EarthModel::GeodeticPosition &position, - LatLonDerivative &position_rate) override; - - private: - EarthModel::GeodeticPosition ComputeLatLon(const EquationsOfMotionState &eqm_state) const; - std::shared_ptr m_tangent_plane_sequence; - EarthModel::GeodeticPosition m_last_resolved_position{}; -}; -} // namespace aaesim::open_source diff --git a/include/public/LineOnEllipsoid.h b/include/public/LineOnEllipsoid.h deleted file mode 100644 index c55775f..0000000 --- a/include/public/LineOnEllipsoid.h +++ /dev/null @@ -1,86 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include - -#include - -#include "public/LatitudeLongitudePoint.h" -#include "public/ShapeOnEllipsoid.h" - -namespace aaesim { -class LineOnEllipsoid final : public ShapeOnEllipsoid { - public: - LineOnEllipsoid() = default; - - LineOnEllipsoid(const geolib_idealab::Geodesic &geodesic); - - ~LineOnEllipsoid() = default; - - kShapeType GetShapeType() const override; - - Units::SignedAngle GetForwardCourseEnuAtStartPoint() const override; - - Units::SignedAngle GetForwardCourseEnuAtEndPoint() const override; - - Units::Length GetShapeLength() const override; - - LatitudeLongitudePoint GetStartPoint() const override; - - LatitudeLongitudePoint GetEndPoint() const override; - - const geolib_idealab::Geodesic &GetGeolibPrimitiveGeodesic() const; - - const geolib_idealab::LineType GetLineType() const; - - bool IsPointOnShape(const LatitudeLongitudePoint &test_point) const override; - - LineOnEllipsoid CreateExtendedLine(const Units::Length extended_distance) const; - - kDirectionRelativeToShape GetRelativeDirection(const LatitudeLongitudePoint &point_not_on_shape) const override; - - Units::Length GetDistanceToEndPoint(const LatitudeLongitudePoint &latitude_longitude_point) const override; - - static LineOnEllipsoid CreateFromPoints(const aaesim::LatitudeLongitudePoint &start_point, - const aaesim::LatitudeLongitudePoint &end_point); - - LatitudeLongitudePoint GetNearestPointOnShape(const LatitudeLongitudePoint &point_not_on_shape) const override; - - LatitudeLongitudePoint CalculatePointAtDistanceFromStartPoint( - const Units::Length &distance_along_shape_from_start_point) const override; - - std::pair CalculateCourseAtDistanceFromStartPoint( - const Units::Length &distance_along_shape_from_start_point) const override; - - protected: - Units::Length CalculateDistanceFromPointOnShapeToEnd(const LatitudeLongitudePoint &point_on_shape) const override; - - private: - static log4cplus::Logger m_logger; - void ComputeUnitVectorNormalToLineStartEnd(); - - geolib_idealab::Geodesic geolib_geodesic_{}; - EarthModel::AbsolutePositionEcef unit_vector_normal_to_line_start_end_; -}; - -inline ShapeOnEllipsoid::kShapeType LineOnEllipsoid::GetShapeType() const { return LINE; } - -} // namespace aaesim diff --git a/include/public/LocalTangentPlane.h b/include/public/LocalTangentPlane.h deleted file mode 100644 index ad7514f..0000000 --- a/include/public/LocalTangentPlane.h +++ /dev/null @@ -1,99 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -/* - * LocalTangentPlane.h - * - * Created on: Jun 25, 2015 - * Author: klewis - */ - -#pragma once - -#include - -#include - -#include "public/DMatrix.h" -#include "public/EarthModel.h" - -class LocalTangentPlane { - public: - static void printCoordinates(const std::string &title, Units::Length x, Units::Length y, Units::Length z); - - LocalTangentPlane(const EarthModel *earthModel, const EarthModel::AbsolutePositionEcef &ecefPointOfTangency, - const EarthModel::LocalPositionEnu &enuPointOfTangency); - - virtual ~LocalTangentPlane(); - - /** - * Sets the rotation matrices to what they would be - * at (0N, 0E) in a typical round-earth model. That is, - * x = Up, y = East, z = North. - * - * The value from the constructor, (x = East, y = North, - * z = Up), is more useful for flat-earth models. - */ - void InitializeRotationForGeodeticOrigin(); - - /** - * This method rotates the ENU frame. The new rotation around - * the vector , angle theta, is appended to the end of - * the existing m_ecef_to_enu rotation. Its inverse is prepended - * to the beginning of the existing m_enu_to_ecef transformation. - */ - void RotateEnuFrame(const double x, const double y, const double z, const Units::Angle theta); - - /* lla2ecef */ - void ConvertGeodeticToAbsolute(const EarthModel::GeodeticPosition &geo, - EarthModel::AbsolutePositionEcef &ecef) const; - - /* ecef2lla */ - void ConvertAbsoluteToGeodetic(const EarthModel::AbsolutePositionEcef &ecef, - EarthModel::GeodeticPosition &geo) const; - - /* enu2ecef */ - void ConvertLocalToAbsolute(const EarthModel::LocalPositionEnu &enu, EarthModel::AbsolutePositionEcef &ecef) const; - - /* ecef2enu */ - void ConvertAbsoluteToLocal(const EarthModel::AbsolutePositionEcef &ecef, EarthModel::LocalPositionEnu &enu) const; - - /* lla2enu */ - void ConvertGeodeticToLocal(const EarthModel::GeodeticPosition &geo, EarthModel::LocalPositionEnu &enu) const; - - /* enu2lla */ - void ConvertLocalToGeodetic(const EarthModel::LocalPositionEnu &enu, EarthModel::GeodeticPosition &geo) const; - - const EarthModel::LocalPositionEnu &getPointOfTangencyEnu() const; - - const EarthModel::AbsolutePositionEcef &getPointOfTangencyEcef() const; - - private: - static log4cplus::Logger logger; - /** The model which created us, and which we use for geodetic conversion */ - const EarthModel *earthModel; - /** The point which maps to pointOfTangencyEnu */ - EarthModel::AbsolutePositionEcef pointOfTangencyEcef; - EarthModel::LocalPositionEnu pointOfTangencyEnu; - /** Rotation matrix for (x,y,z)-->(e,n,u) */ - DMatrix m_ecef_to_enu; - /** Rotation matrix for (e,n,u)-->(x,y,z), the inverse */ - DMatrix m_enu_to_ecef; - const static double identity3x3[3][3]; // = { { 1, 0, 0}, {0, 1, 0}, {0, 0, 1}}; -}; diff --git a/include/public/Log4cplusSetup.h b/include/public/Log4cplusSetup.h deleted file mode 100644 index d4b3231..0000000 --- a/include/public/Log4cplusSetup.h +++ /dev/null @@ -1,50 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// 2023 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include -#include -#include -#include -#include - -static bool logging_initialized_ = false; - -static void LoadLoggerProperties() { - if (logging_initialized_) return; - logging_initialized_ = true; - - auto logger = log4cplus::Logger::getInstance(LOG4CPLUS_TEXT("logging.init")); - auto prop_file = getenv("LOG4CPLUS_PROPERTIES"); - if (prop_file == NULL) { - // fall back to log4cplus.properties - prop_file = (char *)"log4cplus.properties"; - } - - if (access(prop_file, F_OK) == -1) { - log4cplus::BasicConfigurator config; - config.configure(); - return; - } - - log4cplus::PropertyConfigurator config(prop_file); - config.configure(); - LOG4CPLUS_TRACE(logger, "LOG4CPLUS_PROPERTIES file is " << LOG4CPLUS_TEXT(prop_file)); -} diff --git a/include/public/MaintainMetric.h b/include/public/MaintainMetric.h deleted file mode 100644 index 2e28882..0000000 --- a/include/public/MaintainMetric.h +++ /dev/null @@ -1,87 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// 2022 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include "math/Statistics.h" - -#ifndef CYCLE_THRESHOLD -#define CYCLE_THRESHOLD 10 -#endif - - -// Storage metrics class for statistics gathered during the maintain phase -// of the flight. - -class MaintainMetric -{ -public: - MaintainMetric(void); - - ~MaintainMetric(void); - - // Adds data to be added for each pass through an IM::update method. - void AddSpacingErrorSec(double err); - - // Sets time aircraft went by achieve by point. - void SetTimeAtAbp(double time); - - // Boolean to determine if achieveBy set (achieveBy < 0.0) - bool TimeAtAbpRecorded(); - - // Computes total maintain time subtracting the achieveByTime - // from the current time. - void ComputeTotalMaintainTime(double cTime); - - // Gets mean spacing error. - double getMeanErr(); - - // Gets standard deviation of spacing error. - double getStdErr(); - - // Gets 95th bound of spacing error. - double getBound95(); - - // Gets total maintain time. - double getTotMaintain(); - - // Gets number of cycles with spacing errors > cycle threshold - int getNumCycles(); - - // Returns whether there are data samples to collect metrics from. - bool hasSamples(); - bool IsOutputEnabled() const; - void SetOutputEnabled(bool output_enabled); - -private: - // Running sum of time spacing errors between IM and target ac. - Statistics spacingError; - - // Time went by achieve by point. - double achieveByTime; - - // Time spent in maintain stage, (current time - achieve by time) - double totalMaintainTime; - - // Number of cycles with a spacing error > 10 secs. - int numCyclesOutsideThreshold; - - /** Output should be enabled for IM aircraft */ - bool m_output_enabled; -}; diff --git a/include/public/MergePointMetric.h b/include/public/MergePointMetric.h deleted file mode 100644 index 6c0733a..0000000 --- a/include/public/MergePointMetric.h +++ /dev/null @@ -1,86 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// 2022 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include "public/AircraftIntent.h" -#include - -// Class to compute distance between IM aircraft and target aircraft when -// IM aircraft is at the merge point of the IM and target route. - -class MergePointMetric -{ - - -public: - MergePointMetric(void); - - ~MergePointMetric(void); - - // Determines and stores the merge point. - void determineMergePoint(const AircraftIntent &IMIntent, - const AircraftIntent &targIntent); - - // Updates IM and target position. - void update(double imXNew, - double imYNew, - double targXNew, - double targYNew); - - // Gets merge point (waypoint name). - std::string getMergePoint(); - - // Gets computed distance. - Units::Length getDist(); - - // Returns whether merge point is set or not. - bool mergePointFound(); - - bool willReportMetrics() const; - int GetImAcId() const; - int GetTargetAcId() const; - -private: - static log4cplus::Logger logger; - - // Checks if newest IM position closer to waypoint than the stored IM position. - bool newPointCloser(double x, - double y); - - int m_im_ac_id; - int m_target_ac_id; - - std::string mMergePointName; - Units::Length mMergePointX; - Units::Length mMergePointY; - - double mIMX; // ft - double mIMY; // ft - - Units::Length mIMDist; - - double mTargX; // ft - double mTargY; // ft - - Units::Length mMergeDist; - - bool mReportMetrics; - -}; diff --git a/include/public/NMObserver.h b/include/public/NMObserver.h deleted file mode 100644 index f696220..0000000 --- a/include/public/NMObserver.h +++ /dev/null @@ -1,60 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// 2022 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include -#include "public/NMObserverEntry.h" -#include "math/Statistics.h" - -class NMObserver -{ -public: - NMObserver(void); - - ~NMObserver(void); - - // adds a new output entry to the Nautical Mile Observer - void output_NM_values(double predictedDistance, - double trueDistance, - double time, - double currIAS, - double currGS, - double targetGS, - double minIAS, - double maxIAS, - double minTAS, - double maxTAS); - - std::vector entry_list; - - std::vector predictedDistance; - std::vector trueDistance; - std::vector time; - std::vector ac_IAS_stats; - std::vector ac_GS_stats; - std::vector target_GS_stats; - std::vector min_IAS_stats; - std::vector max_IAS_stats; - - int curr_NM; - - void initialize_stats(); -}; - diff --git a/include/public/NMObserverEntry.h b/include/public/NMObserverEntry.h deleted file mode 100644 index 0c03b3b..0000000 --- a/include/public/NMObserverEntry.h +++ /dev/null @@ -1,40 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// 2022 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -class NMObserverEntry -{ -public: - NMObserverEntry(void); - - ~NMObserverEntry(void); - - double predictedDistance; - double trueDistance; - double time; - double acIAS; - double acGS; - double targetGS; - double minIAS; - double maxIAS; - double minTAS; - double maxTAS; -}; - diff --git a/include/public/NullADSBReceiver.h b/include/public/NullADSBReceiver.h deleted file mode 100644 index 58cea85..0000000 --- a/include/public/NullADSBReceiver.h +++ /dev/null @@ -1,61 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include -#include - -#include "public/ADSBReceiver.h" - -namespace aaesim { -namespace open_source { -class NullADSBReceiver final : public ADSBReceiver { - public: - NullADSBReceiver() = default; - virtual ~NullADSBReceiver() = default; - aaesim::open_source::ADSBSVReport GetCurrentADSBReport(int id) const override { - return aaesim::open_source::ADSBSVReport::EMPTY_REPORT; - } - aaesim::open_source::ADSBSVReport GetADSBReportBefore(int id, Units::Time time) const override { - return aaesim::open_source::ADSBSVReport::EMPTY_REPORT; - } - const std::vector &GetReportsReceivedByTime( - const SimulationTime &time) const override { - static std::vector empty; - return empty; - } - const std::map > &GetAllReportsReceived() const override { - static std::map > empty; - return empty; - } - std::map const GetCurrentADSBReport() const override { - static std::map empty; - return empty; - } - void Initialize(Units::Length adsb_reception_range_threshold) override {} - std::map Receive(const aaesim::open_source::SimulationTime &time, - const aaesim::open_source::AircraftState &state) override { - static std::map empty; - return empty; - } -}; -} // namespace open_source - -} // namespace aaesim diff --git a/include/public/NullAdsbTransmitter.h b/include/public/NullAdsbTransmitter.h deleted file mode 100644 index 73a63d1..0000000 --- a/include/public/NullAdsbTransmitter.h +++ /dev/null @@ -1,39 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include -#include - -#include "public/ADSBTransmitter.h" - -namespace aaesim { -namespace open_source { -class NullAdsbTransmitter final : public ADSBTransmitter { - const std::vector &GetAllTransmissions() const { - static std::vector empty; - return empty; - } - void Initialize(const std::list &waypoints_along_route) {} - void Transmit(const aaesim::open_source::SimulationTime &simulation_time, - const aaesim::open_source::AircraftState &nav_measurement) {} -}; -} // namespace open_source -} // namespace aaesim diff --git a/include/public/NullAtmosphere.h b/include/public/NullAtmosphere.h deleted file mode 100644 index 2b4dd21..0000000 --- a/include/public/NullAtmosphere.h +++ /dev/null @@ -1,92 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -/** - * NullAtmosphere is a placeholder class which throws logic_error for most - * methods. It should be replaced with an actual implementation of Atmosphere - * before use. - */ - -#pragma once - -#include - -class NullAtmosphere final : public Atmosphere { - public: - NullAtmosphere() {} - - NullAtmosphere(const Units::Temperature temperature_offset) {} - - virtual ~NullAtmosphere() = default; - - Atmosphere *Clone() const { throw std::logic_error("NullAtmosphere algorithms are not implemented."); } - - void CalibrateTemperatureAtAltitude(const Units::KelvinTemperature temperature, const Units::Length altitude) { - throw std::logic_error("NullAtmosphere algorithms are not implemented."); - } - - void AirDensity(const Units::Length h, Units::Density &rho, Units::Pressure &P) const { - throw std::logic_error("NullAtmosphere algorithms are not implemented."); - } - - Units::KelvinTemperature GetTemperature(const Units::Length altitude_msl) const { - throw std::logic_error("NullAtmosphere algorithms are not implemented."); - } - - Units::KelvinTemperature GetSeaLevelTemperature() const { - throw std::logic_error("NullAtmosphere algorithms are not implemented."); - } - - Units::Density GetSeaLevelDensity() const { - throw std::logic_error("NullAtmosphere algorithms are not implemented."); - } - - Units::MetersLength GetTropopauseHeight() const { - throw std::logic_error("NullAtmosphere algorithms are not implemented."); - } - - Units::Density GetTropopauseDensity() const { - throw std::logic_error("NullAtmosphere algorithms are not implemented."); - } - - Units::Pressure GetTropopausePressure() const { - throw std::logic_error("NullAtmosphere algorithms are not implemented."); - } - - Units::Speed CAS2TAS(const Units::Speed vcas, const Units::Pressure p, const Units::Density rho) const { - throw std::logic_error("NullAtmosphere algorithms are not implemented."); - } - - Units::Speed TAS2CAS(const Units::Speed vtas, const Units::Pressure p, const Units::Density rho) const { - throw std::logic_error("NullAtmosphere algorithms are not implemented."); - } - - Units::Speed SpeedOfSound(Units::KelvinTemperature temperature) const { - throw std::logic_error("NullAtmosphere algorithms are not implemented."); - } - - double ESFconstantCAS(const Units::Speed true_airspeed, const Units::Length altitude_msl, - const Units::KelvinTemperature temperature) const { - throw std::logic_error("NullAtmosphere algorithms are not implemented."); - } - - Units::Length GetMachIASTransition(const Units::Speed ias, const double mach) const { - throw std::logic_error("NullAtmosphere algorithms are not implemented."); - } -}; diff --git a/include/public/NullFlightDeckApplication.h b/include/public/NullFlightDeckApplication.h deleted file mode 100644 index 942a96f..0000000 --- a/include/public/NullFlightDeckApplication.h +++ /dev/null @@ -1,43 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include "public/FlightDeckApplication.h" - -namespace aaesim { -namespace open_source { -class NullFlightDeckApplication final : public aaesim::open_source::FlightDeckApplication { - public: - NullFlightDeckApplication() = default; - virtual ~NullFlightDeckApplication() = default; - void Initialize(FlightDeckApplicationInitializer &initializer_visitor) override { /**/ } - aaesim::open_source::Guidance Update(const aaesim::open_source::SimulationTime &simtime, - const aaesim::open_source::Guidance ¤t_guidance, - const aaesim::open_source::DynamicsState &dynamics_state, - const aaesim::open_source::AircraftState &own_state) override { - aaesim::open_source::Guidance invalid_guidance{current_guidance}; - invalid_guidance.SetValid(false); - return invalid_guidance; - } - bool IsActive() const override { return false; } -}; -} // namespace open_source - -} // namespace aaesim diff --git a/include/public/NullPilotDelay.h b/include/public/NullPilotDelay.h deleted file mode 100644 index f0059db..0000000 --- a/include/public/NullPilotDelay.h +++ /dev/null @@ -1,39 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include "public/PilotDelay.h" - -namespace aaesim::open_source { -class NullPilotDelay final : public aaesim::open_source::PilotDelay { - public: - NullPilotDelay() = default; - - Units::Speed UpdateIAS(Units::Speed previous_speed_command_ias, Units::Speed proposed_speed_command_ias, - Units::Length current_altitude, Units::Length altitude_at_end_of_route) override { - return proposed_speed_command_ias; - }; - - Units::Speed UpdateMach(double previous_speed_command_as_mach, double proposed_speed_command_as_mach, - Units::Length current_altitude, Units::Length altitude_at_end_of_route) override { - return Units::zero(); - } -}; -} // namespace aaesim::open_source diff --git a/include/public/NullPositionEstimator.h b/include/public/NullPositionEstimator.h deleted file mode 100644 index 3505eab..0000000 --- a/include/public/NullPositionEstimator.h +++ /dev/null @@ -1,37 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include "public/EllipsoidalPositionEstimator.h" - -namespace aaesim::open_source { -class NullPositionEstimator final : public EllipsoidalPositionEstimator { - public: - NullPositionEstimator() = default; - ~NullPositionEstimator() = default; - void ComputePosition(const SimulationTime &simtime, const EquationsOfMotionState &eqm_state, - const EquationsOfMotionStateDeriv &eqm_state_derivative, EarthModel::GeodeticPosition &position, - LatLonDerivative &position_rate) override { - position = EarthModel::GeodeticPosition::Of(Units::ZERO_ANGLE, Units::ZERO_ANGLE); - position_rate.latitude_time_derivative = Units::zero(); - position_rate.longitude_time_derivative = Units::zero(); - }; -}; -} // namespace aaesim::open_source diff --git a/include/public/NullSpeedLimiter.h b/include/public/NullSpeedLimiter.h deleted file mode 100644 index 29eacf2..0000000 --- a/include/public/NullSpeedLimiter.h +++ /dev/null @@ -1,44 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include "public/SpeedCommandLimiter.h" - -namespace aaesim { -namespace open_source { -class NullSpeedLimiter final : public SpeedCommandLimiter { - public: - NullSpeedLimiter(); - - Units::Speed LimitSpeedCommand(const Units::Speed previous_ias_speed_command, - const Units::Speed current_ias_speed_command, - const Units::Speed reference_velocity_mps, - const Units::Length speed_quantization_distance, - const Units::Length distance_to_end_of_route, const Units::Length current_altitude, - const aaesim::open_source::bada_utils::FlapConfiguration flap_configuration) override; - - BoundedValue LimitMachCommand(const BoundedValue &previous_reference_speed_command_mach, - const BoundedValue ¤t_mach_command, - const BoundedValue &nominal_mach, - const Units::Mass ¤t_mass, const Units::Length ¤t_altitude, - const WeatherPrediction &weather_prediction) override; -}; -} // namespace open_source -} // namespace aaesim diff --git a/include/public/NullWindEvaluator.h b/include/public/NullWindEvaluator.h deleted file mode 100644 index c7a3147..0000000 --- a/include/public/NullWindEvaluator.h +++ /dev/null @@ -1,49 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include - -#include "public/PredictedWindEvaluator.h" - -/** - * NullWindEvaluator always indicates that winds are accurate. - */ -namespace aaesim { -namespace open_source { - -class NullWindEvaluator final : public PredictedWindEvaluator { - public: - const static std::shared_ptr GetInstance(); - - virtual ~NullWindEvaluator(); - - virtual bool ArePredictedWindsAccurate(const aaesim::open_source::AircraftState &state, - const WeatherPrediction &weather_prediction, const Units::Speed reference_cas, - const Units::Length reference_altitude, - const std::shared_ptr &sensed_atmosphere) const; - - private: - static std::shared_ptr m_instance; - - NullWindEvaluator(); -}; -} // namespace open_source -} // namespace aaesim diff --git a/include/public/OutputHandler.h b/include/public/OutputHandler.h deleted file mode 100644 index 56dcdef..0000000 --- a/include/public/OutputHandler.h +++ /dev/null @@ -1,75 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include - -#include "MiniCSV/minicsv.h" - -struct OutputHandler { - public: - OutputHandler() = default; - - /* - * The constructor takes the scenario name, and the desired file extension to describe the - * type of output contained in this file. - */ - OutputHandler(const std::string &scenario_name, const std::string &file_suffix) - : m_file_suffix(file_suffix), filename(scenario_name + file_suffix), os(), m_finished(false) {} - - virtual ~OutputHandler() = default; - - /** - * Writes the file, closes it, and clears data stores to save memory. - * Must also set m_finished to true. - * Not implemented at the OutputHandler level, must be done in a subclass. - * Finish() should only be called once during the life cycle of the object, - * to avoid overwriting the file. - */ - virtual void Finish() = 0; - - /** - * Implementations which don't open the output file immediately can - * use a dummy scenario name in the constructor and set it later - * using this function. - */ - virtual void SetScenarioName(const std::string &scenario_name); - - virtual std::string GetOutputFilename() const { return filename; } - - const std::string &GetFileSuffix() const { return m_file_suffix; } - - protected: - // Everything in the filename after the scenario name, e.g. "-waypoints.csv" - std::string m_file_suffix; - - // Full name of file to be written, including suffix - std::string filename; - - // Output stream that handles writing when object is destroyed - mini::csv::ofstream os; - - // indicates whether Finish() has been called, to complete writing - bool m_finished{false}; -}; - -inline void OutputHandler::SetScenarioName(const std::string &scenario_name) { - filename.assign(scenario_name + m_file_suffix); -} diff --git a/include/public/PassThroughAssap.h b/include/public/PassThroughAssap.h deleted file mode 100644 index 216ed33..0000000 --- a/include/public/PassThroughAssap.h +++ /dev/null @@ -1,45 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include - -#include "public/ASSAP.h" -#include "utility/CustomUnits.h" - -namespace aaesim { -namespace open_source { -class PassThroughAssap final : public aaesim::open_source::ASSAP { - public: - PassThroughAssap(); - aaesim::open_source::AircraftState Update(const aaesim::open_source::AircraftState &state_to_sync_with, - const aaesim::open_source::ADSBSVReport &most_recent_ads_b) const override; - - void Initialize(std::shared_ptr adsb_receiver) override; - - std::shared_ptr GetAdsbReceiver() const override { return m_adsb_receiver; } - - const Units::SecondsTime GetMaxCoastTime() const override { return Units::ZERO_TIME; } - - private: - std::shared_ptr m_adsb_receiver; -}; -} // namespace open_source -} // namespace aaesim diff --git a/include/public/PilotDelay.h b/include/public/PilotDelay.h deleted file mode 100644 index 768e3af..0000000 --- a/include/public/PilotDelay.h +++ /dev/null @@ -1,34 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include -#include -#include - -namespace aaesim::open_source { -struct PilotDelay { - virtual Units::Speed UpdateIAS(Units::Speed previous_speed_command_ias, Units::Speed proposed_speed_command_ias, - Units::Length current_altitude, Units::Length altitude_at_end_of_route) = 0; - - virtual Units::Speed UpdateMach(double previous_speed_command_as_mach, double proposed_speed_command_as_mach, - Units::Length current_altitude, Units::Length altitude_at_end_of_route) = 0; -}; -} // namespace aaesim::open_source diff --git a/include/public/PositionCalculator.h b/include/public/PositionCalculator.h deleted file mode 100644 index c8e84cb..0000000 --- a/include/public/PositionCalculator.h +++ /dev/null @@ -1,58 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include - -#include - -#include "DirectionOfFlightCourseCalculator.h" -#include "HorizontalPath.h" -#include "HorizontalPathTracker.h" - -namespace aaesim::open_source { - -/** - * Calculates position (euclidean x,y) for a given horizontal path (previously defined in x/y) and a - * distance along that path. - */ -class PositionCalculator : public DirectionOfFlightCourseCalculator { - public: - PositionCalculator(); - PositionCalculator(const std::vector &horizontal_path, - TrajectoryIndexProgressionDirection expected_index_progression); - virtual ~PositionCalculator(); - - /** - * @brief Calculate a position and course along the horizontal trajectory from a provided distance along the path. - */ - bool CalculatePositionFromAlongPathDistance(const Units::Length &distance_along_path, Units::Length &position_x, - Units::Length &position_y, Units::UnsignedAngle &course); - - private: - static log4cplus::Logger m_logger; - - bool CalculatePosition(const Units::Length &distance_along_path, - const std::vector &horizontal_trajectory, - const std::vector::size_type starting_trajectory_index, - Units::Length &x_position, Units::Length &y_position, Units::UnsignedAngle &course, - std::vector::size_type &resolved_trajectory_index); -}; -} // namespace aaesim::open_source diff --git a/include/public/PrecalcConstraint.h b/include/public/PrecalcConstraint.h deleted file mode 100644 index d7b4711..0000000 --- a/include/public/PrecalcConstraint.h +++ /dev/null @@ -1,88 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once -#include -#include - -#include -#include - -namespace aaesim::open_source { - -enum ActiveFlagType { - UNSET = 0, - BELOW_ALT_ON_SPEED = 1, - AT_ALT_ON_SPEED, - BELOW_ALT_SLOW, - SEG_END_LOW_ALT, - SEG_END_MID_ALT, - AT_ALT_SLOW, - SEG_END_AT_ALT, - AT_250_BELOW_10K, - AT_ALT_FAST -}; - -static std::string ActiveFlagAsString(const ActiveFlagType &flag) { - switch (flag) { - case UNSET: - return "UNSET"; - case BELOW_ALT_ON_SPEED: - return "BELOW_ALT_ON_SPEED"; - case AT_ALT_ON_SPEED: - return "AT_ALT_ON_SPEED"; - case BELOW_ALT_SLOW: - return "BELOW_ALT_SLOW"; - case SEG_END_LOW_ALT: - return "SEG_END_LOW_ALT"; - case SEG_END_MID_ALT: - return "SEG_END_MID_ALT"; - case AT_ALT_SLOW: - return "AT_ALT_SLOW"; - case SEG_END_AT_ALT: - return "SEG_END_AT_ALT"; - case AT_250_BELOW_10K: - return "AT_250_BELOW_10K"; - case AT_ALT_FAST: - return "AT_ALT_FAST"; - default: - throw std::runtime_error("Developer Error: This should be impossible"); - } -}; - -struct PrecalcConstraint final { - PrecalcConstraint() = default; - ~PrecalcConstraint() = default; - - PrecalcConstraint &operator=(const PrecalcConstraint &obj); - bool operator<(const PrecalcConstraint &obj) const; - bool operator!=(const PrecalcConstraint &obj) const; - bool operator==(const PrecalcConstraint &obj) const; - - Units::Length constraint_along_path_distance{Units::zero()}; - Units::Length constraint_altHi{Units::zero()}; - Units::Length constraint_altLow{Units::zero()}; - Units::Speed constraint_speedHi{Units::zero()}; - Units::Speed constraint_speedLow{Units::zero()}; - int index{-1}; - ActiveFlagType active_flag{ActiveFlagType::UNSET}; - bool violation_flag{false}; - bool is_last_constraint{false}; -}; -} // namespace aaesim::open_source diff --git a/include/public/PrecalcWaypoint.h b/include/public/PrecalcWaypoint.h deleted file mode 100644 index 95bdb9b..0000000 --- a/include/public/PrecalcWaypoint.h +++ /dev/null @@ -1,52 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include - -#include - -#include "public/PrecalcConstraint.h" - -class PrecalcWaypoint final { - public: - PrecalcWaypoint() = default; - - ~PrecalcWaypoint() = default; - - bool operator==(const PrecalcWaypoint &obj) const; - - std::string m_name{}; - - Units::Length m_leg_length{Units::zero()}; - Units::UnsignedRadiansAngle m_course_angle{Units::zero()}; - - Units::MetersLength m_x_pos_meters{Units::zero()}; - Units::MetersLength m_y_pos_meters{Units::zero()}; - - Units::MetersLength m_rf_leg_center_x{Units::zero()}; - Units::MetersLength m_rf_leg_center_y{Units::zero()}; - Units::MetersLength m_radius_rf_leg{Units::zero()}; - - Units::RadiansAngle m_bank_angle{Units::zero()}; - Units::MetersPerSecondSpeed m_ground_speed{Units::zero()}; - - aaesim::open_source::PrecalcConstraint m_precalc_constraints{}; -}; diff --git a/include/public/PredictedWindEvaluator.h b/include/public/PredictedWindEvaluator.h deleted file mode 100644 index abef822..0000000 --- a/include/public/PredictedWindEvaluator.h +++ /dev/null @@ -1,41 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include - -#include "public/AircraftState.h" -#include "public/WeatherPrediction.h" - -namespace aaesim { -namespace open_source { -struct PredictedWindEvaluator { - public: - PredictedWindEvaluator() = default; - - virtual ~PredictedWindEvaluator() = default; - - virtual bool ArePredictedWindsAccurate(const aaesim::open_source::AircraftState &state, - const aaesim::open_source::WeatherPrediction &weather, - const Units::Speed reference_cas, const Units::Length reference_altitude, - const std::shared_ptr &sensed_atmosphere) const = 0; -}; -} // namespace open_source -} // namespace aaesim diff --git a/include/public/PredictionFileBase.h b/include/public/PredictionFileBase.h deleted file mode 100644 index 3f45e03..0000000 --- a/include/public/PredictionFileBase.h +++ /dev/null @@ -1,105 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include -#include -#include -#include -#include - -#include -#include -#include - -#include "public/AircraftState.h" -#include "public/OutputHandler.h" -#include "public/VerticalPath.h" - -struct PredictionFileBase { - public: - PredictionFileBase() = default; - ~PredictionFileBase() = default; - - struct PredictionData { - enum DataSource { - INVALID_SOURCE = -1, - FMS_DESCENT = 0, - IM_ALGO_OWNSHIP, - IM_ALGO_TARGET, - FMS_ASCENT, - }; - - PredictionData() = default; - - int iteration_number{-1}; - Units::Time simulation_time{Units::SecondsTime(-1.0)}; - std::string acid{}; - DataSource source{PredictionData::INVALID_SOURCE}; - Units::Length altitude{Units::MetersLength(-1.0)}; - Units::Speed IAS{Units::MetersPerSecondSpeed(-1.0)}; - double mach{-1}; - Units::Speed GS{Units::MetersPerSecondSpeed(-1.0)}; - Units::Speed TAS{Units::MetersPerSecondSpeed(-1.0)}; - Units::Time time_to_go{Units::SecondsTime(-1.0)}; - Units::Length distance_to_go{Units::MetersLength(-1.0)}; - Units::MetersPerSecondSpeed VwePred{Units::MetersPerSecondSpeed(-1.0)}; - Units::MetersPerSecondSpeed VwnPred{Units::MetersPerSecondSpeed(-1.0)}; - VerticalPath::PredictionAlgorithmType algorithm{VerticalPath::PredictionAlgorithmType::UNDETERMINED}; - aaesim::open_source::bada_utils::FlapConfiguration flap_setting{ - aaesim::open_source::bada_utils::FlapConfiguration::UNDEFINED}; - Units::MetersPerSecondSpeed vertical_rate{Units::MetersPerSecondSpeed(-1.0)}; - Units::DegreesAngle flight_path_angle{Units::DegreesAngle(-1)}; - }; - - std::vector ExtractPredictionDataFromVerticalPath(const unsigned int &iteration, - const Units::Time simulation_time, - const std::string &acid, - const VerticalPath &vertical_path, - const PredictionData::DataSource &source) { - std::vector prediction_data; - - for (auto m = 0; m < vertical_path.along_path_distance_m.size(); ++m) { - PredictionFileBase::PredictionData pdata; - - pdata.iteration_number = iteration; - pdata.source = source; - pdata.acid = acid; - pdata.simulation_time = simulation_time; - - pdata.altitude = Units::MetersLength(vertical_path.altitude_m[m]); - pdata.IAS = Units::MetersPerSecondSpeed(vertical_path.cas_mps[m]); - pdata.mach = vertical_path.mach[m]; - pdata.GS = Units::MetersPerSecondSpeed(vertical_path.gs_mps[m]); - pdata.TAS = Units::MetersPerSecondSpeed(vertical_path.true_airspeed[m]); - pdata.time_to_go = Units::SecondsTime(vertical_path.time_to_go_sec[m]); - pdata.distance_to_go = Units::MetersLength(vertical_path.along_path_distance_m[m]); - pdata.VwePred = vertical_path.wind_velocity_east[m]; - pdata.VwnPred = vertical_path.wind_velocity_north[m]; - pdata.algorithm = vertical_path.algorithm_type[m]; - pdata.flap_setting = vertical_path.flap_setting[m]; - pdata.vertical_rate = Units::MetersPerSecondSpeed(vertical_path.altitude_rate_mps[m]); - pdata.flight_path_angle = Units::DegreesAngle(Units::RadiansAngle(vertical_path.theta_radians[m])); - - prediction_data.push_back(pdata); - } - return prediction_data; - } -}; diff --git a/include/public/RandomGenerator.h b/include/public/RandomGenerator.h deleted file mode 100644 index 43689ed..0000000 --- a/include/public/RandomGenerator.h +++ /dev/null @@ -1,101 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include - -class RandomGenerator final { - public: - RandomGenerator(); - - RandomGenerator(const double seed); - - ~RandomGenerator() = default; - - void SetSeed(const double seed); - - const double GetSeed(); - - const double UniformSample(); - - const double GaussianSample(); - - const double TruncatedGaussianSample(const double max_standard_deviation); - - const double RayleighSample(); - - const double LaplaceSample(); - - /** - * Returns a random sample from Gaussian distribution - * with average = mean and standard deviation = sigma. - */ - template - const T GaussianSample(const T mean, const T sigma) { - T v1 = mean + sigma * GaussianSample(); - return v1; - } - - /** - * Returns a random sample from Gaussian distribution - * with average = mean and standard deviation = sigma, - * with the deviation not exceeding maxstddev * sigma. - */ - template - const T TruncatedGaussianSample(const T mean, const T sigma, const double max_standard_deviation) { - // check for zero standard deviation - if (sigma * 0 == sigma) { - return mean; - } - T v1 = mean + sigma * TruncatedGaussianSample(max_standard_deviation); - return v1; - } - - /** - * Returns a random sample from a Raleigh distribution sample with - * the given mean and standard deviation. - */ - template - const T RayleighSample(const T mean, const T sigma) { - T v1 = mean + sigma * RayleighSample(); - return v1; - } - - /** - * Returns a random sample from a LaPlace distribution with - * the given value of lambda. - */ - template - const T LaplaceSample(const T lambda) { - T v1 = lambda * LaplaceSample(); - return v1; - } - - private: - static log4cplus::Logger m_logger; - - static const double m_IA; - static const double m_IM; - static const double m_AM; - static const double m_IQ; - static const double m_IR; - - double m_seed; -}; diff --git a/include/public/RefReader.h b/include/public/RefReader.h deleted file mode 100644 index 11427ee..0000000 --- a/include/public/RefReader.h +++ /dev/null @@ -1,49 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// 2022 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -/* - * RefReader.h - * - * Reads a Ref.csv file containing a sequence of reference values. - * - * Created on: Mar 23, 2019 - * Author: klewis - */ - -#pragma once - -#include "public/DataReader.h" - -namespace testvector { - -class RefReader : public DataReader { -public: - RefReader(std::string file_name, int header_lines, size_t expected_columns); - RefReader(std::shared_ptr input_stream, int header_lines, size_t expected_columns); - virtual ~RefReader(); - virtual bool Advance(); - const Units::SecondsTime GetTimeToFly() const; - -private: - Units::SecondsTime m_time_to_fly; // column 1 - -}; - - -} // namespace testvector diff --git a/include/public/RunFile.h b/include/public/RunFile.h deleted file mode 100644 index 44aa6fc..0000000 --- a/include/public/RunFile.h +++ /dev/null @@ -1,32 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// 2022 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include -#include "public/Scenario.h" - -class RunFile { - public: - RunFile(void); - - ~RunFile(void); - - std::vector > > scenarios; -}; diff --git a/include/public/Scenario.h b/include/public/Scenario.h deleted file mode 100644 index 31dddaf..0000000 --- a/include/public/Scenario.h +++ /dev/null @@ -1,40 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include - -namespace aaesim::open_source { -class Scenario { - public: - Scenario() : m_scenario_name() {} - virtual ~Scenario() = default; - virtual void SimulateAllIterations() = 0; - virtual const std::string &GetScenarioName() const; - void SetScenarioName(const std::string &in); - - private: - std::string m_scenario_name; -}; - -inline const std::string &Scenario::GetScenarioName() const { return m_scenario_name; } - -inline void Scenario::SetScenarioName(const std::string &scenario_name) { m_scenario_name = scenario_name; } -} // namespace aaesim::open_source diff --git a/include/public/ScenarioEntity.h b/include/public/ScenarioEntity.h deleted file mode 100644 index 13c0e34..0000000 --- a/include/public/ScenarioEntity.h +++ /dev/null @@ -1,34 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include "public/SimulationTime.h" - -namespace aaesim { -namespace open_source { -struct ScenarioEntity { - ScenarioEntity() = default; - virtual ~ScenarioEntity() = default; - virtual bool Update(const SimulationTime &simulation_time) = 0; - virtual const int GetStartTime() const = 0; - virtual bool IsFinished() const = 0; -}; -} // namespace open_source -} // namespace aaesim diff --git a/include/public/ScenarioEventNotifier.h b/include/public/ScenarioEventNotifier.h deleted file mode 100644 index 81a5907..0000000 --- a/include/public/ScenarioEventNotifier.h +++ /dev/null @@ -1,33 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include -#include - -namespace aaesim::open_source { -struct ScenarioEventNotifier { - virtual void IterationBegin(const int &iteration_number) = 0; - virtual void IterationEnd(const int &iteration_number) = 0; - virtual void ScenarioBegin(const std::string &scenario_name) = 0; - virtual void ScenarioEnd(const std::string &scenario_name) = 0; - virtual void ErrorOccurred(const int &iteration_number, const std::exception &exception_object) = 0; -}; -} // namespace aaesim::open_source diff --git a/include/public/ScenarioUtils.h b/include/public/ScenarioUtils.h deleted file mode 100644 index bdcb20f..0000000 --- a/include/public/ScenarioUtils.h +++ /dev/null @@ -1,78 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include -#include - -#include "public/RandomGenerator.h" - -namespace aaesim::open_source { -class ScenarioUtils { - public: - ~ScenarioUtils() = default; - - static RandomGenerator RANDOM_NUMBER_GENERATOR; - static const int AIRCRAFT_ID_NOT_IN_MAP; - static void ClearAircraftIdMap() { m_aircraft_string_int_map.clear(); } - static std::string GetAircraftIdForUniqueId(const int unique_id) { - for (const auto &element : m_aircraft_string_int_map) { - if (element.second == unique_id) return element.first; - } - return ""; - } - static int GetUniqueIdForAircraftId(const std::string &aircraft_id) { - bool is_in_map = m_aircraft_string_int_map.find(aircraft_id) != m_aircraft_string_int_map.end(); - if (is_in_map) { - return m_aircraft_string_int_map[aircraft_id]; - } - return AIRCRAFT_ID_NOT_IN_MAP; - } - static int GenerateNewUniqueIdForAircraftId(const std::string &aircraft_id) { - int old_id = GetUniqueIdForAircraftId(aircraft_id); - if (old_id == AIRCRAFT_ID_NOT_IN_MAP) { - int new_id = m_aircraft_string_int_map.size(); - m_aircraft_string_int_map[aircraft_id] = new_id; - return new_id; - } else { - return old_id; - } - } - static std::string ResolveScenarioRootName(const std::string &full_scenario_filename) { - std::string local_scenario_name{full_scenario_filename}; - - // remove the leading directory structure if present (search for last instance of "/" or "\\") - auto index = local_scenario_name.find_last_of("/\\"); - if (index != std::string::npos) { - local_scenario_name = local_scenario_name.substr(index + 1); - } - - index = local_scenario_name.find(".txt"); - if (index != std::string::npos) { - local_scenario_name.erase(index, 4); - } - return local_scenario_name; - } - - private: - static std::map m_aircraft_string_int_map; - ScenarioUtils() = default; -}; -} // namespace aaesim::open_source diff --git a/include/public/ShapeOnEllipsoid.h b/include/public/ShapeOnEllipsoid.h deleted file mode 100644 index 862c64a..0000000 --- a/include/public/ShapeOnEllipsoid.h +++ /dev/null @@ -1,133 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include - -#include - -#include "LatitudeLongitudePoint.h" - -/* - * An interface class for shapes on an ellipsoid. Lines, arcs, and any other defined shape - * should inherit from this class. - */ -namespace aaesim { -class ShapeOnEllipsoid { - public: - enum kShapeType { NO_SHAPE = INT32_MIN, LINE = 0, ARC = 1 }; - - enum kDirectionRelativeToShape { UNSET = INT32_MIN, LEFT_OF_SHAPE = -1, RIGHT_OF_SHAPE = 1, ON_SHAPE = 0 }; - - ShapeOnEllipsoid() = default; - - virtual ~ShapeOnEllipsoid() = default; - - virtual kShapeType GetShapeType() const { return NO_SHAPE; }; - - virtual Units::SignedAngle GetForwardCourseEnuAtStartPoint() const = 0; - - virtual Units::SignedAngle GetForwardCourseEnuAtEndPoint() const = 0; - - virtual LatitudeLongitudePoint GetStartPoint() const = 0; - - virtual LatitudeLongitudePoint GetEndPoint() const = 0; - - virtual LatitudeLongitudePoint CalculatePointAtDistanceFromStartPoint( - const Units::Length &distance_along_shape_from_start_point) const = 0; - - virtual std::pair CalculateCourseAtDistanceFromStartPoint( - const Units::Length &distance_along_shape_from_start_point) const = 0; - - /** - * Returns the length of the shape along the ellipsoid. - * - * @return a Units::Length object. Any negative value indicates a failed calculation. - */ - virtual Units::Length GetShapeLength() const = 0; - - /** - * For a given latitude_longitude_point, determines the side relative to the shape's direction (defined by the start - * and end point). - * - * @param latitude_longitude_point - * @return see return enum for possibilities - */ - virtual kDirectionRelativeToShape GetRelativeDirection( - const LatitudeLongitudePoint &latitude_longitude_point) const { - return UNSET; - } - - /** - * Test a point for being on the shape. - * - * Does not project to the shape. - * - * @param test_point - * @return true if on the shape, false otherwise - */ - virtual bool IsPointOnShape(const LatitudeLongitudePoint &test_point) const { return false; } - - /** - * For a test point which may or may not be on the shape, return the distance along the shape to the end - * point. If the test point is not on the shape, it will be projected to the shape and that point used to - * determine the length to return. - * - * @param latlon_point - * @return a Units::Length object. Any negative value indicates a failed calculation. - */ - virtual Units::Length GetDistanceToEndPoint(const LatitudeLongitudePoint &latlon_point) const { - Units::Length distance_to_end = Units::negInfinity(); - if (IsPointOnShape(latlon_point)) { - distance_to_end = CalculateDistanceFromPointOnShapeToEnd(latlon_point); - } else { - const LatitudeLongitudePoint perpendicular_point = GetNearestPointOnShape(latlon_point); - distance_to_end = CalculateDistanceFromPointOnShapeToEnd(perpendicular_point); - } - - return distance_to_end; - } - - /** - * Project from point_not_on_shape to the shape, making a perpendicular line through the shape. Return that point - * that has been calculated as the projected point. - * - * This returned point also defines the nearest location on the shape to point_not_on_shape. - * - * @param point_not_on_shape - * @return - */ - virtual LatitudeLongitudePoint GetNearestPointOnShape(const LatitudeLongitudePoint &point_not_on_shape) const { - return LatitudeLongitudePoint(); - } - - protected: - /** - * This is intentionally not public. - * - * @see GetDistanceToEndPoint() for the public implementation - * @param point_on_shape. The caller is responsible for ensuring that this point is on the shape. - * @return a Units::Length object - */ - virtual Units::Length CalculateDistanceFromPointOnShapeToEnd(const LatitudeLongitudePoint &point_on_shape) const { - return Units::negInfinity(); - } -}; -} // namespace aaesim diff --git a/include/public/SimulationTime.h b/include/public/SimulationTime.h deleted file mode 100644 index d60f9fd..0000000 --- a/include/public/SimulationTime.h +++ /dev/null @@ -1,86 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include - -#include - -namespace aaesim::open_source { -class SimulationTime final { - public: - static inline const Units::SecondsTime SIMULATION_TIME_STEP = Units::SecondsTime(1.0); - - // visible for testing - static void SetSimulationTimeStep(Units::Time in) { m_simulation_time_step = in; } - - static const Units::SecondsTime GetSimulationTimeStep() { return m_simulation_time_step; } - - static const SimulationTime Of(const Units::SecondsTime time) { - int cyc = static_cast(Units::SecondsTime(time / m_simulation_time_step).value()); - SimulationTime simtime; - simtime.SetCycle(cyc); - return simtime; - } - - SimulationTime() = default; - ~SimulationTime() = default; - SimulationTime(const SimulationTime &in) { Copy(in); } - - SimulationTime &operator=(const SimulationTime &in) { - if (this != &in) { - Copy(in); - } - return *this; - } - - bool operator<(const SimulationTime &in) const { return m_cycle < in.m_cycle; } - - bool operator>(const SimulationTime &in) const { return not(*this < in); } - - void Increment() { - ++m_cycle; - m_current_time += m_simulation_time_step; - } - - Units::SecondsTime GetCurrentSimulationTime() const { return m_current_time; } - - std::string GetCurrentSimulationTimeAsString() const { return std::to_string(m_current_time.value()); } - - int GetCycle() const { return m_cycle; } - - // visible for testing - void SetCycle(int cycle_in) { - m_cycle = cycle_in; - m_current_time = m_simulation_time_step * m_cycle; - } - - private: - void Copy(SimulationTime const &in) { - m_current_time = in.m_current_time; - m_cycle = in.m_cycle; - m_simulation_time_step = in.m_simulation_time_step; - } - - int m_cycle{0}; - Units::SecondsTime m_current_time{Units::zero()}; - inline static Units::SecondsTime m_simulation_time_step{SIMULATION_TIME_STEP}; -}; -} // namespace aaesim::open_source diff --git a/include/public/SingleTangentPlaneSequence.h b/include/public/SingleTangentPlaneSequence.h deleted file mode 100644 index 97db6dd..0000000 --- a/include/public/SingleTangentPlaneSequence.h +++ /dev/null @@ -1,35 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include - -#include "public/TangentPlaneSequence.h" - -class SingleTangentPlaneSequence final : public TangentPlaneSequence { - public: - static void ClearStaticMembers(); - SingleTangentPlaneSequence(const std::list &waypoint_list); - - private: - static std::list m_master_waypoint_sequence; - static log4cplus::Logger m_logger; - void Initialize(const std::list &waypoint_list) override; -}; diff --git a/include/public/SpeedBrakeController.h b/include/public/SpeedBrakeController.h deleted file mode 100644 index 851e1fa..0000000 --- a/include/public/SpeedBrakeController.h +++ /dev/null @@ -1,68 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include "utility/BoundedValue.h" - -namespace aaesim::open_source { -class SpeedBrakeController final { - public: - SpeedBrakeController() = default; - ~SpeedBrakeController() = default; - double GetSpeedBrakeGain() const { return gain_speedbrake_; }; - BoundedValue GetCurrentCommand() const { return speed_brake_command_; } - bool IsDeployed() const { return is_speedbrake_deployed_; } - BoundedValue Deploy() { - ++speedbrake_counter_; - speed_brake_command_ = speedbrake_command_maximum_; - is_speedbrake_deployed_ = true; - return GetCurrentCommand(); - } - BoundedValue Retract() { - speedbrake_counter_ = 0; - speed_brake_command_ = speedbrake_command_minimum_; - is_speedbrake_deployed_ = false; - return GetCurrentCommand(); - } - BoundedValue Update(bool thrust_command_is_minimum) { - if (not is_speedbrake_deployed_) return GetCurrentCommand(); - const bool beyond_minimum_deployment_duration = speedbrake_counter_ > speedbrake_counter_maximum; - if (not beyond_minimum_deployment_duration) { - ++speedbrake_counter_; - return GetCurrentCommand(); - } - - if (not thrust_command_is_minimum) { - Retract(); - } - return GetCurrentCommand(); - } - - private: - inline static const double gain_speedbrake_{0.10}; - inline static const unsigned int speedbrake_counter_maximum{30}; - inline static const double speedbrake_command_maximum_{0.5}; - inline static const double speedbrake_command_minimum_{0.0}; - BoundedValue speed_brake_command_{0}; - unsigned int speedbrake_counter_{0}; - bool is_speedbrake_deployed_{false}; -}; - -} // namespace aaesim::open_source diff --git a/include/public/SpeedCommandLimiter.h b/include/public/SpeedCommandLimiter.h deleted file mode 100644 index 1f6fa8c..0000000 --- a/include/public/SpeedCommandLimiter.h +++ /dev/null @@ -1,48 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include -#include -#include - -#include "public/BadaUtils.h" -#include "public/WeatherPrediction.h" -#include "utility/BoundedValue.h" - -namespace aaesim { -namespace open_source { -struct SpeedCommandLimiter { - virtual ~SpeedCommandLimiter() = default; - - virtual Units::Speed LimitSpeedCommand( - const Units::Speed previous_ias_speed_command, const Units::Speed current_ias_speed_command, - const Units::Speed reference_velocity_mps, const Units::Length speed_quantization_distance, - const Units::Length distance_to_end_of_route, const Units::Length current_altitude, - const aaesim::open_source::bada_utils::FlapConfiguration flap_configuration) = 0; - - virtual BoundedValue LimitMachCommand( - const BoundedValue &previous_reference_speed_command_mach, - const BoundedValue &estimated_mach, const BoundedValue &nominal_mach, - const Units::Mass ¤t_mass, const Units::Length ¤t_altitude, - const WeatherPrediction &weather_prediction) = 0; -}; -} // namespace open_source -} // namespace aaesim diff --git a/include/public/SpeedOnPitchControl.h b/include/public/SpeedOnPitchControl.h deleted file mode 100644 index fe572d5..0000000 --- a/include/public/SpeedOnPitchControl.h +++ /dev/null @@ -1,47 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include - -#include "public/AbstractDescentController.h" -#include "public/SpeedOnThrustControl.h" - -namespace aaesim::open_source { -class SpeedOnPitchControl final : public AbstractDescentController { - public: - SpeedOnPitchControl(const Units::Speed speed_threshold, const Units::Length altitude_threshold) - : speed_threshold_{speed_threshold}, altitude_threshold_{altitude_threshold} {}; - SpeedOnPitchControl() = delete; - void Initialize(std::shared_ptr &aircraft_performance) override; - void ComputeVerticalCommands(const Guidance &guidance, const EquationsOfMotionState &equations_of_motion_state, - std::shared_ptr &sensed_weather, - Units::Force &thrust_command, Units::Angle &gamma_command, Units::Speed &tas_command, - BoundedValue &speed_brake_command, - aaesim::open_source::bada_utils::FlapConfiguration &flap_configuration) override; - - private: - inline static log4cplus::Logger logger_{log4cplus::Logger::getInstance("SpeedOnPitchControl")}; - Units::Speed speed_threshold_{Units::KnotsSpeed{20.0}}; - Units::Length altitude_threshold_{Units::FeetLength{500.0}}; - bool is_level_flight_{true}; - std::shared_ptr speed_on_thrust_controller_{std::make_shared()}; -}; -} // namespace aaesim::open_source diff --git a/include/public/SpeedOnThrustControl.h b/include/public/SpeedOnThrustControl.h deleted file mode 100644 index 66fb677..0000000 --- a/include/public/SpeedOnThrustControl.h +++ /dev/null @@ -1,45 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include - -#include "public/AbstractDescentController.h" - -namespace aaesim { -namespace open_source { -class SpeedOnThrustControl final : public AbstractDescentController { - public: - SpeedOnThrustControl() = default; - ~SpeedOnThrustControl() = default; - void ComputeVerticalCommands(const Guidance &guidance, const EquationsOfMotionState &equations_of_motion_state, - std::shared_ptr &sensed_weather, - Units::Force &thrust_command, Units::Angle &gamma_command, Units::Speed &tas_command, - BoundedValue &speed_brake_command, - aaesim::open_source::bada_utils::FlapConfiguration &flap_configuration) override; - - private: - inline static log4cplus::Logger logger_{log4cplus::Logger::getInstance("SpeedOnThrustControl")}; - inline static const Units::Frequency gain_altitude_{Units::HertzFrequency(0.20)}; - inline static const Units::Frequency gain_true_airspeed_{Units::sqr(natural_frequency_) / thrust_gain_}; - unsigned int min_thrust_counter_{0}; -}; -} // namespace open_source -} // namespace aaesim diff --git a/include/public/StandardAtmosphere.h b/include/public/StandardAtmosphere.h deleted file mode 100644 index a0797d0..0000000 --- a/include/public/StandardAtmosphere.h +++ /dev/null @@ -1,73 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// 2023 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -/** - * StandardAtmosphere is an implementation of the ISA - * (International Standard Atmosphere) concept, which - * defines straightforward approximations for calculating - * pressure, temperature, density, and viscosity as a - * function of altitude. - * - * https://en.wikipedia.org/wiki/International_Standard_Atmosphere - */ - -#pragma once - -#include - -class StandardAtmosphere : public Atmosphere { - public: - static StandardAtmosphere *MakeInstance(const Units::KelvinTemperature temperature, const Units::Length altitude); - - static StandardAtmosphere *MakeInstanceFromTemperatureOffset(Units::CelsiusTemperature temperature_offset); - - StandardAtmosphere(const Units::Temperature temperatureOffset); - - virtual ~StandardAtmosphere(); - - /** - * Calculates the temperature at a given altitude MSL. - * - * Note: Only the Troposphere and Tropopause layers are implemented. - * Calculations above 65,000 feet would require adding Stratosphere. - * - * @param altitude_msl - */ - Units::KelvinTemperature GetTemperature(const Units::Length altitude_msl) const; - - Units::Temperature GetTemperatureOffset() const; - - Units::KelvinTemperature GetSeaLevelTemperature() const; - Units::Density GetSeaLevelDensity() const; - Units::MetersLength GetTropopauseHeight() const; - Units::Density GetTropopauseDensity() const; - Units::Pressure GetTropopausePressure() const; - - protected: - void SetTemperatureOffset(Units::Temperature temperature_offset); - - private: - static log4cplus::Logger m_logger; - Units::Temperature m_temperature_offset; - Units::MetersLength m_tropopause_height; - Units::Temperature m_sea_level_temperature; - Units::Density m_sea_level_density; - Units::Density m_tropopause_density; - Units::Pressure m_tropopause_pressure; -}; diff --git a/include/public/StatisticalPilotDelay.h b/include/public/StatisticalPilotDelay.h deleted file mode 100644 index efda97f..0000000 --- a/include/public/StatisticalPilotDelay.h +++ /dev/null @@ -1,116 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include -#include -#include -#include - -#include -#include -#include - -#include "public/Atmosphere.h" -#include "public/PilotDelay.h" - -namespace aaesim::open_source { -class StatisticalPilotDelay final : public PilotDelay { - public: - static StatisticalPilotDelay NoDelay(); - static StatisticalPilotDelay WithDelayDefaults(std::shared_ptr atmosphere); - static StatisticalPilotDelay WithDelay(Units::Time gaussian_mean, Units::Time gaussian_std, - std::shared_ptr atmosphere); - - StatisticalPilotDelay() = default; - - void IterationReset(); - - Units::Speed UpdateIAS(Units::Speed previous_speed_command_ias, Units::Speed proposed_speed_command_ias, - Units::Length current_altitude, Units::Length altitude_at_end_of_route) override; - - Units::Speed UpdateMach(double previous_speed_command_as_mach, double proposed_command_as_mach, - Units::Length current_altitude, Units::Length altitude_at_end_of_route) override; - - void SetUsePilotDelay(const bool delay_enabled); - - bool IsPilotDelayOn() const; - - std::pair GetPilotDelayParameters() const; - - private: - Units::Time ComputeTimeToSpeedChange(Units::Length current_altitude, Units::Length altitude_at_end_of_route); - void SetAtmosphere(std::shared_ptr atmosphere); - void SetInitialIAS(Units::Length current_altitude, Units::Speed fallback_IAS); - void SetPilotDelayParameters(const Units::Time mean, const Units::Time standard_deviation); - - // for speed conversion - std::shared_ptr m_atmosphere{}; - - Units::Time m_time_to_next_speed_change{Units::SecondsTime(-1.0)}; - Units::Speed m_guidance_ias{Units::zero()}; - Units::SecondsTime m_pilot_delay_mean{Units::SecondsTime(12.0)}; - Units::SecondsTime m_pilot_delay_standard_deviation{Units::zero()}; - - double m_guidance_mach{0}; - bool m_pilot_delay_is_on{true}; - - // for statistical output - int m_delay_count{0}; - double m_delay_sum{0}; - double m_delay_square_sum{0}; - std::map m_delay_frequency{}; - - inline static const double STANDARD_DEVIATION_LIMIT{3}; - static log4cplus::Logger m_logger; -}; - -inline void StatisticalPilotDelay::SetAtmosphere(std::shared_ptr atmosphere) { m_atmosphere = atmosphere; } - -inline void StatisticalPilotDelay::SetUsePilotDelay(const bool delay_enabled) { m_pilot_delay_is_on = delay_enabled; } - -inline bool StatisticalPilotDelay::IsPilotDelayOn() const { return m_pilot_delay_is_on; } - -inline std::pair StatisticalPilotDelay::GetPilotDelayParameters() const { - return std::pair(m_pilot_delay_mean, m_pilot_delay_standard_deviation); -} - -inline StatisticalPilotDelay StatisticalPilotDelay::NoDelay() { - StatisticalPilotDelay no_delay{}; - no_delay.SetUsePilotDelay(false); - return no_delay; -} - -inline StatisticalPilotDelay StatisticalPilotDelay::WithDelay(Units::Time gaussian_mean, Units::Time gaussian_std, - std::shared_ptr atmosphere) { - StatisticalPilotDelay delayed{}; - delayed.SetUsePilotDelay(true); - delayed.SetPilotDelayParameters(gaussian_mean, gaussian_std); - delayed.SetAtmosphere(atmosphere); - return delayed; -} - -inline StatisticalPilotDelay StatisticalPilotDelay::WithDelayDefaults(std::shared_ptr atmosphere) { - StatisticalPilotDelay delayed{}; - delayed.SetAtmosphere(atmosphere); - return delayed; -} - -} // namespace aaesim::open_source diff --git a/include/public/StereographicProjection.h b/include/public/StereographicProjection.h deleted file mode 100644 index d6cd27a..0000000 --- a/include/public/StereographicProjection.h +++ /dev/null @@ -1,58 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include -#include - -class StereographicProjection { - public: - StereographicProjection() = default; - - ~StereographicProjection() = default; - - static void init(Units::Angle lat, Units::Angle lon, Units::Length earthRadius); - - static void xy_to_ll(const Units::Length x, const Units::Length y, Units::Angle &lat2, Units::Angle &lon2); - - static void ll_to_xy(const Units::Angle lat2, Units::Angle lon2, Units::Length &x, Units::Length &y); - - private: - static double toConformalSin(double x); - - /* Raw parameters for the NAS conversion */ - static Units::RadiansAngle latTPT; /* North latitude of tangency point (radians) */ - static Units::RadiansAngle lonTPT; /* **WEST** longitude of tangency point (radians) */ - static Units::FeetLength eRadius; // earth radius at tangent point (lon1, lat1), feet - - /* Convienience parameters calculated from raw parameters */ - static double sin_latTPT; /* sin of latitude of tangency point */ - static double sin_clatTPT; /* sin of conformal latitude of tangency point */ - static double cos_clatTPT; /* cos of conformal latitude of tangency point */ - - /* Convienience parameters used only for the reverse NAS projection */ - static double cos_gamma; - static double sin_gamma; - - /* The following constants are used for converting from geodetic to */ - /* conformal latitude. They are found in NAS-MD-312 Appendix D. */ - static double GEOD_CONST_A; - static double GEOD_CONST_B; -}; diff --git a/include/public/TakeOffVerticalController.h b/include/public/TakeOffVerticalController.h deleted file mode 100644 index 5292800..0000000 --- a/include/public/TakeOffVerticalController.h +++ /dev/null @@ -1,46 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include - -#include "public/AbstractAscentController.h" -#include "public/FixedMassAircraftPerformance.h" - -namespace aaesim::open_source { -class TakeOffVerticalController final : public AbstractAscentController { - public: - TakeOffVerticalController() = default; - ~TakeOffVerticalController() = default; - void ComputeAscentCommands(const Guidance &guidance, const EquationsOfMotionState &equations_of_motion_state, - std::shared_ptr &sensed_weather, - Units::Force &thrust_command, Units::Angle &gamma_command, Units::Speed &tas_command, - aaesim::open_source::bada_utils::FlapConfiguration &flap_command) override { - thrust_command = aircraft_performance_->GetMaxThrust( - equations_of_motion_state.altitude_msl, bada_utils::FlapConfiguration::TAKEOFF, - bada_utils::EngineThrustMode::MAXIMUM_CLIMB, Units::ZERO_CELSIUS); - gamma_command = Units::zero(); - flap_command = bada_utils::TAKEOFF; - tas_command = - sensed_weather->GetTrueWeather()->CAS2TAS(guidance.m_ias_command, equations_of_motion_state.altitude_msl); - } -}; - -}; // namespace aaesim::open_source diff --git a/include/public/TangentPlaneSequence.h b/include/public/TangentPlaneSequence.h deleted file mode 100644 index 8ec4501..0000000 --- a/include/public/TangentPlaneSequence.h +++ /dev/null @@ -1,107 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include -#include -#include - -#include "public/LocalTangentPlane.h" -#include "public/Waypoint.h" - -/** - * This class takes a series of waypoints and creates a LocalTangentPlane - * for each, using the waypoint as the point of tangency. - */ -class TangentPlaneSequence { - public: - TangentPlaneSequence(); - - /** - * Constructs a sequence of LocalTangentPlane objects, one - * for each waypoint. The final waypoint in the list maps - * to the origin in ENU coordinates, and each of the other - * waypoints maps to the same coordinates in its own plane - * as it does in its successor's plane. The altitudes of - * the waypoints are ignored and treated as zero (sea level); - * only the latitudes and longitudes are used. - */ - explicit TangentPlaneSequence(std::list &waypoint_list); - - virtual ~TangentPlaneSequence() = default; - - TangentPlaneSequence(const TangentPlaneSequence &in); - - /** - * Converts a local ENU point to geodetic coordinates - * using the default EarthModel and the nearest - * point of tangency in the sequence. - * - * Note that altitude is intentionally ignored (treated as - * zero) by EllipsoidalEarthModel. - * - * @param localPosition - * @param waypoint - */ - void ConvertLocalToGeodetic(EarthModel::LocalPositionEnu localPosition, - EarthModel::GeodeticPosition &geoPosition) const; - - /** - * Converts a geodetic point to local ENU coordinates - * using the default EarthModel and the nearest - * point of tangency in the sequence. - * - * Note that returned altitude is reset to zero by EllipsoidalEarthModel. - * - * @param localPosition - * @param waypoint - */ - void ConvertGeodeticToLocal(EarthModel::GeodeticPosition geoPosition, - EarthModel::LocalPositionEnu &localPosition) const; - - /** - * Returns the ENU coordinates of each of the waypoints - * supplied during construction. - */ - const std::vector &GetLocalPositionsFromInitialization() const; - - /** - * Returns copies of the waypoints used during construction. - */ - const std::vector &GetWaypointsFromInitialization() const; - - /** - * Returns tangent planes for each of the waypoints used during - * construction. - */ - const std::vector > &GetTangentPlanesFromInitialization() const; - - private: - inline static log4cplus::Logger logger_{log4cplus::Logger::getInstance("TangentPlaneSequence")}; - - void Copy(const TangentPlaneSequence &in); - - protected: - virtual void Initialize(const std::list &waypoint_list); - - std::vector waypoints_from_initialization_; - std::vector > tangent_planes_from_initialization_; - std::vector local_positions_from_initialization_; -}; diff --git a/include/public/ThreeDOFDynamics.h b/include/public/ThreeDOFDynamics.h deleted file mode 100644 index 1d679f5..0000000 --- a/include/public/ThreeDOFDynamics.h +++ /dev/null @@ -1,128 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include "public/AircraftControl.h" -#include "public/AircraftState.h" -#include "public/DynamicsState.h" -#include "public/EllipsoidalPositionEstimator.h" -#include "public/EquationsOfMotionState.h" -#include "public/EquationsOfMotionStateDeriv.h" -#include "public/FixedMassAircraftPerformance.h" -#include "public/Guidance.h" -#include "public/SimulationTime.h" -#include "public/TrueWeatherOperator.h" - -namespace aaesim::open_source { -class ThreeDOFDynamics final { - public: - ThreeDOFDynamics() = default; - ~ThreeDOFDynamics() = default; - - AircraftState Update(const int unique_acid, const aaesim::open_source::SimulationTime &simtime, - const Guidance &guidance, const std::shared_ptr &aircraft_control); - - void Initialize(const aaesim::open_source::SimulationTime &simulation_time, - std::shared_ptr aircraft_performance, - const EarthModel::GeodeticPosition &initial_position, - const EarthModel::LocalPositionEnu &initial_position_enu, Units::Length initial_altitude_msl, - Units::Speed initial_true_airspeed, Units::Angle initial_ground_course_enu, - double initial_mass_fraction, - std::shared_ptr position_estimator, - std::shared_ptr true_weather_operator); - - const std::pair GetWindComponents() const; - - const DynamicsState GetDynamicsState() const; - - const EquationsOfMotionState &GetEquationsOfMotionState() const; - - const EquationsOfMotionStateDeriv GetEquationsOfMotionStateDerivative() const; - - const std::map &GetDynamicsStateHistory() const; - - private: - inline static log4cplus::Logger m_logger{log4cplus::Logger::getInstance(LOG4CPLUS_TEXT("ThreeDOFDynamics"))}; - - DynamicsState Integrate(const Guidance &guidance, const std::shared_ptr &aircraft_control); - - // Calculate the trim angle correction necessary and provides an updated state - Units::SignedRadiansAngle CalculateTrimmedPsiForWind(Units::SignedAngle ground_track_enu); - - EquationsOfMotionStateDeriv StatePropagation(Units::Frequency dVwx_dh, Units::Frequency dVwy_dh, - Units::Frequency k_gamma, Units::Frequency k_t, Units::Frequency k_phi, - double k_speedBrake, ControlCommands commands); - - EquationsOfMotionStateDeriv StatePropagationOnRunway(ControlCommands commands, const Guidance &guidance); - - void CalculateKineticForces(Units::Force &lift, Units::Force &drag); - - void UpdateTrueWeatherConditions(); - - DynamicsState ComputeDynamicsState(const EquationsOfMotionState &equations_of_motion_state, - const EquationsOfMotionStateDeriv &equations_of_motion_state_derivative) const; - - std::shared_ptr m_bada_calculator{}; - std::shared_ptr m_position_estimator; - std::map m_dynamics_history{}; - EquationsOfMotionState m_equations_of_motion_state{}; - EquationsOfMotionStateDeriv m_equations_of_motion_state_derivative{}; - EarthModel::GeodeticPosition m_last_resolved_position{}; - Units::Speed m_wind_velocity_east{Units::zero()}; - Units::Speed m_wind_velocity_north{Units::zero()}; - double m_max_thrust_percent{1.0}; - double m_min_thrust_percent{1.0}; - std::shared_ptr m_true_weather_operator; -}; - -inline const std::pair ThreeDOFDynamics::GetWindComponents() const { - return std::make_pair(m_wind_velocity_east, m_wind_velocity_north); -} - -inline const DynamicsState ThreeDOFDynamics::GetDynamicsState() const { - if (m_dynamics_history.empty()) return DynamicsState{}; - return std::prev(m_dynamics_history.cend())->second; -} - -inline const EquationsOfMotionState &ThreeDOFDynamics::GetEquationsOfMotionState() const { - return m_equations_of_motion_state; -} - -inline const EquationsOfMotionStateDeriv ThreeDOFDynamics::GetEquationsOfMotionStateDerivative() const { - return m_equations_of_motion_state_derivative; -} - -inline const std::map & - ThreeDOFDynamics::GetDynamicsStateHistory() const { - return m_dynamics_history; -} - -} // namespace aaesim::open_source diff --git a/include/public/Token.h b/include/public/Token.h deleted file mode 100644 index 5554a64..0000000 --- a/include/public/Token.h +++ /dev/null @@ -1,66 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// 2022 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include - -class Token { - public: - Token(void) {} //-------------------------------------------------------------- - - inline void add_Data(char c) { data += c; } //-------------------------------------------------------------- - inline void add_Data(const std::string &s) { - data += s; - } //-------------------------------------------------------------- - - inline void add_Format(char c) { format += c; } //-------------------------------------------------------------- - inline void add_Format(const std::string &s) { - format += s; - } //-------------------------------------------------------------- - inline void add_Format(const Token &t) { - add_Format(t.get_Format()); - add_Format(t.get_Data()); - } //-------------------------------------------------------------- - inline void merge_data_into_format() { - format += data; - data = ""; - } //-------------------------------------------------------------- - inline std::string get_Data() const { - return data; - } //-------------------------------------------------------------- - inline std::string get_Format() const { - return format; - } //-------------------------------------------------------------- - - inline std::string get_All() const { - return format + data; - } //-------------------------------------------------------------- - - inline void set_data(std::string const &nd) { - data = nd; - } //-------------------------------------------------------------- - inline void set_format(std::string const &nf) { - format = nf; - } //-------------------------------------------------------------- - - private: - std::string data; - std::string format; -}; diff --git a/include/public/TrueWeatherOperator.h b/include/public/TrueWeatherOperator.h deleted file mode 100644 index 0b7c855..0000000 --- a/include/public/TrueWeatherOperator.h +++ /dev/null @@ -1,46 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include -#include -#include - -#include - -#include "public/Atmosphere.h" -#include "public/EarthModel.h" -#include "public/WeatherTruth.h" - -namespace aaesim::open_source { -struct TrueWeatherOperator { - virtual void CalculateEnvironmentalWind(const EarthModel::GeodeticPosition &position, - const Units::Length &altitude_msl) = 0; - virtual Units::Speed GetWindSpeedEast() const = 0; - virtual Units::Speed GetWindSpeedNorth() const = 0; - virtual Units::Frequency GetWindSpeedVerticalDerivativeEast() const = 0; - virtual Units::Frequency GetWindSpeedVerticalDerivativeNorth() const = 0; - virtual Units::KelvinTemperature GetTemperature() const = 0; - virtual Units::Density GetDensity() const = 0; - virtual Units::Pressure GetPressure() const = 0; - virtual std::shared_ptr GetAtmosphere() const = 0; - virtual std::shared_ptr GetTrueWeather() const = 0; -}; -} // namespace aaesim::open_source diff --git a/include/public/TurnAnticipation.h b/include/public/TurnAnticipation.h deleted file mode 100644 index f3319ff..0000000 --- a/include/public/TurnAnticipation.h +++ /dev/null @@ -1,38 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -namespace aaesim { -namespace open_source { - -struct TurnAnticipation { - double distance; // turn anticipation in meters - double bankAngle; // bank angle in radians - double maxAngle; // max bank angle - double radius; // radius of turn - double groundspeed; // meters per second - - TurnAnticipation() : distance(0), bankAngle(0), maxAngle(0), radius(0), groundspeed(0) {}; - - TurnAnticipation(double pDist, double pBank, double pMaxBank, double pRadius, double pgs) - : distance(pDist), bankAngle(pBank), maxAngle(pMaxBank), radius(pRadius), groundspeed(pgs) {}; -}; -} // namespace open_source -} // namespace aaesim diff --git a/include/public/TvReader.h b/include/public/TvReader.h deleted file mode 100644 index 8c7d1f9..0000000 --- a/include/public/TvReader.h +++ /dev/null @@ -1,81 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -/* - * TvReader.h - * - * Reads a TV.csv file containing a sequence of aircraft states. - * - * Created on: Mar 23, 2019 - * Author: klewis - */ - -#pragma once - -#include -#include - -#include - -#include "public/DataReader.h" -#include "utility/CustomUnits.h" - -namespace aaesim { -namespace open_source { - -class TvReader : public aaesim::open_source::DataReader { - public: - static const size_t EXPECTED_TV_COLUMN_COUNT; - TvReader(const std::string &file_name, int header_lines); - TvReader(std::shared_ptr input_stream, int header_lines); - TvReader() = default; - bool Advance(); - const Units::SecondsTime GetTimeOfReceipt() const; - const int GetAcid() const; - const Units::SecondsTime GetToap() const; - const Units::DegreesAngle GetLat() const; - const Units::DegreesAngle GetLon() const; - const Units::FeetLength GetAlt() const; - const Units::KnotsSpeed GetEwvel() const; - const Units::KnotsSpeed GetNsvel() const; - const Units::SecondsTime GetToav() const; - const int GetNacp() const; - const int GetNic() const; - const int GetNacv() const; - const Units::FeetPerMinuteSpeed GetVertRate() const; - - private: - Units::SecondsTime m_time_of_receipt{}; // column 1 - void SetColumnIndexesFromHeader(const int header_lines); - int m_aircraft_id_column{0}; - int m_time_of_applicability_position_column{0}; - int m_latitude_column{0}; - int m_longitude_column{0}; - int m_altitude_column{0}; - int m_east_velocity_column{0}; - int m_north_velocity_column{0}; - int m_time_of_applicability_velocity_column{0}; - int m_nacp_column{0}; - int m_nic_column{0}; - int m_nacv_column{0}; - int m_vert_rate_column{0}; -}; - -} // namespace open_source -} // namespace aaesim diff --git a/include/public/USStandardAtmosphere1976.h b/include/public/USStandardAtmosphere1976.h deleted file mode 100644 index 512d32a..0000000 --- a/include/public/USStandardAtmosphere1976.h +++ /dev/null @@ -1,74 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -/** - * USStandardAtmosphere1976 implements the United States Standard Atmosphere as revised in 1976. - * - * It uses the same value for sea-level temperature, pressure, and density as ISA, so we will - * use - */ - -#pragma once - -#include - -class USStandardAtmosphere1976 final : public Atmosphere { - public: - USStandardAtmosphere1976(); - USStandardAtmosphere1976(const Units::Temperature temperature_offset); - - virtual ~USStandardAtmosphere1976() = default; - - Atmosphere *Clone() const; - - void SetTemperatureOffset(const Units::Temperature temperature_offset); - - void CalibrateTemperatureAtAltitude(const Units::KelvinTemperature temperature, const Units::Length altitude); - - void AirDensity(const Units::Length h, Units::Density &rho, Units::Pressure &P) const; - - Units::KelvinTemperature GetTemperature(const Units::Length altitude_msl) const; - - Units::KelvinTemperature GetSeaLevelTemperature() const; - - Units::Density GetSeaLevelDensity() const; - - Units::MetersLength GetTropopauseHeight() const; - - Units::Density GetTropopauseDensity() const; - - Units::Pressure GetTropopausePressure() const; - - Units::Speed CAS2TAS(const Units::Speed vcas, const Units::Pressure p, const Units::Density rho) const; - - Units::Speed TAS2CAS(const Units::Speed vtas, const Units::Pressure p, const Units::Density rho) const; - - Units::Speed SpeedOfSound(Units::KelvinTemperature temperature) const; - - double ESFconstantCAS(const Units::Speed true_airspeed, const Units::Length altitude_msl, - const Units::KelvinTemperature temperature) const; - - Units::Length GetMachIASTransition(const Units::Speed ias, const double mach) const; - - private: - static log4cplus::Logger m_logger; - - static const double P_T_EXPONENT; - static const double RHO_T_EXPONENT; -}; diff --git a/include/public/VectorDifferenceWindEvaluator.h b/include/public/VectorDifferenceWindEvaluator.h deleted file mode 100644 index 10bd44a..0000000 --- a/include/public/VectorDifferenceWindEvaluator.h +++ /dev/null @@ -1,49 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include -#include - -#include "public/PredictedWindEvaluator.h" - -namespace aaesim { -namespace open_source { - -class VectorDifferenceWindEvaluator : public aaesim::open_source::PredictedWindEvaluator { - public: - const static std::shared_ptr GetInstance( - const Units::Speed maxSpeedDiff); - - virtual ~VectorDifferenceWindEvaluator(); - - virtual bool ArePredictedWindsAccurate(const aaesim::open_source::AircraftState &state, - const aaesim::open_source::WeatherPrediction &weather_prediction, - const Units::Speed reference_cas, const Units::Length reference_altitude, - const std::shared_ptr &sensed_atmosphere) const; - - private: - static std::map > m_instances; - const Units::Speed m_max_allowed_difference; - - explicit VectorDifferenceWindEvaluator(const Units::Speed &max_allowed_difference); -}; -} // namespace open_source -} // namespace aaesim diff --git a/include/public/VerticalController.h b/include/public/VerticalController.h deleted file mode 100644 index 00d1af5..0000000 --- a/include/public/VerticalController.h +++ /dev/null @@ -1,110 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include -#include -#include - -#include - -#include "public/BadaUtils.h" -#include "public/EquationsOfMotionState.h" -#include "public/FixedMassAircraftPerformance.h" -#include "public/Guidance.h" -#include "public/TrueWeatherOperator.h" -#include "utility/BoundedValue.h" - -namespace aaesim::open_source { -struct VerticalController { - virtual void Initialize( - std::shared_ptr &performance_calculator) = 0; - virtual void ComputeVerticalCommands(const Guidance &guidance, - const EquationsOfMotionState &equations_of_motion_state, - std::shared_ptr &sensed_weather, - Units::Force &thrust_command, Units::Angle &gamma_command, - Units::Speed &tas_command, BoundedValue &speed_brake_command, - aaesim::open_source::bada_utils::FlapConfiguration &flap_command) = 0; - - virtual Units::Frequency GetGammaGain() const { return gain_flight_path_angle_; }; - virtual Units::Frequency GetThrustGain() const { return thrust_gain_; }; - virtual double GetSpeedBrakeGain() const = 0; - - protected: - inline static const Units::Frequency natural_frequency_{Units::HertzFrequency(0.20)}; - inline static Units::Frequency CalculateThrustGain() { - const double zeta = 0.88; - return 2 * zeta * natural_frequency_; - } - inline static const Units::Frequency gain_flight_path_angle_{Units::HertzFrequency(0.40)}; - inline static const Units::Frequency thrust_gain_{CalculateThrustGain()}; - inline static void ConfigureFlapsAndEstimateKineticForces( - const EquationsOfMotionState &equations_of_motion_state, - std::shared_ptr &sensed_weather, - std::shared_ptr aircraft_performance, Units::Force &lift, Units::Force &drag, - aaesim::open_source::bada_utils::FlapConfiguration &flap_configuration) { - Units::Speed calibrated_airspeed = sensed_weather->GetTrueWeather()->TAS2CAS( - Units::MetersPerSecondSpeed(equations_of_motion_state.true_airspeed), - Units::MetersLength(equations_of_motion_state.altitude_msl)); - - Units::KilogramsMeterDensity rho{}; - Units::Pressure pressure{}; - sensed_weather->GetTrueWeather()->getAtmosphere()->AirDensity(equations_of_motion_state.altitude_msl, rho, - pressure); - - double cd0{0}, cd2{0}, gear{0}; - aircraft_performance->GetDragCoefficientsAndIncrementFlapConfiguration( - calibrated_airspeed, equations_of_motion_state.altitude_msl, cd0, cd2, gear, flap_configuration); - - const auto ac_mass = aircraft_performance->GetAircraftMass(); - const auto wing_area = aircraft_performance->GetAerodynamicsInformation().S; - double cL = - (2. * ac_mass * Units::ONE_G_ACCELERATION) / (rho * Units::sqr(equations_of_motion_state.true_airspeed) * - wing_area * cos(equations_of_motion_state.phi)); - double cD = cd0 + gear + cd2 * pow(cL, 2); - if (equations_of_motion_state.speed_brake_percentage != 0.0) { - cD = (1.0 + 0.6 * equations_of_motion_state.speed_brake_percentage) * cD; - } - drag = 1. / 2. * rho * cD * Units::sqr(equations_of_motion_state.true_airspeed) * wing_area; - lift = 1. / 2. * rho * cL * Units::sqr(equations_of_motion_state.true_airspeed) * wing_area; - }; -}; - -class NullVerticalController final : public VerticalController { - public: - NullVerticalController() = default; - ~NullVerticalController() = default; - void Initialize( - std::shared_ptr &performance_calculator) override {} - void ComputeVerticalCommands(const Guidance &guidance, const EquationsOfMotionState &equations_of_motion_state, - std::shared_ptr &sensed_weather, - Units::Force &thrust_command, Units::Angle &gamma_command, Units::Speed &tas_command, - BoundedValue &speed_brake_command, - aaesim::open_source::bada_utils::FlapConfiguration &flap_command) override { - thrust_command = equations_of_motion_state.thrust; - gamma_command = equations_of_motion_state.gamma; - tas_command = equations_of_motion_state.true_airspeed; - speed_brake_command = equations_of_motion_state.speed_brake_percentage; - flap_command = equations_of_motion_state.flap_configuration; - } - double GetSpeedBrakeGain() const override { return 0.0; }; -}; - -} // namespace aaesim::open_source diff --git a/include/public/VerticalPath.h b/include/public/VerticalPath.h deleted file mode 100644 index fdbd760..0000000 --- a/include/public/VerticalPath.h +++ /dev/null @@ -1,78 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include - -#include -#include - -#include "public/BadaUtils.h" - -class VerticalPath final { - public: - enum PredictionAlgorithmType { - UNDETERMINED = 0, - LEVEL, - LEVEL_DECEL1, - LEVEL_DECEL2, - CONSTANT_CAS, - CONSTANT_MACH, - CONSTANT_DECEL, - IDLE1, - IDLE2, - FPA, - FPA_DECEL, - FPA_TO_CURRENT_POS, - TAKEOFF_ROLL, - ESF_CLIMB, - CONSTANT_CAS_CLIMB, - CONSTANT_MACH_CLIMB, - LEVEL_ACCEL_ASCENDING, - LEVEL_FLIGHT, - LEVEL_ACCEL_DESCENDING - }; - - VerticalPath(); - - virtual ~VerticalPath(); - - void Append(const VerticalPath &in); - - void operator+=(const VerticalPath &in); - - bool operator==(const VerticalPath &obj) const; - - std::vector along_path_distance_m; - std::vector altitude_m; - std::vector cas_mps; - std::vector mach; - std::vector altitude_rate_mps; - std::vector true_airspeed; - std::vector tas_rate_mps; - std::vector theta_radians; - std::vector gs_mps; - std::vector time_to_go_sec; - std::vector mass_kg; - std::vector wind_velocity_east; - std::vector wind_velocity_north; - std::vector algorithm_type; - std::vector flap_setting; -}; diff --git a/include/public/VerticalPathObserver.h b/include/public/VerticalPathObserver.h deleted file mode 100644 index 803eec4..0000000 --- a/include/public/VerticalPathObserver.h +++ /dev/null @@ -1,80 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// 2022 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include -#include -#include -#include "public/VerticalPath.h" - -// Used to write trajectory data for all the flights over all the iterations for a type of trajectory. Examples of types -// of trajectories are the precalculated trajectories created at the beginning of the run, kinematic trajectories for -// own aircraft, or kinematic trajectories for target aircraft. - -class VerticalPathObserver -{ - -public: - VerticalPathObserver(); - - VerticalPathObserver(std::string scenario_name, - std::string file_name, - bool is_target_aircraft_data); - - virtual ~VerticalPathObserver(); - - void AddTrajectory(int id, - const VerticalPath &vertical_path); - - void SetIterationNumber(int iteration_number); - - int GetIterationNumber(); - - void WriteData(); - -protected: - void Initialize(); - - std::string m_scenario_name; - std::string m_file_name; - std::string m_column_header; - - std::ofstream out_stream; - -private: - static log4cplus::Logger m_logger; - - std::string CreateFullFileName(const std::string &scenario_name, - const std::string &file_name); - - std::string GetHeader(); - - int m_iteration; - - bool m_is_target_aircraft_data; -}; - -inline void VerticalPathObserver::SetIterationNumber(int iteration_number) { - m_iteration = iteration_number; -} - -inline int VerticalPathObserver::GetIterationNumber() { - return m_iteration; -} \ No newline at end of file diff --git a/include/public/VerticalPathUtils.h b/include/public/VerticalPathUtils.h deleted file mode 100644 index bba039f..0000000 --- a/include/public/VerticalPathUtils.h +++ /dev/null @@ -1,232 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include -#include -#include - -#include - -#include "public/CoreUtils.h" - -namespace aaesim { -namespace open_source { -struct VerticalPathUtils { - struct VerticalPathDataSet final { - Units::Length along_path_distance{}; - Units::Length altitude_msl{}; - Units::Speed calibrated_airspeed{}; - double mach{INT32_MIN}; - Units::Speed altitude_rate{}; - Units::Speed true_airspeed{}; - Units::Acceleration tas_rate{}; - Units::Angle theta{}; - Units::Speed ground_speed{}; - Units::Time time_to_go{}; - Units::Mass mass{}; - Units::MetersPerSecondSpeed wind_velocity_east{}; - Units::MetersPerSecondSpeed wind_velocity_north{}; - aaesim::open_source::bada_utils::FlapConfiguration flap_setting{ - aaesim::open_source::bada_utils::FlapConfiguration::UNDEFINED}; - int resolved_index{INT32_MIN}; - VerticalPath::PredictionAlgorithmType algorithm_type{VerticalPath::PredictionAlgorithmType::UNDETERMINED}; - }; - - static Units::Time CalculateTimeToFly(const VerticalPath &vertical_path, - Units::Length estimated_distance_to_path_end); - - static Units::Speed CalculateSpeedGuidance(const VerticalPath &vertical_path, - Units::Length estimated_distance_to_path_end); - - static double CalculateMachGuidance(const VerticalPath &vertical_path, Units::Length estimated_distance_to_path_end); - - static Units::Mass GetExpectedMass(const VerticalPath &vertical_path, Units::Length estimated_distance_to_path_end); - - static VerticalPathDataSet GetVerticalPathData(const VerticalPath &vertical_path, - Units::Length estimated_distance_to_path_end); - - static VerticalPathDataSet GetInterpolatedPathData(const VerticalPath &vertical_path, - Units::Length estimated_distance_to_path_end); - - static VerticalPathDataSet GetPathDataAtIndex(const VerticalPath &vertical_path, int index); - - static std::vector ConvertToPathDataSet( - const VerticalPath &vertical_path); -}; -} // namespace open_source -} // namespace aaesim - -inline aaesim::open_source::VerticalPathUtils::VerticalPathDataSet - aaesim::open_source::VerticalPathUtils::GetVerticalPathData(const VerticalPath &vertical_path, - Units::Length estimated_distance_to_path_end) { - const Units::MetersLength distance_to_go{estimated_distance_to_path_end}; - const auto reference_lookup_index = - CoreUtils::FindNearestIndex(distance_to_go.value(), vertical_path.along_path_distance_m); - return GetPathDataAtIndex(vertical_path, reference_lookup_index); -} - -inline std::vector - aaesim::open_source::VerticalPathUtils::ConvertToPathDataSet(const VerticalPath &vertical_path) { - std::vector path_data_set{}; - for (auto idx = 0; idx < vertical_path.along_path_distance_m.size(); ++idx) { - path_data_set.push_back(aaesim::open_source::VerticalPathUtils::GetPathDataAtIndex(vertical_path, idx)); - } - return path_data_set; -} - -inline aaesim::open_source::VerticalPathUtils::VerticalPathDataSet - aaesim::open_source::VerticalPathUtils::GetPathDataAtIndex(const VerticalPath &vertical_path, int index) { - aaesim::open_source::VerticalPathUtils::VerticalPathDataSet single_data_row{}; - single_data_row.resolved_index = index; - single_data_row.along_path_distance = Units::MetersLength(vertical_path.along_path_distance_m[index]); - single_data_row.altitude_msl = Units::MetersLength(vertical_path.altitude_m[index]); - single_data_row.calibrated_airspeed = Units::MetersPerSecondSpeed(vertical_path.cas_mps[index]); - single_data_row.mach = vertical_path.mach[index]; - single_data_row.altitude_rate = Units::MetersPerSecondSpeed(vertical_path.altitude_rate_mps[index]); - single_data_row.true_airspeed = vertical_path.true_airspeed[index]; - single_data_row.tas_rate = Units::MetersSecondAcceleration(vertical_path.tas_rate_mps[index]); - single_data_row.theta = Units::RadiansAngle(vertical_path.theta_radians[index]); - single_data_row.ground_speed = Units::MetersPerSecondSpeed(vertical_path.gs_mps[index]); - single_data_row.time_to_go = Units::SecondsTime(vertical_path.time_to_go_sec[index]); - single_data_row.mass = Units::KilogramsMass(vertical_path.mass_kg[index]); - single_data_row.wind_velocity_east = vertical_path.wind_velocity_east[index]; - single_data_row.wind_velocity_north = vertical_path.wind_velocity_north[index]; - single_data_row.flap_setting = vertical_path.flap_setting[index]; - single_data_row.algorithm_type = vertical_path.algorithm_type[index]; - return single_data_row; -} - -inline aaesim::open_source::VerticalPathUtils::VerticalPathDataSet - aaesim::open_source::VerticalPathUtils::GetInterpolatedPathData(const VerticalPath &vertical_path, - Units::Length estimated_distance_to_path_end) { - const Units::MetersLength distance_to_go{estimated_distance_to_path_end}; - const auto reference_lookup_index = - CoreUtils::FindNearestIndex(distance_to_go.value(), vertical_path.along_path_distance_m); - - if (reference_lookup_index < 1) { - return GetPathDataAtIndex(vertical_path, reference_lookup_index); - } - - aaesim::open_source::VerticalPathUtils::VerticalPathDataSet single_data_row{}; - single_data_row.resolved_index = reference_lookup_index; - single_data_row.altitude_msl = Units::MetersLength( - CoreUtils::LinearlyInterpolate(reference_lookup_index, distance_to_go.value(), - vertical_path.along_path_distance_m, vertical_path.altitude_m)); - single_data_row.along_path_distance = estimated_distance_to_path_end; - single_data_row.calibrated_airspeed = Units::MetersPerSecondSpeed(CoreUtils::LinearlyInterpolate( - reference_lookup_index, distance_to_go.value(), vertical_path.along_path_distance_m, vertical_path.cas_mps)); - single_data_row.mach = CoreUtils::LinearlyInterpolate(reference_lookup_index, distance_to_go.value(), - vertical_path.along_path_distance_m, vertical_path.mach); - single_data_row.altitude_rate = Units::MetersPerSecondSpeed( - CoreUtils::LinearlyInterpolate(reference_lookup_index, distance_to_go.value(), - vertical_path.along_path_distance_m, vertical_path.altitude_rate_mps)); - single_data_row.true_airspeed = CoreUtils::LinearlyInterpolate( - reference_lookup_index, distance_to_go, vertical_path.along_path_distance_m, vertical_path.true_airspeed); - single_data_row.tas_rate = Units::MetersSecondAcceleration( - CoreUtils::LinearlyInterpolate(reference_lookup_index, distance_to_go.value(), - vertical_path.along_path_distance_m, vertical_path.tas_rate_mps)); - single_data_row.theta = Units::RadiansAngle( - CoreUtils::LinearlyInterpolate(reference_lookup_index, distance_to_go.value(), - vertical_path.along_path_distance_m, vertical_path.theta_radians)); - single_data_row.ground_speed = Units::MetersPerSecondSpeed(CoreUtils::LinearlyInterpolate( - reference_lookup_index, distance_to_go.value(), vertical_path.along_path_distance_m, vertical_path.gs_mps)); - single_data_row.time_to_go = Units::SecondsTime( - CoreUtils::LinearlyInterpolate(reference_lookup_index, distance_to_go.value(), - vertical_path.along_path_distance_m, vertical_path.time_to_go_sec)); - single_data_row.mass = Units::KilogramsMass(CoreUtils::LinearlyInterpolate( - reference_lookup_index, distance_to_go.value(), vertical_path.along_path_distance_m, vertical_path.mass_kg)); - single_data_row.wind_velocity_east = CoreUtils::LinearlyInterpolate( - reference_lookup_index, distance_to_go, vertical_path.along_path_distance_m, vertical_path.wind_velocity_east); - single_data_row.wind_velocity_north = - CoreUtils::LinearlyInterpolate(reference_lookup_index, distance_to_go, vertical_path.along_path_distance_m, - vertical_path.wind_velocity_north); - single_data_row.flap_setting = vertical_path.flap_setting[reference_lookup_index]; - single_data_row.algorithm_type = vertical_path.algorithm_type[reference_lookup_index]; - return single_data_row; -} - -inline Units::Speed aaesim::open_source::VerticalPathUtils::CalculateSpeedGuidance( - const VerticalPath &vertical_path, Units::Length estimated_distance_to_path_end) { - auto reference_lookup_index = CoreUtils::FindNearestIndex( - Units::MetersLength(estimated_distance_to_path_end).value(), vertical_path.along_path_distance_m); - - Units::Speed cas_guidance = Units::zero(); - if (reference_lookup_index == 0) { - cas_guidance = Units::MetersPerSecondSpeed(vertical_path.cas_mps[0]); - } else { - cas_guidance = Units::MetersPerSecondSpeed(CoreUtils::LinearlyInterpolate( - reference_lookup_index, Units::MetersLength(estimated_distance_to_path_end).value(), - vertical_path.along_path_distance_m, vertical_path.cas_mps)); - } - - return cas_guidance; -} - -inline double aaesim::open_source::VerticalPathUtils::CalculateMachGuidance( - const VerticalPath &vertical_path, Units::Length estimated_distance_to_path_end) { - auto reference_lookup_index = CoreUtils::FindNearestIndex( - Units::MetersLength(estimated_distance_to_path_end).value(), vertical_path.along_path_distance_m); - - double mach_guidance = 0; - if (reference_lookup_index == 0) { - mach_guidance = vertical_path.mach[0]; - } else { - mach_guidance = CoreUtils::LinearlyInterpolate(reference_lookup_index, - Units::MetersLength(estimated_distance_to_path_end).value(), - vertical_path.along_path_distance_m, vertical_path.mach); - } - - return mach_guidance; -} - -inline Units::Time aaesim::open_source::VerticalPathUtils::CalculateTimeToFly( - const VerticalPath &vertical_path, Units::Length estimated_distance_to_path_end) { - auto reference_lookup_index = CoreUtils::FindNearestIndex( - Units::MetersLength(estimated_distance_to_path_end).value(), vertical_path.along_path_distance_m); - - Units::Time time_to_fly = Units::zero(); - if (reference_lookup_index == 0) { - time_to_fly = Units::SecondsTime(vertical_path.time_to_go_sec[0]); - } else { - time_to_fly = Units::SecondsTime(CoreUtils::LinearlyInterpolate( - reference_lookup_index, Units::MetersLength(estimated_distance_to_path_end).value(), - vertical_path.along_path_distance_m, vertical_path.time_to_go_sec)); - } - - return time_to_fly; -} - -inline Units::Mass aaesim::open_source::VerticalPathUtils::GetExpectedMass( - const VerticalPath &vertical_path, Units::Length estimated_distance_to_path_end) { - auto reference_lookup_index = CoreUtils::FindNearestIndex( - Units::MetersLength(estimated_distance_to_path_end).value(), vertical_path.along_path_distance_m); - - Units::Mass mass = Units::zero(); - if (reference_lookup_index == 0) { - mass = Units::KilogramsMass(vertical_path.mass_kg[0]); - } else { - mass = Units::KilogramsMass(CoreUtils::LinearlyInterpolate( - reference_lookup_index, Units::MetersLength(estimated_distance_to_path_end).value(), - vertical_path.along_path_distance_m, vertical_path.mass_kg)); - } - - return mass; -} diff --git a/include/public/VerticalPredictor.h b/include/public/VerticalPredictor.h deleted file mode 100644 index 3898447..0000000 --- a/include/public/VerticalPredictor.h +++ /dev/null @@ -1,158 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include -#include -#include - -#include -#include -#include - -#include "public/AircraftState.h" -#include "public/CalcWindGradControl.h" -#include "public/DirectionOfFlightCourseCalculator.h" -#include "public/FixedMassAircraftPerformance.h" -#include "public/Guidance.h" -#include "public/HorizontalPath.h" -#include "public/PrecalcConstraint.h" -#include "public/PrecalcWaypoint.h" -#include "public/VerticalPath.h" -#include "public/WeatherPrediction.h" - -class VerticalPredictor { - public: - VerticalPredictor(); - - virtual ~VerticalPredictor(void) = default; - - VerticalPredictor &operator=(const VerticalPredictor &obj); - - bool operator==(const VerticalPredictor &obj) const; - - bool operator!=(const VerticalPredictor &obj) const; - - virtual void BuildVerticalPrediction(std::vector &horizontal_path, - std::vector &precalc_waypoints, - const aaesim::open_source::WeatherPrediction &weather, - const Units::Length &start_altitude, - const Units::Length &aircraft_distance_to_go) = 0; - - void SetMembers(const VerticalPredictor &vertical_predictor); - - virtual const Units::Length GetAltitudeAtEndOfRoute() const = 0; - - aaesim::open_source::Guidance Update(const aaesim::open_source::AircraftState ¤t_state, - const aaesim::open_source::Guidance ¤t_guidance, - const Units::Length distance_to_go); - - const VerticalPath &GetVerticalPath() const; - - void SetCruiseAltitude(Units::Length cruise_altitude_msl); - - Units::Length GetCruiseAltitude() const; - - const double GetTransitionMach() const; - - Units::Speed GetTransitionIas() const; - - void SetTransitionAltitude(Units::Length transition_altitude_msl); - - Units::Length GetTransitionAltitude() const; - - Units::KnotsSpeed GetIasAtEndOfRoute(); - - std::shared_ptr GetAtmosphere() const; - void SetAtmosphere(std::shared_ptr atmosphere); - - protected: - double CalculateEsfUsingConstantCAS(const double true_airspeed_mps, const double altitude_msl_meter, - const Units::Temperature temperature); - - double CalculateEsfUsingConstantMach(const double true_airspeed_mps, const double altitude_msl_meter, - const Units::Temperature temperature); - - aaesim::open_source::PrecalcConstraint CheckActiveConstraint( - double along_path_distance_to_go_meter, double altitude_msl_meter, double calibrated_airspeed_mps, - const aaesim::open_source::PrecalcConstraint &constraints, double transition_altitude_meter); - - aaesim::open_source::Guidance CalculateGuidanceCommands(const aaesim::open_source::AircraftState &state, - const Units::Length distance_to_go, - const aaesim::open_source::Guidance ¤t_guidance); - - void TrimDuplicatesFromVerticalPath(); - - aaesim::open_source::PrecalcConstraint FindActiveConstraint( - const double &along_path_distance_to_go_meters, const std::vector &precalculated_waypoints); - - const bool IsCruiseMachValid() const; - - inline static const double TIME_STEP_SECONDS{0.5}; - inline static const Units::MetersPerSecondSpeed SPEED_HIGH_CONSTRAINT_TOLERANCE{-0.1}; - inline static const Units::KnotsSpeed SPEED_HIGH_MAXIMUM{1000}; - inline static const Units::FeetLength ALT_HIGH_CONSTRAINT_TOLERANCE{100}; - - const Units::KnotsSpeed LOW_GROUNDSPEED_WARNING; - const Units::KnotsSpeed LOW_GROUNDSPEED_FATAL; - const Units::DegreesAngle DESCENT_ANGLE_MAX; - const Units::DegreesAngle DESCENT_ANGLE_WARNING; - int m_current_trajectory_index; - Units::Length m_cruise_altitude_msl; - Units::Time m_descent_start_time; - Units::Speed m_transition_ias; - Units::Length m_transition_altitude_msl; - double m_cruise_mach; - double m_transition_mach; - aaesim::open_source::PrecalcConstraint m_precalculated_constraints; - aaesim::open_source::CalcWindGradControl m_wind_calculator; - Units::MetersLength m_start_altitude_msl; - VerticalPath m_vertical_path; - aaesim::open_source::DirectionOfFlightCourseCalculator m_course_calculator; - Units::Speed m_ias_at_end_of_route; - std::shared_ptr m_atmosphere; - std::shared_ptr m_bada_calculator; -}; - -inline const VerticalPath &VerticalPredictor::GetVerticalPath() const { return m_vertical_path; } - -inline void VerticalPredictor::SetCruiseAltitude(Units::Length cruise_altitude_msl) { - m_cruise_altitude_msl = cruise_altitude_msl; -} - -inline Units::Length VerticalPredictor::GetCruiseAltitude() const { return m_cruise_altitude_msl; } - -inline const double VerticalPredictor::GetTransitionMach() const { return m_transition_mach; } - -inline Units::Speed VerticalPredictor::GetTransitionIas() const { return m_transition_ias; } - -inline void VerticalPredictor::SetTransitionAltitude(Units::Length transition_altitude_msl) { - m_transition_altitude_msl = transition_altitude_msl; -} - -inline Units::Length VerticalPredictor::GetTransitionAltitude() const { return m_transition_altitude_msl; } - -inline std::shared_ptr VerticalPredictor::GetAtmosphere() const { return m_atmosphere; } - -inline void VerticalPredictor::SetAtmosphere(std::shared_ptr atmosphere) { m_atmosphere = atmosphere; } - -inline const bool VerticalPredictor::IsCruiseMachValid() const { return m_transition_mach > 0; } - -inline Units::KnotsSpeed VerticalPredictor::GetIasAtEndOfRoute() { return m_ias_at_end_of_route; } diff --git a/include/public/WGS84EarthModelConstants.h b/include/public/WGS84EarthModelConstants.h deleted file mode 100644 index 421884f..0000000 --- a/include/public/WGS84EarthModelConstants.h +++ /dev/null @@ -1,29 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include "geolib/Constants.h" -#include "utility/CustomUnits.h" - -namespace aaesim::open_source { -inline static const Units::MetersLength kWgs84SemiMinorAxis{SEMI_MINOR_AXIS_METERS}; -inline static const Units::MetersLength kWgs84SemiMajorAxis{SEMI_MAJOR_AXIS_METERS}; -inline constexpr double kWgs84EccentricitySquared{ECCENTRICITY_SQ}; -} // namespace aaesim::open_source diff --git a/include/public/Waypoint.h b/include/public/Waypoint.h deleted file mode 100644 index 2a1479c..0000000 --- a/include/public/Waypoint.h +++ /dev/null @@ -1,180 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include -#include -#include - -#include "utility/CustomUnits.h" - -class Waypoint { - public: - static const Units::FeetLength MAX_ALTITUDE_CONSTRAINT; - static const Units::FeetLength MIN_ALTITUDE_CONSTRAINT; - static const Units::KnotsSpeed MAX_SPEED_CONSTRAINT; - static const Units::KnotsSpeed MIN_SPEED_CONSTRAINT; - - Waypoint() = default; - - ~Waypoint() = default; - - Waypoint(const std::string &name, Units::Angle latitude, Units::Angle longitude, - Units::Length altitude_constraint_upper = MAX_ALTITUDE_CONSTRAINT, - Units::Length altitude_constraint_lower = MIN_ALTITUDE_CONSTRAINT, - Units::Speed speed_constraint = MAX_SPEED_CONSTRAINT, Units::Length nominal_altitude = Units::ZERO_LENGTH, - Units::Speed nominal_ias = Units::ZERO_SPEED, const std::string &arinc424_leg_type = ""); - - Waypoint &operator=(const Waypoint &in) = default; - - const std::string &GetName() const; - - void SetName(const std::string &name); - - Units::Angle GetLatitude() const; - - void SetLatitude(const Units::Angle &latitude); - - Units::Angle GetLongitude() const; - - void SetLongitude(const Units::Angle &longitude); - - void SetWaypointLatLon(const Units::Angle &latitude, const Units::Angle &longitude); - - Units::Length GetAltitude() const; - - void SetAltitude(const Units::Length &nominal_altitude); - - void SetNominalIas(const Units::Speed &nominal_ias); - - Units::Speed GetNominalIas() const; - - void SetAltitudeConstraintHigh(const Units::Length &altitude_high); - - Units::Length GetAltitudeConstraintHigh() const; - - void SetAltitudeConstraintLow(const Units::Length &altitude_low); - - Units::Length GetAltitudeConstraintLow() const; - - void SetSpeedConstraintHigh(const Units::Speed &speed_high); - - Units::Speed GetSpeedConstraintHigh() const; - - void SetSpeedConstraintLow(const Units::Speed &speed_low); - - Units::Speed GetSpeedConstraintLow() const; - - void SetRfTurnCenterLatitude(const Units::Angle &rf_turn_center_latitude); - - Units::Angle GetRfTurnCenterLatitude() const; - - void SetRfTurnCenterLongitude(const Units::Angle &rf_turn_center_longitude); - - Units::Angle GetRfTurnCenterLongitude() const; - - void SetRfTurnArcRadius(const Units::Length &rf_turn_radius); - - Units::Length GetRfTurnArcRadius() const; - - const std::string &GetArinc424LegType() const; - - private: - std::string m_name{}; - Units::Angle m_latitude{Units::zero()}; - Units::Angle m_longitude{Units::zero()}; - Units::Length m_altitude{Units::zero()}; - Units::Speed m_nominal_ias{Units::zero()}; - Units::Length m_altitude_constraint_high{Units::zero()}; - Units::Length m_altitude_constraint_low{Units::zero()}; - Units::Speed m_speed_constraint_high{Units::zero()}; - Units::Speed m_speed_constraint_low{Units::zero()}; - Units::Angle m_rf_turn_center_latitude{Units::zero()}; - Units::Angle m_rf_turn_center_longitude{Units::zero()}; - Units::Length m_rf_turn_arc_radius{Units::zero()}; - std::string m_arinc424_leg_type{}; -}; - -inline const std::string &Waypoint::GetName() const { return m_name; } - -inline void Waypoint::SetName(const std::string &name) { m_name.assign(name); } - -inline Units::Angle Waypoint::GetLatitude() const { return m_latitude; } - -inline void Waypoint::SetLatitude(const Units::Angle &latitude) { m_latitude = latitude; } - -inline Units::Angle Waypoint::GetLongitude() const { return m_longitude; } - -inline void Waypoint::SetLongitude(const Units::Angle &longitude) { m_longitude = longitude; } - -inline void Waypoint::SetWaypointLatLon(const Units::Angle &latitude, const Units::Angle &longitude) { - SetLatitude(latitude); - SetLongitude(longitude); -} - -inline Units::Length Waypoint::GetAltitude() const { return m_altitude; } - -inline void Waypoint::SetAltitude(const Units::Length &nominal_altitude) { m_altitude = nominal_altitude; } - -inline Units::Speed Waypoint::GetNominalIas() const { return m_nominal_ias; } - -inline void Waypoint::SetNominalIas(const Units::Speed &nominal_ias) { m_nominal_ias = nominal_ias; } - -inline void Waypoint::SetAltitudeConstraintHigh(const Units::Length &altitude_high) { - m_altitude_constraint_high = altitude_high; -} - -inline Units::Length Waypoint::GetAltitudeConstraintHigh() const { return m_altitude_constraint_high; } - -inline void Waypoint::SetAltitudeConstraintLow(const Units::Length &altitude_low) { - m_altitude_constraint_low = altitude_low; -} - -inline Units::Length Waypoint::GetAltitudeConstraintLow() const { return m_altitude_constraint_low; } - -inline void Waypoint::SetSpeedConstraintHigh(const Units::Speed &speed_high) { m_speed_constraint_high = speed_high; } - -inline Units::Speed Waypoint::GetSpeedConstraintHigh() const { return m_speed_constraint_high; } - -inline void Waypoint::SetSpeedConstraintLow(const Units::Speed &speed_low) { m_speed_constraint_low = speed_low; } - -inline Units::Speed Waypoint::GetSpeedConstraintLow() const { return m_speed_constraint_low; } - -inline Units::Angle Waypoint::GetRfTurnCenterLatitude() const { return m_rf_turn_center_latitude; } - -inline void Waypoint::SetRfTurnCenterLatitude(const Units::Angle &rf_turn_center_latitude) { - m_rf_turn_center_latitude = rf_turn_center_latitude; -} - -inline Units::Angle Waypoint::GetRfTurnCenterLongitude() const { return m_rf_turn_center_longitude; } - -inline void Waypoint::SetRfTurnCenterLongitude(const Units::Angle &rf_turn_center_longitude) { - m_rf_turn_center_longitude = rf_turn_center_longitude; -} - -inline Units::Length Waypoint::GetRfTurnArcRadius() const { return m_rf_turn_arc_radius; } - -inline void Waypoint::SetRfTurnArcRadius(const Units::Length &rf_turn_radius) { m_rf_turn_arc_radius = rf_turn_radius; } - -inline const std::string &Waypoint::GetArinc424LegType() const { return m_arinc424_leg_type; } - -std::ostream &operator<<(std::ostream &out, const Waypoint &waypoint); - -std::ostream &operator<<(std::ostream &out, const std::list &waypoints); diff --git a/include/public/WaypointPassingMonitor.h b/include/public/WaypointPassingMonitor.h deleted file mode 100644 index 316b45f..0000000 --- a/include/public/WaypointPassingMonitor.h +++ /dev/null @@ -1,35 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include - -#include "public/AircraftState.h" -#include "public/LatitudeLongitudePoint.h" - -namespace aaesim { -namespace open_source { -struct WaypointPassingMonitor { - virtual void Update(const aaesim::LatitudeLongitudePoint &position, const Units::SignedAngle &ground_course_enu) = 0; - - virtual bool IsPassedWaypoint() const = 0; -}; -} // namespace open_source -} // namespace aaesim diff --git a/include/public/WeatherEstimate.h b/include/public/WeatherEstimate.h deleted file mode 100644 index 2e5acbf..0000000 --- a/include/public/WeatherEstimate.h +++ /dev/null @@ -1,119 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include -#include - -#include -#include - -#include "public/Atmosphere.h" -#include "public/WindStack.h" - -class Wind; - -namespace aaesim { -namespace open_source { -class WeatherEstimate { - friend class Wind_populate_predicted_wind_matrices_Test; - - friend class TrajectoryPredictor_updateWeatherPrediction_Test; - - friend class TrajectoryPredictor_startAltitudeInDescentAltList_Test; - - friend class TrajectoryPredictor_startAndEndAltitudeInDescentAltList_Test; - - struct shared_members { - shared_members() = default; - explicit shared_members(std::shared_ptr atmosphere) - : east_west(), north_south(), m_atmosphere(atmosphere) {} - aaesim::open_source::WindStack east_west{}; - aaesim::open_source::WindStack north_south{}; - std::shared_ptr m_atmosphere{}; - }; - - public: - aaesim::open_source::WindStack &east_west() const { return m_shared_members->east_west; } - aaesim::open_source::WindStack &north_south() const { return m_shared_members->north_south; } - - std::shared_ptr getWind() const; - - std::shared_ptr getAtmosphere() const; - - virtual void LoadConditionsAt(const Units::Angle latitude, const Units::Angle longitude, - const Units::Length altitude); - Units::Density GetDensity() const; - Units::Pressure GetPressure() const; - Units::KelvinTemperature GetTemperature() const; - - Units::Speed MachToTAS(const double mach, const Units::Length altitude) const; - Units::Speed MachToCAS(const double mach, const Units::Length altitude) const; - Units::Speed TAS2CAS(const Units::Speed true_airspeed, const Units::Length altitude) const; - Units::Speed CAS2TAS(const Units::Speed calibrated_airspeed, const Units::Length altitude) const; - double CAS2Mach(const Units::Speed calibrated_airspeed, const Units::Length altitude) const; - double TAS2Mach(const Units::Speed true_airspeed, const Units::Length altitude) const; - double ESFconstantCAS(const Units::Speed true_airspeed, const Units::Length altitude) const; - std::pair, Units::Length> GetCurrentLocationOfWeather() const; - void SetAtmosphere(std::shared_ptr atmosphere); - - protected: - // Constructors are protected to prevent bare instantiation. - // Callers should use WeatherPrediction or WeatherTruth. - WeatherEstimate(); - - WeatherEstimate(std::shared_ptr wind, std::shared_ptr atmosphere); - - virtual ~WeatherEstimate(); - - bool IsTemperatureAvailable(const Units::Angle latitude, const Units::Angle longitude, - const Units::Length altitude) const; - - void SetLocation(const Units::Angle latitude, const Units::Angle longitude, const Units::Length altitude); - - std::shared_ptr m_shared_members{}; - std::shared_ptr m_wind{}; - Units::KelvinTemperature m_temperature{}; - Units::Pressure m_pressure{}; - Units::Density m_density{}; - mutable bool m_temperature_checked{true}, m_temperature_available{false}; - std::pair, Units::Length> m_location_of_current_conditions{}; - - private: - inline static log4cplus::Logger m_logger{log4cplus::Logger::getInstance(LOG4CPLUS_TEXT("WeatherEstimate"))}; -}; - -inline void WeatherEstimate::SetAtmosphere(std::shared_ptr atmosphere) { - m_shared_members->m_atmosphere = atmosphere; -} - -inline void WeatherEstimate::SetLocation(const Units::Angle latitude, const Units::Angle longitude, - const Units::Length altitude) { - m_location_of_current_conditions.first = std::pair(latitude, longitude); - m_location_of_current_conditions.second = altitude; -} - -inline std::pair, Units::Length> WeatherEstimate::GetCurrentLocationOfWeather() - const { - return m_location_of_current_conditions; -} - -} // namespace open_source -} // namespace aaesim diff --git a/include/public/WeatherPrediction.h b/include/public/WeatherPrediction.h deleted file mode 100644 index da4fe3e..0000000 --- a/include/public/WeatherPrediction.h +++ /dev/null @@ -1,72 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include - -#include "public/WeatherEstimate.h" - -namespace aaesim { -namespace test { -class Wind_populate_predicted_wind_matrices_Test; -class TrajectoryPredictor_updateWeatherPrediction_Test; -class TrajectoryPredictor_startAltitudeInDescentAltList_Test; -class TrajectoryPredictor_startAndEndAltitudeInDescentAltList_Test; -} // namespace test - -namespace open_source { - -class WeatherPrediction final : public WeatherEstimate { - friend class aaesim::test::Wind_populate_predicted_wind_matrices_Test; - - friend class aaesim::test::TrajectoryPredictor_updateWeatherPrediction_Test; - - friend class aaesim::test::TrajectoryPredictor_startAltitudeInDescentAltList_Test; - - friend class aaesim::test::TrajectoryPredictor_startAndEndAltitudeInDescentAltList_Test; - - public: - static aaesim::open_source::WeatherPrediction CreateZeroWindPrediction(std::shared_ptr atmosphere); - - WeatherPrediction() = default; - WeatherPrediction(std::shared_ptr wind, std::shared_ptr atmosphere); - virtual ~WeatherPrediction() = default; - - std::shared_ptr GetForecastWind() const; - - std::shared_ptr GetForecastAtmosphere() const; - - void IncrementUpdateCount(); - - int GetUpdateCount() const; - - private: - int update_count_{0}; -}; - -inline void WeatherPrediction::IncrementUpdateCount() { ++update_count_; } - -inline int WeatherPrediction::GetUpdateCount() const { return update_count_; } - -inline std::shared_ptr WeatherPrediction::GetForecastWind() const { return getWind(); } - -inline std::shared_ptr WeatherPrediction::GetForecastAtmosphere() const { return getAtmosphere(); } -} // namespace open_source -} // namespace aaesim diff --git a/include/public/WeatherTruth.h b/include/public/WeatherTruth.h deleted file mode 100644 index 0112a1c..0000000 --- a/include/public/WeatherTruth.h +++ /dev/null @@ -1,37 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include - -#include "public/WeatherEstimate.h" - -namespace aaesim { -namespace open_source { -class WeatherTruth : public WeatherEstimate { - public: - WeatherTruth(); - - WeatherTruth(std::shared_ptr wind, std::shared_ptr atmosphere, bool inhibit_weather_temperature); - - virtual ~WeatherTruth(); -}; -} // namespace open_source -} // namespace aaesim diff --git a/include/public/Wgs84PrecalcWaypoint.h b/include/public/Wgs84PrecalcWaypoint.h deleted file mode 100644 index a172b34..0000000 --- a/include/public/Wgs84PrecalcWaypoint.h +++ /dev/null @@ -1,51 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include -#include - -#include - -#include "public/AircraftIntent.h" -#include "public/LatitudeLongitudePoint.h" -#include "public/PrecalcConstraint.h" - -namespace aaesim::open_source { - -struct Wgs84PrecalcWaypoint final { - Wgs84PrecalcWaypoint() = default; - ~Wgs84PrecalcWaypoint() = default; - bool operator==(const Wgs84PrecalcWaypoint &obj) const; - - std::string m_name{}; - AircraftIntent::Arinc424LegType m_leg_type{AircraftIntent::Arinc424LegType::UNSET}; - Units::Length m_leg_length{Units::zero()}; - Units::SignedRadiansAngle m_enu_course_out_angle{Units::zero()}; - Units::SignedRadiansAngle m_enu_course_in_angle{Units::zero()}; - LatitudeLongitudePoint m_position{}; - LatitudeLongitudePoint m_rf_leg_center{}; - Units::MetersLength m_radius_rf_leg{Units::zero()}; - Units::RadiansAngle m_bank_angle{Units::zero()}; - Units::MetersPerSecondSpeed m_ground_speed{Units::zero()}; - open_source::PrecalcConstraint m_precalc_constraints{}; -}; - -} // namespace aaesim::open_source diff --git a/include/public/Wind.h b/include/public/Wind.h deleted file mode 100644 index 90fc034..0000000 --- a/include/public/Wind.h +++ /dev/null @@ -1,63 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include -#include -#include -#include - -#include - -#include "public/AircraftIntent.h" -#include "public/WeatherPrediction.h" - -class Wind { - public: - Wind() = default; - virtual ~Wind() = default; - - void InterpolateTrueWind(const Units::Angle lat_in, const Units::Angle lon_in, const Units::Length altitude, - aaesim::open_source::WindStack &east_west, aaesim::open_source::WindStack &north_south); - - void InterpolateForecastWind(const std::shared_ptr &tangentPlaneSequence, - const Units::Length x_in, const Units::Length y_in, const Units::Length altitude, - Units::Speed &east_west, Units::Speed &north_south); - - virtual void InterpolateWindScalar(Units::Angle lat_in, Units::Angle lon_in, Units::Length altitude, - Units::Speed &east_west, Units::Speed &north_south) = 0; - - virtual Units::KelvinTemperature InterpolateTemperature(const Units::Angle latitude_in, - const Units::Angle longitude_in, - const Units::Length altitude) = 0; - - virtual Units::Pressure InterpolatePressure(const Units::Angle latitude_in, const Units::Angle longitude_in, - const Units::Length altitude) = 0; - - virtual void InterpolateWind(Units::Angle latitude_in, Units::Angle longitude_in, Units::Length altitude, - Units::Speed &u, Units::Speed &v) = 0; - - virtual void InterpolateWindMatrix(Units::Angle lat_in, Units::Angle lon_in, Units::Length alt_in, - aaesim::open_source::WindStack &east_west, - aaesim::open_source::WindStack &north_south) = 0; - - private: - inline static log4cplus::Logger m_logger{log4cplus::Logger::getInstance(LOG4CPLUS_TEXT("Wind"))}; -}; diff --git a/include/public/WindBlendingAlgorithm.h b/include/public/WindBlendingAlgorithm.h deleted file mode 100644 index 179f5fb..0000000 --- a/include/public/WindBlendingAlgorithm.h +++ /dev/null @@ -1,30 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include "public/AircraftState.h" -#include "public/WeatherPrediction.h" - -namespace aaesim::open_source { -struct WindBlendingAlgorithm { - virtual void BlendSensedWithPredicted(const aaesim::open_source::AircraftState ¤t_state, - aaesim::open_source::WeatherPrediction &weather_prediction) = 0; -}; -} // namespace aaesim::open_source diff --git a/include/public/WindStack.h b/include/public/WindStack.h deleted file mode 100644 index 222beb3..0000000 --- a/include/public/WindStack.h +++ /dev/null @@ -1,84 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include -#include -#include - -#include -#include - -namespace aaesim { -namespace open_source { -class WindStack { - public: - WindStack(); - - WindStack(const WindStack &in); - - WindStack(const int min, const int max); - - ~WindStack() = default; - - WindStack &operator=(const WindStack &in); - - bool operator==(const WindStack &obj) const; - - bool operator!=(const WindStack &obj) const; - - Units::FeetLength GetAltitude(const int index) const; - - Units::KnotsSpeed GetSpeed(const int index) const; - - int GetMinRow() const; - - int GetMaxRow() const; - - void SetBounds(const int min, const int max); - - void Insert(const int index, const Units::Length altitude, const Units::Speed speed); - - void SortAltitudesAscending(); - - void CalculateWindGradientAtAltitude(const Units::Length altitude_in, Units::Speed &wind_speed, - Units::Frequency &wind_gradient) const; - - static WindStack CreateZeroSpeedStack(); - - private: - static bool AltitudeComparator(std::pair item1, - std::pair item2); - void Copy(const WindStack &in); - std::vector m_altitude; - std::vector m_speed; - int m_minimum_data_index, m_maximum_data_index; -}; - -inline int WindStack::GetMinRow() const { return m_minimum_data_index; } - -inline int WindStack::GetMaxRow() const { return m_maximum_data_index; } - -inline bool WindStack::AltitudeComparator(std::pair item1, - std::pair item2) { - return item1.first < item2.first; -} -} // namespace open_source -} // namespace aaesim diff --git a/include/public/WindZero.h b/include/public/WindZero.h deleted file mode 100644 index 9d2c73a..0000000 --- a/include/public/WindZero.h +++ /dev/null @@ -1,54 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include - -#include "public/Atmosphere.h" -#include "public/Wind.h" - -namespace aaesim { -namespace open_source { -class WindZero final : public Wind { - public: - WindZero(std::shared_ptr atmosphere); - - virtual ~WindZero(); - - void InterpolateWind(Units::Angle latitude_in, Units::Angle longitude_in, Units::Length alt, Units::Speed &u, - Units::Speed &v) override; - - void InterpolateWindScalar(Units::Angle lat_in, Units::Angle lon_in, Units::Length altitude, Units::Speed &east_west, - Units::Speed &north_south) override; - - void InterpolateWindMatrix(Units::Angle lat_in, Units::Angle lon_in, Units::Length alt_in, - aaesim::open_source::WindStack &east_west, - aaesim::open_source::WindStack &north_south) override; - - Units::KelvinTemperature InterpolateTemperature(Units::Angle latitude_in, Units::Angle longitude_in, - Units::Length alt) override; - - Units::Pressure InterpolatePressure(Units::Angle latitude_in, Units::Angle longitude_in, Units::Length alt) override; - - private: - std::shared_ptr m_atmosphere; -}; -} // namespace open_source -} // namespace aaesim diff --git a/include/public/ZeroWindTrueWeatherOperator.h b/include/public/ZeroWindTrueWeatherOperator.h deleted file mode 100644 index c9fee24..0000000 --- a/include/public/ZeroWindTrueWeatherOperator.h +++ /dev/null @@ -1,39 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include "public/AbstractTrueWeatherOperator.h" - -namespace aaesim::open_source { -class ZeroWindTrueWeatherOperator final : public AbstractTrueWeatherOperator { - public: - ZeroWindTrueWeatherOperator(std::shared_ptr true_weather); - ~ZeroWindTrueWeatherOperator() = default; - void CalculateEnvironmentalWind(const EarthModel::GeodeticPosition &position, - const Units::Length &altitude_msl) override; - Units::Speed GetWindSpeedEast() const override { return Units::zero(); } - Units::Speed GetWindSpeedNorth() const override { return Units::zero(); } - Units::Frequency GetWindSpeedVerticalDerivativeEast() const override { return Units::zero(); } - Units::Frequency GetWindSpeedVerticalDerivativeNorth() const override { return Units::zero(); } - - private: - inline static WindStack ZERO_STACK{WindStack::CreateZeroSpeedStack()}; -}; -} // namespace aaesim::open_source diff --git a/include/public/minicsv.h b/include/public/minicsv.h deleted file mode 100755 index 773e51b..0000000 --- a/include/public/minicsv.h +++ /dev/null @@ -1,1008 +0,0 @@ -// The MIT License (MIT) -// Minimalistic CSV Streams 1.7.9 -// Copyright (C) 2014 - 2016, by Wong Shao Voon (shaovoon@yahoo.com) -// -// http://opensource.org/licenses/MIT -// -// version 1.2 : make use of make_shared -// version 1.3 : fixed: to when reading the last line and it does not have linefeed -// added: skip_1st_line and skip_line functions to ifstream class -// version 1.4 : Removed the use of smart ptr. -// version 1.5 : Performance increase on writing without flushing every line. -// version 1.6 : Add string streams -// version 1.7 : You MUST specify the escape/unescape string when calling set_delimiter. Option to surround/trim string with quotes -// version 1.7.1 : Add stream operator overload usage in example.cpp -// Disable the surround/trim quote on text by default -// version 1.7.2 : Stream operator overload for const char* -// version 1.7.3 : Add num_of_delimiter method to ifstream and istringstream -// Fix g++ compilation errors -// version 1.7.4 : Add get_rest_of_line -// version 1.7.5 : Add terminate_on_blank_line variable. Set to false if your file format has blank lines in between. -// version 1.7.6 : Ignore delimiters within quotes during reading when enable_trim_quote_on_str is true; -// version 1.7.7 : Fixed multiple symbol linkage errors -// version 1.7.8 : Add quote escape/unescape. Default is """ -// version 1.7.9 : Reading UTF-8 BOM -// version 1.7.10 : separator class for the stream, so that no need to call set_delimiter repeatedly if delimiter keep changing -// version 1.7.11 : Fixed num_of_delimiters function: do not count delimiter within quotes -// version 1.8.0 : Add meaningful error message for data conversion during reading -// version 1.8.1 : Put under the mini namespace - -//#define USE_BOOST_LEXICAL_CAST - -// This was downloaded from https://github.com/shaovoon/minicsv - -#ifndef MiniCSV_H -#define MiniCSV_H - -#include -#include -#include -#include -#include - -#include - -#ifdef USE_BOOST_LEXICAL_CAST -# include -#endif - -#define NEWLINE '\n' - -namespace mini { - - namespace csv { - inline std::string const &replace(std::string &src, - std::string const &to_find, - std::string const &to_replace) { - size_t pos = 0; - while (std::string::npos != pos) { - pos = src.find(to_find, pos); - - if (std::string::npos != pos) { - src.erase(pos, to_find.size()); - src.insert(pos, to_replace); - pos += to_replace.size(); - } - } - - return src; - } - - inline std::string trim_right(const std::string &str, - const std::string &trimChars) { - std::string result = ""; - size_t endpos = str.find_last_not_of(trimChars); - if (std::string::npos != endpos) { - result = str.substr(0, endpos + 1); - } else { - result = str; - } - - return result; - } - - inline std::string trim_left(const std::string &str, - const std::string &trimChars) { - std::string result = ""; - - size_t startpos = str.find_first_not_of(trimChars); - if (std::string::npos != startpos) { - result = str.substr(startpos); - } else { - result = str; - } - - return result; - } - - inline std::string trim(const std::string &str, - const std::string &trimChars) { - return trim_left(trim_right(str, trimChars), trimChars); - } - - class sep // separator class for the stream, so that no need to call set_delimiter - { - public: - sep(const char delimiter_, - const std::string &escape_) - : delimiter(delimiter_), escape(escape_) { - } - - const char get_delimiter() const { - return delimiter; - } - - const std::string &get_escape() const { - return escape; - } - - private: - const char delimiter; - const std::string escape; - }; - - class ifstream - { - public: - ifstream() - : str(""), pos(0), delimiter(","), unescape_str("##"), trim_quote_on_str(false), trim_quote('\"'), - terminate_on_blank_line(true), quote_unescape("""), has_bom(false), first_line_read(false), - filename(""), line_num(0), token_num(0) { - } - - ifstream(const char *file) { - open(file); - } - - void open(const char *file) { - init(); - filename = file; - istm.open(file, std::ios_base::in); - read_bom(); - } - - void read_bom() { - char tt[3] = {0, 0, 0}; - - istm.read(tt, sizeof(tt)); - - if (tt[0] == (char) 0xEF || tt[1] == (char) 0xBB || - tt[2] == (char) 0xBF) { // not the correct BOM, so reset the pos to beginning (file might not have BOM) - has_bom = true; - } - - istm.seekg(0, istm.beg); - } - - void init() { - str = ""; - pos = 0; - delimiter = ','; - unescape_str = "##"; - trim_quote_on_str = false; - trim_quote = '\"'; - terminate_on_blank_line = true; - has_bom = false; - first_line_read = false; - filename = ""; - line_num = 0; - token_num = 0; - } - - void close() { - istm.close(); - } - - bool is_open() { - return istm.is_open(); - } - - void enable_trim_quote_on_str(bool enable, - char quote, - const std::string &unescape = """) { - trim_quote_on_str = enable; - trim_quote = quote; - quote_unescape = unescape; - } - - // eof is replaced by read_line - //bool eof() const - void set_delimiter(char delimiter_, - std::string const &unescape_str_) { - delimiter = delimiter_; - unescape_str = unescape_str_; - } - - std::string const &get_delimiter() const { - return delimiter; - } - - std::string const &get_unescape_str() const { - return unescape_str; - } - - void skip_line() { - if (!istm.eof()) { - std::getline(istm, str); - pos = 0; - - if (first_line_read == false) { - first_line_read = true; - } - } - } - - bool read_line() { - this->str = ""; - while (!istm.eof()) { - std::getline(istm, this->str); - pos = 0; - - if (first_line_read == false) { - first_line_read = true; - if (has_bom) { - this->str = this->str.substr(3); - } - } - - if (this->str.empty()) { - if (terminate_on_blank_line) { - break; - } else { - continue; - } - } - - ++line_num; - token_num = 0; - return true; - } - return false; - } - - std::string get_delimited_str() { - std::string str = ""; - char ch = '\0'; - bool within_quote = false; - do { - if (pos >= this->str.size()) { - this->str = ""; - - ++token_num; - return unescape(str); - } - - ch = this->str[pos]; - if (trim_quote_on_str) { - if (within_quote == false && ch == trim_quote && - ((pos > 0 && this->str[pos - 1] == delimiter[0]) || pos == 0)) { - within_quote = true; - } else if (within_quote && ch == trim_quote) { - within_quote = false; - } - } - - ++(pos); - - if (ch == delimiter[0] && within_quote == false) { - break; - } - if (ch == '\r' || ch == '\n') { - break; - } - - str += ch; - } while (true); - - ++token_num; - return unescape(str); - } - - std::string unescape(std::string &src) { - src = unescape_str.empty() ? src : replace(src, unescape_str, delimiter); - - if (trim_quote_on_str) { - std::string s = trim(src, std::string(1, trim_quote)); - return replace(s, quote_unescape, std::string(1, trim_quote)); - } - - return src; - } - - size_t num_of_delimiter() const { - if (delimiter.size() == 0) { - return 0; - } - - size_t cnt = 0; - if (trim_quote_on_str) { - bool inside_quote = false; - for (size_t i = 0; i < str.size(); ++i) { - if (str[i] == trim_quote) { - inside_quote = !inside_quote; - } - - if (!inside_quote) { - if (str[i] == delimiter[0]) { - ++cnt; - } - } - } - } else { - cnt = std::count(str.begin(), str.end(), delimiter[0]); - } - return cnt; - } - - std::string get_rest_of_line() const { - return str.substr(pos); - } - - const std::string &get_line() const { - return str; - } - - void enable_terminate_on_blank_line(bool enable) { - terminate_on_blank_line = enable; - } - - bool is_terminate_on_blank_line() const { - return terminate_on_blank_line; - } - - std::string error_line(const std::string &token, - const std::string &function_site) { - std::ostringstream is; - is << "csv::ifstream Conversion error at line no.:" << line_num - << ", filename:" << filename << ", token position:" << token_num - << ", token:" << token << ", function:" << function_site; - - return is.str(); - } - - private: - std::ifstream istm; - std::string str; - size_t pos; - std::string delimiter; - std::string unescape_str; - bool trim_quote_on_str; - char trim_quote; - bool terminate_on_blank_line; - std::string quote_unescape; - bool has_bom; - bool first_line_read; - std::string filename; - size_t line_num; - size_t token_num; - }; - - class ofstream - { - public: - - ofstream() - : after_newline(true), delimiter(","), escape_str("##"), surround_quote_on_str(false), - surround_quote('\"'), quote_escape(""") { - std::ostringstream tmp; - mPrecision = tmp.precision(); - } - - ofstream(const char *file) { - std::ostringstream tmp; - mPrecision = tmp.precision(); - - open(file); - } - - void open(const char *file) { - init(); - ostm.open(file, std::ios_base::out); - } - - void init() { - after_newline = true; - delimiter = ','; - escape_str = "##"; - surround_quote_on_str = false; - surround_quote = '\"'; - quote_escape = """; - } - - void flush() { - ostm.flush(); - } - - void close() { - ostm.close(); - } - - bool is_open() { - return ostm.is_open(); - } - - void enable_surround_quote_on_str(bool enable, - char quote, - const std::string &escape = """) { - surround_quote_on_str = enable; - surround_quote = quote; - quote_escape = escape; - } - - void set_delimiter(char delimiter_, - std::string const &escape_str_) { - delimiter = delimiter_; - escape_str = escape_str_; - } - - std::string const &get_delimiter() const { - return delimiter; - } - - std::string const &get_escape_str() const { - return escape_str; - } - - void set_after_newline(bool after_newline_) { - after_newline = after_newline_; - } - - bool get_after_newline() const { - return after_newline; - } - - std::ofstream &get_ofstream() { - return ostm; - } - - void escape_and_output(std::string src) { - ostm << ((escape_str.empty()) ? src : replace(src, delimiter, escape_str)); - } - - void escape_str_and_output(std::string src) { - src = ((escape_str.empty()) ? src : replace(src, delimiter, escape_str)); - if (surround_quote_on_str) { - if (!quote_escape.empty()) { - src = replace(src, std::string(1, surround_quote), quote_escape); - } - ostm << surround_quote << src << surround_quote; - } else { - ostm << src; - } - } - - void precision(int precision) { - mPrecision = precision; - } - - int precision() { - return mPrecision; - } - - private: - std::ofstream ostm; - bool after_newline; - std::string delimiter; - std::string escape_str; - bool surround_quote_on_str; - char surround_quote; - std::string quote_escape; - int mPrecision; - }; - - - } // ns csv -} // ns mini - -template -mini::csv::ifstream &operator>>(mini::csv::ifstream &istm, - T &val) { - std::string str = istm.get_delimited_str(); - -#ifdef USE_BOOST_LEXICAL_CAST - try - { - val = boost::lexical_cast(str); - } - catch (boost::bad_lexical_cast& e) - { -#ifdef _WIN32 - const std::string function_site = __FUNCSIG__; -#else - const std::string function_site = __PRETTY_FUNCTION__; -#endif - - throw std::runtime_error(istm.error_line(str, function_site).c_str()); - } -#else - std::istringstream is(str); - is >> val; - if (!(bool) is) { -#ifdef _WIN32 - const std::string function_site = __FUNCSIG__; -#else - const std::string function_site = __PRETTY_FUNCTION__; -#endif - - throw std::runtime_error(istm.error_line(str, function_site).c_str()); - } -#endif - - return istm; -} - -template<> -inline mini::csv::ifstream &operator>>(mini::csv::ifstream &istm, - std::string &val) { - val = istm.get_delimited_str(); - - return istm; -} - -template<> -inline mini::csv::ifstream &operator>>(mini::csv::ifstream &istm, - mini::csv::sep &val) { - istm.set_delimiter(val.get_delimiter(), val.get_escape()); - - return istm; -} - -template -mini::csv::ofstream &operator<<(mini::csv::ofstream &ostm, - const T &val) { - if (!ostm.get_after_newline()) { - ostm.get_ofstream() << ostm.get_delimiter(); - } - - std::ostringstream os_temp; - - os_temp.precision(ostm.precision()); - - os_temp << val; - - ostm.escape_and_output(os_temp.str()); - - ostm.set_after_newline(false); - - return ostm; -} - -template -mini::csv::ofstream &operator<<(mini::csv::ofstream &ostm, - const T *val) { - if (!ostm.get_after_newline()) { - ostm.get_ofstream() << ostm.get_delimiter(); - } - - std::ostringstream os_temp; - - os_temp << *val; - - ostm.escape_and_output(os_temp.str()); - - ostm.set_after_newline(false); - - return ostm; -} - -template<> -inline mini::csv::ofstream &operator<<(mini::csv::ofstream &ostm, - const std::string &val) { - if (!ostm.get_after_newline()) { - ostm.get_ofstream() << ostm.get_delimiter(); - } - - std::string temp = val; - ostm.escape_str_and_output(temp); - - ostm.set_after_newline(false); - - return ostm; -} - -template<> -inline mini::csv::ofstream &operator<<(mini::csv::ofstream &ostm, - const mini::csv::sep &val) { - ostm.set_delimiter(val.get_delimiter(), val.get_escape()); - - return ostm; -} - -template<> -inline mini::csv::ofstream &operator<<(mini::csv::ofstream &ostm, - const char &val) { - if (val == NEWLINE) { - ostm.get_ofstream() << NEWLINE; - - ostm.set_after_newline(true); - } else { - std::ostringstream os_temp; - - os_temp << val; - - ostm.escape_and_output(os_temp.str()); - } - - return ostm; -} - -template<> -inline mini::csv::ofstream &operator<<(mini::csv::ofstream &ostm, - const char *val) { - const std::string temp = val; - - ostm << temp; - - return ostm; -} - -namespace mini { - namespace csv { - - class istringstream - { - public: - istringstream(const char *text) - : str(""), pos(0), delimiter(","), unescape_str("##"), trim_quote_on_str(false), trim_quote('\"'), - terminate_on_blank_line(true), quote_unescape("""), line_num(0), token_num(0) { - istm.str(text); - } - - void enable_trim_quote_on_str(bool enable, - char quote, - const std::string &unescape = """) { - trim_quote_on_str = enable; - trim_quote = quote; - quote_unescape = unescape; - } - - void set_delimiter(char delimiter_, - std::string const &unescape_str_) { - delimiter = delimiter_; - unescape_str = unescape_str_; - } - - std::string const &get_delimiter() const { - return delimiter; - } - - std::string const &get_unescape_str() const { - return unescape_str; - } - - void skip_line() { - std::getline(istm, str); - pos = 0; - } - - bool read_line() { - this->str = ""; - while (!istm.eof()) { - std::getline(istm, this->str); - pos = 0; - - if (this->str.empty()) { - if (terminate_on_blank_line) { - break; - } else { - continue; - } - } - - ++line_num; - token_num = 0; - return true; - } - return false; - } - - std::string get_delimited_str() { - std::string str = ""; - char ch = '\0'; - bool within_quote = false; - do { - if (pos >= this->str.size()) { - this->str = ""; - - ++token_num; - return unescape(str); - } - - ch = this->str[pos]; - if (trim_quote_on_str) { - if (within_quote == false && ch == trim_quote && - ((pos > 0 && this->str[pos - 1] == delimiter[0]) || pos == 0)) { - within_quote = true; - } else if (within_quote && ch == trim_quote) { - within_quote = false; - } - } - - ++(pos); - - if (ch == delimiter[0] && within_quote == false) { - break; - } - if (ch == '\r' || ch == '\n') { - break; - } - - str += ch; - } while (true); - - ++token_num; - return unescape(str); - } - - std::string unescape(std::string &src) { - src = unescape_str.empty() ? src : replace(src, unescape_str, delimiter); - if (trim_quote_on_str) { - std::string s = trim(src, std::string(1, trim_quote)); - return replace(s, quote_unescape, std::string(1, trim_quote)); - } - return src; - } - - size_t num_of_delimiter() const { - if (delimiter.size() == 0) { - return 0; - } - - size_t cnt = 0; - if (trim_quote_on_str) { - bool inside_quote = false; - for (size_t i = 0; i < str.size(); ++i) { - if (str[i] == trim_quote) { - inside_quote = !inside_quote; - } - - if (!inside_quote) { - if (str[i] == delimiter[0]) { - ++cnt; - } - } - } - } else { - cnt = std::count(str.begin(), str.end(), delimiter[0]); - } - return cnt; - } - - std::string get_rest_of_line() const { - return str.substr(pos); - } - - const std::string &get_line() const { - return str; - } - - void enable_terminate_on_blank_line(bool enable) { - terminate_on_blank_line = enable; - } - - bool is_terminate_on_blank_line() const { - return terminate_on_blank_line; - } - - std::string error_line(const std::string &token, - const std::string &function_site) { - std::ostringstream is; - is << "csv::istringstream conversion error at line no.:" << line_num - << ", token position:" << token_num << ", token:" << token - << ", function:" << function_site; - return is.str(); - } - - private: - std::istringstream istm; - std::string str; - size_t pos; - std::string delimiter; - std::string unescape_str; - bool trim_quote_on_str; - char trim_quote; - bool terminate_on_blank_line; - std::string quote_unescape; - size_t line_num; - size_t token_num; - }; - - class ostringstream - { - public: - - ostringstream() - : after_newline(true), delimiter(","), escape_str("##"), surround_quote_on_str(false), - surround_quote('\"'), quote_escape(""") { - } - - void enable_surround_quote_on_str(bool enable, - char quote, - const std::string &escape = """) { - surround_quote_on_str = enable; - surround_quote = quote; - quote_escape = escape; - } - - void set_delimiter(char delimiter_, - std::string const &escape_str_) { - delimiter = delimiter_; - escape_str = escape_str_; - } - - std::string const &get_delimiter() const { - return delimiter; - } - - std::string const &get_escape_str() const { - return escape_str; - } - - void set_after_newline(bool after_newline_) { - after_newline = after_newline_; - } - - bool get_after_newline() const { - return after_newline; - } - - std::ostringstream &get_ostringstream() { - return ostm; - } - - std::string get_text() { - return ostm.str(); - } - - void escape_and_output(std::string src) { - ostm << ((escape_str.empty()) ? src : replace(src, delimiter, escape_str)); - } - - void escape_str_and_output(std::string src) { - src = ((escape_str.empty()) ? src : replace(src, delimiter, escape_str)); - if (surround_quote_on_str) { - if (!quote_escape.empty()) { - src = replace(src, std::string(1, surround_quote), quote_escape); - } - ostm << surround_quote << src << surround_quote; - } else { - ostm << src; - } - } - - private: - std::ostringstream ostm; - bool after_newline; - std::string delimiter; - std::string escape_str; - bool surround_quote_on_str; - char surround_quote; - std::string quote_escape; - }; - - - } // ns csv -} // ns mini - -template -mini::csv::istringstream &operator>>(mini::csv::istringstream &istm, - T &val) { - std::string str = istm.get_delimited_str(); - -#ifdef USE_BOOST_LEXICAL_CAST - try - { - val = boost::lexical_cast(str); - } - catch (boost::bad_lexical_cast& e) - { -#ifdef _WIN32 - const std::string function_site = __FUNCSIG__; -#else - const std::string function_site = __PRETTY_FUNCTION__; -#endif - - throw std::runtime_error(istm.error_line(str, function_site).c_str()); - } -#else - std::istringstream is(str); - is >> val; - if (!(bool) is) { -#ifdef _WIN32 - const std::string function_site = __FUNCSIG__; -#else - const std::string function_site = __PRETTY_FUNCTION__; -#endif - - throw std::runtime_error(istm.error_line(str, function_site).c_str()); - } -#endif - - return istm; -} - -template<> -inline mini::csv::istringstream &operator>>(mini::csv::istringstream &istm, - std::string &val) { - val = istm.get_delimited_str(); - - return istm; -} - -template<> -inline mini::csv::istringstream &operator>>(mini::csv::istringstream &istm, - mini::csv::sep &val) { - istm.set_delimiter(val.get_delimiter(), val.get_escape()); - - return istm; -} - -template -mini::csv::ostringstream &operator<<(mini::csv::ostringstream &ostm, - const T &val) { - if (!ostm.get_after_newline()) { - ostm.get_ostringstream() << ostm.get_delimiter(); - } - - std::ostringstream os_temp; - - os_temp << val; - - ostm.escape_and_output(os_temp.str()); - - ostm.set_after_newline(false); - - return ostm; -} - -template -mini::csv::ostringstream &operator<<(mini::csv::ostringstream &ostm, - const T *val) { - if (!ostm.get_after_newline()) { - ostm.get_ostringstream() << ostm.get_delimiter(); - } - - std::ostringstream os_temp; - - os_temp << *val; - - ostm.escape_and_output(os_temp.str()); - - ostm.set_after_newline(false); - - return ostm; -} - -template<> -inline mini::csv::ostringstream &operator<<(mini::csv::ostringstream &ostm, - const std::string &val) { - if (!ostm.get_after_newline()) { - ostm.get_ostringstream() << ostm.get_delimiter(); - } - - std::string temp = val; - ostm.escape_str_and_output(temp); - - ostm.set_after_newline(false); - - return ostm; -} - -template<> -inline mini::csv::ostringstream &operator<<(mini::csv::ostringstream &ostm, - const mini::csv::sep &val) { - ostm.set_delimiter(val.get_delimiter(), val.get_escape()); - - return ostm; -} - -template<> -inline mini::csv::ostringstream &operator<<(mini::csv::ostringstream &ostm, - const char &val) { - if (val == NEWLINE) { - ostm.get_ostringstream() << NEWLINE; - - ostm.set_after_newline(true); - } else { - std::ostringstream os_temp; - - os_temp << val; - - ostm.escape_and_output(os_temp.str()); - } - - return ostm; -} - -template<> -inline mini::csv::ostringstream &operator<<(mini::csv::ostringstream &ostm, - const char *val) { - const std::string temp = val; - - ostm << temp; - - return ostm; -} - - -#endif // MiniCSV_H diff --git a/include/public/version.h b/include/public/version.h deleted file mode 100644 index c7a5c63..0000000 --- a/include/public/version.h +++ /dev/null @@ -1,72 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This is the copyright work of The MITRE Corporation, and was produced -// for the U. S. Government under Contract Number DTFAWA-10-C-00080, and -// is subject to Federal Aviation Administration Acquisition Management -// System Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV -// (Oct. 1996). No other use other than that granted to the U. S. -// Government, or to those acting on behalf of the U. S. Government, -// under that Clause is authorized without the express written -// permission of The MITRE Corporation. For further information, please -// contact The MITRE Corporation, Contracts Office, 7515 Colshire Drive, -// McLean, VA 22102-7539, (703) 983-6000. -// -// Copyright 2020 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#if !defined (AAESIM_VERSION_H) -#define AAESIM_VERSION_H - -#if defined (AAESIM_HAVE_PRAGMA_ONCE) -#pragma once -#endif - -#include -#include - -// AAESIM_VERSION_STR is now defined in build_info -#include "aaesim/build_info.h" - -// if suffix is empty, this will end with a hyphen. That will be removed at run-time -#define AAESIM_MAKE_VERSION_STR(major, minor, point, suffix) \ -#major "." #minor "." #point "-" #suffix - -//! This is AAESIM version number as a string. -//! Do not wrap the suffix in quotes, but it may be left empty for a release -//! Do not leave a space before the right parenthesis -// #define AAESIM_VERSION_STR AAESIM_MAKE_VERSION_STR(1, 4, 4, SNAPSHOT) - -namespace aaesim { - static std::string getVersion() { - std::string verStr(AAESIM_VERSION_STR); - bool stripLastChar = verStr.find("-") == verStr.length() - 1; - if (stripLastChar) { - // dump the last char - verStr.resize(verStr.find("-")); - } - return verStr; - } -} -#endif - - -#if !defined (CPPMANIFEST_VERSION_H) -#define CPPMANIFEST_VERSION_H - -#if defined (CPPMANIFEST_HAVE_PRAGMA_ONCE) -#pragma once -#endif - -#include -#include - -// if suffix is empty, this will end with a hyphen. That will be removed at run-time -#define CPPMANIFEST_MAKE_VERSION_STR(major, minor, point, suffix) \ -#major "." #minor "." #point "-" #suffix - -//! This is CPPMANIFEST version number as a string. -//! Do not wrap the suffix in quotes, but it may be left empty for a release -#define CPPMANIFEST_VERSION_STR CPPMANIFEST_MAKE_VERSION_STR(1, 0, 0, alpha) - -#endif diff --git a/include/utility/BoundedValue.h b/include/utility/BoundedValue.h deleted file mode 100644 index 67d56b9..0000000 --- a/include/utility/BoundedValue.h +++ /dev/null @@ -1,133 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -/* -Original idea in the public domain from: https://stackoverflow.com/a/13730310 -*/ - -#pragma once -#include -#include -#include -#include - -#define STRINGIZE(x) #x -#define STRINGIFY(x) STRINGIZE( x ) - -// handling for runtime value errors -#define BOUNDED_VALUE_ASSERT(MIN, MAX, VAL) \ - do { \ - if ((VAL) < (MIN) || (VAL) > (MAX)) { \ - bounded_value_assert_helper(MIN, MAX, VAL, "BOUNDED_VALUE_ASSERT at " __FILE__ ":" STRINGIFY(__LINE__)); \ - } \ - } while (0) - -template -struct BoundedValueException : public std::range_error { - virtual ~BoundedValueException() noexcept = default; - BoundedValueException() = delete; - BoundedValueException(BoundedValueException const &other) = default; - BoundedValueException(BoundedValueException &&source) = default; - - BoundedValueException(T min, T max, T val, std::string const &message) - : std::range_error(message), minval_(min), maxval_(max), val_(val) {} - - T const minval_; - T const maxval_; - T const val_; -}; - -template -void bounded_value_assert_helper(T min, T max, T val, char const *message = nullptr) { - std::ostringstream oss; - oss << "BoundedValueException: !(" << min << "<=" << val << "<=" << max << ")"; - if (message) { - oss << " - " << message; - } - throw BoundedValueException(min, max, val, oss.str()); -} - -template -class BoundedValue { - static_assert(std::is_arithmetic::value, "T must be arithmetic"); - - public: - typedef T value_type; - enum { min_value_int = Tmin, max_value_int = Tmax }; - static constexpr T min_value = static_cast(Tmin); - static constexpr T max_value = static_cast(Tmax); - typedef BoundedValue SelfType; - - // runtime checking constructor: - explicit BoundedValue(T runtime_value) : val_(runtime_value) { - BOUNDED_VALUE_ASSERT(min_value, max_value, runtime_value); - } - // compile-time checked constructors: - constexpr BoundedValue() : val_(min_value) {} - - template - BoundedValue(BoundedValue const &other) - : val_(static_cast(other)) // explicitly convert underlying value - { - static_assert(otherTmin >= Tmin, "conversion disallowed from BoundedValue with lower min"); - static_assert(otherTmax <= Tmax, "conversion disallowed from BoundedValue with higher max"); - } - - // compile-time checked assignments: - BoundedValue &operator=(SelfType const &other) { - val_ = other.val_; - return *this; - } - - template - BoundedValue &operator=(BoundedValue const &other) { - static_assert(otherTmin >= Tmin, "conversion disallowed from BoundedValue with lower min"); - static_assert(otherTmax <= Tmax, "conversion disallowed from BoundedValue with higher max"); - val_ = static_cast(other); // explicit conversion of underlying value - return *this; - } - // run-time checked assignment: - BoundedValue &operator=(T const &val) { - BOUNDED_VALUE_ASSERT(min_value, max_value, val); - val_ = val; - return *this; - } - - // C++20: conversion operator - operator T const &() const { return val_; } - - // Comparison operators (C++11 compatible) - bool operator==(BoundedValue const &other) const { return val_ == other.val_; } - bool operator!=(BoundedValue const &other) const { return val_ != other.val_; } - bool operator<(BoundedValue const &other) const { return val_ < other.val_; } - bool operator<=(BoundedValue const &other) const { return val_ <= other.val_; } - bool operator>(BoundedValue const &other) const { return val_ > other.val_; } - bool operator>=(BoundedValue const &other) const { return val_ >= other.val_; } - - // Comparison operators with raw values - bool operator==(T const &val) const { return val_ == val; } - bool operator<(T const &val) const { return val_ < val; } - bool operator<=(T const &val) const { return val_ <= val; } - bool operator>(T const &val) const { return val_ > val; } - bool operator>=(T const &val) const { return val_ >= val; } - bool operator!=(T const &val) const { return val_ != val; } - - private: - value_type val_; -}; diff --git a/include/utility/CsvParser.h b/include/utility/CsvParser.h deleted file mode 100644 index ae232e8..0000000 --- a/include/utility/CsvParser.h +++ /dev/null @@ -1,114 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -/* - * Originally from http://stackoverflow.com/questions/1120140/how-can-i-read-and-parse-csv-files-in-c - * - * Use in an interator loop, like this: - std::ifstream file("testfile.csv"); - - for(CSVIterator loop(file);loop != CSVIterator();++loop) - { - std::cout << "4th Element(" << (*loop)[3] << ")\n"; - } - * - */ - -#include -#include -#include -#include -#include -#include - -namespace CsvParser { - -class CsvRow { - public: - std::string const &operator[](std::size_t index) const { return m_data[index]; } - - std::size_t Size() const { return m_data.size(); } - - void ReadNextRow(std::istream &str) { - std::string line; - std::getline(str, line); - - std::stringstream lineStream(line); - std::string cell; - - m_data.clear(); - while (std::getline(lineStream, cell, ',')) { - m_data.push_back(cell); - } - } - - private: - std::vector m_data; -}; - -inline std::istream &operator>>(std::istream &str, CsvRow &data) { - data.ReadNextRow(str); - return str; -} - -class CsvIterator { - public: - typedef std::input_iterator_tag iterator_category; - typedef CsvRow value_type; - typedef std::size_t difference_type; - typedef CsvRow *pointer; - typedef CsvRow &reference; - - explicit CsvIterator(std::istream &str) : m_str(str.good() ? &str : NULL) { ++(*this); } - - CsvIterator() : m_str(NULL) {} - - // Pre Increment - CsvIterator &operator++() { - if (m_str) { - (*m_str) >> m_row; - m_str = m_str->good() ? m_str : NULL; - } - return *this; - } - - // Post increment - CsvIterator operator++(int) { - CsvIterator tmp(*this); - ++(*this); - return tmp; - } - - CsvRow const &operator*() const { return m_row; } - - CsvRow const *operator->() const { return &m_row; } - - bool operator==(CsvIterator const &rhs) { - return ((this == &rhs) || ((this->m_str == NULL) && (rhs.m_str == NULL))); - } - - bool operator!=(CsvIterator const &rhs) { return !((*this) == rhs); } - - private: - std::istream *m_str; - CsvRow m_row; -}; -} // namespace CsvParser diff --git a/include/utility/CustomUnits.h b/include/utility/CustomUnits.h deleted file mode 100644 index 3968a37..0000000 --- a/include/utility/CustomUnits.h +++ /dev/null @@ -1,164 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "utility/UtilityConstants.h" - -/* - * Definitions for unit types which are not already defined in unitsLib. - */ -namespace Units { - -/* - * New mathematical operations not provided by units. - */ -// square operation ----------------------- - -#define UNITS_UNIT_TEMPLATE_ARGS_SQR_RESULT1 \ - ValueType1, 2 * massExp1, 2 * lengthExp1, 2 * timeExp1, 2 * currentExp1, 2 * temperatureExp1, 2 * amountExp1, \ - 2 * intensityExp1, AngleExponent<2 * angleExp1, 2 * lengthExp1>::value - -template -inline Unit sqr(Unit const &value) { - return (value * value); -} -// end square operation ----------------------- - -// end new operations------------------------------------------------------------------------------------------------ - -/* - * New unit of measure concepts needed by aaesim. - */ -// Acceleration -UNITS_DECLARE_SPECIFIC_UNIT(Acceleration, KnotsPerSecondAcceleration, "kts/s", 1852.0 / 3600.0); - -// change rate of force -UNITS_DECLARE_BASE_UNIT(ForceChange, 1, 1, -3, 0, 0, 0, 0, 0); -UNITS_DECLARE_SPECIFIC_UNIT(ForceChange, NewtonsPerSecondForceChange, "N/s", 1.0); - -// inverted length, for per-meter -UNITS_DECLARE_BASE_UNIT(InvertedLength, 0, -1, 0, 0, 0, 0, 0, 0); -UNITS_DECLARE_SPECIFIC_UNIT(InvertedLength, PerMeterInvertedLength, "1/m", 1.0); - -UNITS_DECLARE_BASE_UNIT(LengthGain, 0, 1, -2, 0, 0, 0, 0, 0); -UNITS_DECLARE_SPECIFIC_UNIT(LengthGain, MetersPerSecondSquaredLengthGain, "m/s^2", 1.0); - -UNITS_DECLARE_BASE_UNIT(InvertedLengthGain, 0, -1, 2, 0, 0, 0, 0, 0); -UNITS_DECLARE_SPECIFIC_UNIT(InvertedLengthGain, SecondsSquaredPerMeterInvertedLengthGain, "s^2/m", 1.0); - -// Mass -UNITS_DECLARE_SPECIFIC_UNIT(Mass, TonnesMass, "t", 1000.0); - -// Mass Flow Rate -UNITS_DECLARE_SPECIFIC_UNIT(MassFlowRate, KilogramsPerMinuteMassFlowRate, "kg/min", (1.0 / 60.0)); -UNITS_DECLARE_SPECIFIC_UNIT(MassFlowRate, KilogramsPerSecondMassFlowRate, "kg/min", 1.0); - -// Length to Mass Gradient -UNITS_DECLARE_BASE_UNIT(LengthToMassGradient, -1, 1, 0, 0, 0, 0, 0, 0); -UNITS_DECLARE_SPECIFIC_UNIT(LengthToMassGradient, FeetToKilogramsLengthToMassGradient, "ft/kg", (0.3048 / 1.0)); -UNITS_DECLARE_SPECIFIC_UNIT(LengthToMassGradient, MetersToKilogramsLengthToMassGradient, "m/kg", (1.0 / 1.0)); - -// inverted speed -// -// 1 m/s = 3.280839895 ft/s = 2.236936292 mi/hr -// 1 knot = 1 nmi/hr = 1852 m/h -UNITS_DECLARE_BASE_UNIT(InvertedSpeed, 0, -1, 1, 0, 0, 0, 0, 0); -UNITS_DECLARE_SPECIFIC_UNIT(InvertedSpeed, SecondsPerNauticalMileInvertedSpeed, "s/nmi", (1.0 / 1852.0)); -UNITS_DECLARE_SPECIFIC_UNIT(InvertedSpeed, KnotsInvertedSpeed, "1/kts", (3600.0 / 1852.0)); -UNITS_DECLARE_SPECIFIC_UNIT(InvertedSpeed, SecondsPerMeterInvertedSpeed, "s/m", 1.0); - -// Temperature gradient -UNITS_DECLARE_BASE_UNIT(TemperatureGradient, 0, -1, 0, 0, 1, 0, 0, 0); -UNITS_DECLARE_SPECIFIC_UNIT(TemperatureGradient, KelvinPerMeter, "K/m", 1.0); - -// for gas constant -- m^2/K-s^2 -UNITS_DECLARE_BASE_UNIT(SpeedSquaredOverTemperature, 0, 2, -2, 0, -1, 0, 0, 0); -UNITS_DECLARE_SPECIFIC_UNIT(SpeedSquaredOverTemperature, MetersSecondsKelvinGasConstant, "m^2/s^2/K", 1.0); - -// Speed -UNITS_DECLARE_SPECIFIC_UNIT(Speed, FeetPerMinuteSpeed, "fpm", 0.3048 / 60.0); - -// Speed / altitude gradient (same units as Frequency) -UNITS_DECLARE_SPECIFIC_UNIT(Frequency, KnotsPerFootFrequency, "kts/ft", 1852. / (3600. * .3048)); - -// Inverted Acceleration -UNITS_DECLARE_BASE_UNIT(InvertedAcceleration, 0, -1, 2, 0, 0, 0, 0, 0); -UNITS_DECLARE_SPECIFIC_UNIT(InvertedAcceleration, SecondsSquaredPerMeter, "s^2/m", 1.0); -UNITS_DECLARE_SPECIFIC_UNIT(InvertedAcceleration, SecondsPerKnot, "s/kts", 1852. / 3600.); -// end new units of measure------------------------------------------------------------------------------------------ - -/* - * Convenience Constants - */ -// Time -const Units::Time DUMMY_SECONDS_TIME = Units::SecondsTime(-999.0); - -// Convenience Zeros -const Units::Angle ZERO_ANGLE = RadiansAngle(0.0); -const Units::Frequency ZERO_FREQUENCY = Units::HertzFrequency(0.0); -const Units::Length ZERO_LENGTH = Units::MetersLength(0.0); -const Units::Speed ZERO_SPEED = Units::MetersPerSecondSpeed(0.0); -const Units::Time ZERO_TIME = Units::SecondsTime(0.0); -const Units::Force ZERO_FORCE = Units::NewtonsForce(0.0); -const Units::Mass ZERO_MASS = Units::KilogramsMass(0.0); -const Units::AbsCelsiusTemperature ZERO_CELSIUS = Units::AbsCelsiusTemperature(0.0); - -const InvertedLengthGain ZERO_INVERTED_LENGTH_GAIN = Units::SecondsSquaredPerMeterInvertedLengthGain(0.0); -const InvertedSpeed ZERO_INVERTED_SPEED = Units::SecondsPerMeterInvertedSpeed(0.0); - -const Units::Acceleration ONE_G_ACCELERATION = - MetersSecondAcceleration(aaesim::open_source::constants::GRAVITY_METERS_PER_SECOND); - -// angle constants -const Units::Angle PI_RADIANS_ANGLE = DegreesAngle(180.0); -const Units::Angle HALF_PI_RADIANS_ANGLE = PI_RADIANS_ANGLE / 2; -const Units::Angle ONE_RADIAN_ANGLE = RadiansAngle(1.0); -const Units::Angle DUMMY_DEGREES_ANGLE = DegreesAngle(-999.0); -// end constants----------------------------------------------------------------------------------------------------- - -/* -Convenience Methods -*/ - -inline static Units::UnsignedRadiansAngle ToUnsigned(const Units::SignedAngle &signed_angle) { - Units::UnsignedRadiansAngle result{signed_angle}; - result.normalize(); - return result; -} - -inline static Units::SignedRadiansAngle ToSigned(Units::UnsignedAngle unsigned_angle) { - Units::SignedRadiansAngle result{unsigned_angle}; - result.normalize(); - return result; -} - -} // namespace Units diff --git a/include/utility/FilePath.h b/include/utility/FilePath.h deleted file mode 100644 index 047518a..0000000 --- a/include/utility/FilePath.h +++ /dev/null @@ -1,79 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// 2022 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include -#include - -class FilePath { - public: - FilePath(); - - virtual ~FilePath(); - - FilePath(const std::string &name); - - std::string GetFullPath() const; - - std::string GetType() const; - - std::string GetDisk() const; - - std::string RemoveLastDirectory() const; - - int GetNumberOfDirectories() const; - - std::vector ListDirectories() const; - - std::string GetName() const; - - FilePath Pop() const; - - FilePath Push(const FilePath &more) const; - - FilePath Cd(const FilePath &fp) const; - - bool operator==(const FilePath &rhs) const; - - private: - int ExtractDisk(const std::string &name); - - int ExtractType(const std::string &name); - - void ExtractPath(const std::string &name, int &index, int end); - - void ExtractPop(const std::string &name); - - int GetNumberOfWildCards(const std::string &name); - - std::vector m_list_of_directories; - - std::string m_full_path; - std::string m_drive; - std::string m_type; - std::string m_last_directory; - std::string m_file_name; - - int m_number_of_directories; - int m_number_of_wildcards; - bool m_error; -}; - -inline std::string FilePath::GetName() const { return m_file_name; } diff --git a/include/utility/Logging.h b/include/utility/Logging.h deleted file mode 100644 index 1489f9b..0000000 --- a/include/utility/Logging.h +++ /dev/null @@ -1,27 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// 2022 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#define LOG4CPLUS_STATIC // force static usage of log4cplus - -#include -#include - -void InitializeLogging(); diff --git a/include/utility/ProcessingTimeStats.h b/include/utility/ProcessingTimeStats.h deleted file mode 100644 index e7082d7..0000000 --- a/include/utility/ProcessingTimeStats.h +++ /dev/null @@ -1,94 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This is the copyright work of The MITRE Corporation, and was produced -// for the U. S. Government under Contract Number DTFAWA-10-C-00080, and -// is subject to Federal Aviation Administration Acquisition Management -// System Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV -// (Oct. 1996). No other use other than that granted to the U. S. -// Government, or to those acting on behalf of the U. S. Government, -// under that Clause is authorized without the express written -// permission of The MITRE Corporation. For further information, please -// contact The MITRE Corporation, Contracts Office, 7515 Colshire Drive, -// McLean, VA 22102-7539, (703) 983-6000. -// -// Copyright 2018 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -/* - * ProcessingTimeStats - * - * Created on: 18 May 16 - * Author: greanias - */ - - -#pragma once - -#include -#include - - -// Class to gather and dump processor time stats over poritions -// of code. -// -// Sample use of code: -// -// ProcessStuff::processLoopStats defined in header file. -// -// ProcessStuff::ProcessStuff(void) { -// ... -// processingLoopStats.setHdr("Stats for processing loop"); -// ... -// -// ProcessStuff::execute(void) { -// -// ... -// processingLoopStats.start(); -// -// for (auto ix=0;ix -// } -// -// processingLoopStats.stop(); -//... -// -// Processor time data will be collected for the loop. -// When the procssingLoopStats destructor is invoked, -// all the collected stats will be dumped. - - -class ProcessingTimeStats -{ -public: - - ProcessingTimeStats(void); - - ProcessingTimeStats(std::string str); - - ~ProcessingTimeStats(void); - - // Sets header for dump. - void setHdr(std::string str); - - // Gets time at start point in code. - void start(void); - - // Collects processing time in ms between start point and this point. - void stop(void); - -private: - // Gathers stats between start and stop point. - void gather(void); - - // Dumps stats. - void dump(void); - - clock_t t0; - clock_t t1; - - double ms; - int entries; - - std::string hdr; -}; diff --git a/include/utility/UtilityConstants.h b/include/utility/UtilityConstants.h deleted file mode 100644 index e06fa39..0000000 --- a/include/utility/UtilityConstants.h +++ /dev/null @@ -1,39 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -namespace aaesim { -namespace open_source { -namespace constants { -inline constexpr double PI = 3.14159265358979323846; -inline constexpr double DEGREES_PER_RADIAN = 180 / PI; -inline constexpr double TWO_PI = 2 * PI; -inline constexpr double RADIAN_TO_DEGREES = DEGREES_PER_RADIAN; -inline constexpr double DEGREES_TO_RADIAN = 1.0 / RADIAN_TO_DEGREES; -inline constexpr double NAUTICAL_MILES_TO_METERS = 1852.0; -inline constexpr double FEET_TO_METERS = 0.3048; -inline constexpr double KNOTS_TO_METERS_PER_SECOND = (NAUTICAL_MILES_TO_METERS / 3600.0); -inline constexpr double NAUTICAL_MILES_TO_FEET = 6076.115486; -inline constexpr double KNOTS_TO_FEET_PER_SECOND = 1.687809857; -inline constexpr double GRAVITY_METERS_PER_SECOND = 9.80665; -inline constexpr double GRAVITY_FEET_PER_SECOND = GRAVITY_METERS_PER_SECOND / FEET_TO_METERS; -} // namespace constants -} // namespace open_source -} // namespace aaesim diff --git a/include/utility/UtilityTemplates.h b/include/utility/UtilityTemplates.h deleted file mode 100644 index fa41b1e..0000000 --- a/include/utility/UtilityTemplates.h +++ /dev/null @@ -1,25 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -template -int sgn(T t) { - return t < 0 ? -1 : t > 0; -} diff --git a/include/utility/constants.h b/include/utility/constants.h deleted file mode 100644 index d8e5b9a..0000000 --- a/include/utility/constants.h +++ /dev/null @@ -1,71 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// 2022 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once -namespace aaesim { -namespace constants { -const double PI = 3.14159265358979323846; - -const double DEGREES_PER_RADIAN = 180 / PI; -const double TWO_PI = 2 * PI; -const double RADIAN_TO_DEGREES = DEGREES_PER_RADIAN; -const double DEGREES_TO_RADIAN = 1.0 / RADIAN_TO_DEGREES; - -// exact conversion constants -const double NAUTICAL_MILES_TO_METERS = 1852.0; -const double FEET_TO_METERS = 0.3048; -const double KNOTS_TO_METERS_PER_SECOND = (NAUTICAL_MILES_TO_METERS / 3600.0); - -const double NAUTICAL_MILES_TO_FEET = 6076.115486; -const double KNOTS_TO_FEET_PER_SECOND = 1.687809857; -const double BIGNUM = 9.9E99; - -const double BARO_ALT_SIG = 6.0; -const double BARO_ALT_RATE_SIG = 2.5; -const int BARO_ALT_ERR_STEPS = 121; - -const extern double BARO_ALT_ERR_GRAD[BARO_ALT_ERR_STEPS]; -const extern double BARO_ALT_ERR_ALTITUDES[BARO_ALT_ERR_STEPS]; -const extern double BARO_ALT_ERROR[BARO_ALT_ERR_STEPS]; - -const double BARO_GRADIENT_ERR = 0; -const double SA_NORTH_STD_DEV = 128.0; -const double SA_EAST_STD_DEV = 105.0; -const double SA_DOWN_STD_DEV = 220.0; - -const double OMG0 = 0.012; -const double QC = 0.002585; -const double BETA = 0.707106781; - -const int TRACKING = 1; -const int TURNING = 2; - -// Gravitational acceleration is defined by -// International Committee on Weights and Measures (1901) -// and International Bureau of Weights and Measures (current) -// to be 9.80665 m/s^2 -const double GRAVITY_METERS_PER_SECOND = 9.80665; -const double GRAVITY_FEET_PER_SECOND = GRAVITY_METERS_PER_SECOND / FEET_TO_METERS; - -/* The following constants are used for converting from geodetic to */ -/* conformal latitude. They are found in NAS-MD-312 Appendix D. */ -const double GEOD_CONST_A = 0.9932773; -const double GEOD_CONST_B = 0.0066625; -} // namespace constants -} // namespace aaesim diff --git a/include/utility/dev-notes.md b/include/utility/dev-notes.md deleted file mode 100644 index a090815..0000000 --- a/include/utility/dev-notes.md +++ /dev/null @@ -1,3 +0,0 @@ -# Utility Code - -Developers: This `utility` folder is intended to contain header-only code. There is no compiled library (`libutility.a/so`) so there cannot be any `cpp` source files. Do not put any header files in here that are intended to also have source code written against them. \ No newline at end of file diff --git a/include/utility/micros.h b/include/utility/micros.h deleted file mode 100644 index ecd67b1..0000000 --- a/include/utility/micros.h +++ /dev/null @@ -1,48 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This is the copyright work of The MITRE Corporation, and was produced -// for the U. S. Government under Contract Number DTFAWA-10-C-00080, and -// is subject to Federal Aviation Administration Acquisition Management -// System Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV -// (Oct. 1996). No other use other than that granted to the U. S. -// Government, or to those acting on behalf of the U. S. Government, -// under that Clause is authorized without the express written -// permission of The MITRE Corporation. For further information, please -// contact The MITRE Corporation, Contracts Office, 7515 Colshire Drive, -// McLean, VA 22102-7539, (703) 983-6000. -// -// Copyright 2017 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - - -#ifndef SQR -#define SQR(A) ((A)*(A)) -#endif - -#ifndef MAX -#define MAX(A, B) (((A) > (B)) ? (A) : (B)) -#endif - -#ifndef MIN -#define MIN(A, B) (((A) < (B)) ? (A) : (B)) -#endif - - -#ifndef ABS -#define ABS(A) (((A) > (0)) ? (A) : (-(A))) -#endif - -#ifndef NORM -#define NORM(DX, DY) sqrt(SQR(DX) + SQR(DY)) -#endif - -#ifndef LIMIT -#define LIMIT(x,xmin,xmax) ( x < xmin ? xmin : (x > xmax ? xmax : x) ) -#endif - -#ifndef SIGN -#define SIGN(A) (((A) == (0)) ? 0: (((A) > (0)) ? (1) : (-1)) ) -#endif diff --git a/unittest/src/Public/earth_model_tests.cpp b/unittest/src/Public/earth_model_tests.cpp deleted file mode 100644 index c4e8b96..0000000 --- a/unittest/src/Public/earth_model_tests.cpp +++ /dev/null @@ -1,127 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include - -#include - -#include "public/EarthModel.h" -#include "public/EllipsoidalEarthModel.h" -#include "public/LocalTangentPlane.h" -#include "public/TangentPlaneSequence.h" - -const auto ENU_POSITION_TEST_TOLERANCE = Units::KilometersLength{1e-8}; -const auto ECEF_POSITION_TEST_TOLERANCE = Units::KilometersLength{1e-8}; -const auto DEGREE_POSITION_TEST_TOLERANCE = Units::DegreesAngle{1e-8}; - -const auto lat_reference = Units::DegreesAngle(46.017); -const auto lon_reference = Units::DegreesAngle(7.750); -const auto tangency_position_as_waypoint = Waypoint("test", lat_reference, lon_reference); -const auto lat_test_point = Units::DegreesAngle(45.976); -const auto lon_test_point = Units::DegreesAngle(7.658); -const auto test_point = EarthModel::GeodeticPosition::Of(lat_test_point, lon_test_point); - -// ----------------------------------------------------------------------------------------- -// These constants below are from running open source code GeographicLib -// repo: https://geographiclib.sourceforge.io/C++/doc/index.html -// test code: https://mustache.mitre.org/users/sbowman/repos/geographiclib-testing/browse -const auto ecef_x_from_geographiclib = Units::MetersLength{4400636.82668814}; -const auto ecef_y_from_geographiclib = Units::MetersLength{591704.962660163}; -const auto ecef_z_from_geographiclib = Units::MetersLength{4563394.05024529}; -const auto enu_x_from_geographiclib = Units::MetersLength{-7129.70106116624}; -const auto enu_y_from_geographiclib = Units::MetersLength{-4553.08211938867}; -const auto enu_z_from_geographiclib = Units::MetersLength{-5.60559700015367}; -// ----------------------------------------------------------------------------------------- - -TEST(LocalTangentPlane, lla2ecef_forward_reverse) { - const auto earth_model = std::make_unique(); - EarthModel::AbsolutePositionEcef resolved_ecef; - earth_model->ConvertGeodeticToAbsolute(test_point, resolved_ecef); - - ASSERT_NEAR(Units::MetersLength(resolved_ecef.x).value(), ecef_x_from_geographiclib.value(), - Units::MetersLength(ECEF_POSITION_TEST_TOLERANCE).value()); - ASSERT_NEAR(Units::MetersLength(resolved_ecef.y).value(), ecef_y_from_geographiclib.value(), - Units::MetersLength(ECEF_POSITION_TEST_TOLERANCE).value()); - ASSERT_NEAR(Units::MetersLength(resolved_ecef.z).value(), ecef_z_from_geographiclib.value(), - Units::MetersLength(ECEF_POSITION_TEST_TOLERANCE).value()); - - EarthModel::AbsolutePositionEcef ecef_test_point; - ecef_test_point.x = ecef_x_from_geographiclib; - ecef_test_point.y = ecef_y_from_geographiclib; - ecef_test_point.z = ecef_z_from_geographiclib; - EarthModel::GeodeticPosition resolved_position; - earth_model->ConvertAbsoluteToGeodetic(ecef_test_point, resolved_position); - ASSERT_NEAR(Units::DegreesAngle(resolved_position.latitude).value(), lat_test_point.value(), - DEGREE_POSITION_TEST_TOLERANCE.value()); - ASSERT_NEAR(Units::DegreesAngle(resolved_position.longitude).value(), lon_test_point.value(), - DEGREE_POSITION_TEST_TOLERANCE.value()); - ASSERT_DOUBLE_EQ(Units::MetersLength(resolved_position.altitude).value(), 0); -} - -TEST(LocalTangentPlane, ecef2enu_forward_backward) { - EarthModel::AbsolutePositionEcef ecef_position, updated_ecef_position; - ecef_position.x = ecef_x_from_geographiclib; - ecef_position.y = ecef_y_from_geographiclib; - ecef_position.z = ecef_z_from_geographiclib; - auto waypoint_list = std::list{tangency_position_as_waypoint}; - const auto earth_model = std::make_unique(); - const auto converter = std::make_unique(waypoint_list); - EarthModel::LocalPositionEnu enu_position; - - converter->GetTangentPlanesFromInitialization().front()->ConvertAbsoluteToLocal(ecef_position, enu_position); - ASSERT_NEAR(Units::MetersLength(enu_position.x).value(), enu_x_from_geographiclib.value(), - ENU_POSITION_TEST_TOLERANCE.value()); - ASSERT_NEAR(Units::MetersLength(enu_position.y).value(), enu_y_from_geographiclib.value(), - ENU_POSITION_TEST_TOLERANCE.value()); - ASSERT_NEAR(Units::MetersLength(enu_position.z).value(), enu_z_from_geographiclib.value(), - ENU_POSITION_TEST_TOLERANCE.value()); - - converter->GetTangentPlanesFromInitialization().front()->ConvertLocalToAbsolute(enu_position, updated_ecef_position); - ASSERT_NEAR(Units::MetersLength(updated_ecef_position.x).value(), ecef_x_from_geographiclib.value(), - ECEF_POSITION_TEST_TOLERANCE.value()); - ASSERT_NEAR(Units::MetersLength(updated_ecef_position.y).value(), ecef_y_from_geographiclib.value(), - ECEF_POSITION_TEST_TOLERANCE.value()); - ASSERT_NEAR(Units::MetersLength(updated_ecef_position.z).value(), ecef_z_from_geographiclib.value(), - ECEF_POSITION_TEST_TOLERANCE.value()); -} - -TEST(LocalTangentPlane, lla2enu_forward_reverse) { - auto waypoint_list = std::list{tangency_position_as_waypoint}; - const auto earth_model = std::make_unique(); - const auto converter = std::make_unique(waypoint_list); - EarthModel::LocalPositionEnu resolved_enu; - - converter->GetTangentPlanesFromInitialization().front()->ConvertGeodeticToLocal(test_point, resolved_enu); - ASSERT_NEAR(Units::MetersLength(resolved_enu.x).value(), enu_x_from_geographiclib.value(), - Units::MetersLength(ENU_POSITION_TEST_TOLERANCE).value()); - ASSERT_NEAR(Units::MetersLength(resolved_enu.y).value(), enu_y_from_geographiclib.value(), - Units::MetersLength(ENU_POSITION_TEST_TOLERANCE).value()); - ASSERT_NEAR(Units::MetersLength(resolved_enu.z).value(), enu_z_from_geographiclib.value(), - Units::MetersLength(ENU_POSITION_TEST_TOLERANCE).value()); - - EarthModel::GeodeticPosition resolved_geodetic_pos; - converter->GetTangentPlanesFromInitialization().front()->ConvertLocalToGeodetic( - EarthModel::LocalPositionEnu::Of(enu_x_from_geographiclib, enu_y_from_geographiclib, enu_z_from_geographiclib), - resolved_geodetic_pos); - ASSERT_NEAR(Units::DegreesAngle(resolved_geodetic_pos.latitude).value(), lat_test_point.value(), - DEGREE_POSITION_TEST_TOLERANCE.value()); - ASSERT_NEAR(Units::DegreesAngle(resolved_geodetic_pos.longitude).value(), lon_test_point.value(), - DEGREE_POSITION_TEST_TOLERANCE.value()); - ASSERT_DOUBLE_EQ(Units::MetersLength(resolved_geodetic_pos.altitude).value(), 0); -} diff --git a/unittest/src/Public/geolib_tests.cpp b/unittest/src/Public/geolib_tests.cpp deleted file mode 100644 index 4951236..0000000 --- a/unittest/src/Public/geolib_tests.cpp +++ /dev/null @@ -1,1690 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include - -#include -#include -#include -#include - -#include "MiniCSV/minicsv.h" -#include "geolib/Geolib.h" -#include "public/ArcOnEllipsoid.h" -#include "public/EarthModel.h" -#include "public/GeolibUtils.h" -#include "public/LatitudeLongitudePoint.h" -#include "public/LineOnEllipsoid.h" - -using namespace geolib_idealab; -using namespace aaesim; - -namespace aaesim { -namespace test { -namespace open_source { - -TEST(GeolibLibrary, basic_internal_consistency_test) { - /* - * The geolib library has its own extensive testing system (thousands of tests). This test does not - * attempt to recreate that infrastructure. The point here is to ensure a basic operation works. Anything - * more extensive should be pushed back onto the geolib testing infrastructure, not implemented here. - */ - LLPoint origin; - origin.latitude = 0.0; - origin.longitude = 0.0; - - LLPoint destination; - destination.latitude = 1.0 * M_PI / 180.0; - destination.longitude = 1.0 * M_PI / 180.0; - - // Call inverse operation - double epsilon = DEFAULT_EPS; - double crs_radians_calculated = DBL_MIN; - double reverse_course_radians_calculated = DBL_MIN; - double distance_nm_calculated = DBL_MIN; - ErrorSet error_set_inverse = inverse(origin, destination, &crs_radians_calculated, - &reverse_course_radians_calculated, &distance_nm_calculated, epsilon); - if (error_set_inverse != SUCCESS) { - std::cout << "BLAH. Something went wrong with the inverse operation! " << formatErrorMessage(error_set_inverse) - << std::endl; - FAIL(); - } - - // Now call direct() - LLPoint destination_calculated; - ErrorSet error_set_direct = - direct(origin, crs_radians_calculated, distance_nm_calculated, &destination_calculated, epsilon); - if (error_set_direct != SUCCESS) { - std::cout << "BLAH. Something went wrong with the direct operation! " << formatErrorMessage(error_set_direct) - << std::endl; - FAIL(); - } - - // Assert - EXPECT_NEAR(destination.latitude, destination_calculated.latitude, 1e-15); - EXPECT_NEAR(destination.longitude, destination_calculated.longitude, 1e-15); -} - -TEST(LatitudeLongitudePoint, createObject) { - LLPoint test_point; - test_point.latitude = 10 * M_PI / 180; - test_point.longitude = 5 * M_PI / 180; - LatitudeLongitudePoint new_point(Units::SignedRadiansAngle(test_point.latitude), - Units::SignedRadiansAngle(test_point.longitude)); - - EXPECT_DOUBLE_EQ(test_point.latitude, Units::SignedRadiansAngle(new_point.GetLatitude()).value()); - EXPECT_DOUBLE_EQ(test_point.longitude, Units::SignedRadiansAngle(new_point.GetLongitude()).value()); -} - -TEST(LineOnEllipsoid, calculate_point_on_shape) { - LatitudeLongitudePoint start_point(Units::SignedDegreesAngle(4), Units::SignedRadiansAngle(-10)); - LatitudeLongitudePoint end_point(Units::SignedDegreesAngle(3), Units::SignedRadiansAngle(-103)); - - const LineOnEllipsoid geodesic = LineOnEllipsoid::CreateFromPoints(start_point, end_point); - - // This is the tested method ---- - const LatitudeLongitudePoint calculated_point = - geodesic.CalculatePointAtDistanceFromStartPoint(geodesic.GetShapeLength()); - EXPECT_NEAR(Units::RadiansAngle(end_point.GetLatitude()).value(), - Units::RadiansAngle(calculated_point.GetLatitude()).value(), 1e-8); - EXPECT_NEAR(Units::RadiansAngle(end_point.GetLongitude()).value(), - Units::RadiansAngle(calculated_point.GetLongitude()).value(), 1e-8); - // ------------------------------ -} - -TEST(LineOnEllipsoid, calculate_course_at_point_on_shape) { - LatitudeLongitudePoint start_point(Units::SignedDegreesAngle(4), Units::SignedRadiansAngle(-10)); - LatitudeLongitudePoint end_point(Units::SignedDegreesAngle(3), Units::SignedRadiansAngle(-103)); - - const LineOnEllipsoid geodesic = LineOnEllipsoid::CreateFromPoints(start_point, end_point); - - // This is the tested method ---- - const std::pair calculated_course_and_point = - geodesic.CalculateCourseAtDistanceFromStartPoint(geodesic.GetShapeLength()); - const Units::SignedRadiansAngle actual_course = calculated_course_and_point.first; - const LatitudeLongitudePoint actual_point = calculated_course_and_point.second; - EXPECT_NEAR(Units::SignedRadiansAngle(geodesic.GetForwardCourseEnuAtEndPoint()).value(), actual_course.value(), - 1e-8); - EXPECT_NEAR(Units::RadiansAngle(end_point.GetLatitude()).value(), - Units::RadiansAngle(actual_point.GetLatitude()).value(), 1e-8); - EXPECT_NEAR(Units::RadiansAngle(end_point.GetLongitude()).value(), - Units::RadiansAngle(actual_point.GetLongitude()).value(), 1e-8); - // ------------------------------ -} - -TEST(LineOnEllipsoid, createLineFromTwoPoints) { - LatitudeLongitudePoint start_point(Units::SignedDegreesAngle(4), Units::SignedRadiansAngle(-10)); - LatitudeLongitudePoint end_point(Units::SignedDegreesAngle(3), Units::SignedRadiansAngle(-103)); - - // This is the tested method ---- - LineOnEllipsoid geodesic = LineOnEllipsoid::CreateFromPoints(start_point, end_point); - // ------------------------------ - - // Call directly into the geolib library - const LLPoint origin = start_point.GetGeolibPrimitiveLLPoint(); - const LLPoint destination = end_point.GetGeolibPrimitiveLLPoint(); - double course_ned_radians_at_origin_calculated = DBL_MIN; - double reverse_course_radians_at_dest_calculated = DBL_MIN; - double distance_nm_calculated = DBL_MIN; - ErrorSet error_set_inverse = - inverse(origin, destination, &course_ned_radians_at_origin_calculated, - &reverse_course_radians_at_dest_calculated, &distance_nm_calculated, DEFAULT_EPS); - if (error_set_inverse != ErrorCodes::SUCCESS) { - FAIL(); - } - - EXPECT_NEAR(distance_nm_calculated, Units::NauticalMilesLength(geodesic.GetShapeLength()).value(), 1e-12); - EXPECT_NEAR( - course_ned_radians_at_origin_calculated, - Units::UnsignedRadiansAngle(GeolibUtils::ConvertCourseFromEnuToNed(geodesic.GetForwardCourseEnuAtStartPoint())) - .value(), - 1e-12); - const double expected_forward_course_ned = - modpos(reverse_course_radians_at_dest_calculated + M_PI, - M_2PI); // geolib inverse gives the reverse the course, so we need to do adjust to forward here - EXPECT_NEAR( - expected_forward_course_ned, - Units::UnsignedRadiansAngle(GeolibUtils::ConvertCourseFromEnuToNed(geodesic.GetForwardCourseEnuAtEndPoint())) - .value(), - 1e-12); -} - -TEST(GeolibUtils, createArcFromThreeKnownPoints) { - // Calculate an arc - const Units::NauticalMilesLength expected_radius(2.5); - const LatitudeLongitudePoint center_point(Units::SignedDegreesAngle(33.3862), Units::SignedRadiansAngle(-111.887)); - LLPoint start_point_calculated; - ErrorSet error_set_1 = direct(center_point.GetGeolibPrimitiveLLPoint(), 0, expected_radius.value(), - &start_point_calculated, DEFAULT_EPS); - if (error_set_1 != ErrorCodes::SUCCESS) { - FAIL(); - } - LatitudeLongitudePoint start_point = LatitudeLongitudePoint::CreateFromGeolibPrimitive(start_point_calculated); - - LLPoint end_point_calculated; - ErrorSet error_set_2 = direct(center_point.GetGeolibPrimitiveLLPoint(), M_PI / 2, expected_radius.value(), - &end_point_calculated, DEFAULT_EPS); - if (error_set_2 != ErrorCodes::SUCCESS) { - FAIL(); - } - LatitudeLongitudePoint end_point = LatitudeLongitudePoint::CreateFromGeolibPrimitive(end_point_calculated); - - // This is the tested method ---- - ArcOnEllipsoid arc_on_ellipsoid = - GeolibUtils::CreateArcOnEllipsoid(start_point, end_point, center_point, ArcDirection::CLOCKWISE); - // ------------------------------ - - EXPECT_NEAR(M_PI / 2, Units::SignedRadiansAngle(arc_on_ellipsoid.GetArcAngularExtent()).value(), 1e-5); - EXPECT_NEAR(expected_radius.value(), Units::NauticalMilesLength(arc_on_ellipsoid.GetRadius()).value(), 1e-5); -} - -TEST(LatitudeLongitudePoint, calculate_new_point_1nm) { - const LatitudeLongitudePoint start_point(Units::DegreesAngle(0.0), Units::DegreesAngle(0)); - const Units::NauticalMilesLength distance(1.0); - const Units::SignedDegreesAngle go_north_enu(0); - - // This is the tested method ---- - LatitudeLongitudePoint calculated_point = start_point.ProjectDistanceAlongCourse(distance, go_north_enu); - // ------------------------------ - - // Now get comparison data for "truth" measurement by calling the inverse operation - double crs_radians_ned_truth = DBL_MIN; - double tmp_course_radians_calculated = DBL_MIN; - double distance_nm_truth = DBL_MIN; - ErrorSet error_set_inverse = - inverse(start_point.GetGeolibPrimitiveLLPoint(), calculated_point.GetGeolibPrimitiveLLPoint(), - &crs_radians_ned_truth, &tmp_course_radians_calculated, &distance_nm_truth, GEOLIB_EPSILON); - if (error_set_inverse != SUCCESS) { - std::cout << "BLAH. Something went wrong with the inverse operation! " << formatErrorMessage(error_set_inverse) - << std::endl; - FAIL(); - } - - EXPECT_NEAR(distance_nm_truth, distance.value(), 1e-12); - EXPECT_NEAR(crs_radians_ned_truth, - Units::UnsignedRadiansAngle(GeolibUtils::ConvertCourseFromEnuToNed(go_north_enu)).value(), 1e-12); -} - -TEST(LatitudeLongitudePoint, calculate_new_point_1meter) { - const LatitudeLongitudePoint start_point(Units::DegreesAngle(0.0), Units::DegreesAngle(0)); - const Units::MetersLength distance(1.0); - const Units::SignedDegreesAngle go_east_enu(90); - - // This is the tested method ---- - LatitudeLongitudePoint calculated_point = start_point.ProjectDistanceAlongCourse(distance, go_east_enu); - // ------------------------------ - - // Now get comparison data for "truth" measurement by calling the inverse operation - double epsilon = DEFAULT_EPS; - double crs_radians_ned_truth = DBL_MIN; - double tmp_course_radians_calculated = DBL_MIN; - double distance_nm_truth = DBL_MIN; - ErrorSet error_set_inverse = - inverse(start_point.GetGeolibPrimitiveLLPoint(), calculated_point.GetGeolibPrimitiveLLPoint(), - &crs_radians_ned_truth, &tmp_course_radians_calculated, &distance_nm_truth, epsilon); - if (error_set_inverse != SUCCESS) { - std::cout << "BLAH. Something went wrong with the inverse operation! " << formatErrorMessage(error_set_inverse) - << std::endl; - FAIL(); - } - - EXPECT_NEAR(distance_nm_truth, Units::NauticalMilesLength(distance).value(), 1e-12); - EXPECT_NEAR(crs_radians_ned_truth, - Units::UnsignedRadiansAngle(GeolibUtils::ConvertCourseFromEnuToNed(go_east_enu)).value(), 1e-12); -} - -TEST(GeolibUtils, test_CalculateNewPoint) { - const LatitudeLongitudePoint start_point(Units::DegreesAngle(0.0), Units::DegreesAngle(0)); - const Units::MetersLength distance(1.0); - const Units::SignedDegreesAngle go_east_enu(0); - - // This is the tested method ---- - LatitudeLongitudePoint calculated_point = GeolibUtils::CalculateNewPoint(start_point, distance, go_east_enu); - // ------------------------------ - - // Now get comparison data for "truth" measurement by calling the inverse operation - double epsilon = DEFAULT_EPS; - double crs_radians_truth_ned_unsigned = DBL_MIN; - double tmp_course_radians_calculated = DBL_MIN; - double distance_nm_truth = DBL_MIN; - ErrorSet error_set_inverse = - inverse(start_point.GetGeolibPrimitiveLLPoint(), calculated_point.GetGeolibPrimitiveLLPoint(), - &crs_radians_truth_ned_unsigned, &tmp_course_radians_calculated, &distance_nm_truth, epsilon); - if (error_set_inverse != SUCCESS) { - std::cout << "BLAH. Something went wrong with the inverse operation! " << formatErrorMessage(error_set_inverse) - << std::endl; - FAIL(); - } - - EXPECT_NEAR(distance_nm_truth, Units::NauticalMilesLength(distance).value(), 1e-12); - EXPECT_NEAR(crs_radians_truth_ned_unsigned, - Units::SignedRadiansAngle(GeolibUtils::ConvertCourseFromEnuToNed(go_east_enu)).value(), 1e-12); -} - -TEST(LatitudeLongitudePoint, test_RelationshipBewteenPoints_1meter) { - const LatitudeLongitudePoint start_point(Units::DegreesAngle(0.0), Units::DegreesAngle(0)); - const Units::MetersLength distance(1.0); - const Units::SignedDegreesAngle go_east_enu(0); - - // Now call directly into geolib for "truth" data point - LLPoint end_point_from_geolib; - ErrorSet error_set_direct = - direct(start_point.GetGeolibPrimitiveLLPoint(), - Units::UnsignedRadiansAngle(GeolibUtils::ConvertCourseFromEnuToNed(go_east_enu)).value(), - Units::NauticalMilesLength(distance).value(), &end_point_from_geolib, GEOLIB_EPSILON); - if (error_set_direct != SUCCESS) { - std::cout << "BLAH. Something went wrong with the direct operation! " << formatErrorMessage(error_set_direct) - << std::endl; - FAIL(); - } - const LatitudeLongitudePoint end_point = LatitudeLongitudePoint::CreateFromGeolibPrimitive(end_point_from_geolib); - - // This is the tested method ---- - std::pair point_relationship = - start_point.CalculateRelationshipBetweenPoints(end_point); - // ------------------------------ - - EXPECT_NEAR(distance.value(), point_relationship.first.value(), 1e-10); - EXPECT_NEAR(go_east_enu.value(), point_relationship.second.value(), 1e-12); -} - -TEST(GeolibUtils, test_RelationshipBewteenPoints_1meter) { - const LatitudeLongitudePoint start_point(Units::DegreesAngle(0.0), Units::DegreesAngle(0)); - const Units::MetersLength distance(1.0); - const Units::SignedDegreesAngle go_east_enu(0); - - // Now call directly into geolib for "truth" data point - LLPoint end_point_from_geolib; - ErrorSet error_set_direct = - direct(start_point.GetGeolibPrimitiveLLPoint(), - Units::UnsignedRadiansAngle(GeolibUtils::ConvertCourseFromEnuToNed(go_east_enu)).value(), - Units::NauticalMilesLength(distance).value(), &end_point_from_geolib, GEOLIB_EPSILON); - if (error_set_direct != SUCCESS) { - std::cout << "BLAH. Something went wrong with the direct operation! " << formatErrorMessage(error_set_direct) - << std::endl; - FAIL(); - } - const LatitudeLongitudePoint end_point = LatitudeLongitudePoint::CreateFromGeolibPrimitive(end_point_from_geolib); - - // This is the tested method ---- - std::pair point_relationship = - GeolibUtils::CalculateRelationshipBetweenPoints(start_point, end_point); - // ------------------------------ - - EXPECT_NEAR(distance.value(), point_relationship.first.value(), 1e-10); - EXPECT_NEAR(go_east_enu.value(), point_relationship.second.value(), 1e-12); -} - -TEST(GeolibUtils, test_RelationshipBewteenPoints_1nm) { - const LatitudeLongitudePoint start_point(Units::DegreesAngle(0.0), Units::DegreesAngle(0)); - const Units::NauticalMilesLength distance(1.0); - const Units::SignedDegreesAngle go_east_enu(0); - - // Now call directly into geolib for "truth" data point - LLPoint end_point_from_geolib; - ErrorSet error_set_direct = - direct(start_point.GetGeolibPrimitiveLLPoint(), - Units::UnsignedRadiansAngle(GeolibUtils::ConvertCourseFromEnuToNed(go_east_enu)).value(), - Units::NauticalMilesLength(distance).value(), &end_point_from_geolib, GEOLIB_EPSILON); - if (error_set_direct != SUCCESS) { - std::cout << "BLAH. Something went wrong with the direct operation! " << formatErrorMessage(error_set_direct) - << std::endl; - FAIL(); - } - const LatitudeLongitudePoint end_point = LatitudeLongitudePoint::CreateFromGeolibPrimitive(end_point_from_geolib); - - // This is the tested method ---- - std::pair point_relationship = - GeolibUtils::CalculateRelationshipBetweenPoints(start_point, end_point); - // ------------------------------ - - EXPECT_NEAR(distance.value(), point_relationship.first.value(), 1e-10); - EXPECT_NEAR(go_east_enu.value(), point_relationship.second.value(), 1e-12); -} - -TEST(GeolibUtils, test_ConvertCourseFromEnuToNed) { - // Create a mapping of input courses (in ENU coords) to expected ouput (in NED coords) - std::vector> zipped_angles_input_then_expected; - zipped_angles_input_then_expected.push_back(std::pair( - Units::SignedDegreesAngle(0), Units::UnsignedDegreesAngle(90))); - zipped_angles_input_then_expected.push_back(std::pair( - Units::SignedDegreesAngle(45), Units::UnsignedDegreesAngle(45))); - zipped_angles_input_then_expected.push_back(std::pair( - Units::SignedDegreesAngle(90), Units::UnsignedDegreesAngle(0))); - zipped_angles_input_then_expected.push_back(std::pair( - Units::SignedDegreesAngle(135), Units::UnsignedDegreesAngle(315))); - zipped_angles_input_then_expected.push_back(std::pair( - Units::SignedDegreesAngle(-45), Units::UnsignedDegreesAngle(135))); - zipped_angles_input_then_expected.push_back(std::pair( - Units::SignedDegreesAngle(-90), Units::UnsignedDegreesAngle(180))); - zipped_angles_input_then_expected.push_back(std::pair( - Units::SignedDegreesAngle(-95), Units::UnsignedDegreesAngle(185))); - zipped_angles_input_then_expected.push_back(std::pair( - Units::SignedDegreesAngle(180), Units::UnsignedDegreesAngle(270))); - - // Test the conversion method - for (auto test_pair : zipped_angles_input_then_expected) { - const Units::SignedDegreesAngle enu_crs_to_test = test_pair.first; - const Units::UnsignedDegreesAngle expected_result_after_conversion = test_pair.second; - const Units::UnsignedDegreesAngle result_crs_ned = GeolibUtils::ConvertCourseFromEnuToNed(enu_crs_to_test); - EXPECT_NEAR(expected_result_after_conversion.value(), result_crs_ned.value(), 1e-10); - } -} - -TEST(GeolibUtils, test_ConvertCourseFromNedToEnu) { - // Create a mapping of input courses (in NED coords) to expected ouput (in ENU coords) - std::vector> zipped_angles_input_then_expected; - zipped_angles_input_then_expected.push_back(std::pair( - Units::UnsignedDegreesAngle(90), Units::SignedDegreesAngle(0))); - zipped_angles_input_then_expected.push_back(std::pair( - Units::UnsignedDegreesAngle(45), Units::SignedDegreesAngle(45))); - zipped_angles_input_then_expected.push_back(std::pair( - Units::UnsignedDegreesAngle(0), Units::SignedDegreesAngle(90))); - zipped_angles_input_then_expected.push_back(std::pair( - Units::UnsignedDegreesAngle(315), Units::SignedDegreesAngle(135))); - zipped_angles_input_then_expected.push_back(std::pair( - Units::UnsignedDegreesAngle(135), Units::SignedDegreesAngle(-45))); - zipped_angles_input_then_expected.push_back(std::pair( - Units::UnsignedDegreesAngle(180), Units::SignedDegreesAngle(-90))); - zipped_angles_input_then_expected.push_back(std::pair( - Units::UnsignedDegreesAngle(185), Units::SignedDegreesAngle(-95))); - zipped_angles_input_then_expected.push_back(std::pair( - Units::UnsignedDegreesAngle(270), Units::SignedDegreesAngle(180))); - - // Test the conversion method - for (auto test_pair : zipped_angles_input_then_expected) { - const Units::SignedDegreesAngle ned_crs_to_test = test_pair.first; - const Units::UnsignedDegreesAngle expected_result_after_conversion = test_pair.second; - const Units::UnsignedDegreesAngle result_crs_enu = GeolibUtils::ConvertCourseFromNedToEnu(ned_crs_to_test); - EXPECT_NEAR(expected_result_after_conversion.value(), result_crs_enu.value(), 1e-10); - } -} - -TEST(GeolibUtils, test_CalculateLineLineIntersectionPoint) { - // Define an intersection point that is the truth point - const LatitudeLongitudePoint expected_point(Units::DegreesAngle(38.0), Units::DegreesAngle(-77.3)); - - // Define two lines that are guaranteed to pass through the intersection point - const Units::NauticalMilesLength dist1 = Units::NauticalMilesLength(.1); - const LatitudeLongitudePoint end_point1 = - GeolibUtils::CalculateNewPoint(expected_point, dist1, Units::DegreesAngle(0)); - const LineOnEllipsoid tmp_line = LineOnEllipsoid::CreateFromPoints(expected_point, end_point1); - const LatitudeLongitudePoint new_start_point1 = GeolibUtils::CalculateNewPoint( - expected_point, dist1, tmp_line.GetForwardCourseEnuAtStartPoint() + Units::RadiansAngle(M_PI)); - const LineOnEllipsoid line1 = LineOnEllipsoid::CreateFromPoints(new_start_point1, end_point1); - - const Units::NauticalMilesLength dist2 = Units::NauticalMilesLength(.5); - const LatitudeLongitudePoint end_point2 = - GeolibUtils::CalculateNewPoint(expected_point, dist2, Units::DegreesAngle(10)); - LineOnEllipsoid line2 = LineOnEllipsoid::CreateFromPoints(expected_point, end_point2); - - // This is the tested method ---- - auto intersection_values = GeolibUtils::CalculateLineLineIntersectionPoint(line1, line2); - const bool intersection_is_valid = std::get<0>(intersection_values); - const LatitudeLongitudePoint calculated_intersection_point = std::get<1>(intersection_values); - const std::vector distances_to_intersection_point = std::get<2>(intersection_values); - // ------------------------------ - - EXPECT_NEAR(Units::SignedRadiansAngle(expected_point.GetLatitude()).value(), - Units::SignedRadiansAngle(calculated_intersection_point.GetLatitude()).value(), 1e-10); - EXPECT_NEAR(Units::SignedRadiansAngle(expected_point.GetLongitude()).value(), - Units::SignedRadiansAngle(calculated_intersection_point.GetLongitude()).value(), 1e-10); - EXPECT_NEAR(dist1.value(), Units::NauticalMilesLength(distances_to_intersection_point.front()).value(), 1e-10); - EXPECT_NEAR(0.0, Units::NauticalMilesLength(distances_to_intersection_point.back()).value(), 1e-10); - EXPECT_TRUE(intersection_is_valid); -} - -TEST(GeolibUtils, test_NoPossibleLineLineIntersectionPoint) { - // Define two lines that are guaranteed not to intersect - const LatitudeLongitudePoint start_point1(Units::DegreesAngle(38.0), Units::DegreesAngle(-77.3)); - const LatitudeLongitudePoint end_point1 = - GeolibUtils::CalculateNewPoint(start_point1, Units::NauticalMilesLength(.1), Units::DegreesAngle(0)); - const LineOnEllipsoid line1 = LineOnEllipsoid::CreateFromPoints(start_point1, end_point1); - const LatitudeLongitudePoint start_point2(Units::DegreesAngle(40.1), Units::DegreesAngle(-112.8)); - const LatitudeLongitudePoint end_point2 = - GeolibUtils::CalculateNewPoint(start_point2, Units::NauticalMilesLength(.5), Units::DegreesAngle(10)); - const LineOnEllipsoid line2 = LineOnEllipsoid::CreateFromPoints(start_point2, end_point2); - - // This is the tested method ---- - auto intersection_values = GeolibUtils::CalculateLineLineIntersectionPoint(line1, line2); - bool intersection_is_valid = std::get<0>(intersection_values); - // ------------------------------ - - // We expect this to fail -- no intersection found - EXPECT_FALSE(intersection_is_valid); -} - -TEST(GeolibUtils, test_IsPointOnLine) { - // Define a point that will be tested - const LatitudeLongitudePoint test_point(Units::DegreesAngle(38.0), Units::DegreesAngle(-77.3)); - - // Define a line that is guaranteed to pass through the test point - const Units::NauticalMilesLength distance = Units::NauticalMilesLength(1); - const LatitudeLongitudePoint point_to_east = - GeolibUtils::CalculateNewPoint(test_point, distance, Units::SignedDegreesAngle(10)); - const LineOnEllipsoid tmp_line = LineOnEllipsoid::CreateFromPoints(test_point, point_to_east); - const LatitudeLongitudePoint start_point = GeolibUtils::CalculateNewPoint( - test_point, distance, tmp_line.GetForwardCourseEnuAtStartPoint() + Units::SignedRadiansAngle(M_PI)); - const LineOnEllipsoid line_with_test_point_on_it = LineOnEllipsoid::CreateFromPoints(start_point, point_to_east); - - // This is the tested method ---- - bool is_on_line = GeolibUtils::IsPointOnLine(line_with_test_point_on_it, test_point); - // ------------------------------ - - // We expect this to be true - EXPECT_TRUE(is_on_line); -} - -TEST(GeolibUtils, test_PoinIsNotOnLine) { - // Define a point that will be used to build a line - const LatitudeLongitudePoint line_mid_point(Units::DegreesAngle(38.0), Units::DegreesAngle(-77.3)); - const Units::NauticalMilesLength distance = Units::NauticalMilesLength(1); - const LatitudeLongitudePoint point_to_east = - GeolibUtils::CalculateNewPoint(line_mid_point, distance, Units::SignedDegreesAngle(10)); - const LineOnEllipsoid tmp_line = LineOnEllipsoid::CreateFromPoints(line_mid_point, point_to_east); - const LatitudeLongitudePoint start_point = GeolibUtils::CalculateNewPoint( - line_mid_point, distance, tmp_line.GetForwardCourseEnuAtStartPoint() + Units::SignedRadiansAngle(M_PI)); - const LineOnEllipsoid line_with_mid_point_on_it = LineOnEllipsoid::CreateFromPoints(start_point, point_to_east); - - // Define a point that is near the mid-point, but not actually the midpoint - const LatitudeLongitudePoint test_point = - GeolibUtils::CalculateNewPoint(line_mid_point, Units::MetersLength(.001), Units::SignedDegreesAngle(0)); - - // This is the tested method ---- - bool is_on_line = GeolibUtils::IsPointOnLine(line_with_mid_point_on_it, test_point); - // ------------------------------ - - // We expect this to be false - EXPECT_FALSE(is_on_line); -} - -TEST(GeolibUtils, test_IsPointOnArc) { - // Calculate an arc - const Units::NauticalMilesLength defined_radius(2.5); - const LatitudeLongitudePoint center_point(Units::SignedDegreesAngle(33.3862), Units::SignedRadiansAngle(-111.887)); - const LatitudeLongitudePoint start_point = - GeolibUtils::CalculateNewPoint(center_point, defined_radius, Units::SignedDegreesAngle(90)); - const LatitudeLongitudePoint end_point = - GeolibUtils::CalculateNewPoint(center_point, defined_radius, Units::SignedDegreesAngle(0)); - const ArcOnEllipsoid arc_on_ellipsoid = - GeolibUtils::CreateArcOnEllipsoid(start_point, end_point, center_point, ArcDirection::CLOCKWISE); - const LatitudeLongitudePoint point_on_arc = - GeolibUtils::CalculateNewPoint(center_point, defined_radius, Units::SignedDegreesAngle(45)); - - // This is the tested method ---- - bool is_on_arc = GeolibUtils::IsPointOnArc(arc_on_ellipsoid, point_on_arc); - // ------------------------------ - - // We expect this to be true - EXPECT_TRUE(is_on_arc); -} - -TEST(GeolibUtils, test_PoinIsNotOnArc) { - // Calculate an arc - const Units::NauticalMilesLength defined_radius(2.5); - const LatitudeLongitudePoint center_point(Units::SignedDegreesAngle(33.3862), Units::SignedRadiansAngle(-111.887)); - const LatitudeLongitudePoint start_point = - GeolibUtils::CalculateNewPoint(center_point, defined_radius, Units::SignedDegreesAngle(90)); - const LatitudeLongitudePoint end_point = - GeolibUtils::CalculateNewPoint(center_point, defined_radius, Units::SignedDegreesAngle(0)); - const ArcOnEllipsoid arc_on_ellipsoid = - GeolibUtils::CreateArcOnEllipsoid(start_point, end_point, center_point, ArcDirection::CLOCKWISE); - - // Define a point that is near the arc, but not actually on it - const LatitudeLongitudePoint point_not_on_arc = GeolibUtils::CalculateNewPoint( - center_point, defined_radius - Units::MetersLength(0.001), Units::SignedDegreesAngle(45)); - - // This is the tested method ---- - bool is_on_arc = GeolibUtils::IsPointOnArc(arc_on_ellipsoid, point_not_on_arc); - // ------------------------------ - - // We expect this to be false - EXPECT_FALSE(is_on_arc); -} - -TEST(ArcOnEllipsoid, IsPointOnArc) { - // Calculate an arc - const Units::NauticalMilesLength defined_radius(2.5); - const LatitudeLongitudePoint center_point(Units::SignedDegreesAngle(33.3862), Units::SignedRadiansAngle(-111.887)); - const LatitudeLongitudePoint start_point = - GeolibUtils::CalculateNewPoint(center_point, defined_radius, Units::SignedDegreesAngle(90)); - const LatitudeLongitudePoint end_point = - GeolibUtils::CalculateNewPoint(center_point, defined_radius, Units::SignedDegreesAngle(0)); - const ArcOnEllipsoid arc_on_ellipsoid = - GeolibUtils::CreateArcOnEllipsoid(start_point, end_point, center_point, ArcDirection::CLOCKWISE); - const LatitudeLongitudePoint point_on_arc = - GeolibUtils::CalculateNewPoint(center_point, defined_radius, Units::SignedDegreesAngle(45)); - - // This is the tested method ---- - bool is_on_arc = arc_on_ellipsoid.IsPointOnShape(point_on_arc); - // ------------------------------ - - // We expect this to be true - EXPECT_TRUE(is_on_arc); -} - -TEST(ArcOnEllipsoid, calculate_point_on_shape) { - const Units::NauticalMilesLength defined_radius(2.5); - const LatitudeLongitudePoint center_point(Units::SignedDegreesAngle(33.3862), Units::SignedRadiansAngle(-111.887)); - const LatitudeLongitudePoint start_point = - GeolibUtils::CalculateNewPoint(center_point, defined_radius, Units::SignedDegreesAngle(90)); - const LatitudeLongitudePoint end_point = - GeolibUtils::CalculateNewPoint(center_point, defined_radius, Units::SignedDegreesAngle(0)); - const ArcOnEllipsoid arc_on_ellipsoid = - GeolibUtils::CreateArcOnEllipsoid(start_point, end_point, center_point, ArcDirection::CLOCKWISE); - - // This is the tested method ---- - const LatitudeLongitudePoint calculated_point = - arc_on_ellipsoid.CalculatePointAtDistanceFromStartPoint(arc_on_ellipsoid.GetShapeLength()); - EXPECT_NEAR(Units::RadiansAngle(end_point.GetLatitude()).value(), - Units::RadiansAngle(calculated_point.GetLatitude()).value(), 1e-8); - EXPECT_NEAR(Units::RadiansAngle(end_point.GetLongitude()).value(), - Units::RadiansAngle(calculated_point.GetLongitude()).value(), 1e-8); - // ------------------------------ -} - -TEST(ArcOnEllipsoid, calculate_course_at_point_on_shape) { - const Units::NauticalMilesLength defined_radius(2.5); - const LatitudeLongitudePoint center_point(Units::SignedDegreesAngle(33.3862), Units::SignedRadiansAngle(-111.887)); - const LatitudeLongitudePoint start_point = - GeolibUtils::CalculateNewPoint(center_point, defined_radius, Units::SignedDegreesAngle(90)); - const LatitudeLongitudePoint end_point = - GeolibUtils::CalculateNewPoint(center_point, defined_radius, Units::SignedDegreesAngle(0)); - const ArcOnEllipsoid arc_on_ellipsoid = - GeolibUtils::CreateArcOnEllipsoid(start_point, end_point, center_point, ArcDirection::CLOCKWISE); - - // This is the tested method ---- - const std::pair calculated_course_and_point_at_distance = - arc_on_ellipsoid.CalculateCourseAtDistanceFromStartPoint(arc_on_ellipsoid.GetShapeLength()); - EXPECT_NEAR(Units::SignedRadiansAngle(arc_on_ellipsoid.GetCourseEnuTangentToEndPoint()).value(), - calculated_course_and_point_at_distance.first.value(), 1e-8); - EXPECT_NEAR(Units::RadiansAngle(end_point.GetLatitude()).value(), - Units::RadiansAngle(calculated_course_and_point_at_distance.second.GetLatitude()).value(), 1e-8); - EXPECT_NEAR(Units::RadiansAngle(end_point.GetLongitude()).value(), - Units::RadiansAngle(calculated_course_and_point_at_distance.second.GetLongitude()).value(), 1e-8); - // ------------------------------ -} - -TEST(LineOnEllipsoid, IsPointOnLine) { - // Define a point that will be tested - const LatitudeLongitudePoint test_point(Units::DegreesAngle(38.0), Units::DegreesAngle(-77.3)); - - // Define a line that is guaranteed to pass through the test point - const Units::NauticalMilesLength distance = Units::NauticalMilesLength(1); - const LatitudeLongitudePoint point_to_east = - GeolibUtils::CalculateNewPoint(test_point, distance, Units::SignedDegreesAngle(10)); - const LineOnEllipsoid tmp_line = LineOnEllipsoid::CreateFromPoints(test_point, point_to_east); - const LatitudeLongitudePoint start_point = GeolibUtils::CalculateNewPoint( - test_point, distance, tmp_line.GetForwardCourseEnuAtStartPoint() + Units::SignedRadiansAngle(M_PI)); - const LineOnEllipsoid line_with_test_point_on_it = LineOnEllipsoid::CreateFromPoints(start_point, point_to_east); - - // This is the tested method ---- - const bool is_on_line = line_with_test_point_on_it.IsPointOnShape(test_point); - // ------------------------------ - - // We expect this to be true - EXPECT_TRUE(is_on_line); -} - -TEST(GeolibUtils, FindPointOnLineUsingPerpendiculorProjection_valid) { - // Define a point that will be used to build a line - const LatitudeLongitudePoint line_mid_point(Units::DegreesAngle(38.0), Units::DegreesAngle(-77.3)); - const Units::NauticalMilesLength distance = Units::NauticalMilesLength(1); - const LatitudeLongitudePoint point_to_east = - GeolibUtils::CalculateNewPoint(line_mid_point, distance, Units::SignedDegreesAngle(10)); - const LineOnEllipsoid tmp_line = LineOnEllipsoid::CreateFromPoints(line_mid_point, point_to_east); - const LatitudeLongitudePoint start_point = GeolibUtils::CalculateNewPoint( - line_mid_point, distance, tmp_line.GetForwardCourseEnuAtStartPoint() + Units::SignedRadiansAngle(M_PI)); - const LineOnEllipsoid line_with_mid_point_on_it = LineOnEllipsoid::CreateFromPoints(start_point, point_to_east); - - // Define a point that is perpendicular to the mid-point - const LatitudeLongitudePoint test_point = - GeolibUtils::CalculateNewPoint(line_mid_point, Units::MetersLength(.001), - tmp_line.GetForwardCourseEnuAtStartPoint() + Units::SignedDegreesAngle(90)); - - // This is the tested method ---- - std::tuple perp_info = - GeolibUtils::FindNearestPointOnLineUsingPerpendicularProjection(line_with_mid_point_on_it, test_point); - const LatitudeLongitudePoint point_on_line = std::get<0>(perp_info); - - // ------------------------------ - - // We expect these to match - EXPECT_NEAR(Units::RadiansAngle(line_mid_point.GetLatitude()).value(), - Units::RadiansAngle(point_on_line.GetLatitude()).value(), 1e-10); - EXPECT_NEAR(Units::RadiansAngle(line_mid_point.GetLongitude()).value(), - Units::RadiansAngle(point_on_line.GetLongitude()).value(), 1e-10); -} - -TEST(GeolibUtils, IsPointInsideArc) { - // Calculate an arc - const Units::NauticalMilesLength defined_radius(2.5); - const LatitudeLongitudePoint center_point(Units::SignedDegreesAngle(33.3862), Units::SignedRadiansAngle(-111.887)); - const LatitudeLongitudePoint start_point = - GeolibUtils::CalculateNewPoint(center_point, defined_radius, Units::SignedDegreesAngle(90)); - const LatitudeLongitudePoint end_point = - GeolibUtils::CalculateNewPoint(center_point, defined_radius, Units::SignedDegreesAngle(0)); - const ArcOnEllipsoid arc_on_ellipsoid = - GeolibUtils::CreateArcOnEllipsoid(start_point, end_point, center_point, ArcDirection::CLOCKWISE); - const LatitudeLongitudePoint point_inside_arc = GeolibUtils::CalculateNewPoint( - center_point, defined_radius - Units::MetersLength(1e-5), Units::SignedDegreesAngle(45)); - - // This is the tested method ---- - bool is_inside_arc = GeolibUtils::IsPointInsideArcSegment(arc_on_ellipsoid, point_inside_arc); - // ------------------------------ - - // We expect this to be true - EXPECT_TRUE(is_inside_arc); -} - -TEST(GeolibUtils, IsPointInsideArc_false) { - // Calculate an arc - const Units::NauticalMilesLength defined_radius(2.5); - const LatitudeLongitudePoint center_point(Units::SignedDegreesAngle(33.3862), Units::SignedRadiansAngle(-111.887)); - const LatitudeLongitudePoint start_point = - GeolibUtils::CalculateNewPoint(center_point, defined_radius, Units::SignedDegreesAngle(90)); - const LatitudeLongitudePoint end_point = - GeolibUtils::CalculateNewPoint(center_point, defined_radius, Units::SignedDegreesAngle(0)); - const ArcOnEllipsoid arc_on_ellipsoid = - GeolibUtils::CreateArcOnEllipsoid(start_point, end_point, center_point, ArcDirection::CLOCKWISE); - const LatitudeLongitudePoint point_not_inside_arc = GeolibUtils::CalculateNewPoint( - center_point, defined_radius + Units::MetersLength(1e-5), Units::SignedDegreesAngle(45)); - - // This is the tested method ---- - bool is_inside_arc = GeolibUtils::IsPointInsideArcSegment(arc_on_ellipsoid, point_not_inside_arc); - // ------------------------------ - - // We expect this to be false - EXPECT_FALSE(is_inside_arc); -} - -TEST(ArcOnEllipsoid, IsPointInsideArc) { - // Calculate an arc - const Units::NauticalMilesLength defined_radius(2.5); - const LatitudeLongitudePoint center_point(Units::SignedDegreesAngle(33.3862), Units::SignedRadiansAngle(-111.887)); - const LatitudeLongitudePoint start_point = - GeolibUtils::CalculateNewPoint(center_point, defined_radius, Units::SignedDegreesAngle(90)); - const LatitudeLongitudePoint end_point = - GeolibUtils::CalculateNewPoint(center_point, defined_radius, Units::SignedDegreesAngle(0)); - const ArcOnEllipsoid arc_on_ellipsoid = - GeolibUtils::CreateArcOnEllipsoid(start_point, end_point, center_point, ArcDirection::CLOCKWISE); - const LatitudeLongitudePoint point_inside_arc = GeolibUtils::CalculateNewPoint( - center_point, defined_radius - Units::MetersLength(1e-5), Units::SignedDegreesAngle(45)); - - // This is the tested method ---- - bool is_inside_arc = arc_on_ellipsoid.IsPointInsideArc(point_inside_arc); - // ------------------------------ - - // We expect this to be true - EXPECT_TRUE(is_inside_arc); -} - -TEST(GeolibUtils, FindNearestPointOnArc) { - // Calculate an arc - const Units::NauticalMilesLength defined_radius(2.5); - const LatitudeLongitudePoint center_point(Units::SignedDegreesAngle(33.3862), Units::SignedRadiansAngle(-111.887)); - const LatitudeLongitudePoint start_point = - GeolibUtils::CalculateNewPoint(center_point, defined_radius, Units::SignedDegreesAngle(90)); - const LatitudeLongitudePoint end_point = - GeolibUtils::CalculateNewPoint(center_point, defined_radius, Units::SignedDegreesAngle(0)); - const ArcOnEllipsoid arc_on_ellipsoid = - GeolibUtils::CreateArcOnEllipsoid(start_point, end_point, center_point, ArcDirection::CLOCKWISE); - const LatitudeLongitudePoint point_on_arc_to_find = - GeolibUtils::CalculateNewPoint(center_point, defined_radius, Units::SignedDegreesAngle(45)); - const LatitudeLongitudePoint point_outside_arc = GeolibUtils::CalculateNewPoint( - center_point, defined_radius + Units::NauticalMilesLength(1), Units::SignedDegreesAngle(45)); - - // This is the tested method ---- - std::pair ret = - GeolibUtils::FindNearestPointOnArcUsingPerpendiculorProjection(arc_on_ellipsoid, point_outside_arc); - // ------------------------------ - - // Expect this to be true - EXPECT_TRUE(ret.first); - - // Expect these to match - EXPECT_NEAR(Units::RadiansAngle(point_on_arc_to_find.GetLatitude()).value(), - Units::RadiansAngle(ret.second.GetLatitude()).value(), 1e-10); - EXPECT_NEAR(Units::RadiansAngle(point_on_arc_to_find.GetLongitude()).value(), - Units::RadiansAngle(ret.second.GetLongitude()).value(), 1e-10); -} - -TEST(GeolibUtils, PointsAreMathematicallyEqual) { - // Random location with lots of precision - const LatitudeLongitudePoint point1(Units::SignedDegreesAngle(34.23423432341236), - Units::SignedDegreesAngle(-112.1234123569987)); - const LatitudeLongitudePoint point2(point1); - - // This is the tested method ----------- - const bool is_equal = GeolibUtils::ArePointsMathematicallyEqual(point1, point2); - // ------------------------------------- - - EXPECT_TRUE(is_equal); -} - -TEST(GeolibUtils, PointsAreNotMathematicallyEqual) { - // Random location with lots of precision - const LatitudeLongitudePoint point1(Units::SignedDegreesAngle(34.23423432341236), - Units::SignedDegreesAngle(-112.1234123569987)); - const LatitudeLongitudePoint point_very_close_to_point1 = - point1.ProjectDistanceAlongCourse(Units::MetersLength(1e-5), Units::DegreesAngle(2)); - - // This is the tested method ----------- - const bool is_equal = GeolibUtils::ArePointsMathematicallyEqual(point1, point_very_close_to_point1); - // ------------------------------------- - - // They should not be equal - EXPECT_FALSE(is_equal); -} - -TEST(LatitudeLongitudePoint, PointsAreMathematicallyEqual) { - // Random location with lots of precision - const LatitudeLongitudePoint point1(Units::SignedDegreesAngle(34.23423432341236), - Units::SignedDegreesAngle(-112.1234123569987)); - const LatitudeLongitudePoint point2(point1); - - // This is the tested method ----------- - const bool is_equal = point1.ArePointsEqual(point2); - // ------------------------------------- - - EXPECT_TRUE(is_equal); -} - -TEST(LatitudeLongitudePoint, PointsAreNotMathematicallyEqual) { - // Random location with lots of precision - const LatitudeLongitudePoint point1(Units::SignedDegreesAngle(34.23423432341236), - Units::SignedDegreesAngle(-112.1234123569987)); - const LatitudeLongitudePoint point_very_close_to_point1 = - point1.ProjectDistanceAlongCourse(Units::MetersLength(1e-5), Units::DegreesAngle(2)); - - // This is the tested method ----------- - const bool is_equal = point1.ArePointsEqual(point_very_close_to_point1); - // ------------------------------------- - - // They should not be equal - EXPECT_FALSE(is_equal); -} - -TEST(GeolibUtils, CreateArcTangentToTwoLines_valid_case) { - /* - * Use "real" data from in an operational AAESim scenario. This came from the - * IM Test Vector work that Stuart did in 2020. These waypoints below are publicly published values - * from a KDEN arrival. - * HIMOM 39.788108 -104.808950 11000 0 210 0.78 0 11000 11000 200 200 0 0 0 - MCMUL 39.738073 -104.810275 10000 0 210 0.78 0 10000 10000 190 190 0 0 0 - KUGLN 39.691536 -104.752289 9000 0 170 0.78 0 9000 9000 190 190 2.8 39.737141 -104.751188 - BSAYN 39.739938 -104.688628 8000 0 170 0 0 9000 8000 170 170 2.9 39.74037993 -104.7518702 - CORDE 39.780965 -104.688147 7000 0 170 0 0 50000 7000 170 170 0 0 0 - */ - std::vector> waypoints_to_connect = { - // tuple holds: bool for is_line, waypoint, rf_radius - std::make_tuple( - true, - Waypoint("HIMOM", Units::DegreesAngle(39.788108), Units::DegreesAngle(-104.808950), Units::ZERO_LENGTH, - Units::ZERO_LENGTH, Units::ZERO_SPEED, Units::ZERO_LENGTH, Units::ZERO_SPEED), - Units::infinity()), - std::make_tuple( - true, - Waypoint("MCMUL", Units::DegreesAngle(39.738073), Units::DegreesAngle(-104.810275), Units::ZERO_LENGTH, - Units::ZERO_LENGTH, Units::ZERO_SPEED, Units::ZERO_LENGTH, Units::ZERO_SPEED), - Units::infinity()), - std::make_tuple( - false, - Waypoint("KUGLN", Units::DegreesAngle(39.691536), Units::DegreesAngle(-104.752289), Units::ZERO_LENGTH, - Units::ZERO_LENGTH, Units::ZERO_SPEED, Units::ZERO_LENGTH, Units::ZERO_SPEED), - Units::NauticalMilesLength(2.8)), - std::make_tuple( - false, - Waypoint("BSAYN", Units::DegreesAngle(39.739938), Units::DegreesAngle(-104.688628), Units::ZERO_LENGTH, - Units::ZERO_LENGTH, Units::ZERO_SPEED, Units::ZERO_LENGTH, Units::ZERO_SPEED), - Units::NauticalMilesLength(2.9)), - std::make_tuple( - true, - Waypoint("CORDE", Units::DegreesAngle(39.780965), Units::DegreesAngle(-104.688147), Units::ZERO_LENGTH, - Units::ZERO_LENGTH, Units::ZERO_SPEED, Units::ZERO_LENGTH, Units::ZERO_SPEED), - Units::infinity()), - }; - Waypoint prev_waypoint; - Units::SignedAngle course_enu_tangency_end_of_prev_shape; - std::vector simple_path_along_ground; - for (auto item : waypoints_to_connect) { - const bool is_line = std::get<0>(item); - const Waypoint wpt = std::get<1>(item); - const Units::Length radius = std::get<2>(item); - - if (wpt.GetName() != "HIMOM") { - ShapeOnEllipsoid *shape; - if (is_line) { - LineOnEllipsoid tf_line = - LineOnEllipsoid::CreateFromPoints(LatitudeLongitudePoint::CreateFromWaypoint(prev_waypoint), - LatitudeLongitudePoint::CreateFromWaypoint(wpt)); - course_enu_tangency_end_of_prev_shape = tf_line.GetForwardCourseEnuAtEndPoint(); - shape = &tf_line; - } else { - const LatitudeLongitudePoint where_rf_began = LatitudeLongitudePoint::CreateFromWaypoint(prev_waypoint); - const LatitudeLongitudePoint projected_pt_after_rf_began = where_rf_began.ProjectDistanceAlongCourse( - Units::NauticalMilesLength(100), course_enu_tangency_end_of_prev_shape); - const LineOnEllipsoid fake_line_behind_rf_leg = - LineOnEllipsoid::CreateFromPoints(where_rf_began, projected_pt_after_rf_began); - const LatitudeLongitudePoint where_rf_ends = LatitudeLongitudePoint::CreateFromWaypoint(wpt); - std::tuple perp_info = - GeolibUtils::FindNearestPointOnLineUsingPerpendicularProjection(fake_line_behind_rf_leg, - where_rf_ends); - const LatitudeLongitudePoint intersection_point_for_rf_outbound_leg = std::get<0>(perp_info); - const LineOnEllipsoid fake_line_outbound_from_rf_leg = - LineOnEllipsoid::CreateFromPoints(intersection_point_for_rf_outbound_leg, where_rf_ends); - const LineOnEllipsoid fake_line_outbound_from_rf_leg_extended = - fake_line_outbound_from_rf_leg.CreateExtendedLine(Units::NauticalMilesLength(100)); - - // This is the tested method ----------- - std::pair data = GeolibUtils::CreateArcTangentToTwoLines( - fake_line_behind_rf_leg, fake_line_outbound_from_rf_leg_extended, radius); - bool rf_leg_found = data.first; - ArcOnEllipsoid rf_leg = data.second; - // ------------------------------------- - - // Test assertions --------------------- - const double latitude_tolerance_degrees = 1e-2; - const double longitude_tolerance_degrees = 1e-2; - const double distance_tolerance_meters = 200; - - // Expect the arc to be found - EXPECT_TRUE(rf_leg_found); - - // Expect that the arc's start point will be close to the previous waypoint - EXPECT_NEAR(Units::DegreesAngle(where_rf_began.GetLatitude()).value(), - Units::DegreesAngle(rf_leg.GetStartPoint().GetLatitude()).value(), latitude_tolerance_degrees); - EXPECT_NEAR(Units::DegreesAngle(where_rf_began.GetLongitude()).value(), - Units::DegreesAngle(rf_leg.GetStartPoint().GetLongitude()).value(), - longitude_tolerance_degrees); - std::pair return_info = - where_rf_began.CalculateRelationshipBetweenPoints(rf_leg.GetStartPoint()); - Units::MetersLength actual_distance = std::get<0>(return_info); - EXPECT_LE(actual_distance.value(), distance_tolerance_meters); - - // Expect that the arc's end point will be close to the current waypoint - EXPECT_NEAR(Units::DegreesAngle(where_rf_ends.GetLatitude()).value(), - Units::DegreesAngle(rf_leg.GetEndPoint().GetLatitude()).value(), latitude_tolerance_degrees); - EXPECT_NEAR(Units::DegreesAngle(where_rf_ends.GetLongitude()).value(), - Units::DegreesAngle(rf_leg.GetEndPoint().GetLongitude()).value(), longitude_tolerance_degrees); - return_info = where_rf_ends.CalculateRelationshipBetweenPoints(rf_leg.GetEndPoint()); - actual_distance = std::get<0>(return_info); - EXPECT_LE(actual_distance.value(), distance_tolerance_meters); - - // Expect that the arc's radius will be mathematically close to the input radius - EXPECT_NEAR(Units::NauticalMilesLength(radius).value(), - Units::NauticalMilesLength(rf_leg.GetRadius()).value(), 1e-10); - // ------------------------------------- - - // Setup for next iteration of for loop - shape = &rf_leg; - course_enu_tangency_end_of_prev_shape = rf_leg.GetCourseEnuTangentToEndPoint(); - } - simple_path_along_ground.push_back(shape); - } - prev_waypoint = wpt; - } - - // Clean up - simple_path_along_ground.clear(); -} - -TEST(GeolibUtils, CreateArcInboundLineAndEndPoint_valid_case) { - /* - * Use "real" data from in an operational AAESim scenario. This came from the - * IM Test Vector work that Stuart did in 2020. These waypoints below are publicly published values - * from a KDEN arrival. - * HIMOM 39.788108 -104.808950 11000 0 210 0.78 0 11000 11000 200 200 0 0 0 - MCMUL 39.738073 -104.810275 10000 0 210 0.78 0 10000 10000 190 190 0 0 0 - KUGLN 39.691536 -104.752289 9000 0 170 0.78 0 9000 9000 190 190 2.8 39.737141 -104.751188 - BSAYN 39.739938 -104.688628 8000 0 170 0 0 9000 8000 170 170 2.9 39.74037993 -104.7518702 - CORDE 39.780965 -104.688147 7000 0 170 0 0 50000 7000 170 170 0 0 0 - */ - std::vector> waypoints_to_connect = { - // tuple holds: bool for is_line, waypoint, rf_radius - std::make_tuple( - true, - Waypoint("HIMOM", Units::DegreesAngle(39.788108), Units::DegreesAngle(-104.808950), Units::ZERO_LENGTH, - Units::ZERO_LENGTH, Units::ZERO_SPEED, Units::ZERO_LENGTH, Units::ZERO_SPEED), - Units::infinity()), - std::make_tuple( - true, - Waypoint("MCMUL", Units::DegreesAngle(39.738073), Units::DegreesAngle(-104.810275), Units::ZERO_LENGTH, - Units::ZERO_LENGTH, Units::ZERO_SPEED, Units::ZERO_LENGTH, Units::ZERO_SPEED), - Units::infinity()), - std::make_tuple( - false, - Waypoint("KUGLN", Units::DegreesAngle(39.691536), Units::DegreesAngle(-104.752289), Units::ZERO_LENGTH, - Units::ZERO_LENGTH, Units::ZERO_SPEED, Units::ZERO_LENGTH, Units::ZERO_SPEED), - Units::NauticalMilesLength(2.8)), - std::make_tuple( - false, - Waypoint("BSAYN", Units::DegreesAngle(39.739938), Units::DegreesAngle(-104.688628), Units::ZERO_LENGTH, - Units::ZERO_LENGTH, Units::ZERO_SPEED, Units::ZERO_LENGTH, Units::ZERO_SPEED), - Units::NauticalMilesLength(2.9)), - std::make_tuple( - true, - Waypoint("CORDE", Units::DegreesAngle(39.780965), Units::DegreesAngle(-104.688147), Units::ZERO_LENGTH, - Units::ZERO_LENGTH, Units::ZERO_SPEED, Units::ZERO_LENGTH, Units::ZERO_SPEED), - Units::infinity()), - }; - Waypoint prev_waypoint; - Units::SignedAngle course_enu_tangency_end_of_prev_shape; - std::vector simple_path_along_ground; - for (auto item : waypoints_to_connect) { - const bool is_line = std::get<0>(item); - const Waypoint wpt = std::get<1>(item); - const Units::Length radius = std::get<2>(item); - - if (wpt.GetName() != "HIMOM") { - ShapeOnEllipsoid *shape; - if (is_line) { - LineOnEllipsoid tf_line = - LineOnEllipsoid::CreateFromPoints(LatitudeLongitudePoint::CreateFromWaypoint(prev_waypoint), - LatitudeLongitudePoint::CreateFromWaypoint(wpt)); - course_enu_tangency_end_of_prev_shape = tf_line.GetForwardCourseEnuAtEndPoint(); - shape = &tf_line; - } else { - const LatitudeLongitudePoint where_rf_began = LatitudeLongitudePoint::CreateFromWaypoint(prev_waypoint); - const LatitudeLongitudePoint where_rf_ends = LatitudeLongitudePoint::CreateFromWaypoint(wpt); - const LatitudeLongitudePoint projected_start_point = where_rf_began.ProjectDistanceAlongCourse( - Units::NauticalMilesLength(100), - course_enu_tangency_end_of_prev_shape + Units::SignedRadiansAngle(180)); - const LineOnEllipsoid fake_inbound_line_to_rf_leg = - LineOnEllipsoid::CreateFromPoints(projected_start_point, where_rf_began); - - // This is the tested method ----------- - ArcOnEllipsoid rf_leg = - GeolibUtils::CreateArcFromInboundShapeAndEndPoint(&fake_inbound_line_to_rf_leg, where_rf_ends); - // ------------------------------------- - - // Test assertions --------------------- - const double latitude_tolerance_degrees = 1e-8; - const double longitude_tolerance_degrees = 1e-8; - const double distance_tolerance_meters = 20; - - // Expect that the arc's start point will be close to the previous waypoint - EXPECT_NEAR(Units::DegreesAngle(where_rf_began.GetLatitude()).value(), - Units::DegreesAngle(rf_leg.GetStartPoint().GetLatitude()).value(), latitude_tolerance_degrees); - EXPECT_NEAR(Units::DegreesAngle(where_rf_began.GetLongitude()).value(), - Units::DegreesAngle(rf_leg.GetStartPoint().GetLongitude()).value(), - longitude_tolerance_degrees); - std::pair return_info = - where_rf_began.CalculateRelationshipBetweenPoints(rf_leg.GetStartPoint()); - Units::MetersLength actual_distance = std::get<0>(return_info); - EXPECT_LE(actual_distance.value(), distance_tolerance_meters); - - // Expect that the arc's end point will be close to the current waypoint - EXPECT_NEAR(Units::DegreesAngle(where_rf_ends.GetLatitude()).value(), - Units::DegreesAngle(rf_leg.GetEndPoint().GetLatitude()).value(), latitude_tolerance_degrees); - EXPECT_NEAR(Units::DegreesAngle(where_rf_ends.GetLongitude()).value(), - Units::DegreesAngle(rf_leg.GetEndPoint().GetLongitude()).value(), longitude_tolerance_degrees); - return_info = where_rf_ends.CalculateRelationshipBetweenPoints(rf_leg.GetEndPoint()); - actual_distance = std::get<0>(return_info); - EXPECT_LE(actual_distance.value(), distance_tolerance_meters); - - // Expect that the arc's radius will be mathematically close to the input radius - EXPECT_NEAR(Units::NauticalMilesLength(radius).value(), - Units::NauticalMilesLength(rf_leg.GetRadius()).value(), distance_tolerance_meters); - // ------------------------------------- - - // Setup for next iteration of for loop - shape = &rf_leg; - course_enu_tangency_end_of_prev_shape = rf_leg.GetCourseEnuTangentToEndPoint(); - } - simple_path_along_ground.push_back(shape); - } - prev_waypoint = wpt; - } - - // Clean up - simple_path_along_ground.clear(); -} - -TEST(GeolibUtils, CreateArcInboundLineAndEndPoint_valid_case2) { - /* - * Use published data from Jeppesen 2012. This is an arrival into KPSP. - * FERNN,33.97281666666667,-116.54705277777778,IF - JEVOK,33.88010833333333,-116.46062500000001,TF - CUXIT,33.95426944444445,-116.29662777777777,RF,4.72 - HOPLI,34.04785277777778,-116.33561388888889,TF - YOCUL,34.111425000000004,-116.40140833333334,RF,6.83 - WASAK,34.09063888888889,-116.52534166666666,RF,5.00 - */ - std::vector> waypoints_to_connect = { - // tuple holds: bool for is_line, waypoint, rf_radius - std::make_tuple(true, - Waypoint("FERNN", Units::DegreesAngle(33.97281666666667), - Units::DegreesAngle(-116.54705277777778), Units::ZERO_LENGTH, Units::ZERO_LENGTH, - Units::ZERO_SPEED, Units::ZERO_LENGTH, Units::ZERO_SPEED), - Units::infinity()), - std::make_tuple(true, - Waypoint("JEVOK", Units::DegreesAngle(33.88010833333333), - Units::DegreesAngle(-116.46062500000001), Units::ZERO_LENGTH, Units::ZERO_LENGTH, - Units::ZERO_SPEED, Units::ZERO_LENGTH, Units::ZERO_SPEED), - Units::infinity()), - std::make_tuple(false, - Waypoint("CUSIT", Units::DegreesAngle(33.95426944444445), - Units::DegreesAngle(-116.29662777777777), Units::ZERO_LENGTH, Units::ZERO_LENGTH, - Units::ZERO_SPEED, Units::ZERO_LENGTH, Units::ZERO_SPEED), - Units::NauticalMilesLength(4.72)), - std::make_tuple(true, - Waypoint("HOPLI", Units::DegreesAngle(34.04785277777778), - Units::DegreesAngle(-116.33561388888889), Units::ZERO_LENGTH, Units::ZERO_LENGTH, - Units::ZERO_SPEED, Units::ZERO_LENGTH, Units::ZERO_SPEED), - Units::infinity()), - std::make_tuple(false, - Waypoint("YOCUL", Units::DegreesAngle(34.111425000000004), - Units::DegreesAngle(-116.40140833333334), Units::ZERO_LENGTH, Units::ZERO_LENGTH, - Units::ZERO_SPEED, Units::ZERO_LENGTH, Units::ZERO_SPEED), - Units::NauticalMilesLength(6.83)), - std::make_tuple(false, - Waypoint("WASAK", Units::DegreesAngle(34.09063888888889), - Units::DegreesAngle(-116.52534166666666), Units::ZERO_LENGTH, Units::ZERO_LENGTH, - Units::ZERO_SPEED, Units::ZERO_LENGTH, Units::ZERO_SPEED), - Units::NauticalMilesLength(5.00)), - }; - Waypoint prev_waypoint; - Units::SignedAngle course_enu_tangency_end_of_prev_shape; - std::vector simple_path_along_ground; - for (auto item : waypoints_to_connect) { - const bool is_line = std::get<0>(item); - const Waypoint wpt = std::get<1>(item); - const Units::Length radius = std::get<2>(item); - - if (wpt.GetName() != "FERNN") { - ShapeOnEllipsoid *shape; - if (is_line) { - LineOnEllipsoid tf_line = - LineOnEllipsoid::CreateFromPoints(LatitudeLongitudePoint::CreateFromWaypoint(prev_waypoint), - LatitudeLongitudePoint::CreateFromWaypoint(wpt)); - course_enu_tangency_end_of_prev_shape = tf_line.GetForwardCourseEnuAtEndPoint(); - shape = &tf_line; - } else { - const LatitudeLongitudePoint where_rf_began = LatitudeLongitudePoint::CreateFromWaypoint(prev_waypoint); - const LatitudeLongitudePoint where_rf_ends = LatitudeLongitudePoint::CreateFromWaypoint(wpt); - const LatitudeLongitudePoint projected_start_point = where_rf_began.ProjectDistanceAlongCourse( - Units::NauticalMilesLength(100), - course_enu_tangency_end_of_prev_shape + Units::SignedRadiansAngle(180)); - const LineOnEllipsoid fake_inbound_line_to_rf_leg = - LineOnEllipsoid::CreateFromPoints(projected_start_point, where_rf_began); - - // This is the tested method ----------- - ArcOnEllipsoid rf_leg = - GeolibUtils::CreateArcFromInboundShapeAndEndPoint(&fake_inbound_line_to_rf_leg, where_rf_ends); - // ------------------------------------- - - // Test assertions --------------------- - const double latitude_tolerance_degrees = 1e-8; - const double longitude_tolerance_degrees = 1e-8; - const double distance_tolerance_meters = 20; - - // Expect that the arc's start point will be close to the previous waypoint - EXPECT_NEAR(Units::DegreesAngle(where_rf_began.GetLatitude()).value(), - Units::DegreesAngle(rf_leg.GetStartPoint().GetLatitude()).value(), latitude_tolerance_degrees); - EXPECT_NEAR(Units::DegreesAngle(where_rf_began.GetLongitude()).value(), - Units::DegreesAngle(rf_leg.GetStartPoint().GetLongitude()).value(), - longitude_tolerance_degrees); - std::pair return_info = - where_rf_began.CalculateRelationshipBetweenPoints(rf_leg.GetStartPoint()); - Units::MetersLength actual_distance = std::get<0>(return_info); - EXPECT_LE(actual_distance.value(), distance_tolerance_meters); - - // Expect that the arc's end point will be close to the current waypoint - EXPECT_NEAR(Units::DegreesAngle(where_rf_ends.GetLatitude()).value(), - Units::DegreesAngle(rf_leg.GetEndPoint().GetLatitude()).value(), latitude_tolerance_degrees); - EXPECT_NEAR(Units::DegreesAngle(where_rf_ends.GetLongitude()).value(), - Units::DegreesAngle(rf_leg.GetEndPoint().GetLongitude()).value(), longitude_tolerance_degrees); - return_info = where_rf_ends.CalculateRelationshipBetweenPoints(rf_leg.GetEndPoint()); - actual_distance = std::get<0>(return_info); - EXPECT_LE(actual_distance.value(), distance_tolerance_meters); - - // Expect that the arc's radius will be mathematically close to the input radius - EXPECT_NEAR(Units::NauticalMilesLength(radius).value(), - Units::NauticalMilesLength(rf_leg.GetRadius()).value(), distance_tolerance_meters); - // ------------------------------------- - - // Setup for next iteration of for loop - shape = &rf_leg; - course_enu_tangency_end_of_prev_shape = rf_leg.GetCourseEnuTangentToEndPoint(); - } - simple_path_along_ground.push_back(shape); - } - prev_waypoint = wpt; - } - - // Clean up - simple_path_along_ground.clear(); -} - -TEST(GeolibUtils, LineArcIntersection_valid_case) { - /* - * Use published data from Jeppesen 2012. This is an arrival into KPSP. - * FERNN,33.97281666666667,-116.54705277777778,IF - JEVOK,33.88010833333333,-116.46062500000001,TF - CUXIT,33.95426944444445,-116.29662777777777,RF,4.72 - HOPLI,34.04785277777778,-116.33561388888889,TF - YOCUL,34.111425000000004,-116.40140833333334,RF,6.83 - WASAK,34.09063888888889,-116.52534166666666,RF,5.00 - */ - std::vector> waypoints_to_connect = { - // tuple holds: bool for is_line, waypoint, rf_radius - std::make_tuple(true, - Waypoint("FERNN", Units::DegreesAngle(33.97281666666667), - Units::DegreesAngle(-116.54705277777778), Units::ZERO_LENGTH, Units::ZERO_LENGTH, - Units::ZERO_SPEED, Units::ZERO_LENGTH, Units::ZERO_SPEED), - Units::infinity()), - std::make_tuple(true, - Waypoint("JEVOK", Units::DegreesAngle(33.88010833333333), - Units::DegreesAngle(-116.46062500000001), Units::ZERO_LENGTH, Units::ZERO_LENGTH, - Units::ZERO_SPEED, Units::ZERO_LENGTH, Units::ZERO_SPEED), - Units::infinity()), - std::make_tuple(false, - Waypoint("CUSIT", Units::DegreesAngle(33.95426944444445), - Units::DegreesAngle(-116.29662777777777), Units::ZERO_LENGTH, Units::ZERO_LENGTH, - Units::ZERO_SPEED, Units::ZERO_LENGTH, Units::ZERO_SPEED), - Units::NauticalMilesLength(4.72)), - std::make_tuple(true, - Waypoint("HOPLI", Units::DegreesAngle(34.04785277777778), - Units::DegreesAngle(-116.33561388888889), Units::ZERO_LENGTH, Units::ZERO_LENGTH, - Units::ZERO_SPEED, Units::ZERO_LENGTH, Units::ZERO_SPEED), - Units::infinity()), - std::make_tuple(false, - Waypoint("YOCUL", Units::DegreesAngle(34.111425000000004), - Units::DegreesAngle(-116.40140833333334), Units::ZERO_LENGTH, Units::ZERO_LENGTH, - Units::ZERO_SPEED, Units::ZERO_LENGTH, Units::ZERO_SPEED), - Units::NauticalMilesLength(6.83)), - std::make_tuple(false, - Waypoint("WASAK", Units::DegreesAngle(34.09063888888889), - Units::DegreesAngle(-116.52534166666666), Units::ZERO_LENGTH, Units::ZERO_LENGTH, - Units::ZERO_SPEED, Units::ZERO_LENGTH, Units::ZERO_SPEED), - Units::NauticalMilesLength(5.00)), - }; - Waypoint prev_waypoint; - Units::SignedAngle course_enu_tangency_end_of_prev_shape; - std::vector simple_path_along_ground; - for (auto item : waypoints_to_connect) { - const bool is_line = std::get<0>(item); - const Waypoint wpt = std::get<1>(item); - - if (wpt.GetName() != "FERNN") { - ShapeOnEllipsoid *shape; - if (is_line) { - LineOnEllipsoid tf_line = - LineOnEllipsoid::CreateFromPoints(LatitudeLongitudePoint::CreateFromWaypoint(prev_waypoint), - LatitudeLongitudePoint::CreateFromWaypoint(wpt)); - course_enu_tangency_end_of_prev_shape = tf_line.GetForwardCourseEnuAtEndPoint(); - shape = &tf_line; - } else { - const LatitudeLongitudePoint where_rf_began = LatitudeLongitudePoint::CreateFromWaypoint(prev_waypoint); - const LatitudeLongitudePoint where_rf_ends = LatitudeLongitudePoint::CreateFromWaypoint(wpt); - const LatitudeLongitudePoint projected_start_point = where_rf_began.ProjectDistanceAlongCourse( - Units::NauticalMilesLength(100), - course_enu_tangency_end_of_prev_shape + Units::SignedRadiansAngle(180)); - const LineOnEllipsoid fake_inbound_line_to_rf_leg = - LineOnEllipsoid::CreateFromPoints(projected_start_point, where_rf_began); - ArcOnEllipsoid rf_leg = - GeolibUtils::CreateArcFromInboundShapeAndEndPoint(&fake_inbound_line_to_rf_leg, where_rf_ends); - - // This is the tested method ----------- - std::vector> intersection_data = - GeolibUtils::CalculateLineArcIntersectionPoints(fake_inbound_line_to_rf_leg, rf_leg); - // ------------------------------------- - - // Test assertions --------------------- - const double latitude_tolerance_degrees = 1e-8; - const double longitude_tolerance_degrees = 1e-8; - - for (auto pr : intersection_data) { - // Expect that an intersection was found and is on both shapes - EXPECT_TRUE(pr.first); - - // Expect that the intersection point will be close to the arc's start point - const LatitudeLongitudePoint intersection_point = pr.second; - EXPECT_NEAR(Units::DegreesAngle(intersection_point.GetLatitude()).value(), - Units::DegreesAngle(rf_leg.GetStartPoint().GetLatitude()).value(), - latitude_tolerance_degrees); - EXPECT_NEAR(Units::DegreesAngle(intersection_point.GetLongitude()).value(), - Units::DegreesAngle(rf_leg.GetStartPoint().GetLongitude()).value(), - longitude_tolerance_degrees); - } - // ------------------------------------- - - // Setup for next iteration of for loop - shape = &rf_leg; - course_enu_tangency_end_of_prev_shape = rf_leg.GetCourseEnuTangentToEndPoint(); - } - simple_path_along_ground.push_back(shape); - } - prev_waypoint = wpt; - } - - // Clean up - simple_path_along_ground.clear(); -} - -TEST(LineOnEllipsoid, GetRelativeDirection) { - const std::vector start_points = { - Waypoint("kphx_airport", Units::DegreesAngle(33.4342778), Units::DegreesAngle(-112.0115833), - Units::ZERO_LENGTH, Units::ZERO_LENGTH, Units::ZERO_SPEED, Units::ZERO_LENGTH, Units::ZERO_SPEED), - Waypoint("ksfo_airport", Units::DegreesAngle(37.6188056), Units::DegreesAngle(-122.3754167), - Units::ZERO_LENGTH, Units::ZERO_LENGTH, Units::ZERO_SPEED, Units::ZERO_LENGTH, Units::ZERO_SPEED), - Waypoint("kden_airport", Units::DegreesAngle(39.8616667), Units::DegreesAngle(-104.6731667), - Units::ZERO_LENGTH, Units::ZERO_LENGTH, Units::ZERO_SPEED, Units::ZERO_LENGTH, Units::ZERO_SPEED), - Waypoint("ksfo_airport", Units::DegreesAngle(42.3629444), Units::DegreesAngle(-71.0063889), Units::ZERO_LENGTH, - Units::ZERO_LENGTH, Units::ZERO_SPEED, Units::ZERO_LENGTH, Units::ZERO_SPEED), - Waypoint("katl_airport", Units::DegreesAngle(33.6366996), Units::DegreesAngle(-84.4278640), Units::ZERO_LENGTH, - Units::ZERO_LENGTH, Units::ZERO_SPEED, Units::ZERO_LENGTH, Units::ZERO_SPEED)}; - - const Units::MetersLength distance_off_line(1e-5); // this should be small to test navigation situations that are - // "very close" to the line being followed - for (const Waypoint wpt : start_points) { - const LatitudeLongitudePoint start_point = LatitudeLongitudePoint::CreateFromWaypoint(wpt); - - for (double crs_ned = 0; crs_ned <= 2 * M_PI; crs_ned += M_PI / 20) { - const Units::SignedRadiansAngle course_enu = - GeolibUtils::ConvertCourseFromNedToEnu(Units::UnsignedRadiansAngle(crs_ned)); - const LatitudeLongitudePoint end_point = - start_point.ProjectDistanceAlongCourse(Units::NauticalMilesLength(100), course_enu); - const LineOnEllipsoid line_on_ellipsoid = LineOnEllipsoid::CreateFromPoints(start_point, end_point); - - // Test: point on line result - const LatitudeLongitudePoint test_point_actually_on_line = - start_point.ProjectDistanceAlongCourse(Units::NauticalMilesLength(1), course_enu); - // This is the tested method ----------- - ShapeOnEllipsoid::kDirectionRelativeToShape actual_dir1 = - line_on_ellipsoid.GetRelativeDirection(test_point_actually_on_line); - EXPECT_TRUE(actual_dir1 == ShapeOnEllipsoid::ON_SHAPE); - // ------------------------------------- - - // Test: point to left result - const LatitudeLongitudePoint test_point_actually_left_of_line = - test_point_actually_on_line.ProjectDistanceAlongCourse( - distance_off_line, GeolibUtils::ConvertCourseFromNedToEnu(Units::UnsignedRadiansAngle(crs_ned) - - Units::RadiansAngle(M_PI_2))); - // This is the tested method ----------- - ShapeOnEllipsoid::kDirectionRelativeToShape actual_dir2 = - line_on_ellipsoid.GetRelativeDirection(test_point_actually_left_of_line); - EXPECT_TRUE(actual_dir2 == ShapeOnEllipsoid::LEFT_OF_SHAPE); - // ------------------------------------- - - // Test: point to right result - const LatitudeLongitudePoint test_point_actually_right_of_line = - test_point_actually_on_line.ProjectDistanceAlongCourse( - distance_off_line, GeolibUtils::ConvertCourseFromNedToEnu(Units::UnsignedRadiansAngle(crs_ned) + - Units::RadiansAngle(M_PI_2))); - // This is the tested method ----------- - ShapeOnEllipsoid::kDirectionRelativeToShape actual_dir3 = - line_on_ellipsoid.GetRelativeDirection(test_point_actually_right_of_line); - EXPECT_TRUE(actual_dir3 == ShapeOnEllipsoid::RIGHT_OF_SHAPE); - // ------------------------------------- - } - } -} - -TEST(LineOnEllipsoid, test_GetRelativeDirection_aaes1666) { - const std::vector waypoints = { - Waypoint("GUNUD", Units::DegreesAngle(1.1783333333333335), Units::DegreesAngle(105.10499999999999), - Units::ZERO_LENGTH, Units::ZERO_LENGTH, Units::ZERO_SPEED, Units::ZERO_LENGTH, Units::ZERO_SPEED), - Waypoint("KEXAS", Units::DegreesAngle(1.1719444444444445), Units::DegreesAngle(104.80499999999998), - Units::ZERO_LENGTH, Units::ZERO_LENGTH, Units::ZERO_SPEED, Units::ZERO_LENGTH, Units::ZERO_SPEED), - Waypoint("VIMAL", Units::DegreesAngle(1.1616666666666666), Units::DegreesAngle(104.39805555555556), - Units::ZERO_LENGTH, Units::ZERO_LENGTH, Units::ZERO_SPEED, Units::ZERO_LENGTH, Units::ZERO_SPEED)}; - const Units::MetersLength distance_off_line(1); - - // create a point to the right of the line. Call the test method and see if the enumeration is correct - const LatitudeLongitudePoint start_point = LatitudeLongitudePoint::CreateFromWaypoint(waypoints[0]); - const LineOnEllipsoid line_on_ellipsoid = - LineOnEllipsoid::CreateFromPoints(start_point, LatitudeLongitudePoint::CreateFromWaypoint(waypoints[1])); - const auto point_on_line = line_on_ellipsoid.CalculatePointAtDistanceFromStartPoint(Units::NauticalMilesLength(1)); - const auto relationship_info = start_point.CalculateRelationshipBetweenPoints(point_on_line); - const auto course_enu = std::get<1>(relationship_info); - const LatitudeLongitudePoint test_point_actually_right_of_line = - point_on_line.ProjectDistanceAlongCourse(distance_off_line, Units::RadiansAngle(M_PI_2)); - // This is the tested method ----------- - EXPECT_TRUE(point_on_line.GetLatitude() < test_point_actually_right_of_line.GetLatitude()); - ShapeOnEllipsoid::kDirectionRelativeToShape actual_dir = - line_on_ellipsoid.GetRelativeDirection(test_point_actually_right_of_line); - EXPECT_TRUE(actual_dir == ShapeOnEllipsoid::RIGHT_OF_SHAPE); - // ------------------------------------- - - // create a point to the left of the line. Call the test method and see if the enumeration is correct - const LatitudeLongitudePoint test_point_actually_left_of_line = - point_on_line.ProjectDistanceAlongCourse(distance_off_line, -Units::RadiansAngle(M_PI_2)); - // This is the tested method ----------- - EXPECT_TRUE(point_on_line.GetLatitude() > test_point_actually_left_of_line.GetLatitude()); - actual_dir = line_on_ellipsoid.GetRelativeDirection(test_point_actually_left_of_line); - EXPECT_TRUE(actual_dir == ShapeOnEllipsoid::LEFT_OF_SHAPE); - // ------------------------------------- -} - -TEST(EarthModel, ToUnitVector) { - EarthModel::AbsolutePositionEcef test_vector, unit_vector; - test_vector.x = Units::MetersLength(3); - test_vector.y = Units::MetersLength(4); - test_vector.z = Units::MetersLength(5); - // Test method--------------------------- - unit_vector = test_vector.ToUnitVector(); - // -------------------------------------- - - const Units::MetersLength unity(1); - EXPECT_NEAR(unity.value(), - Units::MetersLength( - Units::sqrt(Units::sqr(unit_vector.x) + Units::sqr(unit_vector.y) + Units::sqr(unit_vector.z))) - .value(), - 1e-10); -} - -TEST(ArcOnEllipsoid, GetRelativeDirection) { - const std::vector center_points = { - Waypoint("kphx_airport", Units::DegreesAngle(33.4342778), Units::DegreesAngle(-112.0115833), - Units::ZERO_LENGTH, Units::ZERO_LENGTH, Units::ZERO_SPEED, Units::ZERO_LENGTH, Units::ZERO_SPEED), - Waypoint("ksfo_airport", Units::DegreesAngle(37.6188056), Units::DegreesAngle(-122.3754167), - Units::ZERO_LENGTH, Units::ZERO_LENGTH, Units::ZERO_SPEED, Units::ZERO_LENGTH, Units::ZERO_SPEED), - Waypoint("kden_airport", Units::DegreesAngle(39.8616667), Units::DegreesAngle(-104.6731667), - Units::ZERO_LENGTH, Units::ZERO_LENGTH, Units::ZERO_SPEED, Units::ZERO_LENGTH, Units::ZERO_SPEED), - Waypoint("ksfo_airport", Units::DegreesAngle(42.3629444), Units::DegreesAngle(-71.0063889), Units::ZERO_LENGTH, - Units::ZERO_LENGTH, Units::ZERO_SPEED, Units::ZERO_LENGTH, Units::ZERO_SPEED), - Waypoint("katl_airport", Units::DegreesAngle(33.6366996), Units::DegreesAngle(-84.4278640), Units::ZERO_LENGTH, - Units::ZERO_LENGTH, Units::ZERO_SPEED, Units::ZERO_LENGTH, Units::ZERO_SPEED)}; - - const Units::MetersLength distance_off_arc(1e-5); // this should be small to test navigation situations that are - // "very close" to the shape being followed - const Units::NauticalMilesLength arc_radius_approximate(5); - const geolib_idealab::ArcDirection arc_directions[2] = {geolib_idealab::ArcDirection::CLOCKWISE, - geolib_idealab::ArcDirection::COUNTERCLOCKWISE}; - - for (const Waypoint wpt : center_points) { - const LatitudeLongitudePoint center_point_approximate = LatitudeLongitudePoint::CreateFromWaypoint(wpt); - - for (geolib_idealab::ArcDirection arc_direction : arc_directions) { - for (double arc_start_course_ned = 3 * M_PI / 2; arc_start_course_ned <= 2 * M_PI; - arc_start_course_ned += M_PI / 2) { - const Units::SignedRadiansAngle arc_start_course_enu = - GeolibUtils::ConvertCourseFromNedToEnu(Units::UnsignedRadiansAngle(arc_start_course_ned)); - LatitudeLongitudePoint line_end_point = - center_point_approximate.ProjectDistanceAlongCourse(arc_radius_approximate, arc_start_course_enu); - LatitudeLongitudePoint line_start_point, arc_end_point; - if (arc_direction == geolib_idealab::ArcDirection::CLOCKWISE) { - line_start_point = line_end_point.ProjectDistanceAlongCourse( - arc_radius_approximate, arc_start_course_enu + Units::PI_RADIANS_ANGLE / 2); - arc_end_point = center_point_approximate.ProjectDistanceAlongCourse( - arc_radius_approximate, arc_start_course_enu - Units::PI_RADIANS_ANGLE / 2); - } else { - line_start_point = line_end_point.ProjectDistanceAlongCourse( - arc_radius_approximate, arc_start_course_enu - Units::PI_RADIANS_ANGLE / 2); - arc_end_point = center_point_approximate.ProjectDistanceAlongCourse( - arc_radius_approximate, arc_start_course_enu + Units::PI_RADIANS_ANGLE / 2); - } - const LineOnEllipsoid inbound_line = LineOnEllipsoid::CreateFromPoints(line_start_point, line_end_point); - const ArcOnEllipsoid arc_on_ellipsoid = - GeolibUtils::CreateArcFromInboundShapeAndEndPoint(&inbound_line, arc_end_point); - EXPECT_TRUE(arc_on_ellipsoid.GetArcDirection() == arc_direction); - - // Test: point on arc result - LatitudeLongitudePoint test_point_actually_on_arc; - if (arc_direction == geolib_idealab::ArcDirection::CLOCKWISE) { - test_point_actually_on_arc = arc_on_ellipsoid.GetCenterPoint().ProjectDistanceAlongCourse( - arc_on_ellipsoid.GetRadius(), arc_start_course_enu - Units::PI_RADIANS_ANGLE / 4); - } else { - test_point_actually_on_arc = arc_on_ellipsoid.GetCenterPoint().ProjectDistanceAlongCourse( - arc_on_ellipsoid.GetRadius(), arc_start_course_enu + Units::PI_RADIANS_ANGLE / 4); - } - // This is the tested method ----------- - ShapeOnEllipsoid::kDirectionRelativeToShape actual_dir1 = - arc_on_ellipsoid.GetRelativeDirection(test_point_actually_on_arc); - EXPECT_TRUE(actual_dir1 == ShapeOnEllipsoid::ON_SHAPE); - - // Test: point to left result - LatitudeLongitudePoint test_point_actually_left_of_line; - ShapeOnEllipsoid::kDirectionRelativeToShape actual_dir2; - if (arc_direction == geolib_idealab::ArcDirection::CLOCKWISE) { - test_point_actually_left_of_line = arc_on_ellipsoid.GetCenterPoint().ProjectDistanceAlongCourse( - arc_on_ellipsoid.GetRadius() + distance_off_arc, - GeolibUtils::ConvertCourseFromNedToEnu( - Units::UnsignedRadiansAngle(arc_start_course_ned + M_PI_4))); - // This is the tested method ----------- - actual_dir2 = arc_on_ellipsoid.GetRelativeDirection(test_point_actually_left_of_line); - EXPECT_TRUE(actual_dir2 == ShapeOnEllipsoid::LEFT_OF_SHAPE); - // ------------------------------------- - } else { - test_point_actually_left_of_line = arc_on_ellipsoid.GetCenterPoint().ProjectDistanceAlongCourse( - arc_on_ellipsoid.GetRadius() - distance_off_arc, - GeolibUtils::ConvertCourseFromNedToEnu( - Units::UnsignedRadiansAngle(arc_start_course_ned - M_PI_4))); - // This is the tested method ----------- - actual_dir2 = arc_on_ellipsoid.GetRelativeDirection(test_point_actually_left_of_line); - EXPECT_TRUE(actual_dir2 == ShapeOnEllipsoid::LEFT_OF_SHAPE); - // ------------------------------------- - } - - // Test: point to right result - LatitudeLongitudePoint test_point_actually_right_of_line; - ShapeOnEllipsoid::kDirectionRelativeToShape actual_dir3; - if (arc_direction == geolib_idealab::ArcDirection::CLOCKWISE) { - test_point_actually_right_of_line = arc_on_ellipsoid.GetCenterPoint().ProjectDistanceAlongCourse( - arc_on_ellipsoid.GetRadius() - distance_off_arc, - GeolibUtils::ConvertCourseFromNedToEnu( - Units::UnsignedRadiansAngle(arc_start_course_ned + M_PI_4))); - // This is the tested method ----------- - actual_dir3 = arc_on_ellipsoid.GetRelativeDirection(test_point_actually_right_of_line); - EXPECT_TRUE(actual_dir3 == ShapeOnEllipsoid::RIGHT_OF_SHAPE); - // ------------------------------------- - } else { - test_point_actually_right_of_line = arc_on_ellipsoid.GetCenterPoint().ProjectDistanceAlongCourse( - arc_on_ellipsoid.GetRadius() + distance_off_arc, - GeolibUtils::ConvertCourseFromNedToEnu( - Units::UnsignedRadiansAngle(arc_start_course_ned - M_PI_4))); - // This is the tested method ----------- - actual_dir3 = arc_on_ellipsoid.GetRelativeDirection(test_point_actually_right_of_line); - EXPECT_TRUE(actual_dir3 == ShapeOnEllipsoid::RIGHT_OF_SHAPE); - // ------------------------------------- - } - } - } - } -} - -TEST(LineOnEllipsoid, GetDistanceToEndPoint) { - // Test 1: point that is on line already-------- - const LatitudeLongitudePoint zero_zero = LatitudeLongitudePoint(Units::ZERO_ANGLE, Units::ZERO_ANGLE); - const LatitudeLongitudePoint one_zero = LatitudeLongitudePoint(Units::SignedDegreesAngle(1), Units::ZERO_ANGLE); - const LineOnEllipsoid test_line_north = LineOnEllipsoid::CreateFromPoints(zero_zero, one_zero); - const LatitudeLongitudePoint test_point_on_line = zero_zero.ProjectDistanceAlongCourse( - test_line_north.GetShapeLength() / 2, test_line_north.GetForwardCourseEnuAtStartPoint()); - const Units::MetersLength expected_distance_to_end_point = test_line_north.GetShapeLength() / 2; - - // This is the test method ------------------ - const Units::MetersLength actual_distance_to_end_point_1 = test_line_north.GetDistanceToEndPoint(test_point_on_line); - // ------------------------------------------ - EXPECT_NEAR(expected_distance_to_end_point.value(), actual_distance_to_end_point_1.value(), 1e-9); - - // Test 2: point not on line----------------- - const LatitudeLongitudePoint test_point_not_on_line = test_point_on_line.ProjectDistanceAlongCourse( - Units::NauticalMilesLength(1), - test_line_north.GetForwardCourseEnuAtStartPoint() + Units::PI_RADIANS_ANGLE / 2); - - // This is the test method ------------------ - const Units::MetersLength actual_distance_to_end_point_2 = - test_line_north.GetDistanceToEndPoint(test_point_not_on_line); - // ------------------------------------------ - EXPECT_NEAR(expected_distance_to_end_point.value(), actual_distance_to_end_point_2.value(), 1e-9); -} - -TEST(ArcOnEllipsoid, GetDistanceToEndPoint) { - // Test 1: point that is on arc-------- - const LatitudeLongitudePoint one_zero = LatitudeLongitudePoint(Units::SignedDegreesAngle(1), Units::ZERO_ANGLE); - const LatitudeLongitudePoint inbound_line_start_point = - one_zero.ProjectDistanceAlongCourse(Units::NauticalMilesLength(1), Units::PI_RADIANS_ANGLE); - const LineOnEllipsoid inbound_line_to_arc = LineOnEllipsoid::CreateFromPoints(inbound_line_start_point, one_zero); - const LatitudeLongitudePoint arc_end_point = one_zero.ProjectDistanceAlongCourse( - Units::NauticalMilesLength(5), - inbound_line_to_arc.GetForwardCourseEnuAtEndPoint() - Units::PI_RADIANS_ANGLE / 2); - const ArcOnEllipsoid arc_on_ellipsoid = - GeolibUtils::CreateArcFromInboundShapeAndEndPoint(&inbound_line_to_arc, arc_end_point); - LatitudeLongitudePoint test_point_on_arc; - if (arc_on_ellipsoid.GetArcDirection() == geolib_idealab::ArcDirection::CLOCKWISE) { - test_point_on_arc = arc_on_ellipsoid.GetCenterPoint().ProjectDistanceAlongCourse( - arc_on_ellipsoid.GetRadius(), - arc_on_ellipsoid.GetStartAzimuthEnu() + arc_on_ellipsoid.GetArcAngularExtent() / 2); - } else { - test_point_on_arc = arc_on_ellipsoid.GetCenterPoint().ProjectDistanceAlongCourse( - arc_on_ellipsoid.GetRadius(), - arc_on_ellipsoid.GetStartAzimuthEnu() - arc_on_ellipsoid.GetArcAngularExtent() / 2); - } - const Units::MetersLength expected_distance_to_end_point = arc_on_ellipsoid.GetShapeLength() / 2; - - // This is the test method ------------------ - const Units::MetersLength actual_distance_to_end_point_1 = arc_on_ellipsoid.GetDistanceToEndPoint(test_point_on_arc); - // ------------------------------------------ - EXPECT_NEAR(expected_distance_to_end_point.value(), actual_distance_to_end_point_1.value(), 1e-8); - - // Test 2: point not on line----------------- - LatitudeLongitudePoint test_point_not_on_line; - if (arc_on_ellipsoid.GetArcDirection() == geolib_idealab::ArcDirection::CLOCKWISE) { - test_point_not_on_line = arc_on_ellipsoid.GetCenterPoint().ProjectDistanceAlongCourse( - arc_on_ellipsoid.GetRadius() + Units::NauticalMilesLength(1), - arc_on_ellipsoid.GetStartAzimuthEnu() + arc_on_ellipsoid.GetArcAngularExtent() / 2); - } else { - test_point_not_on_line = arc_on_ellipsoid.GetCenterPoint().ProjectDistanceAlongCourse( - arc_on_ellipsoid.GetRadius() - Units::NauticalMilesLength(1), - arc_on_ellipsoid.GetStartAzimuthEnu() - arc_on_ellipsoid.GetArcAngularExtent() / 2); - } - - // This is the test method ------------------ - const Units::MetersLength actual_distance_to_end_point_2 = - arc_on_ellipsoid.GetDistanceToEndPoint(test_point_not_on_line); - // ------------------------------------------ - EXPECT_NEAR(expected_distance_to_end_point.value(), actual_distance_to_end_point_2.value(), 1e-8); -} - -TEST(EarthModel, VectorDotProduct) { - EarthModel::AbsolutePositionEcef one_zero_zero, zero_one_zero; - one_zero_zero.x = Units::MetersLength(1); - one_zero_zero.y = Units::ZERO_LENGTH; - one_zero_zero.z = Units::ZERO_LENGTH; - zero_one_zero.x = Units::ZERO_LENGTH; - zero_one_zero.y = one_zero_zero.x; - zero_one_zero.z = Units::ZERO_LENGTH; - EXPECT_NEAR(0, Units::MetersLength(VectorDotProduct(one_zero_zero, zero_one_zero)).value(), 1e-10); -} - -TEST(EarthModel, VectorCrossProduct) { - EarthModel::AbsolutePositionEcef one_zero_zero, zero_one_zero; - one_zero_zero.x = Units::MetersLength(1); - one_zero_zero.y = Units::ZERO_LENGTH; - one_zero_zero.z = Units::ZERO_LENGTH; - zero_one_zero.x = Units::ZERO_LENGTH; - zero_one_zero.y = one_zero_zero.x; - zero_one_zero.z = Units::ZERO_LENGTH; - - // Cross [1,0,0] and [0,1,0] - EarthModel::AbsolutePositionEcef actual_cp_result_test1 = VectorCrossProduct(one_zero_zero, zero_one_zero); - - EarthModel::AbsolutePositionEcef expect_zero_zero_one; - expect_zero_zero_one.x = Units::ZERO_LENGTH; - expect_zero_zero_one.y = Units::ZERO_LENGTH; - expect_zero_zero_one.z = Units::MetersLength(1); - - EXPECT_NEAR(expect_zero_zero_one.x.value(), Units::MetersLength(actual_cp_result_test1.x).value(), 1e-10); - EXPECT_NEAR(expect_zero_zero_one.y.value(), Units::MetersLength(actual_cp_result_test1.y).value(), 1e-10); - EXPECT_NEAR(expect_zero_zero_one.z.value(), Units::MetersLength(actual_cp_result_test1.z).value(), 1e-10); - - // Cross [0,1,0] and [1,0,0] - EarthModel::AbsolutePositionEcef actual_cp_result_test2 = VectorCrossProduct(zero_one_zero, one_zero_zero); - - EarthModel::AbsolutePositionEcef expect_zero_zero_negative_one; - expect_zero_zero_negative_one.x = Units::ZERO_LENGTH; - expect_zero_zero_negative_one.y = Units::ZERO_LENGTH; - expect_zero_zero_negative_one.z = Units::MetersLength(-1); - - EXPECT_NEAR(expect_zero_zero_negative_one.x.value(), Units::MetersLength(actual_cp_result_test2.x).value(), 1e-10); - EXPECT_NEAR(expect_zero_zero_negative_one.y.value(), Units::MetersLength(actual_cp_result_test2.y).value(), 1e-10); - EXPECT_NEAR(expect_zero_zero_negative_one.z.value(), Units::MetersLength(actual_cp_result_test2.z).value(), 1e-10); -} - -TEST(GeolibUtils, IsSuccess_ReturnsFalse) { - // Define two lines that are guaranteed -not- to intersect - const LatitudeLongitudePoint start_point1(Units::DegreesAngle(38.0), Units::DegreesAngle(-77.3)); - const LatitudeLongitudePoint end_point1 = - GeolibUtils::CalculateNewPoint(start_point1, Units::NauticalMilesLength(.1), Units::DegreesAngle(0)); - const LineOnEllipsoid line1 = LineOnEllipsoid::CreateFromPoints(start_point1, end_point1); - const LatitudeLongitudePoint start_point2(Units::DegreesAngle(40.1), Units::DegreesAngle(-112.8)); - const LatitudeLongitudePoint end_point2 = - GeolibUtils::CalculateNewPoint(start_point2, Units::NauticalMilesLength(.5), Units::DegreesAngle(10)); - const LineOnEllipsoid line2 = LineOnEllipsoid::CreateFromPoints(start_point2, end_point2); - - double crs31, distance_line1_start_to_intx_point, crs32, distance_line2_start_to_intx_point; - geolib_idealab::LLPoint intersection_point; - ErrorSet error_set = - geoIntx(line1.GetStartPoint().GetGeolibPrimitiveLLPoint(), line1.GetEndPoint().GetGeolibPrimitiveLLPoint(), - line1.GetLineType(), &crs31, &distance_line1_start_to_intx_point, - line2.GetStartPoint().GetGeolibPrimitiveLLPoint(), line2.GetEndPoint().GetGeolibPrimitiveLLPoint(), - line2.GetLineType(), &crs32, &distance_line2_start_to_intx_point, &intersection_point, - GEOLIB_TOLERANCE, GEOLIB_EPSILON); - - EXPECT_FALSE(GeolibUtils::IsSuccess(error_set)); - EXPECT_TRUE(GeolibUtils::HasErrorBitSet(error_set, geolib_idealab::ErrorCodes::NO_INTERSECTION_ERR)); -} - -TEST(GeolibUtils, IsSuccess_ReturnsTrue) { - // Define two lines that are guaranteed to intersect - const LatitudeLongitudePoint start_point1(Units::DegreesAngle(38.0), Units::DegreesAngle(-77.3)); - const LatitudeLongitudePoint end_point1 = - GeolibUtils::CalculateNewPoint(start_point1, Units::NauticalMilesLength(1), Units::DegreesAngle(0)); - const LineOnEllipsoid line1 = LineOnEllipsoid::CreateFromPoints(start_point1, end_point1); - const LatitudeLongitudePoint start_point2 = - line1.CalculatePointAtDistanceFromStartPoint(Units::NauticalMilesLength(.1)); - const LatitudeLongitudePoint end_point2 = - GeolibUtils::CalculateNewPoint(start_point2, Units::NauticalMilesLength(.5), Units::DegreesAngle(10)); - const LineOnEllipsoid line2 = LineOnEllipsoid::CreateFromPoints(start_point2, end_point2); - - double crs31, distance_line1_start_to_intx_point, crs32, distance_line2_start_to_intx_point; - geolib_idealab::LLPoint intersection_point; - ErrorSet error_set = - geoIntx(line1.GetStartPoint().GetGeolibPrimitiveLLPoint(), line1.GetEndPoint().GetGeolibPrimitiveLLPoint(), - line1.GetLineType(), &crs31, &distance_line1_start_to_intx_point, - line2.GetStartPoint().GetGeolibPrimitiveLLPoint(), line2.GetEndPoint().GetGeolibPrimitiveLLPoint(), - line2.GetLineType(), &crs32, &distance_line2_start_to_intx_point, &intersection_point, - GEOLIB_TOLERANCE, GEOLIB_EPSILON); - - EXPECT_TRUE(GeolibUtils::IsSuccess(error_set)); - EXPECT_FALSE(GeolibUtils::HasErrorBitSet(error_set, geolib_idealab::ErrorCodes::NO_INTERSECTION_ERR)); -} - -TEST(GeolibUtils, HasErrorBitSet_SUCCESS) { - const ErrorSet error_set{geolib_idealab::ErrorCodes::SUCCESS}; - EXPECT_TRUE(GeolibUtils::HasErrorBitSet(error_set, geolib_idealab::ErrorCodes::SUCCESS)); -} - -TEST(GeolibUtils, HasErrorBitSet) { - const ErrorSet error_set{geolib_idealab::ErrorCodes::NO_INTERSECTION_ERR}; - EXPECT_TRUE(GeolibUtils::HasErrorBitSet(error_set, geolib_idealab::ErrorCodes::NO_INTERSECTION_ERR)); -} - -TEST(GeolibUtils, HasErrorBitSet_Combo) { - ErrorSet error_set{geolib_idealab::ErrorCodes::NO_INTERSECTION_ERR}; - error_set |= geolib_idealab::ErrorCodes::ERROR_MAX_REACHED_ERR; - EXPECT_TRUE(GeolibUtils::HasErrorBitSet(error_set, geolib_idealab::ErrorCodes::NO_INTERSECTION_ERR)); - EXPECT_TRUE(GeolibUtils::HasErrorBitSet(error_set, geolib_idealab::ErrorCodes::ERROR_MAX_REACHED_ERR)); -} - -} // namespace open_source -} // namespace test -} // namespace aaesim diff --git a/unittest/src/Public/public.cmake b/unittest/src/Public/public.cmake deleted file mode 100644 index 882db55..0000000 --- a/unittest/src/Public/public.cmake +++ /dev/null @@ -1,40 +0,0 @@ -cmake_minimum_required(VERSION 3.14) - - -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fPIC") - -set(PUBLIC_LIBRARY_TEST_SOURCE - ${UNITTEST_DIR}/src/Public/geolib_tests.cpp - ${UNITTEST_DIR}/src/Public/windstack_tests.cpp - ${UNITTEST_DIR}/src/Public/utility_tests.cpp - ${UNITTEST_DIR}/src/Public/public_tests.cpp - ${UNITTEST_DIR}/src/Public/public_atmosphere_tests.cpp - ${UNITTEST_DIR}/src/Public/tangent_plane_tests.cpp - ${UNITTEST_DIR}/src/Public/wind_blending_tests.cpp - ${UNITTEST_DIR}/src/Public/earth_model_tests.cpp - ${UNITTEST_DIR}/src/Public/threedof_glider_tests.cpp -) - -add_executable(public_test - ${PUBLIC_LIBRARY_TEST_SOURCE} - ${PUBLIC_TEST_SUPPORT_SOURCE} - ${UNITTEST_DIR}/src/main.cpp -) -target_link_libraries(public_test - gtest - pub -) -target_include_directories(public_test PUBLIC - ${aaesim_INCLUDE_DIRS} - ${minicsv_INCLUDE_DIR} - ${nlohmann_json_INCLUDE_DIR} - ${UNITTEST_DIR}/src - ${geolib_idealab_INCLUDE_DIRS}) -set_target_properties(public_test PROPERTIES - RUNTIME_OUTPUT_DIRECTORY ${CMAKE_SOURCE_DIR}/unittest/bin - EXCLUDE_FROM_ALL TRUE) -add_custom_target(run_public_test - ${CMAKE_SOURCE_DIR}/unittest/bin/public_test --gtest_output=xml:public_unit_test_results.xml - DEPENDS ${CMAKE_SOURCE_DIR}/unittest/bin/public_test - WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}/unittest/ -) diff --git a/unittest/src/Public/public_atmosphere_tests.cpp b/unittest/src/Public/public_atmosphere_tests.cpp deleted file mode 100644 index 29ee657..0000000 --- a/unittest/src/Public/public_atmosphere_tests.cpp +++ /dev/null @@ -1,115 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include - -#include - -#include "public/USStandardAtmosphere1976.h" - -using namespace std; -using namespace aaesim::open_source; - -namespace aaesim { -namespace test { - -TEST(USStandardAtmosphere1976, temperature) { - Atmosphere *atm = new USStandardAtmosphere1976(); - // check for discontinuity at tropopause - Units::MetersLength half_eps(1e-6); - Units::KelvinTemperature delta(1e-3); - Units::KelvinTemperature temp_pre_tropo = atm->GetTemperature(atm->GetTropopauseHeight() - half_eps); - Units::KelvinTemperature temp_post_tropo = atm->GetTemperature(atm->GetTropopauseHeight() + half_eps); - EXPECT_NEAR(temp_pre_tropo.value(), temp_post_tropo.value(), delta.value()); - - // check for temperature below absolute zero - Units::KelvinTemperature temp_very_high_alt = atm->GetTemperature(Units::MetersLength(1e8)); - EXPECT_GE(temp_very_high_alt.value(), 0); - EXPECT_EQ(Units::CelsiusTemperature(0), static_cast(atm)->GetTemperatureOffset()); - delete atm; -} - -TEST(USStandardAtmosphere1976, density_low_altitude) { - Atmosphere *atm = new USStandardAtmosphere1976(); - - // below H_TROP - const Units::MetersLength alt(3000.0); - const Units::KilogramsMeterDensity expectedRho(9.0926e-1); // from tabular standard atmosphere - const Units::PascalsPressure expectedPressure(7.0121e4); // from tabular standard atmosphere - Units::KilogramsMeterDensity rho; - Units::PascalsPressure pressure; - atm->AirDensity(alt, rho, pressure); - EXPECT_NEAR(rho.value(), expectedRho.value(), 1e-3); - EXPECT_NEAR(pressure.value(), expectedPressure.value(), 1e2); // tolerance can be large; Pascals are big numbers! - - delete atm; -} - -TEST(USStandardAtmosphere1976, density_above_htrop) { - Atmosphere *atm = new USStandardAtmosphere1976(); - - // above H_TROP - const Units::MetersLength alt(3000.0); - const Units::MetersLength higher_alt = alt + atm->GetTropopauseHeight(); - const Units::KilogramsMeterDensity expectedRho(2.268e-1); // from tabular standard atmosphere - const Units::PascalsPressure expectedPressure(1.4101e4); // from tabular standard atmosphere - Units::KilogramsMeterDensity rho; - Units::PascalsPressure pressure; - atm->AirDensity(higher_alt, rho, pressure); - EXPECT_NEAR(rho.value(), expectedRho.value(), 1e-3); - EXPECT_NEAR(pressure.value(), expectedPressure.value(), 1e2); // tolerance can be large; Pascals are big numbers! - - delete atm; -} - -TEST(USStandardAtmosphere1976, speed) { - Atmosphere *atm = new USStandardAtmosphere1976(); - - const Units::Length alt = Units::MetersLength(3000.0); - Units::Speed cas = Units::MetersPerSecondSpeed(400.0); - Units::Speed tas = atm->CAS2TAS(cas, alt); - EXPECT_NEAR(Units::MetersPerSecondSpeed(tas).value(), 464.32, .02); - Units::Speed cas2 = atm->TAS2CAS(tas, alt); - EXPECT_DOUBLE_EQ(Units::MetersPerSecondSpeed(cas).value(), Units::MetersPerSecondSpeed(cas2).value()); - delete atm; -} - -TEST(USStandardAtmosphere1976, mach_ias_transition_isa) { - // Test mach/ias conversion at altitude using mach of unity. - const std::vector test_machs = {.6, .65, .713, .868}; - const std::vector test_altitudes = {Units::FeetLength(27800.0), Units::FeetLength(30000.0), - Units::FeetLength(35000.0)}; - const Units::FeetLength tolerance(50.0); - - for (double test_mach : test_machs) { - for (Units::FeetLength test_altitude : test_altitudes) { - // zero temp offset - const USStandardAtmosphere1976 atmosphere_0; - Units::Speed ias_at_test_mach = atmosphere_0.MachToIAS(test_mach, test_altitude); - Units::FeetLength actual_alt_trans_sea_level_old = - atmosphere_0.GetMachIASTransition(ias_at_test_mach, test_mach); - Units::FeetLength actual_alt_trans_sea_level = atmosphere_0.GetMachIASTransition(ias_at_test_mach, test_mach); - EXPECT_NEAR(test_altitude.value(), actual_alt_trans_sea_level_old.value(), tolerance.value()); - EXPECT_NEAR(test_altitude.value(), actual_alt_trans_sea_level.value(), tolerance.value()); - } - } -} - -} // namespace test -} // namespace aaesim diff --git a/unittest/src/Public/public_tests.cpp b/unittest/src/Public/public_tests.cpp deleted file mode 100644 index 7b31f9d..0000000 --- a/unittest/src/Public/public_tests.cpp +++ /dev/null @@ -1,1292 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include - -#include -#include -#include -#include - -#include "public/AircraftCalculations.h" -#include "public/AircraftIntent.h" -#include "public/AlongPathDistanceCalculator.h" -#include "public/CoreUtils.h" -#include "public/CustomMath.h" -#include "public/DirectionOfFlightCourseCalculator.h" -#include "public/EuclideanWaypointMonitor.h" -#include "public/FlightEnvelopeSpeedLimiter.h" -#include "public/Guidance.h" -#include "public/HorizontalPathTracker.h" -#include "public/InvalidIndexException.h" -#include "public/PositionCalculator.h" -#include "public/ScenarioUtils.h" -#include "public/SimulationTime.h" -#include "public/VectorDifferenceWindEvaluator.h" -#include "public/Wgs84PrecalcWaypoint.h" -#include "public/WindZero.h" -#include "utility/CustomUnits.h" -#include "utils/public/OldCustomMathUtils.h" -#include "utils/public/PublicUtils.h" - -using namespace std; -using namespace aaesim::test::utils; -using namespace aaesim::open_source; - -namespace aaesim { -namespace test { -namespace open_source { - -class TestHorizontalPathTracker : public HorizontalPathTracker { - // A mock implementation that allows us to get at protected methods - public: - TestHorizontalPathTracker(const std::vector &horizontal_trajectory, - TrajectoryIndexProgressionDirection expected_index_progression) - : HorizontalPathTracker(horizontal_trajectory, expected_index_progression) {} - bool TestIsPositionOnNode(Units::Length position_x, Units::Length position_y) { - std::vector::size_type node_index; - bool is_on_node = IsPositionOnNode(position_x, position_y, node_index); - if (is_on_node) UpdateCurrentIndex(node_index); - return is_on_node; - } - - bool TestIsDistanceAlongPathOnNode(Units::Length distance_along_path) { - std::vector::size_type node_index; - bool is_on_node = IsDistanceAlongPathOnNode(distance_along_path, node_index); - if (is_on_node) UpdateCurrentIndex(node_index); - return is_on_node; - } -}; - -TEST(CoreUtils, interpolate_trivial) { - const int upper_index = 1; - const double value = 0.5; - std::vector x_vals{0.0, 1.0}, y_vals{0.0, 1.0}; - const double y_expected = 0.5; - - // Test - double y_actual = CoreUtils::LinearlyInterpolate(upper_index, value, x_vals, y_vals); - ASSERT_EQ(y_expected, y_actual); -} - -TEST(CoreUtils, interpolate_domain_error) { - const int upper_index = 1; - const double value = -0.5; - std::vector x_vals{0.0, 1.0}, y_vals{0.0, 1.0}; - - // Test - try { - // Expect throw - CoreUtils::LinearlyInterpolate(upper_index, value, x_vals, - y_vals); // value is not on the x_vals vector. Should throw. - FAIL(); // if here, then throw didn't occur. Fail test. - } catch (std::domain_error &e) { - // Test passes. - } -} - -TEST(CoreUtils, interpolate_outofrange_error) { - const int upper_index = -1; - const double value = 0.5; - std::vector x_vals{0.0, 1.0}, y_vals{0.0, 1.0}; - - // Test - try { - // Expect throw - CoreUtils::LinearlyInterpolate(upper_index, value, x_vals, - y_vals); // value is not on the x_vals vector. Should throw. - FAIL(); // if here, then throw didn't occur. Fail test. - } catch (std::out_of_range &e) { - // Test passes. - } -} - -TEST(CoreUtils, SignOfValue) { - const double value = 10.0; - ASSERT_EQ(1.0, CoreUtils::SignOfValue(value)); - ASSERT_EQ(-1.0, CoreUtils::SignOfValue(value * -1.0)); - ASSERT_EQ(0, CoreUtils::SignOfValue(0.0)); -} - -TEST(CoreUtils, LimitOnInterval) { - const double value = 10.0, upper_limit = 15, lower_limit = 5; - ASSERT_EQ(value, CoreUtils::LimitOnInterval(value, lower_limit, upper_limit)); - ASSERT_EQ(upper_limit, CoreUtils::LimitOnInterval(value * 2, lower_limit, upper_limit)); // too big - ASSERT_EQ(lower_limit, CoreUtils::LimitOnInterval(value * -1, lower_limit, upper_limit)); // too small - ASSERT_EQ(upper_limit, - CoreUtils::LimitOnInterval(upper_limit, lower_limit, upper_limit)); // on interval lower limit - ASSERT_EQ(lower_limit, - CoreUtils::LimitOnInterval(lower_limit, lower_limit, upper_limit)); // on interval upper limit -} - -TEST(CoreUtils, IM_Index) { - // Set up a simple test vector - std::vector input_vector = {0.0, 1.0, 2.0, 3.0, 4.0}; - - // Value-to-find greater than max value in vector - double test_value = 4.3; - - int returned_index = CoreUtils::FindNearestIndex(test_value, input_vector); - - EXPECT_DOUBLE_EQ(returned_index, 4); - - // Value-to-find less than min value in vector - test_value = -0.5; - - returned_index = CoreUtils::FindNearestIndex(test_value, input_vector); - - EXPECT_DOUBLE_EQ(returned_index, 0); - - // Value-to-find less than value at given start index - test_value = 1.5; - - returned_index = CoreUtils::FindNearestIndex(test_value, input_vector); - - EXPECT_DOUBLE_EQ(returned_index, 2); - - // Value-to-find greater than value at given start index - test_value = 3.5; - - returned_index = CoreUtils::FindNearestIndex(test_value, input_vector); - - EXPECT_DOUBLE_EQ(returned_index, 4); -} - -TEST(CoreUtils, calculateEuclideanDistance) { - // Calculate from a 3-4-5 triangle - const Units::FeetLength expected = Units::FeetLength(5.0); - const Units::FeetLength l4 = Units::FeetLength(4.0); - const Units::FeetLength l3 = Units::FeetLength(3.0); - const std::pair pair1(l3, Units::ZERO_LENGTH); - const std::pair pair2(Units::ZERO_LENGTH, l4); - const Units::FeetLength actual = CoreUtils::CalculateEuclideanDistance(pair1, pair2); - EXPECT_EQ(expected, actual); -} - -TEST(AircraftCalculations, anglebetweenvectors) { - // zero angle - const Units::SignedRadiansAngle expectedAngle0(0.0), tol(1e-5); - Units::SignedRadiansAngle actual = AircraftCalculations::ComputeAngleBetweenVectors( - Units::ZERO_LENGTH, Units::ZERO_LENGTH, Units::MetersLength(1.0), Units::MetersLength(1.0), - Units::MetersLength(1.0), Units::MetersLength(1.0)); - EXPECT_NEAR(expectedAngle0.value(), actual.value(), tol.value()); - - // positive 45 - const Units::SignedRadiansAngle expectedAngle1 = Units::SignedRadiansAngle(M_PI / 4); - actual = AircraftCalculations::ComputeAngleBetweenVectors( - Units::ZERO_LENGTH, Units::ZERO_LENGTH, Units::MetersLength(1), Units::MetersLength(0), - Units::MetersLength(sqrt(2)), Units::MetersLength(sqrt(2))); - EXPECT_NEAR(expectedAngle1.value(), actual.value(), tol.value()); - - // negative 45 - const Units::SignedRadiansAngle expectedAngle2 = Units::SignedRadiansAngle(M_PI / 4); - actual = AircraftCalculations::ComputeAngleBetweenVectors( - Units::ZERO_LENGTH, Units::ZERO_LENGTH, Units::MetersLength(1), Units::MetersLength(0), - Units::MetersLength(sqrt(2)), Units::MetersLength(-sqrt(2))); - EXPECT_NEAR(expectedAngle2.value(), actual.value(), tol.value()); -} - -TEST(AircraftCalculations, ComputeCrossProduct_trivial) { - const Units::MetersLength x_vertex(0.0), y_vertex(0.0); - const Units::MetersLength vector1_x(1.0), vector1_y(0.0); - const Units::MetersLength vector2_x(0.0), vector2_y(1.0); - const Units::MetersArea expected(1.0); - Units::Area actual = - AircraftCalculations::ComputeCrossProduct(x_vertex, y_vertex, vector1_x, vector1_y, vector2_x, vector2_y); - ASSERT_EQ(expected, actual); -} - -TEST(SimulationTime, basicTests) { - // Tests various SimulationTime functions. - - // NOTE:This not a complete test of SimulationTime. These - // tests setup basically to test the make function. - - SimulationTime simtime; - - // Basic test - - SimulationTime::SetSimulationTimeStep(Units::SecondsTime(1.0)); - - simtime.SetCycle(4); - - EXPECT_DOUBLE_EQ(SimulationTime::GetSimulationTimeStep().value(), 1.0); - EXPECT_EQ(simtime.GetCycle(), 4); - EXPECT_DOUBLE_EQ(simtime.GetCurrentSimulationTime().value(), 4.0); - - // Increment test. - - simtime.Increment(); - - EXPECT_DOUBLE_EQ(simtime.GetCurrentSimulationTime().value(), 5.0); - - // Half time test. - - SimulationTime::SetSimulationTimeStep(Units::SecondsTime(0.5)); - simtime.SetCycle(21); - - EXPECT_DOUBLE_EQ(SimulationTime::GetSimulationTimeStep().value(), 0.5); - EXPECT_EQ(simtime.GetCycle(), 21); - EXPECT_DOUBLE_EQ(simtime.GetCurrentSimulationTime().value(), 10.5); - - // Make test 1 - - Units::SecondsTime maketime(43.5); - - SimulationTime simmake = SimulationTime::Of(maketime); - - EXPECT_DOUBLE_EQ(SimulationTime::GetSimulationTimeStep().value(), 0.5); - EXPECT_EQ(simmake.GetCycle(), 87); - EXPECT_DOUBLE_EQ(simmake.GetCurrentSimulationTime().value(), 43.5); - - // Make test 2-0.2 step. - - SimulationTime::SetSimulationTimeStep(Units::SecondsTime(0.2)); - - maketime = Units::SecondsTime(24.6); - - simmake = SimulationTime::Of(maketime); - - EXPECT_DOUBLE_EQ(SimulationTime::GetSimulationTimeStep().value(), 0.2); - EXPECT_EQ(simmake.GetCycle(), 123); - EXPECT_DOUBLE_EQ(simmake.GetCurrentSimulationTime().value(), 24.6); - SimulationTime::SetSimulationTimeStep(Units::SecondsTime(1.0)); // be kind & reset for future tests -} - -TEST(HorizontalPathTracker, consistency_check_straight_line_reverse) { - /* - * Test the behavior of HorizontalPathTracker. The point - * here is to make sure the internal behavior is self-consistent. - */ - const Units::MetersLength tolXY(100.0), tol_distance(1e-9); - const Units::DegreesAngle tolCrs(0.5); - for (int quad = aaesim::test::utils::Quadrant::FIRST; quad <= aaesim::test::utils::Quadrant::FOURTH; ++quad) { - const std::vector horizontal_trajectory = - aaesim::test::utils::PublicUtils::CreateStraightHorizontalPath( - static_cast(quad)); - PositionCalculator position_calculator(horizontal_trajectory, TrajectoryIndexProgressionDirection::DECREMENTING); - AlongPathDistanceCalculator distance_calculator(horizontal_trajectory, - TrajectoryIndexProgressionDirection::DECREMENTING); - - std::vector::const_reverse_iterator reverse_iterator = horizontal_trajectory.rbegin(); - for (; reverse_iterator != horizontal_trajectory.rend(); ++reverse_iterator) { - Units::MetersLength x1(reverse_iterator->GetXPositionMeters()), y1(reverse_iterator->GetYPositionMeters()), - dist; - Units::UnsignedAngle crs1; - distance_calculator.CalculateAlongPathDistanceFromPosition(x1, y1, dist, crs1); - - // Expect the distance to match the same distance in the horizontal path - // description - EXPECT_NEAR(reverse_iterator->m_path_length_cumulative_meters, Units::MetersLength(dist).value(), - tol_distance.value()); - - Units::MetersLength x2, y2; - Units::UnsignedAngle crs2; - position_calculator.CalculatePositionFromAlongPathDistance(dist, x2, y2, crs2); - - EXPECT_NEAR(x1.value(), x2.value(), tolXY.value()); - EXPECT_NEAR(y1.value(), y2.value(), tolXY.value()); - EXPECT_NEAR(Units::SignedDegreesAngle(normalize(crs1)).value(), - Units::SignedDegreesAngle(normalize(crs2)).value(), tolCrs.value()); - EXPECT_FALSE(position_calculator.IsPassedEndOfRoute()); - EXPECT_FALSE(distance_calculator.IsPassedEndOfRoute()); - } - - // test off end of route - Units::MetersLength x1(0), y1(0); - switch (quad) { - case aaesim::test::utils::Quadrant::FIRST: - x1 = Units::MetersLength(-10); - y1 = Units::MetersLength(-10); - break; - case aaesim::test::utils::Quadrant::SECOND: - x1 = Units::MetersLength(10); - y1 = Units::MetersLength(-10); - break; - case aaesim::test::utils::Quadrant::THIRD: - x1 = Units::MetersLength(10); - y1 = Units::MetersLength(10); - break; - case aaesim::test::utils::Quadrant::FOURTH: - x1 = Units::MetersLength(-10); - y1 = Units::MetersLength(10); - break; - default: - break; - } - Units::MetersLength dist; - Units::UnsignedAngle crs1; - distance_calculator.CalculateAlongPathDistanceFromPosition(x1, y1, dist, crs1); - - Units::MetersLength x2, y2; - Units::UnsignedAngle crs2; - position_calculator.CalculatePositionFromAlongPathDistance(dist, x2, y2, crs2); - - EXPECT_NEAR(x1.value(), x2.value(), tolXY.value()); - EXPECT_NEAR(y1.value(), y2.value(), tolXY.value()); - EXPECT_NEAR(Units::SignedDegreesAngle(normalize(crs1)).value(), - Units::SignedDegreesAngle(normalize(crs2)).value(), tolCrs.value()); - EXPECT_TRUE(position_calculator.IsPassedEndOfRoute()); - EXPECT_TRUE(distance_calculator.IsPassedEndOfRoute()); - } -} - -TEST(HorizontalPathTracker, consistency_check_straight_line_forward) { - /* - * Test the behavior of HorizontalPathTracker. The point - * here is to make sure the internal behavior is self-consistent. - */ - - const std::vector horizontal_trajectory = - aaesim::test::utils::PublicUtils::CreateStraightHorizontalPath(aaesim::test::utils::Quadrant::FIRST); - PositionCalculator position_calculator(horizontal_trajectory, TrajectoryIndexProgressionDirection::INCREMENTING); - AlongPathDistanceCalculator distance_calculator(horizontal_trajectory, - TrajectoryIndexProgressionDirection::INCREMENTING); - - const Units::MetersLength tolXY(100.0), tol_distance(1e-9); - const Units::DegreesAngle tolCrs(0.5); - - // test off end of route - Units::MetersLength x1(-10), y1(-10), dist; - Units::UnsignedAngle crs1; - distance_calculator.CalculateAlongPathDistanceFromPosition(x1, y1, dist, crs1); - - Units::MetersLength x2, y2; - Units::UnsignedAngle crs2; - position_calculator.CalculatePositionFromAlongPathDistance(dist, x2, y2, crs2); - - EXPECT_NEAR(x1.value(), x2.value(), tolXY.value()); - EXPECT_NEAR(y1.value(), y2.value(), tolXY.value()); - EXPECT_NEAR(Units::SignedDegreesAngle(normalize(crs1)).value(), Units::SignedDegreesAngle(normalize(crs2)).value(), - tolCrs.value()); - EXPECT_TRUE(position_calculator.IsPassedEndOfRoute()); - EXPECT_TRUE(distance_calculator.IsPassedEndOfRoute()); - - std::vector::const_iterator iterator = horizontal_trajectory.begin(); - for (; iterator != horizontal_trajectory.end(); ++iterator) { - Units::MetersLength x1(iterator->GetXPositionMeters()), y1(iterator->GetYPositionMeters()), dist; - Units::UnsignedAngle crs1; - distance_calculator.CalculateAlongPathDistanceFromPosition(x1, y1, dist, crs1); - - // Expect the distance to match the same distance in the horizontal path - // description - EXPECT_NEAR(iterator->m_path_length_cumulative_meters, Units::MetersLength(dist).value(), tol_distance.value()); - - Units::MetersLength x2, y2; - Units::UnsignedAngle crs2; - if (dist.value() > iterator->m_path_length_cumulative_meters) { - // The assert above has ensured general accuracy of the calculated - // distance. However, the calculated distance can be slightly larger than - // the cummulative distance. This is allowable in the typical operations - // of these calls. However in the case of this test, we need to drop a - // little accuracy to correctly call - // CalculatePositionFromAlongPathDistance as the next test. - dist = Units::MetersLength(static_cast(Units::MetersLength(dist).value())); - } - position_calculator.CalculatePositionFromAlongPathDistance(dist, x2, y2, crs2); - - EXPECT_NEAR(x1.value(), x2.value(), tolXY.value()); - EXPECT_NEAR(y1.value(), y2.value(), tolXY.value()); - EXPECT_NEAR(Units::SignedDegreesAngle(normalize(crs1)).value(), - Units::SignedDegreesAngle(normalize(crs2)).value(), tolCrs.value()); - EXPECT_FALSE(position_calculator.IsPassedEndOfRoute()); - EXPECT_FALSE(distance_calculator.IsPassedEndOfRoute()); - } -} - -TEST(HorizontalPathTracker, consistency_check_straight_line_nodirection) { - /* - * Test the behavior of HorizontalPathTracker. The point - * here is to make sure the internal behavior is self-consistent. - */ - - const std::vector horizontal_trajectory = - aaesim::test::utils::PublicUtils::CreateStraightHorizontalPath(aaesim::test::utils::Quadrant::FIRST); - PositionCalculator position_calculator(horizontal_trajectory, TrajectoryIndexProgressionDirection::UNDEFINED); - AlongPathDistanceCalculator distance_calculator(horizontal_trajectory, - TrajectoryIndexProgressionDirection::UNDEFINED); - - const Units::MetersLength tolXY(100.0), tol_distance(1e-9); - const Units::DegreesAngle tolCrs(0.5); - - // test off end of route - Units::MetersLength x1(-10), y1(-10), dist; - Units::UnsignedAngle crs1; - distance_calculator.CalculateAlongPathDistanceFromPosition(x1, y1, dist, crs1); - - Units::MetersLength x2, y2; - Units::UnsignedAngle crs2; - position_calculator.CalculatePositionFromAlongPathDistance(dist, x2, y2, crs2); - - EXPECT_NEAR(x1.value(), x2.value(), tolXY.value()); - EXPECT_NEAR(y1.value(), y2.value(), tolXY.value()); - EXPECT_NEAR(Units::SignedDegreesAngle(normalize(crs1)).value(), Units::SignedDegreesAngle(normalize(crs2)).value(), - tolCrs.value()); - EXPECT_TRUE(position_calculator.IsPassedEndOfRoute()); - EXPECT_TRUE(distance_calculator.IsPassedEndOfRoute()); - - // Test at beginning of route - const HorizontalPath front = horizontal_trajectory.front(); - { - x1 = Units::MetersLength(front.GetXPositionMeters()); - y1 = Units::MetersLength(front.GetYPositionMeters()); - distance_calculator.CalculateAlongPathDistanceFromPosition(x1, y1, dist, crs1); - - // Expect the distance to match the same distance in the horizontal path - // description - EXPECT_NEAR(front.m_path_length_cumulative_meters, Units::MetersLength(dist).value(), tol_distance.value()); - - position_calculator.CalculatePositionFromAlongPathDistance(dist, x2, y2, crs2); - - EXPECT_NEAR(x1.value(), x2.value(), tolXY.value()); - EXPECT_NEAR(y1.value(), y2.value(), tolXY.value()); - EXPECT_NEAR(Units::SignedDegreesAngle(normalize(crs1)).value(), - Units::SignedDegreesAngle(normalize(crs2)).value(), tolCrs.value()); - EXPECT_FALSE(position_calculator.IsPassedEndOfRoute()); - EXPECT_FALSE(distance_calculator.IsPassedEndOfRoute()); - } - - // Jump to the end of the route and test - const HorizontalPath back = horizontal_trajectory.back(); - { - x1 = Units::MetersLength(back.GetXPositionMeters()); - y1 = Units::MetersLength(back.GetYPositionMeters()); - distance_calculator.CalculateAlongPathDistanceFromPosition(x1, y1, dist, crs1); - - // Expect the distance to match the same distance in the horizontal path - // description - EXPECT_NEAR(back.m_path_length_cumulative_meters, Units::MetersLength(dist).value(), tol_distance.value()); - - position_calculator.CalculatePositionFromAlongPathDistance(dist, x2, y2, crs2); - - EXPECT_NEAR(x1.value(), x2.value(), tolXY.value()); - EXPECT_NEAR(y1.value(), y2.value(), tolXY.value()); - EXPECT_NEAR(Units::SignedDegreesAngle(normalize(crs1)).value(), - Units::SignedDegreesAngle(normalize(crs2)).value(), tolCrs.value()); - EXPECT_FALSE(position_calculator.IsPassedEndOfRoute()); - EXPECT_FALSE(distance_calculator.IsPassedEndOfRoute()); - } -} - -TEST(AlongPathDistanceCalculator, check_for_throw_when_invalid_call_made_incrementing) { - const std::vector horizontal_trajectory = - aaesim::test::utils::PublicUtils::CreateStraightHorizontalPath(aaesim::test::utils::Quadrant::FIRST); - AlongPathDistanceCalculator distance_calculator(horizontal_trajectory, - TrajectoryIndexProgressionDirection::INCREMENTING); - - // Jump to the end of the route and test. Expect a throw - Units::Length dist; - const HorizontalPath front_node = horizontal_trajectory.front(); - const HorizontalPath test_node_should_throw = horizontal_trajectory.back(); - try { - distance_calculator.CalculateAlongPathDistanceFromPosition(Units::MetersLength(front_node.GetXPositionMeters()), - Units::MetersLength(front_node.GetYPositionMeters()), - dist); // should pass - distance_calculator.CalculateAlongPathDistanceFromPosition( - Units::MetersLength(test_node_should_throw.GetXPositionMeters()), - Units::MetersLength(test_node_should_throw.GetYPositionMeters()), dist); // should throw - FAIL(); // this should NOT be hit - } catch (exception &e) { - // if here, the test has passed - } -} - -TEST(CustomMath, atan3_values) { - EXPECT_DOUBLE_EQ(atan3(5, 5), M_PI * .25); - EXPECT_DOUBLE_EQ(atan3(5, -5), M_PI * .75); - EXPECT_DOUBLE_EQ(atan3(-5, -5), M_PI * 1.25); - EXPECT_DOUBLE_EQ(atan3(-5, 5), M_PI * 1.75); -} - -TEST(CustomMath, quantize) { - Units::Speed s1 = Units::FeetPerSecondSpeed(12); - Units::Speed sq = Units::FeetPerSecondSpeed(5); - Units::Speed s2 = quantize(s1, sq); - double result = Units::FeetPerSecondSpeed(s2).value(); - EXPECT_DOUBLE_EQ(result, 10); - - s1 = Units::FeetPerSecondSpeed(-3); - s2 = quantize(s1, sq); - result = Units::FeetPerSecondSpeed(s2).value(); - EXPECT_DOUBLE_EQ(result, -5); - - Units::Length d1 = Units::NauticalMilesLength(19); - Units::Length dq = Units::NauticalMilesLength(4); - Units::Length d2 = quantize(d1, dq); - result = Units::NauticalMilesLength(d2).value(); - EXPECT_DOUBLE_EQ(result, 20); -} - -TEST(RandomGenerator, uniformSample) { - double seed = 15; - ScenarioUtils::RANDOM_NUMBER_GENERATOR.SetSeed(seed); - - double s1 = 0, s2 = 0, s3 = 0, s4 = 0; - int n = 100000; - for (int i = 0; i < n; i++) { - double x = ScenarioUtils::RANDOM_NUMBER_GENERATOR.UniformSample(); - EXPECT_GE(x, 0); - EXPECT_LE(x, 1); - double x2 = x * x; - s1 += x; - s2 += x2; - s3 += x * x2; - s4 += x2 * x2; - } - double ee = sqrt(1 / (double)n); - double m1 = s1 / n; - EXPECT_NEAR(.5, m1, ee); - double m2 = s2 / n; - EXPECT_NEAR(1.0 / 3.0, m2, ee); - double m3 = s3 / n; - EXPECT_NEAR(0.25, m3, ee); - double m4 = s4 / n; - EXPECT_NEAR(0.2, m4, ee); -} - -TEST(RandomGenerator, uniformConsistencyTest) { - OldCustomMath cm; - - double seed = 54321.0; - ScenarioUtils::RANDOM_NUMBER_GENERATOR.SetSeed(seed); - - bool same = true; - - for (int ix = 0; ix < 1000; ix++) { - double c = cm.uniform(seed); - double r = ScenarioUtils::RANDOM_NUMBER_GENERATOR.UniformSample(); - - same = same && (c == r); - } - - EXPECT_TRUE(same); - EXPECT_DOUBLE_EQ(seed, ScenarioUtils::RANDOM_NUMBER_GENERATOR.GetSeed()); -} - -TEST(RandomGenerator, rayleighConsistencyTest) { - OldCustomMath cm; - - double seed = 54321.0; - ScenarioUtils::RANDOM_NUMBER_GENERATOR.SetSeed(seed); - - bool same = true; - - for (int ix = 0; ix < 1000; ix++) { - double c = cm.Rayleigh(0.0, 12.0, seed); - double r = ScenarioUtils::RANDOM_NUMBER_GENERATOR.RayleighSample(0.0, 12.0); - - same = same && (c == r); - } - - EXPECT_TRUE(same); - EXPECT_DOUBLE_EQ(seed, ScenarioUtils::RANDOM_NUMBER_GENERATOR.GetSeed()); -} - -TEST(RandomGenerator, laplaceConsistencyTest) { - OldCustomMath cm; - - double seed = 54321.0; - ScenarioUtils::RANDOM_NUMBER_GENERATOR.SetSeed(seed); - - bool same = true; - - for (int ix = 0; ix < 1000; ix++) { - double c = cm.laplace(69.0, seed); - double r = ScenarioUtils::RANDOM_NUMBER_GENERATOR.LaplaceSample(69.0); - - same = same && (c == r); - } - - EXPECT_TRUE(same); - EXPECT_DOUBLE_EQ(seed, ScenarioUtils::RANDOM_NUMBER_GENERATOR.GetSeed()); -} - -TEST(RandomGenerator, gaussConsistencyTest) { - OldCustomMath cm; - - double seed = 54321.0; - ScenarioUtils::RANDOM_NUMBER_GENERATOR.SetSeed(seed); - - bool same = true; - - for (int ix = 0; ix < 1000; ix++) { - double c = cm.gauss(0.0, 1.0, seed); - double r = ScenarioUtils::RANDOM_NUMBER_GENERATOR.GaussianSample(0.0, 1.0); - - same = same && (c == r); - } - - EXPECT_TRUE(same); - EXPECT_DOUBLE_EQ(seed, ScenarioUtils::RANDOM_NUMBER_GENERATOR.GetSeed()); -} - -TEST(RandomGenerator, truncateGaussConsistencyTest) { - OldCustomMath cm; - - double seed = 54321.0; - ScenarioUtils::RANDOM_NUMBER_GENERATOR.SetSeed(seed); - - bool same = true; - - for (int ix = 0; ix < 1000; ix++) { - double c = cm.trunc_gauss(0.0, (15.0 / 1.96), 3.0, seed); - double r = ScenarioUtils::RANDOM_NUMBER_GENERATOR.TruncatedGaussianSample(0.0, (15.0 / 1.96), 3.0); - same = same && (c == r); - } - - EXPECT_TRUE(same); - EXPECT_DOUBLE_EQ(seed, ScenarioUtils::RANDOM_NUMBER_GENERATOR.GetSeed()); -} - -TEST(DVector, access) { - DVector v(1000, 1003); - for (int i = v.GetMin(); i <= v.GetMax(); i++) { - v[i] = i; - } - EXPECT_DOUBLE_EQ(v[1002], 1002); - EXPECT_THROW(v[1005], InvalidIndexException); - EXPECT_DOUBLE_EQ(v[1003], 1003); -} - -TEST(DMatrix, multiply) { - double a1[2][3] = {{1, 2, 3}, {4, 5, 6}}; - DMatrix a((double **)&a1, 0, 1, 0, 2); - double b1[2][2] = {{7, 8}, {9, 10}}; - DMatrix b((double **)&b1, 0, 1, 0, 1); - EXPECT_THROW(a * b, DMatrix::IncompatibleDimensionsException); - - DMatrix &ba = b * a; - EXPECT_EQ(ba[0][0], 39); - EXPECT_EQ(ba.GetMaxRow(), 1); - EXPECT_EQ(ba.GetMaxColumn(), 2); - delete &ba; -} - -TEST(AircraftState, extrapolate) { - const auto state_in = AircraftState::Builder(0, 0) - .GroundSpeed(Units::FeetPerSecondSpeed(100), Units::FeetPerSecondSpeed(-100)) - ->AltitudeRate(Units::FeetPerSecondSpeed(100)) - ->Build(); - - AircraftState state_out; - double extrapolate_time = 10.0; - state_out.Extrapolate(state_in, Units::SecondsTime(extrapolate_time)); - if (state_out.GetTime().value() == -1) { - printf("Nothing was extrapolated!"); - FAIL(); - } - - EXPECT_DOUBLE_EQ(state_out.GetTime().value(), extrapolate_time); - EXPECT_DOUBLE_EQ(Units::FeetLength(state_out.GetPositionEnuX()).value(), 1000.0); - EXPECT_DOUBLE_EQ(Units::FeetLength(state_out.GetPositionEnuY()).value(), -1000.0); - EXPECT_DOUBLE_EQ(Units::FeetLength(state_out.GetAltitudeMsl()).value(), 1000.0); -} - -TEST(Units, CustomUnits) { - Units::InvertedLength pm1 = Units::PerMeterInvertedLength(.25); - Units::Length pm2 = Units::MetersLength(1); - double pm = pm1 * pm2; - EXPECT_DOUBLE_EQ(pm, .25); - - Units::InvertedSpeed pk1 = Units::SecondsPerNauticalMileInvertedSpeed(3600); - Units::Speed pk2 = Units::KnotsSpeed(1); - double pk = pk1 * pk2; - EXPECT_DOUBLE_EQ(pk, 1); - - Units::InvertedLengthGain lg1 = Units::SecondsSquaredPerMeterInvertedLengthGain(5.0); - Units::LengthGain lg2 = Units::MetersPerSecondSquaredLengthGain(0.7); - double lg = lg1 * lg2; - EXPECT_DOUBLE_EQ(lg, 3.5); -} - -TEST(AlongPathDistanceCalculator, check_for_throw_when_invalid_call_made_decrementing) { - const std::vector horizontal_trajectory = - aaesim::test::utils::PublicUtils::CreateStraightHorizontalPath(aaesim::test::utils::Quadrant::FIRST); - AlongPathDistanceCalculator distance_calculator(horizontal_trajectory, - TrajectoryIndexProgressionDirection::DECREMENTING); - - // Jump to the end of the route and test. Expect a throw - Units::Length dist; - const HorizontalPath front_node = horizontal_trajectory.back(); - const HorizontalPath test_node_should_throw = horizontal_trajectory.front(); - try { - distance_calculator.CalculateAlongPathDistanceFromPosition(Units::MetersLength(front_node.GetXPositionMeters()), - Units::MetersLength(front_node.GetYPositionMeters()), - dist); // should pass - distance_calculator.CalculateAlongPathDistanceFromPosition( - Units::MetersLength(test_node_should_throw.GetXPositionMeters()), - Units::MetersLength(test_node_should_throw.GetYPositionMeters()), dist); // should throw - FAIL(); // this should NOT be hit - } catch (exception &e) { - // if here, the test has passed - } -} -TEST(PositionCalculator, check_for_throw_when_invalid_call_made) { - const std::vector horizontal_trajectory = - aaesim::test::utils::PublicUtils::CreateStraightHorizontalPath(aaesim::test::utils::Quadrant::FIRST); - PositionCalculator position_calculator(horizontal_trajectory, TrajectoryIndexProgressionDirection::INCREMENTING); - - // Jump to the end of the route and test. Expect a throw - Units::Length x, y; - Units::UnsignedAngle crs; - const HorizontalPath test_node_should_throw = horizontal_trajectory.back(); - try { - position_calculator.CalculatePositionFromAlongPathDistance( - Units::MetersLength(test_node_should_throw.m_path_length_cumulative_meters), x, y, crs); // should throw - FAIL(); // this should NOT be hit - } catch (exception &e) { - // if here, the test has passed - } -} - -TEST(HorizontalPathCourseCalculator, consistency_check_straight_line_nodirection) { - const Units::SignedDegreesAngle known_course(45.0); - const Units::SignedDegreesAngle expected_reciprocal_course(known_course + Units::SignedDegreesAngle(180.0)); - const std::vector horizontal_trajectory = - aaesim::test::utils::PublicUtils::CreateStraightHorizontalPath(aaesim::test::utils::Quadrant::FIRST); - DirectionOfFlightCourseCalculator course_calculator(horizontal_trajectory, - TrajectoryIndexProgressionDirection::UNDEFINED); - - const Units::DegreesAngle tol_crs(0.5); - - // test off end of route - Units::UnsignedAngle actual_crs; - course_calculator.CalculateCourseAtAlongPathDistance(Units::MetersLength(-50.0), actual_crs); - EXPECT_NEAR(expected_reciprocal_course.value(), Units::SignedDegreesAngle(normalize(actual_crs)).value(), - tol_crs.value()); - EXPECT_TRUE(course_calculator.IsPassedEndOfRoute()); - - // Test at beginning of route - const HorizontalPath front = horizontal_trajectory.front(); - { - course_calculator.CalculateCourseAtAlongPathDistance(Units::MetersLength(front.m_path_length_cumulative_meters), - actual_crs); - EXPECT_NEAR(expected_reciprocal_course.value(), Units::SignedDegreesAngle(normalize(actual_crs)).value(), - tol_crs.value()); - EXPECT_FALSE(course_calculator.IsPassedEndOfRoute()); - } - - // Jump to the end of the route and test - const HorizontalPath back = horizontal_trajectory.back(); - { - course_calculator.CalculateCourseAtAlongPathDistance(Units::MetersLength(back.m_path_length_cumulative_meters), - actual_crs); - EXPECT_NEAR(expected_reciprocal_course.value(), Units::SignedDegreesAngle(normalize(actual_crs)).value(), - tol_crs.value()); - EXPECT_FALSE(course_calculator.IsPassedEndOfRoute()); - } -} - -TEST(BackwardCourseCalculator, consistency_check_start_end_course) { - const Units::DegreesAngle tol_crs(0.005); - const Units::SignedDegreesAngle known_course(45.0); - const Units::SignedDegreesAngle expected_reciprocal_course(known_course + Units::SignedDegreesAngle(180.0)); - const std::vector horizontal_trajectory = - aaesim::test::utils::PublicUtils::CreateStraightHorizontalPath(aaesim::test::utils::Quadrant::FIRST); - DirectionOfFlightCourseCalculator course_calculator(horizontal_trajectory, - TrajectoryIndexProgressionDirection::UNDEFINED); - - Units::UnsignedAngle actual_start_course = course_calculator.GetCourseAtPathEnd(); - EXPECT_NEAR(expected_reciprocal_course.value(), Units::SignedDegreesAngle(normalize(actual_start_course)).value(), - tol_crs.value()); - - Units::UnsignedAngle actual_end_course = course_calculator.GetCourseAtPathStart(); - EXPECT_NEAR(expected_reciprocal_course.value(), Units::SignedDegreesAngle(normalize(actual_end_course)).value(), - tol_crs.value()); -} - -TEST(BackwardCourseCalculator, consistency_check_incrementing) { - const Units::DegreesAngle tol_crs(0.005); - const Units::SignedDegreesAngle known_course(45.0); - const Units::SignedDegreesAngle expected_reciprocal_course(known_course + Units::SignedDegreesAngle(180.0)); - const std::vector horizontal_trajectory = - aaesim::test::utils::PublicUtils::CreateStraightHorizontalPath(aaesim::test::utils::Quadrant::FIRST); - DirectionOfFlightCourseCalculator course_calculator(horizontal_trajectory, - TrajectoryIndexProgressionDirection::INCREMENTING); - - Units::UnsignedAngle actual_back_course; - bool actual_return_bool = - course_calculator.CalculateCourseAtAlongPathDistance(Units::MetersLength(-10.), actual_back_course); - EXPECT_TRUE(actual_return_bool); - EXPECT_TRUE(course_calculator.IsPassedEndOfRoute()); - EXPECT_NEAR(expected_reciprocal_course.value(), Units::SignedDegreesAngle(normalize(actual_back_course)).value(), - tol_crs.value()); - - std::vector::const_iterator iterator = horizontal_trajectory.begin(); - for (; iterator != horizontal_trajectory.end(); ++iterator) { - Units::UnsignedAngle actual_back_course; - bool actual_return_bool = course_calculator.CalculateCourseAtAlongPathDistance( - Units::MetersLength(iterator->m_path_length_cumulative_meters), actual_back_course); - EXPECT_TRUE(actual_return_bool); - EXPECT_FALSE(course_calculator.IsPassedEndOfRoute()); - EXPECT_NEAR(expected_reciprocal_course.value(), Units::SignedDegreesAngle(normalize(actual_back_course)).value(), - tol_crs.value()); - } -} - -TEST(BackwardCourseCalculator, course_throws_for_wrong_progression) { - const Units::DegreesAngle tol_crs(0.005); - const Units::SignedDegreesAngle known_course(45.0); - const Units::SignedDegreesAngle expected_reciprocal_course(known_course + Units::SignedDegreesAngle(180.0)); - const std::vector horizontal_trajectory = - aaesim::test::utils::PublicUtils::CreateStraightHorizontalPath(aaesim::test::utils::Quadrant::FIRST); - DirectionOfFlightCourseCalculator course_calculator(horizontal_trajectory, - TrajectoryIndexProgressionDirection::INCREMENTING); - HorizontalPath start_of_path = horizontal_trajectory.back(); - try { - Units::UnsignedAngle actual_back_course; - course_calculator.CalculateCourseAtAlongPathDistance( - Units::MetersLength(start_of_path.m_path_length_cumulative_meters), actual_back_course); - FAIL(); - } catch (logic_error &e) { - // This is correct. Test passed. - } - - course_calculator = - DirectionOfFlightCourseCalculator(horizontal_trajectory, TrajectoryIndexProgressionDirection::DECREMENTING); - try { - Units::UnsignedAngle actual_back_course; - course_calculator.CalculateCourseAtAlongPathDistance(Units::zero(), actual_back_course); - FAIL(); - } catch (logic_error &e) { - // This is correct. Test passed. - } -} - -TEST(BackwardCourseCalculator, consistency_check_decrementing) { - const Units::DegreesAngle tol_crs(0.005); - const Units::SignedDegreesAngle known_course(45.0); - const Units::SignedDegreesAngle expected_reciprocal_course(known_course + Units::SignedDegreesAngle(180.0)); - const std::vector horizontal_trajectory = - aaesim::test::utils::PublicUtils::CreateStraightHorizontalPath(aaesim::test::utils::Quadrant::FIRST); - DirectionOfFlightCourseCalculator course_calculator(horizontal_trajectory, - TrajectoryIndexProgressionDirection::DECREMENTING); - - std::vector::const_reverse_iterator iterator = horizontal_trajectory.crbegin(); - for (; iterator != horizontal_trajectory.crend(); ++iterator) { - Units::UnsignedAngle actual_back_course; - bool actual_return_bool = course_calculator.CalculateCourseAtAlongPathDistance( - Units::MetersLength(iterator->m_path_length_cumulative_meters), actual_back_course); - - EXPECT_TRUE(actual_return_bool); - EXPECT_FALSE(course_calculator.IsPassedEndOfRoute()); - EXPECT_NEAR(expected_reciprocal_course.value(), Units::SignedDegreesAngle(normalize(actual_back_course)).value(), - tol_crs.value()); - } - - Units::UnsignedAngle actual_back_course; - bool actual_return_bool = - course_calculator.CalculateCourseAtAlongPathDistance(Units::MetersLength(-10.0), actual_back_course); - EXPECT_TRUE(actual_return_bool); - EXPECT_TRUE(course_calculator.IsPassedEndOfRoute()); - EXPECT_NEAR(expected_reciprocal_course.value(), Units::SignedDegreesAngle(normalize(actual_back_course)).value(), - tol_crs.value()); -} - -TEST(AlongPathDistanceCalculator, consistency_check_two_public_methods) { - const Units::DegreesAngle tol_crs(0.5); - const Units::SignedDegreesAngle known_course(45.0); - const Units::SignedDegreesAngle expected_reciprocal_course(known_course + Units::SignedDegreesAngle(180.0)); - const std::vector horizontal_trajectory = - aaesim::test::utils::PublicUtils::CreateStraightHorizontalPath(aaesim::test::utils::Quadrant::FIRST); - AlongPathDistanceCalculator distance_calculator(horizontal_trajectory, - TrajectoryIndexProgressionDirection::UNDEFINED); - - // test at end of route - const HorizontalPath horizontal_path_node = horizontal_trajectory.back(); - Units::UnsignedAngle actual_crs; - Units::MetersLength actual_distance_to_go_method_1, actual_distance_to_go_method_2; - distance_calculator.CalculateAlongPathDistanceFromPosition( - Units::MetersLength(horizontal_path_node.GetXPositionMeters()), - Units::MetersLength(horizontal_path_node.GetYPositionMeters()), actual_distance_to_go_method_1, actual_crs); - distance_calculator.CalculateAlongPathDistanceFromPosition( - Units::MetersLength(horizontal_path_node.GetXPositionMeters()), - Units::MetersLength(horizontal_path_node.GetYPositionMeters()), actual_distance_to_go_method_2); - EXPECT_NEAR(horizontal_path_node.m_path_length_cumulative_meters, actual_distance_to_go_method_1.value(), 1e-13); - EXPECT_NEAR(horizontal_path_node.m_path_length_cumulative_meters, actual_distance_to_go_method_2.value(), 1e-13); - EXPECT_NEAR(expected_reciprocal_course.value(), Units::SignedDegreesAngle(normalize(actual_crs)).value(), - tol_crs.value()); -} - -/* Broken because we cannot access bada classes from here -TEST(WindZero, test_for_zero_behavior) { - std::shared_ptr -atm(aaesim::bada::Bada3Factory::MakeAtmosphereFromTemperatureOffset(Atmosphere::AtmosphereType::BADA37, - Units::CelsiusTemperature(0))); - Units::MetersPerSecondSpeed u, v; - WindZero zero_wind(atm); - zero_wind.InterpolateWind(Units::zero(), Units::zero(), Units::zero(), u, v); - EXPECT_EQ(0., u.value()); - EXPECT_EQ(0., v.value()); - - Units::CelsiusTemperature temperature = - zero_wind.InterpolateTemperature(Units::zero(), Units::zero(), Units::zero()); - EXPECT_NEAR(288.15, temperature.value(), 1e-1); -} */ - -TEST(TestHorizontalPathTracker, check_is_on_node_position) { - const std::vector horizontal_trajectory = - aaesim::test::utils::PublicUtils::CreateStraightHorizontalPath(aaesim::test::utils::Quadrant::FIRST); - - // Decrementing - { - TestHorizontalPathTracker tracker(horizontal_trajectory, TrajectoryIndexProgressionDirection::DECREMENTING); - for (std::vector::size_type index = horizontal_trajectory.size() - 1; index > 0; --index) { - const HorizontalPath hp = horizontal_trajectory[index]; - bool actual_return_bool = tracker.TestIsPositionOnNode(Units::MetersLength(hp.GetXPositionMeters()), - Units::MetersLength(hp.GetYPositionMeters())); - std::vector::size_type actual_node_index = tracker.GetCurrentTrajectoryIndex(); - - EXPECT_TRUE(actual_return_bool); - EXPECT_EQ(index, actual_node_index); - } - } - - // Incrementing - { - TestHorizontalPathTracker tracker(horizontal_trajectory, TrajectoryIndexProgressionDirection::INCREMENTING); - for (std::vector::size_type index = 0; index < horizontal_trajectory.size(); ++index) { - const HorizontalPath hp = horizontal_trajectory[index]; - bool actual_return_bool = tracker.TestIsPositionOnNode(Units::MetersLength(hp.GetXPositionMeters()), - Units::MetersLength(hp.GetYPositionMeters())); - std::vector::size_type actual_node_index = tracker.GetCurrentTrajectoryIndex(); - EXPECT_TRUE(actual_return_bool); - EXPECT_EQ(index, actual_node_index); - } - } - - // Undefined - { - TestHorizontalPathTracker tracker(horizontal_trajectory, TrajectoryIndexProgressionDirection::UNDEFINED); - for (std::vector::size_type index = 0; index < horizontal_trajectory.size(); ++index) { - const HorizontalPath hp = horizontal_trajectory[index]; - bool actual_return_bool = tracker.TestIsPositionOnNode(Units::MetersLength(hp.GetXPositionMeters()), - Units::MetersLength(hp.GetYPositionMeters())); - std::vector::size_type actual_node_index = tracker.GetCurrentTrajectoryIndex(); - EXPECT_TRUE(actual_return_bool); - EXPECT_EQ(index, actual_node_index); - } - } -} - -TEST(TestHorizontalPathTracker, check_is_on_node_distance) { - const std::vector horizontal_trajectory = - aaesim::test::utils::PublicUtils::CreateStraightHorizontalPath(aaesim::test::utils::Quadrant::FIRST); - - // Decrementing - { - TestHorizontalPathTracker tracker(horizontal_trajectory, TrajectoryIndexProgressionDirection::DECREMENTING); - for (std::vector::size_type index = horizontal_trajectory.size() - 1; index > 0; --index) { - const HorizontalPath hp = horizontal_trajectory[index]; - bool actual_return_bool = - tracker.TestIsDistanceAlongPathOnNode(Units::MetersLength(hp.m_path_length_cumulative_meters)); - std::vector::size_type actual_node_index = tracker.GetCurrentTrajectoryIndex(); - - EXPECT_TRUE(actual_return_bool); - EXPECT_EQ(index, actual_node_index); - } - } - - // Incrementing - { - TestHorizontalPathTracker tracker(horizontal_trajectory, TrajectoryIndexProgressionDirection::INCREMENTING); - for (std::vector::size_type index = 0; index < horizontal_trajectory.size(); ++index) { - const HorizontalPath hp = horizontal_trajectory[index]; - bool actual_return_bool = - tracker.TestIsDistanceAlongPathOnNode(Units::MetersLength(hp.m_path_length_cumulative_meters)); - std::vector::size_type actual_node_index = tracker.GetCurrentTrajectoryIndex(); - - EXPECT_TRUE(actual_return_bool); - EXPECT_EQ(index, actual_node_index); - } - } - - // Undefined - { - TestHorizontalPathTracker tracker(horizontal_trajectory, TrajectoryIndexProgressionDirection::UNDEFINED); - for (std::vector::size_type index = 0; index < horizontal_trajectory.size(); ++index) { - const HorizontalPath hp = horizontal_trajectory[index]; - bool actual_return_bool = - tracker.TestIsDistanceAlongPathOnNode(Units::MetersLength(hp.m_path_length_cumulative_meters)); - std::vector::size_type actual_node_index = tracker.GetCurrentTrajectoryIndex(); - - EXPECT_TRUE(actual_return_bool); - EXPECT_EQ(index, actual_node_index); - } - } -} - -TEST(AircraftState, GetHeadingCcwFromEastRadians) { - const Units::SecondsTime delta_time{1}; - for (int q = Quadrant::FIRST; q <= Quadrant::FOURTH; ++q) { - vector known_course_path = PublicUtils::CreateStraightHorizontalPath(static_cast(q)); - - const auto state1 = - AircraftState::Builder(0, 0) - .Position(Units::MetersLength(known_course_path[0].GetXPositionMeters()), - Units::MetersLength(known_course_path[0].GetYPositionMeters())) - ->GroundSpeed(Units::MetersLength(known_course_path[1].GetXPositionMeters()) / delta_time, - Units::MetersLength(known_course_path[1].GetYPositionMeters()) / delta_time) - ->Build(); - const auto state2 = AircraftState::Builder(0, delta_time) - .Position(Units::MetersLength(known_course_path[1].GetXPositionMeters()), - Units::MetersLength(known_course_path[1].GetYPositionMeters())) - ->GroundSpeed(state1.GetSpeedEnuX(), state1.GetSpeedEnuY()) - ->Build(); - - AircraftState test_state; - test_state.Interpolate(state1, state2, 0.5); - const Units::SignedRadiansAngle reported_heading = test_state.GetHeadingCcwFromEastRadians(); - EXPECT_NEAR(known_course_path[0].m_path_course, reported_heading.value(), 1e-3); - } -} - -TEST(Guidance, GetIasCommandIntegerKnots) { - Guidance guidance; - - // test rounding up - guidance.m_ias_command = Units::KnotsSpeed(209.9); - EXPECT_EQ(210, guidance.GetIasCommandIntegerKnots()); - - // test rounding down - guidance.m_ias_command = Units::KnotsSpeed(209.1); - EXPECT_EQ(209, guidance.GetIasCommandIntegerKnots()); -} - -/* Broken because we cannot access bada classes from here -class PredictedWindEvaluatorTest : public ::testing::Test { - protected: - PredictedWindEvaluatorTest() - : aircraft_state(), - reference_cas(Units::ZERO_SPEED), - reference_altitude(Units::ZERO_LENGTH), - sensed_atmosphere(aaesim::bada::Bada3Factory::MakeAtmosphereFromTemperatureOffset(Atmosphere::AtmosphereType::BADA37, - Units::CelsiusTemperature(0))), - weather_prediction(Wind::CreateZeroWindPrediction(sensed_atmosphere)) {} - - aaesim::open_source::AircraftState aircraft_state; - Units::Speed reference_cas; - Units::Length reference_altitude; - std::shared_ptr sensed_atmosphere; - WeatherPrediction weather_prediction; -}; - - -TEST_F(PredictedWindEvaluatorTest, VectorDifferenceWindEvaluator) { - const std::shared_ptr vector_difference_wind_evaluator = - VectorDifferenceWindEvaluator::GetInstance(Units::KnotsSpeed(1)); - ASSERT_TRUE(vector_difference_wind_evaluator->ArePredictedWindsAccurate( - aircraft_state, weather_prediction, reference_cas, reference_altitude, sensed_atmosphere.get())); - - aircraft_state.m_Vwx = 1.1; - ASSERT_FALSE(vector_difference_wind_evaluator->ArePredictedWindsAccurate( - aircraft_state, weather_prediction, reference_cas, reference_altitude, sensed_atmosphere.get())); -} -*/ - -TEST(EuclideanWaypointMonitor, passed_waypoint_first_update) { - const Waypoint test_waypoint("monitor_me", Units::DegreesAngle(38), Units::DegreesAngle(-77)); - aaesim::open_source::Wgs84PrecalcWaypoint point_to_monitor; - point_to_monitor.m_position = aaesim::LatitudeLongitudePoint::CreateFromWaypoint(test_waypoint); - std::shared_ptr test_monitor = - aaesim::open_source::EuclideanWaypointMonitor::OfWgs84PrecalcWaypoint(point_to_monitor); - - aaesim::LatitudeLongitudePoint test_point = point_to_monitor.m_position.ProjectDistanceAlongCourse( - Units::MetersLength(100), Units::SignedDegreesAngle(80)); - - // This is the tested method - test_monitor->Update(test_point, Units::SignedDegreesAngle(0)); - - EXPECT_TRUE(test_monitor->IsPassedWaypoint()); -} - -TEST(EuclideanWaypointMonitor, passes_waypoint_second_call) { - const Waypoint test_waypoint("monitor_me", Units::DegreesAngle(38), Units::DegreesAngle(-77)); - aaesim::open_source::Wgs84PrecalcWaypoint point_to_monitor; - point_to_monitor.m_position = aaesim::LatitudeLongitudePoint::CreateFromWaypoint(test_waypoint); - std::shared_ptr test_monitor = - aaesim::open_source::EuclideanWaypointMonitor::OfWgs84PrecalcWaypoint(point_to_monitor); - - const aaesim::LatitudeLongitudePoint test_point1 = point_to_monitor.m_position.ProjectDistanceAlongCourse( - Units::MetersLength(100), Units::SignedDegreesAngle(91)); - test_monitor->Update(test_point1, Units::SignedDegreesAngle(0)); - EXPECT_FALSE(test_monitor->IsPassedWaypoint()); - - const aaesim::LatitudeLongitudePoint test_point2 = point_to_monitor.m_position.ProjectDistanceAlongCourse( - Units::MetersLength(100), Units::SignedDegreesAngle(89)); - test_monitor->Update(test_point2, Units::SignedDegreesAngle(0)); - EXPECT_TRUE(test_monitor->IsPassedWaypoint()); -} - -TEST(FlightEnvelopeSpeedLimiter, limit_speed_command) { - aaesim::open_source::bada_utils::FlightEnvelope flight_envelope; - flight_envelope.V_mo = Units::KnotsSpeed(250); - flight_envelope.M_mo = 1.0; - flight_envelope.h_mo = Units::ZERO_LENGTH; - flight_envelope.h_max = Units::ZERO_LENGTH; - flight_envelope.G_w = Units::zero(); - flight_envelope.G_t = 0.0; - - aaesim::open_source::bada_utils::FlapSpeeds flap_speeds; - flap_speeds.cas_approach_minimum = Units::KnotsSpeed(200); - flap_speeds.cas_approach_maximum = Units::KnotsSpeed(210); - flap_speeds.cas_landing_minimum = Units::KnotsSpeed(200); - flap_speeds.cas_landing_maximum = Units::KnotsSpeed(210); - flap_speeds.cas_gear_out_minimum = Units::KnotsSpeed(200); - flap_speeds.cas_gear_out_maximum = Units::KnotsSpeed(210); - flap_speeds.cas_takeoff_minimum = Units::KnotsSpeed(210); - flap_speeds.cas_climb_minimum = Units::KnotsSpeed(200); - flap_speeds.cas_cruise_minimum = Units::KnotsSpeed(210); - - FlightEnvelopeSpeedLimiter flight_envelope_speed_limiter(flap_speeds, flight_envelope); - Units::Speed limited_speed = flight_envelope_speed_limiter.LimitSpeedCommand( - Units::ZERO_SPEED, flap_speeds.cas_takeoff_minimum - Units::KnotsSpeed(1), Units::ZERO_SPEED, - Units::ZERO_LENGTH, Units::ZERO_LENGTH, Units::ZERO_LENGTH, - aaesim::open_source::bada_utils::FlapConfiguration::TAKEOFF); - ASSERT_EQ(limited_speed, flap_speeds.cas_takeoff_minimum); - - limited_speed = flight_envelope_speed_limiter.LimitSpeedCommand( - Units::ZERO_SPEED, flap_speeds.cas_cruise_minimum - Units::KnotsSpeed(1), Units::ZERO_SPEED, - Units::ZERO_LENGTH, Units::ZERO_LENGTH, Units::ZERO_LENGTH, bada_utils::FlapConfiguration::CRUISE); - ASSERT_EQ(limited_speed, flap_speeds.cas_cruise_minimum); - - limited_speed = flight_envelope_speed_limiter.LimitSpeedCommand( - Units::ZERO_SPEED, flap_speeds.cas_approach_maximum + Units::KnotsSpeed(1), Units::ZERO_SPEED, - Units::ZERO_LENGTH, Units::ZERO_LENGTH, Units::ZERO_LENGTH, bada_utils::FlapConfiguration::APPROACH); - ASSERT_EQ(limited_speed, flap_speeds.cas_approach_maximum); - - limited_speed = flight_envelope_speed_limiter.LimitSpeedCommand( - Units::ZERO_SPEED, FlightEnvelopeSpeedLimiter::MINIMUM_IAS_LIMIT - Units::KnotsSpeed(1), Units::ZERO_SPEED, - Units::ZERO_LENGTH, Units::ZERO_LENGTH, Units::ZERO_LENGTH, bada_utils::FlapConfiguration::UNDEFINED); - ASSERT_EQ(limited_speed, FlightEnvelopeSpeedLimiter::MINIMUM_IAS_LIMIT); -} - -TEST(FlightEnvelopeSpeedLimiter, limit_mach_command) { - aaesim::open_source::bada_utils::FlightEnvelope flight_envelope; - flight_envelope.V_mo = Units::KnotsSpeed(10); - flight_envelope.M_mo = 1.0; - flight_envelope.h_mo = Units::ZERO_LENGTH; - flight_envelope.h_max = Units::ZERO_LENGTH; - flight_envelope.G_w = Units::zero(); - flight_envelope.G_t = 0.0; - - aaesim::open_source::bada_utils::FlapSpeeds flap_speeds; - flap_speeds.cas_approach_minimum = Units::KnotsSpeed(1); - flap_speeds.cas_approach_maximum = Units::KnotsSpeed(10); - flap_speeds.cas_landing_minimum = Units::KnotsSpeed(1); - flap_speeds.cas_landing_maximum = Units::KnotsSpeed(10); - flap_speeds.cas_gear_out_minimum = Units::KnotsSpeed(1); - flap_speeds.cas_gear_out_maximum = Units::KnotsSpeed(10); - flap_speeds.cas_takeoff_minimum = Units::KnotsSpeed(10); - flap_speeds.cas_climb_minimum = Units::KnotsSpeed(1); - flap_speeds.cas_cruise_minimum = Units::KnotsSpeed(10); - - FlightEnvelopeSpeedLimiter flight_envelope_speed_limiter(flap_speeds, flight_envelope); - BoundedValue current_mach(flight_envelope.M_mo + BoundedValue(0.1)); - - BoundedValue limited_mach = flight_envelope_speed_limiter.LimitMachCommand( - BoundedValue(0), current_mach, BoundedValue(0), Units::ZERO_MASS, - Units::ZERO_LENGTH, WeatherPrediction()); - ASSERT_EQ(limited_mach, flight_envelope.M_mo); - - current_mach = FlightEnvelopeSpeedLimiter::MINIMUM_MACH_LIMIT - BoundedValue(0.1); - limited_mach = flight_envelope_speed_limiter.LimitMachCommand(BoundedValue(0), current_mach, - BoundedValue(0), Units::ZERO_MASS, - Units::ZERO_LENGTH, WeatherPrediction()); - ASSERT_EQ(limited_mach, FlightEnvelopeSpeedLimiter::MINIMUM_MACH_LIMIT); - - current_mach = FlightEnvelopeSpeedLimiter::MINIMUM_MACH_LIMIT + BoundedValue(0.1); - limited_mach = flight_envelope_speed_limiter.LimitMachCommand(BoundedValue(0), current_mach, - BoundedValue(0), Units::ZERO_MASS, - Units::ZERO_LENGTH, WeatherPrediction()); - ASSERT_EQ(limited_mach, current_mach); -} - -TEST(Units, TemperatureAdd) { - Units::Temperature celsius1(Units::CelsiusTemperature(1)); - Units::Temperature celsius2(Units::CelsiusTemperature(2)); - Units::Temperature celsius3; - celsius3 = celsius1 + celsius2; - EXPECT_NEAR(3, Units::CelsiusTemperature(celsius3).value(), 1e-6); - - Units::Temperature kelvin1(Units::KelvinTemperature(1)); - Units::Temperature kelvin2(Units::KelvinTemperature(2)); - Units::Temperature kelvin3; - kelvin3 = kelvin1 + kelvin2; - EXPECT_NEAR(3, Units::KelvinTemperature(kelvin3).value(), 1e-6); -} - -TEST(AircraftState, GetTrueAirspeed) { - const Units::MetersPerSecondSpeed five_mps(5); - const auto aircraft_state_x_only = AircraftState::Builder(0, 0).GroundSpeed(five_mps, Units::ZERO_SPEED)->Build(); - const Units::MetersPerSecondSpeed tas_calculated_x_only = aircraft_state_x_only.GetTrueAirspeed(); - ASSERT_NEAR(five_mps.value(), tas_calculated_x_only.value(), 1e-10); - - const auto aircraft_state_y_only = AircraftState::Builder(0, 0).GroundSpeed(Units::ZERO_SPEED, five_mps)->Build(); - const Units::MetersPerSecondSpeed tas_calculated_y_only = aircraft_state_y_only.GetTrueAirspeed(); - ASSERT_NEAR(five_mps.value(), tas_calculated_y_only.value(), 1e-10); -} - -TEST(CustomUnits, ToUnsigned) { - const Units::SignedDegreesAngle quadrant_1{45}; - const Units::DegreesAngle quad1_unsigned_angle = Units::ToUnsigned(quadrant_1); - ASSERT_DOUBLE_EQ(45, quad1_unsigned_angle.value()); - - const Units::SignedDegreesAngle quadrant_2{125}; - const Units::DegreesAngle quad2_unsigned_angle = Units::ToUnsigned(quadrant_2); - ASSERT_DOUBLE_EQ(125, quad2_unsigned_angle.value()); - - const Units::SignedDegreesAngle quadrant_3{-135}; - const Units::DegreesAngle quad3_unsigned_angle = Units::ToUnsigned(quadrant_3); - ASSERT_DOUBLE_EQ(225, quad3_unsigned_angle.value()); - - const Units::SignedDegreesAngle quadrant_3k{-495}; - const Units::UnsignedDegreesAngle quad3k_unsigned_angle = Units::UnsignedDegreesAngle(quadrant_3k); - ASSERT_DOUBLE_EQ(225, quad3k_unsigned_angle.value()); - - const Units::SignedDegreesAngle quadrant_4{-45}; - const Units::DegreesAngle quad4_unsigned_angle = Units::ToUnsigned(quadrant_4); - ASSERT_DOUBLE_EQ(315, quad4_unsigned_angle.value()); -} - -TEST(CustomUnits, ToSigned) { - const Units::UnsignedDegreesAngle quadrant_1{45}; - const Units::SignedDegreesAngle quad1_signed_angle = Units::ToSigned(quadrant_1); - ASSERT_DOUBLE_EQ(45, quad1_signed_angle.value()); - - const Units::UnsignedDegreesAngle quadrant_2{135}; - const Units::SignedDegreesAngle quad2_signed_angle = Units::ToSigned(quadrant_2); - ASSERT_DOUBLE_EQ(135, quad2_signed_angle.value()); - - const Units::UnsignedDegreesAngle quadrant_3{225}; - const Units::SignedDegreesAngle quad3_signed_angle = Units::ToSigned(quadrant_3); - ASSERT_DOUBLE_EQ(-135, quad3_signed_angle.value()); - - const Units::UnsignedDegreesAngle quadrant_3k{585}; - const Units::SignedDegreesAngle quad3k_signed_angle = Units::SignedDegreesAngle(quadrant_3k); - ASSERT_DOUBLE_EQ(-135, quad3k_signed_angle.value()); - - const Units::UnsignedDegreesAngle quadrant_4{315}; - const Units::SignedDegreesAngle quad4_signed_angle = Units::ToSigned(quadrant_4); - ASSERT_DOUBLE_EQ(-45, quad4_signed_angle.value()); -} - -} // namespace open_source -} // namespace test -} // namespace aaesim diff --git a/unittest/src/Public/tangent_plane_tests.cpp b/unittest/src/Public/tangent_plane_tests.cpp deleted file mode 100644 index 8786562..0000000 --- a/unittest/src/Public/tangent_plane_tests.cpp +++ /dev/null @@ -1,131 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include - -#include -#include -#include - -#include "public/AircraftIntent.h" -#include "public/SingleTangentPlaneSequence.h" -#include "public/TangentPlaneSequence.h" -#include "public/Waypoint.h" - -namespace aaesim { -namespace open_source { -namespace test { -static double TIGHT_TOLERANCE_DEGREES{1e-12}; -TEST(TangentPlaneSequence, StraightLineConsistency) { - Waypoint start_waypoint{"start", Units::DegreesAngle(38.0), Units::DegreesAngle(-77.0)}; - Waypoint end_waypoint{"end", Units::DegreesAngle(40.0), Units::DegreesAngle(-70.0)}; - auto waypoints = std::list{start_waypoint, end_waypoint}; - auto tangent_plane_sequence = std::make_unique(waypoints); - - auto comparator = [&tangent_plane_sequence](const Waypoint &waypoint) { - EarthModel::LocalPositionEnu enu_position; - tangent_plane_sequence->ConvertGeodeticToLocal(EarthModel::GeodeticPosition::CreateFromWaypoint(waypoint), - enu_position); - EarthModel::GeodeticPosition computed_waypoint; - tangent_plane_sequence->ConvertLocalToGeodetic(enu_position, computed_waypoint); - EXPECT_NEAR(Units::DegreesAngle(computed_waypoint.latitude).value(), - Units::DegreesAngle(waypoint.GetLatitude()).value(), TIGHT_TOLERANCE_DEGREES); - EXPECT_NEAR(Units::DegreesAngle(computed_waypoint.longitude).value(), - Units::DegreesAngle(waypoint.GetLongitude()).value(), TIGHT_TOLERANCE_DEGREES); - }; - std::for_each(waypoints.begin(), waypoints.end(), comparator); -} - -TEST(TangentPlaneSequence, LineSequenceConsistency) { - Waypoint start_waypoint{"start", Units::DegreesAngle(35.0), Units::DegreesAngle(-77.0)}; - Waypoint wp1{"wp1", Units::DegreesAngle(37.5), Units::DegreesAngle(-76.0)}; - Waypoint wp2{"wp2", Units::DegreesAngle(37.6), Units::DegreesAngle(-71.0)}; - Waypoint end_waypoint{"end", Units::DegreesAngle(40.0), Units::DegreesAngle(-70.0)}; - auto waypoints = std::list{start_waypoint, wp1, wp2, end_waypoint}; - auto tangent_plane_sequence = std::make_unique(waypoints); - - auto comparator = [&tangent_plane_sequence](const Waypoint &waypoint) { - EarthModel::LocalPositionEnu enu_position; - tangent_plane_sequence->ConvertGeodeticToLocal(EarthModel::GeodeticPosition::CreateFromWaypoint(waypoint), - enu_position); - EarthModel::GeodeticPosition computed_waypoint; - tangent_plane_sequence->ConvertLocalToGeodetic(enu_position, computed_waypoint); - EXPECT_NEAR(Units::DegreesAngle(computed_waypoint.latitude).value(), - Units::DegreesAngle(waypoint.GetLatitude()).value(), TIGHT_TOLERANCE_DEGREES); - EXPECT_NEAR(Units::DegreesAngle(computed_waypoint.longitude).value(), - Units::DegreesAngle(waypoint.GetLongitude()).value(), TIGHT_TOLERANCE_DEGREES); - }; - std::for_each(waypoints.begin(), waypoints.end(), comparator); -} - -TEST(TangentPlaneSequence, LineSequenceConsistency2) { - SingleTangentPlaneSequence::ClearStaticMembers(); - Waypoint start_waypoint{"start", Units::DegreesAngle(35.0), Units::DegreesAngle(-77.0)}; - Waypoint wp1{"wp1", Units::DegreesAngle(37.5), Units::DegreesAngle(-76.0)}; - Waypoint wp2{"wp2", Units::DegreesAngle(37.6), Units::DegreesAngle(-71.0)}; - Waypoint end_waypoint{"end", Units::DegreesAngle(40.0), Units::DegreesAngle(-70.0)}; - auto waypoints = std::list{start_waypoint, wp1, wp2, end_waypoint}; - AircraftIntent aircraft_intent; - aircraft_intent.LoadWaypointsFromList(waypoints, std::list(), std::list()); - auto wplist = aircraft_intent.GetWaypointList(); - auto tangent_plane_sequence = std::make_shared(wplist); - auto route_data = aircraft_intent.GetRouteData(); - - struct ZippedData { - Units::MetersLength x; - Units::MetersLength y; - Units::Angle lat; - Units::Angle lon; - static ZippedData Of(Units::MetersLength x, Units::MetersLength y, Units::Angle lat, Units::Angle lon) { - ZippedData zd; - zd.x = x; - zd.y = y; - zd.lat = lat; - zd.lon = lon; - return zd; - }; - }; - std::vector zipped_route; - for (auto idx = 0; idx < route_data.m_high_altitude_constraint.size(); ++idx) { - zipped_route.push_back(ZippedData::Of(route_data.m_x[idx], route_data.m_y[idx], route_data.m_latitude[idx], - route_data.m_longitude[idx])); - } - - auto enu_comparator_high_tolerance = [&tangent_plane_sequence](const ZippedData &zd) { - EarthModel::LocalPositionEnu enu_position_off_path; - enu_position_off_path.x = zd.x + Units::MetersLength(100.0); - enu_position_off_path.y = zd.y - Units::MetersLength(100.0); - EarthModel::GeodeticPosition computed_lat_lon; - tangent_plane_sequence->ConvertLocalToGeodetic(enu_position_off_path, computed_lat_lon); - - EarthModel::LocalPositionEnu recomputed_enu_position; - tangent_plane_sequence->ConvertGeodeticToLocal(computed_lat_lon, recomputed_enu_position); - - static double TOLERANCE_METERS{50}; - EXPECT_NEAR(Units::MetersLength(enu_position_off_path.x).value(), - Units::MetersLength(recomputed_enu_position.x).value(), TOLERANCE_METERS); - EXPECT_NEAR(Units::MetersLength(enu_position_off_path.y).value(), - Units::MetersLength(recomputed_enu_position.y).value(), TOLERANCE_METERS); - }; - std::for_each(zipped_route.begin(), zipped_route.end(), enu_comparator_high_tolerance); -} - -} // namespace test -} // namespace open_source -} // namespace aaesim diff --git a/unittest/src/Public/threedof_glider_tests.cpp b/unittest/src/Public/threedof_glider_tests.cpp deleted file mode 100644 index 14ff496..0000000 --- a/unittest/src/Public/threedof_glider_tests.cpp +++ /dev/null @@ -1,274 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include - -#include -#include -#include - -#include "public/AircraftControl.h" -#include "public/FixedMassAircraftPerformance.h" -#include "public/NullPositionEstimator.h" -#include "public/SimulationTime.h" -#include "public/ThreeDOFDynamics.h" -#include "public/USStandardAtmosphere1976.h" -#include "public/WeatherTruth.h" -#include "public/WindZero.h" -#include "public/ZeroWindTrueWeatherOperator.h" - -using namespace aaesim::open_source; - -namespace aaesim { -namespace test { -namespace { - -class SimulationTimeStepGuard { - public: - explicit SimulationTimeStepGuard(Units::Time time_step) - : original_time_step_(SimulationTime::GetSimulationTimeStep()) { - SimulationTime::SetSimulationTimeStep(time_step); - } - ~SimulationTimeStepGuard() { SimulationTime::SetSimulationTimeStep(original_time_step_); } - - private: - Units::SecondsTime original_time_step_; -}; - -class IdealGliderPerformance final : public FixedMassAircraftPerformance { - public: - void GetDragCoefficients(const Units::Speed &calibrated_airspeed, const Units::Length &altitude_msl, - const bada_utils::FlapConfiguration ¤t_flap_configuration, double &cd0, double &cd2, - double &gear, bada_utils::FlapConfiguration &flap_configuration) const override { - SetZeroDrag(cd0, cd2, gear); - flap_configuration = current_flap_configuration; - } - - void GetDragCoefficientsAndIncrementFlapConfiguration(const Units::Speed &calibrated_airspeed, - const Units::Length &altitude_msl, double &cd0, double &cd2, - double &gear, - bada_utils::FlapConfiguration &updated_flap_setting) override { - SetZeroDrag(cd0, cd2, gear); - updated_flap_setting = bada_utils::FlapConfiguration::CRUISE; - } - - void GetCurrentDragCoefficients(double &cd0, double &cd2, double &gear) const override { - SetZeroDrag(cd0, cd2, gear); - } - - void GetConfigurationForIncreasedDrag(const Units::Speed &calibrated_airspeed, const Units::Length &altitude_msl, - bada_utils::FlapConfiguration &updated_flap_setting) override { - updated_flap_setting = bada_utils::FlapConfiguration::CRUISE; - } - - Units::NewtonsForce GetMaxThrust(const Units::Length &altitude_msl, bada_utils::FlapConfiguration flap_configuration, - bada_utils::EngineThrustMode engine_thrust_mode, - Units::AbsCelsiusTemperature temperature_offset) const override { - return Units::NewtonsForce(0); - } - - void GetCoefficientsForFlapConfiguration(bada_utils::FlapConfiguration flap_configuration, double &cd0, double &cd2, - double &gear) const override { - SetZeroDrag(cd0, cd2, gear); - } - - bada_utils::FlapConfiguration GetFlapConfigurationForState( - const Units::Speed &calibrated_airspeed, const Units::Length &altitude_msl, - const bada_utils::FlapConfiguration ¤t_flap_configuration) const override { - return current_flap_configuration; - } - - Units::Mass GetAircraftMass() const override { return Units::KilogramsMass(50000.0); } - - double GetAircraftMassPercentile() const override { return 0.5; } - - bada_utils::FlapSpeeds GetFlapSpeeds() const override { return bada_utils::FlapSpeeds{}; } - - bada_utils::FlapConfiguration GetCurrentFlapConfiguration() const override { - return bada_utils::FlapConfiguration::CRUISE; - } - - void UpdateMassFraction(BoundedValue mass_fraction) override {} - - bada_utils::AircraftType GetAircraftTypeInformation() const override { return bada_utils::AircraftType{}; } - - bada_utils::Mass GetAircraftMassInformation() const override { return bada_utils::Mass{}; } - - bada_utils::FlightEnvelope GetFlightEnvelopeInformation() const override { return bada_utils::FlightEnvelope{}; } - - bada_utils::Aerodynamics GetAerodynamicsInformation() const override { - bada_utils::Aerodynamics aerodynamics{}; - aerodynamics.S = Units::MetersArea(100.0); - aerodynamics.cruise.V_stall = Units::MetersPerSecondSpeed(1.0); - return aerodynamics; - } - - bada_utils::EngineThrust GetEngineThrustInformation() const override { return bada_utils::EngineThrust{}; } - - bada_utils::FuelFlow GetFuelFlowInformation() const override { return bada_utils::FuelFlow{}; } - - bada_utils::GroundMovement GetGroundMovementInformation() const override { return bada_utils::GroundMovement{}; } - - bada_utils::Procedure GetProcedureInformation(unsigned int index) const override { return bada_utils::Procedure{}; } - - bada_utils::AircraftPerformance GetAircraftPerformanceInformation() const override { - return bada_utils::AircraftPerformance{}; - } - - std::string GetAircraftTypeIdentifier() const override { return "IDEAL_GLIDER"; } - - private: - static void SetZeroDrag(double &cd0, double &cd2, double &gear) { - cd0 = 0.0; - cd2 = 0.0; - gear = 0.0; - } -}; - -std::shared_ptr MakeZeroWindStandardAtmosphereOperator() { - auto atmosphere = std::make_shared(); - auto wind = std::make_shared(atmosphere); - auto weather_truth = std::make_shared(wind, atmosphere, true); - return std::make_shared(weather_truth); -} - -std::shared_ptr MakeNullAircraftControl( - const std::shared_ptr &aircraft_performance) { - auto aircraft_control = AircraftControl::Builder() - .WithCruiseDescentLateralController(std::make_shared()) - .WithCruiseDescentVerticalController(std::make_shared()) - .Build(); - aircraft_control->Initialize(aircraft_performance); - return aircraft_control; -} - -Guidance MakeCruiseDescentGuidance(Units::Speed true_airspeed, Units::Length altitude_msl, - Units::SignedAngle track_enu) { - Guidance guidance; - guidance.m_active_guidance_phase = GuidanceFlightPhase::CRUISE_DESCENT; - guidance.m_ground_speed = true_airspeed; - guidance.m_ias_command = true_airspeed; - guidance.m_reference_altitude = altitude_msl; - guidance.m_enu_track_angle = track_enu; - guidance.m_reference_bank_angle = Units::zero(); - guidance.m_vertical_speed = Units::zero(); - guidance.m_cross_track_error = Units::zero(); - guidance.m_use_cross_track = false; - return guidance; -} - -ThreeDOFDynamics MakeInitializedGlider(const std::shared_ptr &aircraft_performance, - Units::Length initial_altitude_msl, Units::Speed initial_true_airspeed, - Units::SignedAngle initial_track_enu, - const EarthModel::LocalPositionEnu &initial_position_enu) { - ThreeDOFDynamics dynamics; - dynamics.Initialize(SimulationTime::Of(Units::ZERO_TIME), aircraft_performance, - EarthModel::GeodeticPosition::Of(Units::ZERO_ANGLE, Units::ZERO_ANGLE), initial_position_enu, - initial_altitude_msl, initial_true_airspeed, initial_track_enu, 0.5, - std::make_shared(), MakeZeroWindStandardAtmosphereOperator()); - return dynamics; -} - -AircraftState RunUpdates(ThreeDOFDynamics &dynamics, const Guidance &guidance, - const std::shared_ptr &aircraft_control, int update_count) { - AircraftState state; - for (int update_index = 1; update_index <= update_count; ++update_index) { - state = dynamics.Update(42, SimulationTime::Of(Units::SecondsTime(update_index)), guidance, aircraft_control); - } - return state; -} - -} // namespace - -TEST(ThreeDofGliderKinematics, level_eastbound_motion_matches_constant_velocity_solution) { - const SimulationTimeStepGuard time_step_guard(Units::SecondsTime(1.0)); - const auto aircraft_performance = std::make_shared(); - const auto aircraft_control = MakeNullAircraftControl(aircraft_performance); - - const Units::MetersLength initial_x(1200.0); - const Units::MetersLength initial_y(-300.0); - const Units::MetersLength initial_altitude_msl(3000.0); - const Units::MetersPerSecondSpeed initial_true_airspeed(210.0); - const Units::SignedDegreesAngle initial_track_enu(0.0); - const int update_count = 5; - - auto dynamics = - MakeInitializedGlider(aircraft_performance, initial_altitude_msl, initial_true_airspeed, initial_track_enu, - EarthModel::LocalPositionEnu::Of(initial_x, initial_y, Units::zero())); - - const auto state = - RunUpdates(dynamics, MakeCruiseDescentGuidance(initial_true_airspeed, initial_altitude_msl, initial_track_enu), - aircraft_control, update_count); - - const Units::SecondsTime elapsed_time(update_count * SimulationTime::GetSimulationTimeStep().value()); - const Units::MetersLength expected_x = initial_x + initial_true_airspeed * elapsed_time; - - EXPECT_NEAR(expected_x.value(), Units::MetersLength(state.GetPositionEnuX()).value(), 1e-9); - EXPECT_NEAR(initial_y.value(), Units::MetersLength(state.GetPositionEnuY()).value(), 1e-9); - EXPECT_NEAR(initial_altitude_msl.value(), Units::MetersLength(state.GetAltitudeMsl()).value(), 1e-9); - EXPECT_NEAR(initial_true_airspeed.value(), Units::MetersPerSecondSpeed(state.GetTrueAirspeed()).value(), 1e-9); - EXPECT_NEAR(initial_true_airspeed.value(), Units::MetersPerSecondSpeed(state.GetGroundSpeed()).value(), 1e-9); - EXPECT_NEAR(0.0, Units::MetersPerSecondSpeed(state.GetVerticalSpeed()).value(), 1e-12); - EXPECT_NEAR( - 0.0, - Units::MetersSecondAcceleration(dynamics.GetEquationsOfMotionStateDerivative().true_airspeed_deriv).value(), - 1e-12); - EXPECT_NEAR(0.0, - Units::RadiansPerSecondAngularSpeed(dynamics.GetEquationsOfMotionStateDerivative().gamma_deriv).value(), - 1e-12); - EXPECT_NEAR( - 0.0, Units::RadiansPerSecondAngularSpeed(dynamics.GetEquationsOfMotionStateDerivative().heading_deriv).value(), - 1e-12); -} - -TEST(ThreeDofGliderKinematics, level_motion_resolves_three_four_five_heading_components) { - const SimulationTimeStepGuard time_step_guard(Units::SecondsTime(1.0)); - const auto aircraft_performance = std::make_shared(); - const auto aircraft_control = MakeNullAircraftControl(aircraft_performance); - - const Units::MetersLength initial_x(-75.0); - const Units::MetersLength initial_y(40.0); - const Units::MetersLength initial_altitude_msl(1800.0); - const Units::MetersPerSecondSpeed initial_true_airspeed(250.0); - const Units::SignedRadiansAngle initial_track_enu(std::atan2(4.0, 3.0)); - const int update_count = 4; - - auto dynamics = - MakeInitializedGlider(aircraft_performance, initial_altitude_msl, initial_true_airspeed, initial_track_enu, - EarthModel::LocalPositionEnu::Of(initial_x, initial_y, Units::zero())); - - const auto state = - RunUpdates(dynamics, MakeCruiseDescentGuidance(initial_true_airspeed, initial_altitude_msl, initial_track_enu), - aircraft_control, update_count); - - const Units::SecondsTime elapsed_time(update_count * SimulationTime::GetSimulationTimeStep().value()); - const Units::MetersLength expected_x = initial_x + initial_true_airspeed * elapsed_time * 3.0 / 5.0; - const Units::MetersLength expected_y = initial_y + initial_true_airspeed * elapsed_time * 4.0 / 5.0; - - EXPECT_NEAR(expected_x.value(), Units::MetersLength(state.GetPositionEnuX()).value(), 1e-9); - EXPECT_NEAR(expected_y.value(), Units::MetersLength(state.GetPositionEnuY()).value(), 1e-9); - EXPECT_NEAR(initial_altitude_msl.value(), Units::MetersLength(state.GetAltitudeMsl()).value(), 1e-9); - EXPECT_NEAR(initial_true_airspeed.value(), Units::MetersPerSecondSpeed(state.GetTrueAirspeed()).value(), 1e-9); - EXPECT_NEAR(initial_true_airspeed.value(), Units::MetersPerSecondSpeed(state.GetGroundSpeed()).value(), 1e-9); - EXPECT_NEAR(Units::RadiansAngle(initial_track_enu).value(), - Units::RadiansAngle(state.GetHeadingCcwFromEastRadians()).value(), 1e-12); -} - -} // namespace test -} // namespace aaesim diff --git a/unittest/src/Public/utility_tests.cpp b/unittest/src/Public/utility_tests.cpp deleted file mode 100644 index c0333be..0000000 --- a/unittest/src/Public/utility_tests.cpp +++ /dev/null @@ -1,53 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include - -#include "utility/BoundedValue.h" -#include "utility/UtilityTemplates.h" - -TEST(BoundedValue, doubles) { - const double not_zero = -0.5; - BoundedValue bounded_value_1(not_zero); - EXPECT_DOUBLE_EQ(not_zero, bounded_value_1); // test that double values are equal with no cast operation - - try { - bounded_value_1 = 10; // out-of-bounds: throw a run-time exception - FAIL(); - } catch (const BoundedValueException &e) { - // std::cerr << e.what() << '\n'; - } - - const double updated_value(1); - try { - BoundedValue bounded_value_2(updated_value); - bounded_value_1 = bounded_value_2; - EXPECT_DOUBLE_EQ(updated_value, bounded_value_1); - } catch (const BoundedValueException &e) { - FAIL(); - } -} - -TEST(Utility_Functions, sgn) { - EXPECT_DOUBLE_EQ(sgn(1), 1); // positive - EXPECT_DOUBLE_EQ(sgn(-1), -1); // negative - EXPECT_DOUBLE_EQ(sgn(0.5), 1); // positive - EXPECT_DOUBLE_EQ(sgn(-0.5), -1); // negative - EXPECT_DOUBLE_EQ(sgn(0), 0); // zero -} diff --git a/unittest/src/Public/wind_blending_tests.cpp b/unittest/src/Public/wind_blending_tests.cpp deleted file mode 100644 index c2ff8f8..0000000 --- a/unittest/src/Public/wind_blending_tests.cpp +++ /dev/null @@ -1,134 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include - -#include "public/BlendWindsVerticallyByAltitude.h" -#include "public/WeatherPrediction.h" - -using namespace std; -using namespace aaesim::open_source; - -namespace aaesim::test { - -TEST(BlendWindsVerticallyByAltitude, update_predicted_winds_at_altitude_from_sensed_wind) { - // Get expected values - const int predicted_matrix_rows = 4; - const Units::Speed predicted_wind_at_20k_x(Units::KnotsSpeed(20)); - const Units::Speed predicted_wind_at_10k_x(Units::KnotsSpeed(10)); - const Units::Speed predicted_wind_at_5k_x = predicted_wind_at_10k_x; // make 5k and 10k match - const Units::Speed predicted_wind_at_0k_x(Units::KnotsSpeed(5)); - - // Set up test objects that will have an obvious expected output - const Units::Speed Vwx(Units::KnotsSpeed(20)); - const Units::Speed Vwy = Vwx; - const auto test_state = - AircraftState::Builder(0, 0).AltitudeMsl(Units::FeetLength(7500))->SensedWindComponents(Vwx, Vwy)->Build(); - - WindStack predicted_wind_x(1, predicted_matrix_rows); // set up bounds just like in main simulation - predicted_wind_x.Insert(1, Units::FeetLength(20000), predicted_wind_at_20k_x); - predicted_wind_x.Insert(2, Units::FeetLength(10000), predicted_wind_at_10k_x); - predicted_wind_x.Insert(3, Units::FeetLength(5000), predicted_wind_at_5k_x); - predicted_wind_x.Insert(4, Units::FeetLength(0), predicted_wind_at_0k_x); - predicted_wind_x.SortAltitudesAscending(); - - WeatherPrediction predicted_wind; - predicted_wind.east_west() = predicted_wind_x; - predicted_wind.north_south() = predicted_wind_x; // copy - - // Test - aaesim::open_source::BlendWindsVerticallyByAltitude wind_blender{}; - wind_blender.BlendSensedWithPredicted(test_state, predicted_wind); - - // Assert - // Expecting an update to occur between rows 1 & 2. Row 3 values should be unaffected, but it will be shifted to - // row 4. - EXPECT_EQ(predicted_matrix_rows + 1, predicted_wind.east_west().GetMaxRow()); // There should be one new row - EXPECT_EQ(Units::FeetLength(test_state.GetAltitudeMsl()).value(), - predicted_wind.east_west().GetAltitude(3).value()); // test that new altitude is in correct location - EXPECT_EQ( - Units::KnotsSpeed(Vwx).value(), - predicted_wind.east_west().GetSpeed(3).value()); // test that sensed Vwx is in correct location & same value - EXPECT_EQ( - Units::KnotsSpeed(Vwy).value(), - predicted_wind.north_south().GetSpeed(3).value()); // test that sensed Vwy is in correct location & same value - EXPECT_EQ(Units::KnotsSpeed(predicted_wind_at_10k_x).value() * 1.5, - predicted_wind.east_west().GetSpeed(4).value()); // test that blending occurred correctly at the next - // altitude band up - EXPECT_EQ(Units::KnotsSpeed(predicted_wind_at_10k_x).value() * 1.5, - predicted_wind.north_south().GetSpeed(4).value()); // test that blending occurred correctly at the next - // altitude band up - EXPECT_EQ(Units::KnotsSpeed(predicted_wind_at_5k_x).value() * 1.5, - predicted_wind.east_west().GetSpeed(2).value()); // test that blending occurred correctly at the next - // altitude band up - EXPECT_EQ(Units::KnotsSpeed(predicted_wind_at_5k_x).value() * 1.5, - predicted_wind.north_south().GetSpeed(2).value()); // test that blending occurred correctly at the next - // altitude band up - EXPECT_EQ(Units::KnotsSpeed(predicted_wind_at_20k_x).value(), - predicted_wind.east_west().GetSpeed(predicted_matrix_rows + 1).value()); // test that blending DID NOT - // occur at the 20k row - EXPECT_EQ(Units::KnotsSpeed(predicted_wind_at_20k_x).value(), - predicted_wind.north_south().GetSpeed(predicted_matrix_rows + 1).value()); // test that blending DID NOT - // occur at the 20k row - EXPECT_EQ(Units::KnotsSpeed(predicted_wind_at_0k_x).value(), - predicted_wind.east_west().GetSpeed(1).value()); // test that blending DID NOT occur at the 0k row - EXPECT_EQ(Units::KnotsSpeed(predicted_wind_at_0k_x).value(), - predicted_wind.north_south().GetSpeed(1).value()); // test that blending DID NOT occur at the 0k row -} - -TEST(BlendWindsVerticallyByAltitude, update_predicted_winds_at_higher_altitude) { - // Set expected values - const int predicted_matrix_rows = 4; - const Units::Speed predicted_wind_at_20k_x(Units::KnotsSpeed(20)); - const Units::Speed predicted_wind_at_10k_x(Units::KnotsSpeed(10)); - const Units::Speed predicted_wind_at_5k_x = predicted_wind_at_10k_x; // make 5k and 10k match - const Units::Speed predicted_wind_at_0k_x(Units::KnotsSpeed(5)); - - // Set up test objects that will have an obvious expected output - const Units::Speed Vwx(Units::KnotsSpeed(20)); - const Units::Speed Vwy = Vwx; - const auto test_state = - AircraftState::Builder(0, 0).AltitudeMsl(Units::FeetLength(25000))->SensedWindComponents(Vwx, Vwy)->Build(); - - WindStack predicted_wind_x(1, predicted_matrix_rows); // set up bounds just like in main simulation - predicted_wind_x.Insert(1, Units::FeetLength(20000), predicted_wind_at_20k_x); - predicted_wind_x.Insert(2, Units::FeetLength(10000), predicted_wind_at_10k_x); - predicted_wind_x.Insert(3, Units::FeetLength(5000), predicted_wind_at_5k_x); - predicted_wind_x.Insert(4, Units::FeetLength(0), predicted_wind_at_0k_x); - predicted_wind_x.SortAltitudesAscending(); - - WeatherPrediction predicted_wind; - predicted_wind.east_west() = predicted_wind_x; - predicted_wind.north_south() = predicted_wind_x; // copy - - // Test - aaesim::open_source::BlendWindsVerticallyByAltitude wind_blender{}; - wind_blender.BlendSensedWithPredicted(test_state, predicted_wind); - - // Assert - // Expecting an update to occur at max_row - EXPECT_EQ(predicted_matrix_rows + 1, predicted_wind.east_west().GetMaxRow()); // There should be one new row - EXPECT_EQ( - Units::FeetLength(test_state.GetAltitudeMsl()).value(), - predicted_wind.east_west().GetAltitude(predicted_wind.east_west().GetMaxRow()).value()); // test that new - // altitude is in - // correct location -} - -} // namespace aaesim::test diff --git a/unittest/src/Public/windstack_tests.cpp b/unittest/src/Public/windstack_tests.cpp deleted file mode 100644 index 6ca87d4..0000000 --- a/unittest/src/Public/windstack_tests.cpp +++ /dev/null @@ -1,215 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include - -#include -#include -#include -#include - -#include "public/WindStack.h" -#include "utility/CustomUnits.h" - -using namespace aaesim::open_source; - -namespace aaesim { -namespace test { -namespace open_source { -TEST(WindStack, operatorEqEq) { - // 1.empty vs empty - WindStack ws0; - WindStack ws1; - EXPECT_EQ(ws0, ws1); - - // 2.empty vs something - WindStack ws2; - ws2.SetBounds(0, 4); - for (auto ix = 0; ix <= 4; ix++) { - ws2.Insert(ix, Units::MetersLength((double)ix * 20.0), Units::MetersPerSecondSpeed((double)(ix - 2) * 3 + 175.0)); - } - if (ws0 == ws2) { - std::string err = "WindStack::operator== empty vs something fails, expected false."; - FAIL(); - } - - // 3.something vs same something - WindStack ws3; - ws3.SetBounds(0, 4); - for (auto ix = 0; ix <= 4; ix++) { - ws3.Insert(ix, Units::MetersLength((double)ix * 20.0), Units::MetersPerSecondSpeed((double)(ix - 2) * 3 + 175.0)); - } - if (!(ws2 == ws3)) { - std::string err = "WindStack::operator== something vs same something fails, expected true."; - FAIL(); - } - - // 4.something vs. something-different low row. - WindStack ws4; - ws4.SetBounds(1, 4); - for (auto ix = 1; ix <= 4; ix++) { - ws4.Insert(ix, Units::MetersLength((double)ix * 20.0), Units::MetersPerSecondSpeed((double)(ix - 2) * 3 + 175.0)); - } - if (ws2 == ws4) { - std::string err = "WindStack::operator== something vs something different low row fails, expected false."; - FAIL(); - } - - // 5.something vs. something-different high row. - WindStack ws5; - ws5.SetBounds(0, 5); - for (auto ix = 0; ix <= 5; ix++) { - ws5.Insert(ix, Units::MetersLength((double)ix * 20.0), Units::MetersPerSecondSpeed((double)(ix - 2) * 3 + 175.0)); - } - if (ws2 == ws5) { - std::string err = "WindStack::operator== something vs something different high row fails, expected false."; - FAIL(); - } - - // 6.something vs something with 1 different altitude. - WindStack ws6; - ws6.SetBounds(0, 4); - for (auto ix = 0; ix <= 4; ix++) { - if (ix != 2) { - ws6.Insert(ix, Units::MetersLength((double)ix * 20.0), - Units::MetersPerSecondSpeed((double)(ix - 2) * 3 + 175.0)); - } else { - ws6.Insert(ix, Units::MetersLength(-15.0), Units::MetersPerSecondSpeed((double)(ix - 2) * 3 + 175.0)); - } - } - if (ws2 == ws6) { - std::string err = "WindStack::operator== something vs something with 1 different altitude fails, expected false."; - FAIL(); - } - - // 7.something vs something with 1 different speed. - WindStack ws7; - ws7.SetBounds(0, 4); - for (auto ix = 0; ix <= 4; ix++) { - if (ix != 4) { - ws7.Insert(ix, Units::MetersLength((double)ix * 20.0), - Units::MetersPerSecondSpeed((double)(ix - 2) * 3 + 175.0)); - } else { - ws7.Insert(ix, Units::MetersLength((double)ix * 20.0), Units::MetersPerSecondSpeed(-93.0)); - } - } - if (ws2 == ws7) { - std::string err = "WindStack::operator== something vs something with 1 different speed fails, expected false."; - FAIL(); - } -} - -TEST(WindStack, operatorNotEq) { - // 1.something vs. equals something - WindStack ws0; - ws0.SetBounds(0, 4); - WindStack ws1; - ws1.SetBounds(0, 4); - for (auto ix = 0; ix <= 4; ix++) { - ws0.Insert(ix, Units::MetersLength((double)ix * 20.0), Units::MetersPerSecondSpeed((double)(ix - 2) * 3 + 175.0)); - ws1.Insert(ix, Units::MetersLength((double)ix * 20.0), Units::MetersPerSecondSpeed((double)(ix - 2) * 3 + 175.0)); - } - - if (ws0 != ws1) { - std::string err = "WindStack::operator!= something vs equals something fails, expected false."; - FAIL(); - } - - // 2.something vs. not equals something - WindStack ws2; - ws2.SetBounds(0, 4); - for (auto ix = 0; ix <= 4; ix++) { - if (ix != 3) { - ws2.Insert(ix, Units::MetersLength((double)ix * 20.0), - Units::MetersPerSecondSpeed((double)(ix - 2) * 3 + 175.0)); - } else { - ws2.Insert(ix, Units::MetersLength(-((double)ix * 20.0)), - Units::MetersPerSecondSpeed((double)(ix - 2) * 3 + 175.0)); - } - } - if (!(ws0 != ws2)) { - std::string err = "WindStack::operator!= something vs not equals something fails, expected true."; - FAIL(); - } -} - -TEST(WindStack, sort_basic) { - WindStack unsorted_stack(0, 2); - unsorted_stack.Insert(0, Units::FeetLength(10), Units::KnotsSpeed(20)); - unsorted_stack.Insert(1, Units::ZERO_LENGTH, Units::ZERO_SPEED); - unsorted_stack.Insert(2, Units::FeetLength(5), Units::ZERO_SPEED); - - WindStack sorted_stack = unsorted_stack; - sorted_stack.SortAltitudesAscending(); - - EXPECT_FALSE(unsorted_stack == sorted_stack); - EXPECT_EQ(sorted_stack.GetAltitude(0), Units::ZERO_LENGTH); - EXPECT_EQ(sorted_stack.GetSpeed(0), Units::ZERO_SPEED); - EXPECT_EQ(sorted_stack.GetAltitude(1), Units::FeetLength(5)); - EXPECT_EQ(sorted_stack.GetSpeed(1), Units::ZERO_SPEED); - EXPECT_EQ(sorted_stack.GetAltitude(2), Units::FeetLength(10)); - EXPECT_EQ(sorted_stack.GetSpeed(2), Units::KnotsSpeed(20)); -} - -TEST(WindStack, sort_shifted_min_max) { - WindStack unsorted_stack(2, 4); - unsorted_stack.Insert(2, Units::FeetLength(10), Units::KnotsSpeed(20)); - unsorted_stack.Insert(3, Units::ZERO_LENGTH, Units::ZERO_SPEED); - unsorted_stack.Insert(4, Units::FeetLength(5), Units::ZERO_SPEED); - - WindStack sorted_stack = unsorted_stack; - sorted_stack.SortAltitudesAscending(); - - EXPECT_FALSE(unsorted_stack == sorted_stack); - EXPECT_EQ(sorted_stack.GetAltitude(2), Units::ZERO_LENGTH); - EXPECT_EQ(sorted_stack.GetSpeed(2), Units::ZERO_SPEED); - EXPECT_EQ(sorted_stack.GetAltitude(3), Units::FeetLength(5)); - EXPECT_EQ(sorted_stack.GetSpeed(3), Units::ZERO_SPEED); - EXPECT_EQ(sorted_stack.GetAltitude(4), Units::FeetLength(10)); - EXPECT_EQ(sorted_stack.GetSpeed(4), Units::KnotsSpeed(20)); -} - -TEST(WindStack, AscendSort) { - std::vector > test_values; - test_values.push_back(std::make_pair(Units::MetersLength(100), Units::KnotsSpeed(5))); - test_values.push_back(std::make_pair(Units::MetersLength(0), Units::KnotsSpeed(5))); - test_values.push_back(std::make_pair(Units::MetersLength(10), Units::KnotsSpeed(5))); - test_values.push_back(std::make_pair(Units::MetersLength(200), Units::KnotsSpeed(5))); - test_values.push_back(std::make_pair(Units::MetersLength(20), Units::KnotsSpeed(5))); - - WindStack test_stack(0, test_values.size() - 1); - auto index = 0; - for (auto test_value : test_values) { - test_stack.Insert(index, test_value.first, test_value.second); - ++index; - } - - // This is the tested method - test_stack.SortAltitudesAscending(); - - EXPECT_EQ(Units::MetersLength(test_stack.GetAltitude(0)).value(), Units::MetersLength(0).value()); - EXPECT_EQ(Units::MetersLength(test_stack.GetAltitude(1)).value(), Units::MetersLength(10).value()); - EXPECT_EQ(Units::MetersLength(test_stack.GetAltitude(2)).value(), Units::MetersLength(20).value()); - EXPECT_EQ(Units::MetersLength(test_stack.GetAltitude(3)).value(), Units::MetersLength(100).value()); - EXPECT_EQ(Units::MetersLength(test_stack.GetAltitude(4)).value(), Units::MetersLength(200).value()); -} - -} // namespace open_source -} // namespace test -} // namespace aaesim diff --git a/unittest/src/utils/public/OldCustomMathUtils.cpp b/unittest/src/utils/public/OldCustomMathUtils.cpp deleted file mode 100644 index 737117c..0000000 --- a/unittest/src/utils/public/OldCustomMathUtils.cpp +++ /dev/null @@ -1,409 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -/* OldCustomMath.cpp Initial code from Survsim 2.00R1 11/2/99*/ - -#include "OldCustomMathUtils.h" - -#include -#include -#include - -#include "utility/UtilityConstants.h" - -#define IA 16807 -#define IM 2147483647 -#define AM (1.0 / IM) -#define IQ 127773 -#define IR 2836 - -using namespace aaesim::test::utils; -using namespace aaesim::open_source::constants; - -log4cplus::Logger OldCustomMath::logger = log4cplus::Logger::getInstance(LOG4CPLUS_TEXT("OldCustomMath")); - -// generate a uniform random number between 0 and 1 -// From "Numerical Recipe" - -double OldCustomMath::uniform(double &seed) // seed should not be 0.0 -{ - // TODO: consider replacing this method with the C++11 equivalent - double ans; - const double seed0 = seed; - - long k = (long)(seed / (double)IQ); - seed = (double)IA * (seed - (double)(k * IQ)) - (double)(IR * k); - if (seed < 0) { - seed += (double)IM; - } - ans = (double)AM * seed; - - LOG4CPLUS_TRACE(OldCustomMath::logger, seed0 << "," << seed << "," << ans); - - return ans; -} - -double OldCustomMath::gauss(double mean, double sigma, double &seed) { - // returns gaussian distributed random variable with mean mean and standard - // deviation sigma. - // usage example: x = gauss(0., 32.); - - const double seed0 = seed; - double u1, u2, eln, ang, v1; - - u1 = uniform(seed); - u2 = uniform(seed); - eln = -2.0 * log(u1); // ALOG(U1) - ang = 2.0 * PI * u2; - v1 = sqrt(eln) * cos(ang); - v1 = mean + sigma * v1; - - LOG4CPLUS_TRACE(OldCustomMath::logger, mean << "," << sigma << "," << seed0 << "," << seed << "," << v1); - - return (v1); - -} // gauss - -double OldCustomMath::trunc_gauss(double mean, double sigma, double max_std_dev, double &seed) { - // returns truncated gaussian random varialbe with mean mean, standard - // deviation sigma, and maximum standard deviation max_std_dev - const double seed0 = seed; - double val; - - val = mean + sigma * max_std_dev + 1.; - // gwang 09/05/2002 - // while (val > (mean + sigma * max_std_dev) ) - while (val > (mean + sigma * max_std_dev) || val < (mean - sigma * max_std_dev)) { - // end gwang 09/05/2002 - val = gauss(mean, sigma, seed); - } - - LOG4CPLUS_TRACE(OldCustomMath::logger, - mean << "," << sigma << "," << max_std_dev << "," << seed0 << "," << seed << "," << val); - - return (val); - -} // trunc_gauss - -double OldCustomMath::Rayleigh(double mean, double sigma, double &seed) { - // returns Rayleigh distributed random variable with mean mean and standard - // deviation sigma. - // usage example: x = Rayleigh(0., 32.); - const double seed0 = seed; - - double u1, v1; - - u1 = uniform(seed); - v1 = (sqrt(-2.0 * log(u1)) - 1.253) / sqrt(0.429); - v1 = mean + sigma * v1; - - LOG4CPLUS_TRACE(OldCustomMath::logger, mean << "," << sigma << "," << seed0 << "," << seed << "," << v1); - - return (v1); - -} // Rayleigh - -double OldCustomMath::atan3(double x, double y) { - // returns arc tangent as an angle measured from north in the range 0, 2pi - - double temp; - - temp = (double)atan2(x, y); - - if (temp < 0.0) { - temp = temp + 2.0 * PI; - } - - return (temp); - -} // atan3 - -double OldCustomMath::laplace(double lambda, double &seed) { - // returns laplacian r.v. with parameter lambda. - const double seed0 = seed; - - double uni, err; - - uni = uniform(seed); - err = -lambda * log(uni); - uni = uniform(seed); - - if (uni < 0.5) { - err = -err; - } - - LOG4CPLUS_TRACE(OldCustomMath::logger, lambda << "," << seed0 << "," << seed << "," << err); - - return (err); - -} // laplace - -double OldCustomMath::quantize(double value, double lsb) { - // quantizes value to lsb (least significant bit) - - int j; - - if (value > 0.) { - j = (int)(0.5 + (double)(value / lsb)); - } else { - j = (int)((double)(value / lsb) - 0.5); - } - - return (lsb * (double)j); - -} // quantize - -Units::Length OldCustomMath::quantize(Units::Length value, Units::Length lsb) { - // quantizes value to lsb (least significant bit) - double r = value / lsb; - if (r > 0.) { - r += .5; - } else { - r -= .5; - } - - return (lsb * (long)r); -} - -Units::Speed OldCustomMath::quantize(Units::Speed value, Units::Speed lsb) { - // quantizes value to lsb (least significant bit) - double r = value / lsb; - if (r > 0.) { - r += .5; - } else { - r -= .5; - } - - return (lsb * (long)r); -} - -bool OldCustomMath::hit(double probability, double &seed) { - if (uniform(seed) < probability) { - return true; - } else { - return false; - } -} - -double OldCustomMath::subtract_headings(double hd1, double hd2) { - // subtract heading 2 from heading 1 with the following convention: - // negative (counterclockwise) deltas are indicated by being greater than pi. - // positive (clockwise) deltas are less than pi. - - double t; - - t = hd1 - hd2; - - if (t < 0.) { - t = TWO_PI + t; - } - - return (t); - -} // subtract_headings - -//------------------------------------------------------------- -// Speed conversion using MACH & altitude as inputs; unit of output is FPS -//------------------------------------------------------------- -double OldCustomMath::MachToTas(double mach, double altitude) { - float speedOfSound; - double tas; - - if (0 <= altitude && altitude <= 36000) { - speedOfSound = 662.4 - 243.0 * altitude / 100000.0; - } else if (36000 < altitude && altitude <= 82000) { - speedOfSound = 573.8; - } else if (82000 < altitude && altitude <= 99900) { - speedOfSound = 120 * altitude / 100000. + 475.4; - } else { - printf("Unexpected altitude in MachToTas: %f\n", altitude); - exit(1); - } - - tas = (mach * speedOfSound); - - // before this point tas is in knots - // gwang 2009-03 - tas *= KNOTS_TO_FEET_PER_SECOND; - // end gwang - - return (tas); // FPS -} - -// output CAS in FPS -double OldCustomMath::MachToCas_MITRE(double mach, double alt) { - double cas, thetas, deltam; - - if (alt < 36089.24) { - thetas = (1.0 - 6.8755856E-6 * alt); - deltam = pow(thetas, 5.2558797); - } else { - thetas = 0.7519; - deltam = 0.2233609 * pow(2.718, (-((alt - 36089.24) / 20806.0))); - } - cas = 661.4786 * - sqrt(5.0 * ((pow((1.0 + deltam * ((pow((1.0 + 0.2 * mach * mach), 3.5) - 1.0))), (2.0 / 7.0))) - 1.0)); - - cas *= KNOTS_TO_FEET_PER_SECOND; - - return (cas); - -} /* MachToCas_MITRE */ - -// inverse = inverse(in) -/* Gauss-Jordan elimination from Numerical recipe:*/ -bool OldCustomMath::inverse(DMatrix &in, int n, DMatrix &out) { - int irow = -1, icol = -1; - - DVector indxc(1, n); - DVector indxr(1, n); - DVector ipiv(1, n); - DMatrix a(1, n, 1, n); - - // copy the "in" matrix into the "a" matrix: - int in_min_row = in.GetMinRow(); - int in_min_column = in.GetMinColumn(); - for (int i = 1; i <= n; i++) { - for (int j = 1; j <= n; j++) { - a.Set(i, j, in.Get(i - 1 + in_min_row, j - 1 + in_min_column)); - } - } - - for (int j = 1; j <= n; j++) { - ipiv.Set(j, 0.); - } - - for (int i = 1; i <= n; i++) { - double big = 0.0; - for (int j = 1; j <= n; j++) { - if (ipiv.Get(j) != 1.) { - for (int k = 1; k <= n; k++) { - if (ipiv.Get(k) == 0.0) { - if (fabs(a.Get(j, k)) >= big) { - big = fabs(a.Get(j, k)); - irow = j; - icol = k; - } - } else if (ipiv.Get(k) > 1.) { - // singular matrix - printf("\nWarning: Inversion of a singular matrix in the inverse() function (> 1 val).\n"); - return false; - } - } // end for(int k=1; k<=n; k++) - } // end if(ipiv.get(j) != 1.) - } // end for(int j=1; i<=n; j++) - ipiv.Set(icol, ipiv.Get(icol) + 1); - if (irow != icol) { - // swap - for (int l = 1; l <= n; l++) { - double temp_swap; - temp_swap = a.Get(irow, l); - a.Set(irow, l, a.Get(icol, l)); - a.Set(icol, l, temp_swap); - } // end for(int l=1; l<=n; l++) - } // end if(irow != icol) - indxr.Set(i, (double)irow); - indxc.Set(i, (double)icol); - if (a.Get(icol, icol) == 0.0) { - // singular matrix - printf("\nWarning: Inversion of a singular matrix in the inverse() function (0 val).\n"); - return false; - } - double pivinv = 1.0 / a.Get(icol, icol); - a.Set(icol, icol, 1.); - for (int l = 1; l <= n; l++) { - a.Set(icol, l, pivinv * a.Get(icol, l)); - } // end for(int l=1; l<=n; l++) - - for (int ll = 1; ll <= n; ll++) { - if (ll != icol) { - double dum = a.Get(ll, icol); - a.Set(ll, icol, 0.); - for (int l = 1; l <= n; l++) { - a.Set(ll, l, a.Get(ll, l) - dum * a.Get(icol, l)); - } // end for(int l=1; l<=n; l++) - } // end if(ll != icol) - } // end for(int ll=1; ll<=n; ll++) - } // end for(int i=1; i<=n; i++) - - for (int l = n; l >= 1; l--) { - if (indxr.Get(l) != indxc.Get(l)) { - for (int k = 1; k <= n; k++) { - // swap: - double temp; - temp = a.Get(k, (int)indxr.Get(l)); - a.Set(k, (int)indxr.Get(l), a.Get(k, (int)indxc.Get(l))); - a.Set(k, (int)indxc.Get(l), temp); - } - } // end if(indxr.get(l) != indxc.get(l)) - } // end for(int l=n; l>=1; l--) - - // copy the "a" matrix into the "out" matrix: - int out_min_row = out.GetMinRow(); - int out_min_column = out.GetMinColumn(); - for (int i = 1; i <= n; i++) { - for (int j = 1; j <= n; j++) { - out.Set(i - 1 + out_min_row, j - 1 + out_min_column, a.Get(i, j)); - } - } - return true; -} - -void OldCustomMath::matrix_times_vector(DMatrix &matrix_in, DVector &vector_in, int n, DVector &vector_out) { - for (int i = 0; i < n; i++) { - int ii = i + vector_out.GetMin(); - vector_out[ii] = 0.0; - for (int j = 0; j < n; j++) { - vector_out[ii] += - matrix_in[i + matrix_in.GetMinRow()][j + matrix_in.GetMinColumn()] * vector_in[j + vector_in.GetMin()]; - } - } -} - -/** - * Create a matrix which executes a 3-D rotation of a - * point around a vector when a single-row - * matrix [x y z] is post-multiplied by the rotation - * matrix. - */ -DMatrix &OldCustomMath::createRotationMatrix(double l, double m, double n, const Units::Angle theta) { - // basic formula acquired from: - // https://en.wikipedia.org/wiki/Transformation_matrix#Rotation_2 - // Wikipedia uses T * coord_column, while we use coord_row * T. - // Therefore, we must transpose the matrix. - - // we need a unit vector - double mag2 = l * l + m * m + n * n; - if (mag2 != 1) { - double mag = sqrt(mag2); - l /= mag; - m /= mag; - n /= mag; - } - - double cosT = cos(theta); - double sinT = sin(theta); - double cosT1 = 1 - cosT; - - double a[3][3] = {{l * l * cosT1 + cosT, m * l * cosT1 + n * sinT, n * l * cosT1 - m * sinT}, - {l * m * cosT1 - n * sinT, m * m * cosT1 + cosT, n * m * cosT1 + l * sinT}, - {l * n * cosT1 + m * sinT, m * n * cosT1 - l * sinT, n * n * cosT1 + cosT}}; - DMatrix *result = new DMatrix((double **)&a, 0, 2, 0, 2); - return *result; -} diff --git a/unittest/src/utils/public/OldCustomMathUtils.h b/unittest/src/utils/public/OldCustomMathUtils.h deleted file mode 100644 index 2f8f331..0000000 --- a/unittest/src/utils/public/OldCustomMathUtils.h +++ /dev/null @@ -1,85 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -/* OldCustomMath.h Initial code from Survsim 2.00R1 11/2/99*/ - -#pragma once - -#include -#include -#include -#include -#include - -#include "public/DMatrix.h" -#include "public/DVector.h" - -/** - * This contains the version of Math/CustomMath before Math/RandomGenerator - * was implemented. The sampling functions that were in CustomMath are - * replaced by similar sampling code in RandomGenerator and removed from - * CustomMath. The CustomMath functions are all wrapped inside the class - * here on 19 Nov 15. The sampling methods saved here are used in some - * of the tests. - **/ -namespace aaesim { -namespace test { -namespace utils { -class OldCustomMath { - public: - static log4cplus::Logger logger; - - double atan3(double x, - double y); // arc tangent from 0 - 2pi - double uniform(double &seed); // seed should not be 0.0 - double gauss(double mean, double sigma, double &seed); - - double trunc_gauss(double mean, double sigma, double max_std_dev, double &seed); - - double Rayleigh(double mean, double sigma, double &seed); - - Units::Time Rayleigh(Units::Time mean, Units::Time sigma, double &seed); - - double laplace(double lambda, - double &seed); // returns laplacian rv given lambda - double quantize(double value, - double lsb); // quantizes value to lsb - Units::Length quantize(Units::Length value, Units::Length lsb); - - Units::Speed quantize(Units::Speed value, Units::Speed lsb); - - bool hit(double probability, double &seed); - - double MachToCas_MITRE(double mach, double alt); - - double subtract_headings(double hd1, double hd2); - - double MachToTas(double mach, double altitude); - - // for stereographic convertion: - - bool inverse(DMatrix &in, int n, DMatrix &inverse); - - void matrix_times_vector(DMatrix &matrix_in, DVector &vector_in, int n, DVector &vector_out); - - DMatrix &createRotationMatrix(double l, double m, double n, const Units::Angle theta); -}; -} // namespace utils -} // namespace test -} // namespace aaesim diff --git a/unittest/src/utils/public/PublicUtils.cpp b/unittest/src/utils/public/PublicUtils.cpp deleted file mode 100644 index c214ac1..0000000 --- a/unittest/src/utils/public/PublicUtils.cpp +++ /dev/null @@ -1,76 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include "utils/public/PublicUtils.h" - -#include - -using namespace std; -using namespace aaesim::open_source; - -std::vector aaesim::test::utils::PublicUtils::CreateStraightHorizontalPath(Quadrant quadrant) { - static const Units::MetersLength unity = Units::MetersLength(1); - double course_radians = INFINITY; - Units::MetersLength x_sign(0), y_sign(0); - - switch (quadrant) { - case Quadrant::FIRST: - x_sign = unity; - y_sign = unity; - course_radians = M_PI / 4; - break; - case Quadrant ::SECOND: - x_sign = -unity; - y_sign = unity; - course_radians = 3 * M_PI / 4; - break; - case Quadrant ::THIRD: - x_sign = -unity; - y_sign = -unity; - course_radians = -3 * M_PI / 4; - break; - case Quadrant ::FOURTH: - x_sign = unity; - y_sign = -unity; - course_radians = -M_PI / 4; - break; - default: - break; - } - - vector horizontal_traj; - HorizontalPath hp0, hp1, hp2; - hp0.m_segment_type = HorizontalPath::SegmentType::STRAIGHT; - hp0.SetXYPositionMeters(0 * unity.value(), 0 * unity.value()); - hp0.m_path_length_cumulative_meters = 0; - hp0.m_path_course = course_radians; - horizontal_traj.push_back(hp0); - hp1.m_segment_type = HorizontalPath::SegmentType::STRAIGHT; - hp1.SetXYPositionMeters(1 * unity.value() * x_sign.value(), 1 * unity.value() * y_sign.value()); - hp1.m_path_length_cumulative_meters = hp0.m_path_length_cumulative_meters + sqrt(2); - hp1.m_path_course = course_radians; - horizontal_traj.push_back(hp1); - hp2.m_segment_type = HorizontalPath::SegmentType::STRAIGHT; - hp2.SetXYPositionMeters(2 * unity.value() * x_sign.value(), 2 * unity.value() * y_sign.value()); - hp2.m_path_length_cumulative_meters = hp1.m_path_length_cumulative_meters + sqrt(2); - hp2.m_path_course = course_radians; - horizontal_traj.push_back(hp2); - - return horizontal_traj; -} diff --git a/unittest/src/utils/public/PublicUtils.h b/unittest/src/utils/public/PublicUtils.h deleted file mode 100644 index 7712244..0000000 --- a/unittest/src/utils/public/PublicUtils.h +++ /dev/null @@ -1,45 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy of these views. -// -// For further information, please contact The MITRE Corporation, Contracts Management -// Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -// -// (c) 2026 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include -#include - -#include "public/AircraftIntent.h" -#include "public/HorizontalPath.h" - -namespace aaesim { -namespace test { -namespace utils { -static const double TOLERANCE_RADIANS = 1e-15; -static const double TOLERANCE_METERS = 1.0; -static const double TOLERANCE_METERS_TIGHT = 1e-8; - -enum Quadrant { FIRST, SECOND, THIRD, FOURTH }; - -class PublicUtils { - public: - static std::vector CreateStraightHorizontalPath(Quadrant quadrant); - static AircraftIntent LoadAircraftIntent(std::string parmsfile); - static AircraftIntent PrepareAircraftIntent(std::string parmsfile); -}; -} // namespace utils -} // namespace test -} // namespace aaesim diff --git a/unittest/unittest.cmake b/unittest/unittest.cmake index 224fcbb..00d43b4 100644 --- a/unittest/unittest.cmake +++ b/unittest/unittest.cmake @@ -4,12 +4,6 @@ # the /unittest library as possible. add_subdirectory(${UNITTEST_DIR}) -# include MITRE-open source test code projects -set(PUBLIC_TEST_SUPPORT_SOURCE - ${UNITTEST_DIR}/src/utils/public/OldCustomMathUtils.cpp - ${UNITTEST_DIR}/src/utils/public/PublicUtils.cpp -) -include(${UNITTEST_DIR}/src/Public/public.cmake) include(${UNITTEST_DIR}/src/AircraftDynamicsTestFramework/framework.cmake) # add a target for running all of the unit test binaries at one time @@ -18,5 +12,4 @@ add_custom_target(run_tests ) add_dependencies(run_tests run_fmacm_test - run_public_test -) \ No newline at end of file +) From 2908e671e4e31a56765cc515e3a859c832a82986 Mon Sep 17 00:00:00 2001 From: Stuart Bowman Date: Fri, 26 Jun 2026 16:12:48 -0400 Subject: [PATCH 3/8] chore: now include fsloader dep --- AircraftDynamicsTestFramework/CMakeLists.txt | 2 ++ CMakeLists.txt | 9 +++++++++ 2 files changed, 11 insertions(+) diff --git a/AircraftDynamicsTestFramework/CMakeLists.txt b/AircraftDynamicsTestFramework/CMakeLists.txt index 075f77d..b6827ee 100644 --- a/AircraftDynamicsTestFramework/CMakeLists.txt +++ b/AircraftDynamicsTestFramework/CMakeLists.txt @@ -49,8 +49,10 @@ target_include_directories(framework PUBLIC ${aaesim_INCLUDE_DIRS}) target_link_libraries(framework PUBLIC + mitre::fsloader ${BADA_LIBRARY} ${SAMPLE_ALGORITHM_LIBRARY} + log4cplus::log4cplus mitre::oss::simcore) if (DEFINED BADA_LIBRARY) # Add a compile definition to the build diff --git a/CMakeLists.txt b/CMakeLists.txt index f6198b0..bc24603 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -54,6 +54,15 @@ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fno-strict-aliasing") set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -Wall -Wno-unused-function -Wno-sign-compare -O0 -g") set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -Wall -Wno-unused-function -Wno-sign-compare -O3 -g3") +CPMAddPackage( + NAME fsloader + GIT_REPOSITORY https://github.com/mitre/fsloader.git + GIT_TAG 1.1.0 + OPTIONS + "FSLOADER_BUILD_EXAMPLE OFF" + "BUILD_SHARED_LIBS FALSE" +) + CPMAddPackage( NAME aircraft_simulation_core GITHUB_REPOSITORY mitre/aircraft_simulation_core From d137ef2e529a97b5ba829862f388b624d045d8b2 Mon Sep 17 00:00:00 2001 From: Stuart Bowman Date: Fri, 26 Jun 2026 16:22:17 -0400 Subject: [PATCH 4/8] chore: fixed stale cmake targets --- .github/workflows/ci.yml | 6 +++--- docs/README.md | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index c49ad1d..97565ac 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -52,14 +52,14 @@ jobs: run: | cmake -S. -Bbuild -DCMAKE_BUILD_TYPE=Release - - name: Run public tests + - name: Run tests run: | - cmake --build build --target run_public_test run_fmacm_test -j + cmake --build build --target run_tests -j - name: Upload test results uses: actions/upload-artifact@v4 with: - name: public-test-results + name: test-results path: unittest/*results.xml # - name: Publish JUnit test results diff --git a/docs/README.md b/docs/README.md index 6e81c2c..617b52b 100644 --- a/docs/README.md +++ b/docs/README.md @@ -57,7 +57,7 @@ Unit tests can be run via the CMake infrastructure. Assuming the software already compiles: ```bash -cmake --build build --target run_public_test run_fmacm_test +cmake --build build --target run_tests ``` ### Run a Simulation From b1cfaefed347688b74b98feda7807f2086448f88 Mon Sep 17 00:00:00 2001 From: Stuart Bowman Date: Tue, 30 Jun 2026 08:28:19 -0400 Subject: [PATCH 5/8] chore: update to newer constants include & namespace --- .../TestFrameworkApplication.cpp | 4 +- .../TestFrameworkFMS.cpp | 33 +-- .../TrajectoryFromFile.cpp | 25 +- unittest/src/utils/OldCustomMathUtils.cpp | 219 +++++++----------- 4 files changed, 124 insertions(+), 157 deletions(-) diff --git a/AircraftDynamicsTestFramework/TestFrameworkApplication.cpp b/AircraftDynamicsTestFramework/TestFrameworkApplication.cpp index c65de01..74e6e03 100644 --- a/AircraftDynamicsTestFramework/TestFrameworkApplication.cpp +++ b/AircraftDynamicsTestFramework/TestFrameworkApplication.cpp @@ -18,7 +18,9 @@ // **************************************************************************** #include "framework/TestFrameworkApplication.h" -#include "utility/constants.h" + +#include + #include "public/AircraftCalculations.h" using namespace std; diff --git a/AircraftDynamicsTestFramework/TestFrameworkFMS.cpp b/AircraftDynamicsTestFramework/TestFrameworkFMS.cpp index 70387cb..dd95b4e 100644 --- a/AircraftDynamicsTestFramework/TestFrameworkFMS.cpp +++ b/AircraftDynamicsTestFramework/TestFrameworkFMS.cpp @@ -17,11 +17,18 @@ // 2022 The MITRE Corporation. All Rights Reserved. // **************************************************************************** -#include -#include #include "framework/TestFrameworkFMS.h" -#include "math/CustomMath.h" + +#include +#include + +#include "public/CoreUtils.h" +#include "public/AlongPathDistanceCalculator.h" #include "public/AircraftCalculations.h" +#include "math/CustomMath.h" // FIXME Stuart why is thsi here? +#include "utility/UtilityConstants.h" + +namespace constants = aaesim::open_source::constants; Units::DegreesAngle TestFrameworkFMS::MAX_BANK_ANGLE(25.0); @@ -113,7 +120,7 @@ void TestFrameworkFMS::Update(const aaesim::open_source::AircraftState &state, if (bank_angle_rad != 0.00) { double gs = Units::FeetPerSecondSpeed(state.GetGroundSpeed()).value(); - m_turn_radius = gs * gs / (GRAVITY_METERS_PER_SECOND / FEET_TO_METERS * tan(bank_angle_rad)); + m_turn_radius = gs * gs / (constants::GRAVITY_FEET_PER_SECOND * tan(bank_angle_rad)); m_range_start_turn = fabs(m_turn_radius * tan(0.5 * m_delta_track)); } else { @@ -151,29 +158,29 @@ void TestFrameworkFMS::Update(const aaesim::open_source::AircraftState &state, double track_error = course - desired_course; - if (track_error > M_PI) { - while (track_error > M_PI) { - track_error = track_error - 2.0 * M_PI; + if (track_error > constants::PI) { + while (track_error > constants::PI) { + track_error = track_error - constants::TWO_PI; } } - if (track_error < -M_PI) { - while (track_error < -M_PI) { - track_error = track_error + 2.0 * M_PI; + if (track_error < -constants::PI) { + while (track_error < -constants::PI) { + track_error = track_error + constants::TWO_PI; } } if (m_mode == TURNING) { double DeltaGroundTrack = -1.0 * track_error; - if (cross_track_error < 0.00 && DeltaGroundTrack < 10.00 * DEGREES_TO_RADIAN) { + if (cross_track_error < 0.00 && DeltaGroundTrack < 10.00 * constants::DEGREES_TO_RADIAN) { m_mode = TRACKING; } - if (cross_track_error > 0.00 && DeltaGroundTrack > 10.00 * DEGREES_TO_RADIAN) { + if (cross_track_error > 0.00 && DeltaGroundTrack > 10.00 * constants::DEGREES_TO_RADIAN) { m_mode = TRACKING; } - if (fabs(cross_track_error) < 1000.0 && fabs(track_error) < 5.0 * DEGREES_TO_RADIAN) { + if (fabs(cross_track_error) < 1000.0 && fabs(track_error) < 5.0 * constants::DEGREES_TO_RADIAN) { m_mode = TRACKING; } } diff --git a/AircraftDynamicsTestFramework/TrajectoryFromFile.cpp b/AircraftDynamicsTestFramework/TrajectoryFromFile.cpp index 608c6ee..7706520 100644 --- a/AircraftDynamicsTestFramework/TrajectoryFromFile.cpp +++ b/AircraftDynamicsTestFramework/TrajectoryFromFile.cpp @@ -18,12 +18,18 @@ // **************************************************************************** #include "framework/TrajectoryFromFile.h" + +#include +#include + +#include "scalar/AngularSpeed.h" #include "public/AircraftCalculations.h" #include "public/CoreUtils.h" #include "framework/HfpReader2020.h" #include "utility/CsvParser.h" +#include "utility/UtilityConstants.h" -#include +namespace constants = aaesim::open_source::constants; TrajectoryFromFile::TrajectoryFromFile() : m_vertical_data(), @@ -107,12 +113,15 @@ aaesim::open_source::Guidance TrajectoryFromFile::Update(const aaesim::open_sour result.m_enu_track_angle = course_at_position; - double unsigned_cross_track_meters = sqrt(pow(state.m_x * FEET_TO_METERS - estimated_position_on_path_x.value(), 2) + - pow(state.m_y * FEET_TO_METERS - estimated_position_on_path_y.value(), 2)); + const double state_x_meters = state.m_x * constants::FEET_TO_METERS; + const double state_y_meters = state.m_y * constants::FEET_TO_METERS; + + double unsigned_cross_track_meters = sqrt(pow(state_x_meters - estimated_position_on_path_x.value(), 2) + + pow(state_y_meters - estimated_position_on_path_y.value(), 2)); double center_dist_meters = - sqrt(pow(state.m_x * FEET_TO_METERS - m_horizontal_trajectory[traj_index].m_turn_info.x_position_meters, 2) + - pow(state.m_y * FEET_TO_METERS - m_horizontal_trajectory[traj_index].m_turn_info.y_position_meters, 2)); + sqrt(pow(state_x_meters - m_horizontal_trajectory[traj_index].m_turn_info.x_position_meters, 2) + + pow(state_y_meters - m_horizontal_trajectory[traj_index].m_turn_info.y_position_meters, 2)); if (m_horizontal_trajectory[traj_index].m_segment_type == HorizontalPath::SegmentType::TURN) { Units::FeetLength distance_to_waypoint = @@ -162,10 +171,8 @@ aaesim::open_source::Guidance TrajectoryFromFile::Update(const aaesim::open_sour } } else { result.m_cross_track_error = Units::MetersLength( - -(state.m_y * FEET_TO_METERS - m_horizontal_trajectory[traj_index].GetYPositionMeters()) * - cos(estimated_course) + - (state.m_x * FEET_TO_METERS - m_horizontal_trajectory[traj_index].GetXPositionMeters()) * - sin(estimated_course)); + -(state_y_meters - m_horizontal_trajectory[traj_index].GetYPositionMeters()) * cos(estimated_course) + + (state_x_meters - m_horizontal_trajectory[traj_index].GetXPositionMeters()) * sin(estimated_course)); } result.m_use_cross_track = true; diff --git a/unittest/src/utils/OldCustomMathUtils.cpp b/unittest/src/utils/OldCustomMathUtils.cpp index c02d1de..5aeb674 100644 --- a/unittest/src/utils/OldCustomMathUtils.cpp +++ b/unittest/src/utils/OldCustomMathUtils.cpp @@ -10,7 +10,7 @@ // under that Clause is authorized without the express written // permission of The MITRE Corporation. For further information, please // contact The MITRE Corporation, Contracts Office, 7515 Colshire Drive, -// McLean, VA 22102-7539, (703) 983-6000. +// McLean, VA 22102-7539, (703) 983-6000. // // Copyright 2019 The MITRE Corporation. All Rights Reserved. // **************************************************************************** @@ -18,48 +18,46 @@ /* OldCustomMath.cpp Initial code from Survsim 2.00R1 11/2/99*/ #include "OldCustomMathUtils.h" -#include #include +#include -#include "utility/constants.h" -//uniform +#include "utility/UtilityConstants.h" + +// uniform #define IA 16807 #define IM 2147483647 -#define AM (1.0/IM) +#define AM (1.0 / IM) #define IQ 127773 #define IR 2836 using namespace aaesim::test::utils; -using namespace aaesim::constants; +using namespace aaesim::open_source::constants; log4cplus::Logger OldCustomMath::logger = log4cplus::Logger::getInstance(LOG4CPLUS_TEXT("OldCustomMath")); -//generate a uniform random number between 0 and 1 -//From "Numerical Recipe" +// generate a uniform random number between 0 and 1 +// From "Numerical Recipe" -double OldCustomMath::uniform(double &seed) //seed should not be 0.0 +double OldCustomMath::uniform(double &seed) // seed should not be 0.0 { // TODO: consider replacing this method with the C++11 equivalent double ans; const double seed0 = seed; - long k = (long) (seed / (double) IQ); - seed = (double) IA * (seed - (double) (k * IQ)) - (double) (IR * k); + long k = (long)(seed / (double)IQ); + seed = (double)IA * (seed - (double)(k * IQ)) - (double)(IR * k); if (seed < 0) { - seed += (double) IM; + seed += (double)IM; } - ans = (double) AM * seed; + ans = (double)AM * seed; LOG4CPLUS_TRACE(OldCustomMath::logger, seed0 << "," << seed << "," << ans); return ans; } - -double OldCustomMath::gauss(double mean, - double sigma, - double &seed) { +double OldCustomMath::gauss(double mean, double sigma, double &seed) { // returns gaussian distributed random variable with mean mean and standard // deviation sigma. @@ -70,7 +68,7 @@ double OldCustomMath::gauss(double mean, u1 = uniform(seed); u2 = uniform(seed); - eln = -2.0 * log(u1); // ALOG(U1) + eln = -2.0 * log(u1); // ALOG(U1) ang = 2.0 * PI * u2; v1 = sqrt(eln) * cos(ang); v1 = mean + sigma * v1; @@ -79,13 +77,9 @@ double OldCustomMath::gauss(double mean, return (v1); -} // gauss +} // gauss - -double OldCustomMath::trunc_gauss(double mean, - double sigma, - double max_std_dev, - double &seed) { +double OldCustomMath::trunc_gauss(double mean, double sigma, double max_std_dev, double &seed) { // returns truncated gaussian random varialbe with mean mean, standard // deviation sigma, and maximum standard deviation max_std_dev @@ -93,10 +87,10 @@ double OldCustomMath::trunc_gauss(double mean, double val; val = mean + sigma * max_std_dev + 1.; - //gwang 09/05/2002 - //while (val > (mean + sigma * max_std_dev) ) + // gwang 09/05/2002 + // while (val > (mean + sigma * max_std_dev) ) while (val > (mean + sigma * max_std_dev) || val < (mean - sigma * max_std_dev)) { - //end gwang 09/05/2002 + // end gwang 09/05/2002 val = gauss(mean, sigma, seed); } @@ -105,12 +99,9 @@ double OldCustomMath::trunc_gauss(double mean, return (val); -} // trunc_gauss - +} // trunc_gauss -double OldCustomMath::Rayleigh(double mean, - double sigma, - double &seed) { +double OldCustomMath::Rayleigh(double mean, double sigma, double &seed) { // returns Rayleigh distributed random variable with mean mean and standard // deviation sigma. @@ -127,17 +118,15 @@ double OldCustomMath::Rayleigh(double mean, return (v1); -} // Rayleigh - +} // Rayleigh -double OldCustomMath::atan3(double x, - double y) { +double OldCustomMath::atan3(double x, double y) { // returns arc tangent as an angle measured from north in the range 0, 2pi double temp; - temp = (double) atan2(x, y); + temp = (double)atan2(x, y); if (temp < 0.0) { temp = temp + 2.0 * PI; @@ -145,11 +134,9 @@ double OldCustomMath::atan3(double x, return (temp); -} // atan3 +} // atan3 - -double OldCustomMath::laplace(double lambda, - double &seed) { +double OldCustomMath::laplace(double lambda, double &seed) { // returns laplacian r.v. with parameter lambda. const double seed0 = seed; @@ -168,28 +155,24 @@ double OldCustomMath::laplace(double lambda, return (err); -} // laplace - +} // laplace -double OldCustomMath::quantize(double value, - double lsb) { +double OldCustomMath::quantize(double value, double lsb) { // quantizes value to lsb (least significant bit) int j; if (value > 0.) { - j = (int) (0.5 + (double) (value / lsb)); + j = (int)(0.5 + (double)(value / lsb)); } else { - j = (int) ((double) (value / lsb) - 0.5); + j = (int)((double)(value / lsb) - 0.5); } - return (lsb * (double) j); - -} // quantize + return (lsb * (double)j); +} // quantize -Units::Length OldCustomMath::quantize(Units::Length value, - Units::Length lsb) { +Units::Length OldCustomMath::quantize(Units::Length value, Units::Length lsb) { // quantizes value to lsb (least significant bit) double r = value / lsb; if (r > 0.) { @@ -198,12 +181,10 @@ Units::Length OldCustomMath::quantize(Units::Length value, r -= .5; } - return (lsb * (long) r); + return (lsb * (long)r); } - -Units::Speed OldCustomMath::quantize(Units::Speed value, - Units::Speed lsb) { +Units::Speed OldCustomMath::quantize(Units::Speed value, Units::Speed lsb) { // quantizes value to lsb (least significant bit) double r = value / lsb; if (r > 0.) { @@ -212,12 +193,10 @@ Units::Speed OldCustomMath::quantize(Units::Speed value, r -= .5; } - return (lsb * (long) r); + return (lsb * (long)r); } - -bool OldCustomMath::hit(double probability, - double &seed) { +bool OldCustomMath::hit(double probability, double &seed) { if (uniform(seed) < probability) { return true; } else { @@ -225,9 +204,7 @@ bool OldCustomMath::hit(double probability, } } - -double OldCustomMath::subtract_headings(double hd1, - double hd2) { +double OldCustomMath::subtract_headings(double hd1, double hd2) { // subtract heading 2 from heading 1 with the following convention: // negative (counterclockwise) deltas are indicated by being greater than pi. // positive (clockwise) deltas are less than pi. @@ -242,19 +219,16 @@ double OldCustomMath::subtract_headings(double hd1, return (t); -} // subtract_headings - +} // subtract_headings //------------------------------------------------------------- // Speed conversion using MACH & altitude as inputs; unit of output is FPS //------------------------------------------------------------- -double OldCustomMath::MachToTas(double mach, - double altitude) { +double OldCustomMath::MachToTas(double mach, double altitude) { float speedOfSound; double tas; - if (0 <= altitude && altitude <= 36000) { speedOfSound = 662.4 - 243.0 * altitude / 100000.0; } else if (36000 < altitude && altitude <= 82000) { @@ -268,18 +242,16 @@ double OldCustomMath::MachToTas(double mach, tas = (mach * speedOfSound); - //before this point tas is in knots - //gwang 2009-03 + // before this point tas is in knots + // gwang 2009-03 tas *= KNOTS_TO_FEET_PER_SECOND; - //end gwang + // end gwang - return (tas); //FPS + return (tas); // FPS } - -//output CAS in FPS -double OldCustomMath::MachToCas_MITRE(double mach, - double alt) { +// output CAS in FPS +double OldCustomMath::MachToCas_MITRE(double mach, double alt) { double cas, thetas, deltam; if (alt < 36089.24) { @@ -289,9 +261,8 @@ double OldCustomMath::MachToCas_MITRE(double mach, thetas = 0.7519; deltam = 0.2233609 * pow(2.718, (-((alt - 36089.24) / 20806.0))); } - cas = 661.4786 * sqrt(5.0 * ((pow((1.0 + deltam * - ((pow((1.0 + 0.2 * mach * mach), 3.5) - 1.0))), - (2.0 / 7.0))) - 1.0)); + cas = 661.4786 * + sqrt(5.0 * ((pow((1.0 + deltam * ((pow((1.0 + 0.2 * mach * mach), 3.5) - 1.0))), (2.0 / 7.0))) - 1.0)); cas *= KNOTS_TO_FEET_PER_SECOND; @@ -299,11 +270,9 @@ double OldCustomMath::MachToCas_MITRE(double mach, } /* MachToCas_MITRE */ -//inverse = inverse(in) +// inverse = inverse(in) /* Gauss-Jordan elimination from Numerical recipe:*/ -bool OldCustomMath::inverse(DMatrix &in, - int n, - DMatrix &out) { +bool OldCustomMath::inverse(DMatrix &in, int n, DMatrix &out) { int irow = -1, icol = -1; DVector indxc(1, n); @@ -311,7 +280,7 @@ bool OldCustomMath::inverse(DMatrix &in, DVector ipiv(1, n); DMatrix a(1, n, 1, n); - //copy the "in" matrix into the "a" matrix: + // copy the "in" matrix into the "a" matrix: int in_min_row = in.GetMinRow(); int in_min_column = in.GetMinColumn(); for (int i = 1; i <= n; i++) { @@ -320,7 +289,6 @@ bool OldCustomMath::inverse(DMatrix &in, } } - for (int j = 1; j <= n; j++) { ipiv.Set(j, 0.); } @@ -337,27 +305,27 @@ bool OldCustomMath::inverse(DMatrix &in, icol = k; } } else if (ipiv.Get(k) > 1.) { - //singular matrix + // singular matrix printf("\nWarning: Inversion of a singular matrix in the inverse() function (> 1 val).\n"); return false; } - } //end for(int k=1; k<=n; k++) - } //end if(ipiv.get(j) != 1.) - } //end for(int j=1; i<=n; j++) + } // end for(int k=1; k<=n; k++) + } // end if(ipiv.get(j) != 1.) + } // end for(int j=1; i<=n; j++) ipiv.Set(icol, ipiv.Get(icol) + 1); if (irow != icol) { - //swap + // swap for (int l = 1; l <= n; l++) { double temp_swap; temp_swap = a.Get(irow, l); a.Set(irow, l, a.Get(icol, l)); a.Set(icol, l, temp_swap); - } //end for(int l=1; l<=n; l++) - } //end if(irow != icol) - indxr.Set(i, (double) irow); - indxc.Set(i, (double) icol); + } // end for(int l=1; l<=n; l++) + } // end if(irow != icol) + indxr.Set(i, (double)irow); + indxc.Set(i, (double)icol); if (a.Get(icol, icol) == 0.0) { - //singular matrix + // singular matrix printf("\nWarning: Inversion of a singular matrix in the inverse() function (0 val).\n"); return false; } @@ -365,7 +333,7 @@ bool OldCustomMath::inverse(DMatrix &in, a.Set(icol, icol, 1.); for (int l = 1; l <= n; l++) { a.Set(icol, l, pivinv * a.Get(icol, l)); - }//end for(int l=1; l<=n; l++) + } // end for(int l=1; l<=n; l++) for (int ll = 1; ll <= n; ll++) { if (ll != icol) { @@ -373,28 +341,24 @@ bool OldCustomMath::inverse(DMatrix &in, a.Set(ll, icol, 0.); for (int l = 1; l <= n; l++) { a.Set(ll, l, a.Get(ll, l) - dum * a.Get(icol, l)); - } //end for(int l=1; l<=n; l++) - } //end if(ll != icol) - }//end for(int ll=1; ll<=n; ll++) - }//end for(int i=1; i<=n; i++) - - + } // end for(int l=1; l<=n; l++) + } // end if(ll != icol) + } // end for(int ll=1; ll<=n; ll++) + } // end for(int i=1; i<=n; i++) for (int l = n; l >= 1; l--) { if (indxr.Get(l) != indxc.Get(l)) { for (int k = 1; k <= n; k++) { - //swap: + // swap: double temp; - temp = a.Get(k, (int) indxr.Get(l)); - a.Set(k, (int) indxr.Get(l), a.Get(k, (int) indxc.Get(l))); - a.Set(k, (int) indxc.Get(l), temp); + temp = a.Get(k, (int)indxr.Get(l)); + a.Set(k, (int)indxr.Get(l), a.Get(k, (int)indxc.Get(l))); + a.Set(k, (int)indxc.Get(l), temp); } - }//end if(indxr.get(l) != indxc.get(l)) - }//end for(int l=n; l>=1; l--) - + } // end if(indxr.get(l) != indxc.get(l)) + } // end for(int l=n; l>=1; l--) - - //copy the "a" matrix into the "out" matrix: + // copy the "a" matrix into the "out" matrix: int out_min_row = out.GetMinRow(); int out_min_column = out.GetMinColumn(); for (int i = 1; i <= n; i++) { @@ -405,27 +369,20 @@ bool OldCustomMath::inverse(DMatrix &in, return true; } - -void OldCustomMath::matrix_times_vector(DMatrix &matrix_in, - DVector &vector_in, - int n, - DVector &vector_out) { - +void OldCustomMath::matrix_times_vector(DMatrix &matrix_in, DVector &vector_in, int n, DVector &vector_out) { for (int i = 0; i < n; i++) { int ii = i + vector_out.GetMin(); vector_out[ii] = 0.0; for (int j = 0; j < n; j++) { - vector_out[ii] += matrix_in[i + matrix_in.GetMinRow()][j + matrix_in.GetMinColumn()] * - vector_in[j + vector_in.GetMin()]; + vector_out[ii] += + matrix_in[i + matrix_in.GetMinRow()][j + matrix_in.GetMinColumn()] * vector_in[j + vector_in.GetMin()]; } } } - #ifndef _LINUX_ -int OldCustomMath::roundToInt(double d) -{ +int OldCustomMath::roundToInt(double d) { // Rounds double to int, away from 0 for the midpoint values. // // d:double value to be rounded @@ -438,8 +395,7 @@ int OldCustomMath::roundToInt(double d) if (val > 0) { val = val + 0.5; i = floor(val); - } - else if (val < 0) { + } else if (val < 0) { val = val - 0.5; i = ceil(val); } @@ -454,10 +410,7 @@ int OldCustomMath::roundToInt(double d) * matrix [x y z] is post-multiplied by the rotation * matrix. */ -DMatrix &OldCustomMath::createRotationMatrix(double l, - double m, - double n, - const Units::Angle theta) { +DMatrix &OldCustomMath::createRotationMatrix(double l, double m, double n, const Units::Angle theta) { // basic formula acquired from: // https://en.wikipedia.org/wiki/Transformation_matrix#Rotation_2 @@ -477,11 +430,9 @@ DMatrix &OldCustomMath::createRotationMatrix(double l, double sinT = sin(theta); double cosT1 = 1 - cosT; - double a[3][3] = { - {l * l * cosT1 + cosT, m * l * cosT1 + n * sinT, n * l * cosT1 - m * sinT}, - {l * m * cosT1 - n * sinT, m * m * cosT1 + cosT, n * m * cosT1 + l * sinT}, - {l * n * cosT1 + m * sinT, m * n * cosT1 - l * sinT, n * n * cosT1 + cosT} - }; - DMatrix *result = new DMatrix((double **) &a, 0, 2, 0, 2); + double a[3][3] = {{l * l * cosT1 + cosT, m * l * cosT1 + n * sinT, n * l * cosT1 - m * sinT}, + {l * m * cosT1 - n * sinT, m * m * cosT1 + cosT, n * m * cosT1 + l * sinT}, + {l * n * cosT1 + m * sinT, m * n * cosT1 - l * sinT, n * n * cosT1 + cosT}}; + DMatrix *result = new DMatrix((double **)&a, 0, 2, 0, 2); return *result; } From f8459140df7aa9e5eec339fe427fb9ba0a89bb63 Mon Sep 17 00:00:00 2001 From: Stuart Bowman Date: Tue, 30 Jun 2026 11:05:42 -0400 Subject: [PATCH 6/8] chore: update simcore sha --- CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index bc24603..98c2520 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -65,8 +65,8 @@ CPMAddPackage( CPMAddPackage( NAME aircraft_simulation_core - GITHUB_REPOSITORY mitre/aircraft_simulation_core - GIT_TAG feat/add-code # mitre/aircraft_simulation_core#1 + GIT_REPOSITORY https://github.com/mitre/aircraft_simulation_core.git + GIT_TAG e7409164173c0bb043a846bfeb952d5cf046d679 # FIXME switch to a release tag when available OPTIONS "SIMCORE_BUILD_TESTING OFF" ) From 48ebd8f2449d6fea3e0e802aa6d6f25efcc88d0d Mon Sep 17 00:00:00 2001 From: Stuart Bowman Date: Tue, 30 Jun 2026 11:37:44 -0400 Subject: [PATCH 7/8] chore: updated simcore sha --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 98c2520..242a26c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -66,7 +66,7 @@ CPMAddPackage( CPMAddPackage( NAME aircraft_simulation_core GIT_REPOSITORY https://github.com/mitre/aircraft_simulation_core.git - GIT_TAG e7409164173c0bb043a846bfeb952d5cf046d679 # FIXME switch to a release tag when available + GIT_TAG a84dc78805bf35b6a45c68ac99f1c47fe47ad892 # FIXME switch to a release tag when available OPTIONS "SIMCORE_BUILD_TESTING OFF" ) From 757481de01010838fbce13175bdc1ec9aed9639c Mon Sep 17 00:00:00 2001 From: Stuart Bowman Date: Tue, 30 Jun 2026 11:48:25 -0400 Subject: [PATCH 8/8] chore: delete old, useless code --- unittest/src/utils/OldCustomMathUtils.cpp | 438 ---------------------- unittest/src/utils/OldCustomMathUtils.h | Bin 4081 -> 0 bytes 2 files changed, 438 deletions(-) delete mode 100644 unittest/src/utils/OldCustomMathUtils.cpp delete mode 100644 unittest/src/utils/OldCustomMathUtils.h diff --git a/unittest/src/utils/OldCustomMathUtils.cpp b/unittest/src/utils/OldCustomMathUtils.cpp deleted file mode 100644 index 5aeb674..0000000 --- a/unittest/src/utils/OldCustomMathUtils.cpp +++ /dev/null @@ -1,438 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This is the copyright work of The MITRE Corporation, and was produced -// for the U. S. Government under Contract Number DTFAWA-10-C-00080, and -// is subject to Federal Aviation Administration Acquisition Management -// System Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV -// (Oct. 1996). No other use other than that granted to the U. S. -// Government, or to those acting on behalf of the U. S. Government, -// under that Clause is authorized without the express written -// permission of The MITRE Corporation. For further information, please -// contact The MITRE Corporation, Contracts Office, 7515 Colshire Drive, -// McLean, VA 22102-7539, (703) 983-6000. -// -// Copyright 2019 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -/* OldCustomMath.cpp Initial code from Survsim 2.00R1 11/2/99*/ - -#include "OldCustomMathUtils.h" -#include -#include - -#include "utility/UtilityConstants.h" - -// uniform - -#define IA 16807 -#define IM 2147483647 -#define AM (1.0 / IM) -#define IQ 127773 -#define IR 2836 - -using namespace aaesim::test::utils; -using namespace aaesim::open_source::constants; - -log4cplus::Logger OldCustomMath::logger = log4cplus::Logger::getInstance(LOG4CPLUS_TEXT("OldCustomMath")); - -// generate a uniform random number between 0 and 1 -// From "Numerical Recipe" - -double OldCustomMath::uniform(double &seed) // seed should not be 0.0 -{ - // TODO: consider replacing this method with the C++11 equivalent - double ans; - const double seed0 = seed; - - long k = (long)(seed / (double)IQ); - seed = (double)IA * (seed - (double)(k * IQ)) - (double)(IR * k); - if (seed < 0) { - seed += (double)IM; - } - ans = (double)AM * seed; - - LOG4CPLUS_TRACE(OldCustomMath::logger, seed0 << "," << seed << "," << ans); - - return ans; -} - -double OldCustomMath::gauss(double mean, double sigma, double &seed) { - - // returns gaussian distributed random variable with mean mean and standard - // deviation sigma. - // usage example: x = gauss(0., 32.); - - const double seed0 = seed; - double u1, u2, eln, ang, v1; - - u1 = uniform(seed); - u2 = uniform(seed); - eln = -2.0 * log(u1); // ALOG(U1) - ang = 2.0 * PI * u2; - v1 = sqrt(eln) * cos(ang); - v1 = mean + sigma * v1; - - LOG4CPLUS_TRACE(OldCustomMath::logger, mean << "," << sigma << "," << seed0 << "," << seed << "," << v1); - - return (v1); - -} // gauss - -double OldCustomMath::trunc_gauss(double mean, double sigma, double max_std_dev, double &seed) { - - // returns truncated gaussian random varialbe with mean mean, standard - // deviation sigma, and maximum standard deviation max_std_dev - const double seed0 = seed; - double val; - - val = mean + sigma * max_std_dev + 1.; - // gwang 09/05/2002 - // while (val > (mean + sigma * max_std_dev) ) - while (val > (mean + sigma * max_std_dev) || val < (mean - sigma * max_std_dev)) { - // end gwang 09/05/2002 - val = gauss(mean, sigma, seed); - } - - LOG4CPLUS_TRACE(OldCustomMath::logger, - mean << "," << sigma << "," << max_std_dev << "," << seed0 << "," << seed << "," << val); - - return (val); - -} // trunc_gauss - -double OldCustomMath::Rayleigh(double mean, double sigma, double &seed) { - - // returns Rayleigh distributed random variable with mean mean and standard - // deviation sigma. - // usage example: x = Rayleigh(0., 32.); - const double seed0 = seed; - - double u1, v1; - - u1 = uniform(seed); - v1 = (sqrt(-2.0 * log(u1)) - 1.253) / sqrt(0.429); - v1 = mean + sigma * v1; - - LOG4CPLUS_TRACE(OldCustomMath::logger, mean << "," << sigma << "," << seed0 << "," << seed << "," << v1); - - return (v1); - -} // Rayleigh - -double OldCustomMath::atan3(double x, double y) { - - // returns arc tangent as an angle measured from north in the range 0, 2pi - - double temp; - - temp = (double)atan2(x, y); - - if (temp < 0.0) { - temp = temp + 2.0 * PI; - } - - return (temp); - -} // atan3 - -double OldCustomMath::laplace(double lambda, double &seed) { - - // returns laplacian r.v. with parameter lambda. - const double seed0 = seed; - - double uni, err; - - uni = uniform(seed); - err = -lambda * log(uni); - uni = uniform(seed); - - if (uni < 0.5) { - err = -err; - } - - LOG4CPLUS_TRACE(OldCustomMath::logger, lambda << "," << seed0 << "," << seed << "," << err); - - return (err); - -} // laplace - -double OldCustomMath::quantize(double value, double lsb) { - // quantizes value to lsb (least significant bit) - - int j; - - if (value > 0.) { - j = (int)(0.5 + (double)(value / lsb)); - } else { - j = (int)((double)(value / lsb) - 0.5); - } - - return (lsb * (double)j); - -} // quantize - -Units::Length OldCustomMath::quantize(Units::Length value, Units::Length lsb) { - // quantizes value to lsb (least significant bit) - double r = value / lsb; - if (r > 0.) { - r += .5; - } else { - r -= .5; - } - - return (lsb * (long)r); -} - -Units::Speed OldCustomMath::quantize(Units::Speed value, Units::Speed lsb) { - // quantizes value to lsb (least significant bit) - double r = value / lsb; - if (r > 0.) { - r += .5; - } else { - r -= .5; - } - - return (lsb * (long)r); -} - -bool OldCustomMath::hit(double probability, double &seed) { - if (uniform(seed) < probability) { - return true; - } else { - return false; - } -} - -double OldCustomMath::subtract_headings(double hd1, double hd2) { - // subtract heading 2 from heading 1 with the following convention: - // negative (counterclockwise) deltas are indicated by being greater than pi. - // positive (clockwise) deltas are less than pi. - - double t; - - t = hd1 - hd2; - - if (t < 0.) { - t = TWO_PI + t; - } - - return (t); - -} // subtract_headings - -//------------------------------------------------------------- -// Speed conversion using MACH & altitude as inputs; unit of output is FPS -//------------------------------------------------------------- -double OldCustomMath::MachToTas(double mach, double altitude) { - - float speedOfSound; - double tas; - - if (0 <= altitude && altitude <= 36000) { - speedOfSound = 662.4 - 243.0 * altitude / 100000.0; - } else if (36000 < altitude && altitude <= 82000) { - speedOfSound = 573.8; - } else if (82000 < altitude && altitude <= 99900) { - speedOfSound = 120 * altitude / 100000. + 475.4; - } else { - printf("Unexpected altitude in MachToTas: %f\n", altitude); - exit(1); - } - - tas = (mach * speedOfSound); - - // before this point tas is in knots - // gwang 2009-03 - tas *= KNOTS_TO_FEET_PER_SECOND; - // end gwang - - return (tas); // FPS -} - -// output CAS in FPS -double OldCustomMath::MachToCas_MITRE(double mach, double alt) { - double cas, thetas, deltam; - - if (alt < 36089.24) { - thetas = (1.0 - 6.8755856E-6 * alt); - deltam = pow(thetas, 5.2558797); - } else { - thetas = 0.7519; - deltam = 0.2233609 * pow(2.718, (-((alt - 36089.24) / 20806.0))); - } - cas = 661.4786 * - sqrt(5.0 * ((pow((1.0 + deltam * ((pow((1.0 + 0.2 * mach * mach), 3.5) - 1.0))), (2.0 / 7.0))) - 1.0)); - - cas *= KNOTS_TO_FEET_PER_SECOND; - - return (cas); - -} /* MachToCas_MITRE */ - -// inverse = inverse(in) -/* Gauss-Jordan elimination from Numerical recipe:*/ -bool OldCustomMath::inverse(DMatrix &in, int n, DMatrix &out) { - int irow = -1, icol = -1; - - DVector indxc(1, n); - DVector indxr(1, n); - DVector ipiv(1, n); - DMatrix a(1, n, 1, n); - - // copy the "in" matrix into the "a" matrix: - int in_min_row = in.GetMinRow(); - int in_min_column = in.GetMinColumn(); - for (int i = 1; i <= n; i++) { - for (int j = 1; j <= n; j++) { - a.Set(i, j, in.Get(i - 1 + in_min_row, j - 1 + in_min_column)); - } - } - - for (int j = 1; j <= n; j++) { - ipiv.Set(j, 0.); - } - - for (int i = 1; i <= n; i++) { - double big = 0.0; - for (int j = 1; j <= n; j++) { - if (ipiv.Get(j) != 1.) { - for (int k = 1; k <= n; k++) { - if (ipiv.Get(k) == 0.0) { - if (fabs(a.Get(j, k)) >= big) { - big = fabs(a.Get(j, k)); - irow = j; - icol = k; - } - } else if (ipiv.Get(k) > 1.) { - // singular matrix - printf("\nWarning: Inversion of a singular matrix in the inverse() function (> 1 val).\n"); - return false; - } - } // end for(int k=1; k<=n; k++) - } // end if(ipiv.get(j) != 1.) - } // end for(int j=1; i<=n; j++) - ipiv.Set(icol, ipiv.Get(icol) + 1); - if (irow != icol) { - // swap - for (int l = 1; l <= n; l++) { - double temp_swap; - temp_swap = a.Get(irow, l); - a.Set(irow, l, a.Get(icol, l)); - a.Set(icol, l, temp_swap); - } // end for(int l=1; l<=n; l++) - } // end if(irow != icol) - indxr.Set(i, (double)irow); - indxc.Set(i, (double)icol); - if (a.Get(icol, icol) == 0.0) { - // singular matrix - printf("\nWarning: Inversion of a singular matrix in the inverse() function (0 val).\n"); - return false; - } - double pivinv = 1.0 / a.Get(icol, icol); - a.Set(icol, icol, 1.); - for (int l = 1; l <= n; l++) { - a.Set(icol, l, pivinv * a.Get(icol, l)); - } // end for(int l=1; l<=n; l++) - - for (int ll = 1; ll <= n; ll++) { - if (ll != icol) { - double dum = a.Get(ll, icol); - a.Set(ll, icol, 0.); - for (int l = 1; l <= n; l++) { - a.Set(ll, l, a.Get(ll, l) - dum * a.Get(icol, l)); - } // end for(int l=1; l<=n; l++) - } // end if(ll != icol) - } // end for(int ll=1; ll<=n; ll++) - } // end for(int i=1; i<=n; i++) - - for (int l = n; l >= 1; l--) { - if (indxr.Get(l) != indxc.Get(l)) { - for (int k = 1; k <= n; k++) { - // swap: - double temp; - temp = a.Get(k, (int)indxr.Get(l)); - a.Set(k, (int)indxr.Get(l), a.Get(k, (int)indxc.Get(l))); - a.Set(k, (int)indxc.Get(l), temp); - } - } // end if(indxr.get(l) != indxc.get(l)) - } // end for(int l=n; l>=1; l--) - - // copy the "a" matrix into the "out" matrix: - int out_min_row = out.GetMinRow(); - int out_min_column = out.GetMinColumn(); - for (int i = 1; i <= n; i++) { - for (int j = 1; j <= n; j++) { - out.Set(i - 1 + out_min_row, j - 1 + out_min_column, a.Get(i, j)); - } - } - return true; -} - -void OldCustomMath::matrix_times_vector(DMatrix &matrix_in, DVector &vector_in, int n, DVector &vector_out) { - - for (int i = 0; i < n; i++) { - int ii = i + vector_out.GetMin(); - vector_out[ii] = 0.0; - for (int j = 0; j < n; j++) { - vector_out[ii] += - matrix_in[i + matrix_in.GetMinRow()][j + matrix_in.GetMinColumn()] * vector_in[j + vector_in.GetMin()]; - } - } -} - -#ifndef _LINUX_ -int OldCustomMath::roundToInt(double d) { - // Rounds double to int, away from 0 for the midpoint values. - // - // d:double value to be rounded - // - // returns rounded integer value. - - double val = d; - int i = 0; - - if (val > 0) { - val = val + 0.5; - i = floor(val); - } else if (val < 0) { - val = val - 0.5; - i = ceil(val); - } - - return i; -} -#endif - -/** - * Create a matrix which executes a 3-D rotation of a - * point around a vector when a single-row - * matrix [x y z] is post-multiplied by the rotation - * matrix. - */ -DMatrix &OldCustomMath::createRotationMatrix(double l, double m, double n, const Units::Angle theta) { - - // basic formula acquired from: - // https://en.wikipedia.org/wiki/Transformation_matrix#Rotation_2 - // Wikipedia uses T * coord_column, while we use coord_row * T. - // Therefore, we must transpose the matrix. - - // we need a unit vector - double mag2 = l * l + m * m + n * n; - if (mag2 != 1) { - double mag = sqrt(mag2); - l /= mag; - m /= mag; - n /= mag; - } - - double cosT = cos(theta); - double sinT = sin(theta); - double cosT1 = 1 - cosT; - - double a[3][3] = {{l * l * cosT1 + cosT, m * l * cosT1 + n * sinT, n * l * cosT1 - m * sinT}, - {l * m * cosT1 - n * sinT, m * m * cosT1 + cosT, n * m * cosT1 + l * sinT}, - {l * n * cosT1 + m * sinT, m * n * cosT1 - l * sinT, n * n * cosT1 + cosT}}; - DMatrix *result = new DMatrix((double **)&a, 0, 2, 0, 2); - return *result; -} diff --git a/unittest/src/utils/OldCustomMathUtils.h b/unittest/src/utils/OldCustomMathUtils.h deleted file mode 100644 index c1a821298cc173ac20f3030175d2836639ab2676..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 4081 zcmb_fYj4{&6y4YQS6s9}kp#B<$kI0LFu+LK!ayEC;&%IH&=PHNlSq}MoMc1)`_850 zhrGn`h6V+qT=Jg#KD_OA>F5Xkw7Olox*m_tKH`i27^lh+{`pi=Y>Ee~=BcMUWB(*G z!>Odp(fH;gof%sgE4(tgO+qJhCma>lBxNiU2AUZgK)ytD8_{R8kXGkXdn$DzE%ddw zBKCAu=2QHAKK?ZPZP@AeI%l0;ulKGOP#Fl$xN`c3#9eRblZ3X&Xt+?pI1Q6rY2^UM zzR)oKRw}2$_odKcF4-vK+&(xjb2`gJ=_DOTC!PLrn{GHhMMt0*Q}`^AGFX=Pg_X|HomJjT z&FY1;xpIy(_EcFT`ULN0r40cpje_QNy^Bl=CmAP38M%t%#~Wi=Wk=VunTlnb-k$VN zQ2flL%1S!7Y9ZSUbQxbrfmZrDBpM9*y+P;g$?<8M4&L^T59##Xapw)%A|l-7>u0Oh z9Q699kDHHB#B5pM8|kE7$RuLzA2>iqtycGluCwHAd#1<|IlNhHmz z$?3MV3#W1#M7`ckpQzvO4!WnON8MJd^{TL9o(q&dmiSBQI4hy|%TUiV8Ku8&h%Tg_ zvx!aFZ6Og@Lo`--Q}HEgmv+zLpH=r!`f?3fpM1rD8QVN5y~>n-=w6ul9IeTRi1w&O zN60w(EmU9WiY+kU%YNc4bk_+%KcjiT94x*8S(v<(sd!Wo2?1S_6APFu@GQ>P`YmY1+USRx*y2MJ7N$G<_hXT4lnnQ9>o4wq-xex0N~gDG7yQpfz;u z%UZDE4Xlzthuv9G6wpQ@6spD(7iYpbMglj07&Hh2T(O}3$#!tL#GEFeIKd8t>x>IN zVL);w524jm;HC2}0x;S}i(I+_@sbddJL~Tj5v`?sw~{r)TvNA_<0$yr>|h3}egbCo zS(MXE#qZZ=5IYY>h$%DkpW`Ac-TU|4D&Rv%@aqFN^9+1HqshhS>dWtwhPJ|MU=5gT zY)0B2BnB2FC5ImfBNYxD)k2UYZg<3;c&Nsy7ao{ibV3V}mC_M1@=EGN%~}l`wLZ9A zA1^K3w;NaLP^(|{{ReiUOTxwk%{jkYVTyayp+TV<`n5`_6;tHW$ZLn83@H3T4s4ty zq>aZkP%r8|1~JFe%MmR|*GB@gBD8GBovI>=4AYED`^%FNAmY zU*;<5-qspekJy?TlToTxbcmgLDyHEbej<^3G{mKd)3G@dZW6ZlWqERlp8(wqf7Ko$ z^V_MZjblR(>-;2@BEh4*VzD&o?`2_SBTWWR*r2^&-V%hNT9p3@;q-n_UnpqxjItgL;#q*J>9@^9#4@d;=xGZu_?|Dj{rc*x4o? zWaB1_u%@le*M~d3*sSGwwdc|63TU#o;&Y+wGJ^kzY)7_=JhoDJd1H8!Fd8c&VqNxU zwrA@E+jXh)|Dm(1M4NGiZ}bNit9p#XhR!RW7l?3m_xtkuJ2TpPCkx9LU)z%NuGZR; JRa@Op{{S=nRNMdn