diff --git a/.cmake/get_cpm.cmake b/.cmake/get_cpm.cmake new file mode 100644 index 0000000..9c27c51 --- /dev/null +++ b/.cmake/get_cpm.cmake @@ -0,0 +1,24 @@ +# SPDX-License-Identifier: MIT +# +# SPDX-FileCopyrightText: Copyright (c) 2019-2023 Lars Melchior and contributors + +set(CPM_DOWNLOAD_VERSION 0.40.8) +set(CPM_HASH_SUM "78ba32abdf798bc616bab7c73aac32a17bbd7b06ad9e26a6add69de8f3ae4791") + +if(CPM_SOURCE_CACHE) + set(CPM_DOWNLOAD_LOCATION "${CPM_SOURCE_CACHE}/cpm/CPM_${CPM_DOWNLOAD_VERSION}.cmake") +elseif(DEFINED ENV{CPM_SOURCE_CACHE}) + set(CPM_DOWNLOAD_LOCATION "$ENV{CPM_SOURCE_CACHE}/cpm/CPM_${CPM_DOWNLOAD_VERSION}.cmake") +else() + set(CPM_DOWNLOAD_LOCATION "${CMAKE_BINARY_DIR}/cmake/CPM_${CPM_DOWNLOAD_VERSION}.cmake") +endif() + +# Expand relative path. This is important if the provided path contains a tilde (~) +get_filename_component(CPM_DOWNLOAD_LOCATION ${CPM_DOWNLOAD_LOCATION} ABSOLUTE) + +file(DOWNLOAD + https://github.com/cpm-cmake/CPM.cmake/releases/download/v${CPM_DOWNLOAD_VERSION}/CPM.cmake + ${CPM_DOWNLOAD_LOCATION} EXPECTED_HASH SHA256=${CPM_HASH_SUM} +) + +include(${CPM_DOWNLOAD_LOCATION}) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml new file mode 100644 index 0000000..9441c85 --- /dev/null +++ b/.github/workflows/ci.yml @@ -0,0 +1,70 @@ +name: CI + +on: + push: + branches: [main, master] + pull_request: + branches: [main, master] + +jobs: + build: + runs-on: ${{ matrix.os }} + container: ${{ matrix.container }} + strategy: + matrix: + include: + - os: macos-latest + container: '' + install: | + brew update + brew upgrade + - os: ubuntu-latest + container: rockylinux:9 + install: | + dnf -y install gcc gcc-c++ make cmake git + steps: + - uses: actions/checkout@v4 + + - name: Install dependencies + run: ${{ matrix.install }} + + - name: Configure and Build + run: | + cmake -S. -Bbuild -DCMAKE_BUILD_TYPE=Release + cmake --build build --target imalgs -j + + + run_all_tests: + runs-on: ubuntu-latest + container: + image: rockylinux:9 + steps: + - name: Checkout repository + uses: actions/checkout@v4 + + - name: Install dependencies + run: | + dnf -y install gcc gcc-c++ make cmake git + + - name: Build project + run: | + cmake -S. -Bbuild -DCMAKE_BUILD_TYPE=Release -DBUILD_TESTING=ON + + - name: Run tests + run: | + cmake --build build --target run_imalgs_test -j + + - name: Upload test results + uses: actions/upload-artifact@v4 + with: + name: public-test-results + path: unittest/*results.xml + + # - name: Publish JUnit test results + # uses: dorny/test-reporter@v2 + # if: always() + # with: + # name: Public Library Tests + # path: unittest/*results.xml + # reporter: jest-junit + # fail-on-error: true diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..567609b --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +build/ diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..ab4a2d7 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,75 @@ +cmake_minimum_required(VERSION 3.26...4.1.2) +project(im_sample_algorithm VERSION 5.2.0.0) # the nano value is a boolean. 1 == SNAPSHOT, 0 == release + +set (CMAKE_CXX_STANDARD 20) + +if (NOT DEFINED CMAKE_BUILD_TYPE OR CMAKE_BUILD_TYPE STREQUAL "") + set (CMAKE_BUILD_TYPE "Release") +endif() +message (STATUS "${PROJECT_NAME}: Build type is ${CMAKE_BUILD_TYPE}") + +option(BUILD_TESTING "Enable building ${PROJECT_NAME} tests" ON) +if(NOT ${BUILD_TESTING}) + message(STATUS "${PROJECT_NAME}: skipping test targets") +endif() + + +include(${PROJECT_SOURCE_DIR}/.cmake/get_cpm.cmake) + +if (${PROJECT_VERSION_TWEAK}) + # Append -SNAPSHOT to the version name if this is a pre-release version + set(msg_VERSION ${PROJECT_VERSION_MAJOR}.${PROJECT_VERSION_MINOR}.${PROJECT_VERSION_PATCH}-SNAPSHOT) +else() + set(msg_VERSION ${PROJECT_VERSION_MAJOR}.${PROJECT_VERSION_MINOR}.${PROJECT_VERSION_PATCH}) +endif() +message(STATUS "${PROJECT_NAME}: Generating build version ${msg_VERSION}") + +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") + +# Begin project dependencies ------------------------------------------------------ +CPMAddPackage( + NAME fmacm + GITHUB_REPOSITORY mitre/FMACM + GIT_TAG v5.2.0 + OPTIONS + "BUILD_SHARED_LIBS FALSE" + "BUILD_TESTING ${BUILD_TESTING}" +) + +# CPMAddPackage( +# NAME unitslib +# GIT_REPOSITORY https://mustache.mitre.org/scm/aaes/cpp-uom.git # FIXME use external repo when available +# GIT_TAG 07445f7ec0f98b55cdd6e6188083145a293415a7 +# DOWNLOAD_ONLY TRUE +# ) + +CPMAddPackage( + NAME log4cplus + GITHUB_REPOSITORY log4cplus/log4cplus + GIT_TAG REL_2_1_2 + OPTIONS + "BUILD_SHARED_LIBS FALSE" + "LOG4CPLUS_BUILD_TESTING FALSE" + "LOG4CPLUS_BUILD_LOGGINGSERVER FALSE" +) + +# Create some common paths +if (log4cplus_ADDED) + set (LOG4CPLUS_DIRS + ${log4cplus_BINARY_DIR}/include + ${log4cplus_SOURCE_DIR}/include) +endif () + +set (unitslib_INCLUDE_DIRS ${fmacm_SOURCE_DIR}/units-2.1/src/) +set (LOADER_DIR ${fmacm_SOURCE_DIR}/Loader) +set (PUBLIC_DIR ${fmacm_SOURCE_DIR}/Public) +set (IM_DIR ${CMAKE_CURRENT_SOURCE_DIR}/IntervalManagement) +set (fmacm_INCLUDE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include + ${fmacm_SOURCE_DIR}/include) +set (UNITTEST_DIR ${CMAKE_CURRENT_SOURCE_DIR}/unittest) + +add_subdirectory(${IM_DIR}) +add_subdirectory(${UNITTEST_DIR}) +include(${UNITTEST_DIR}/unittest.cmake) diff --git a/IntervalManagement/AchieveObserver.cpp b/IntervalManagement/AchieveObserver.cpp index 93c35e8..78aa1ed 100644 --- a/IntervalManagement/AchieveObserver.cpp +++ b/IntervalManagement/AchieveObserver.cpp @@ -17,9 +17,10 @@ // 2023 The MITRE Corporation. All Rights Reserved. // **************************************************************************** -#include #include "imalgs/AchieveObserver.h" +#include + using namespace std; using namespace interval_management::open_source; @@ -60,9 +61,9 @@ string AchieveObserver::ToString() { 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()); + snprintf(txt, 301, "%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; diff --git a/IntervalManagement/AchievePointCalcs.cpp b/IntervalManagement/AchievePointCalcs.cpp index 020ee66..4375fb8 100644 --- a/IntervalManagement/AchievePointCalcs.cpp +++ b/IntervalManagement/AchievePointCalcs.cpp @@ -58,7 +58,8 @@ AchievePointCalcs::AchievePointCalcs(const string &waypoint, const AircraftInten AchievePointCalcs::AchievePointCalcs(const string &waypoint, const AircraftIntent &intent, const VerticalPath &vpath, const vector &htraj, const AchievePointCalcs &ownship_calcs, - const AircraftIntent &ownship_intent) { + const AircraftIntent &ownship_intent, + const std::shared_ptr &position_converter) { Clear(); m_waypoint_name = waypoint; @@ -69,8 +70,8 @@ AchievePointCalcs::AchievePointCalcs(const string &waypoint, const AircraftInten if (m_waypoint_name == "CALCULATED_TRP") { Waypoint traffic_reference_point; size_t waypoint_index; - ComputeDefaultTRP(ownship_calcs, ownship_intent, intent, m_horizontal_path, traffic_reference_point, m_waypoint_x, - m_waypoint_y, waypoint_index); + ComputeDefaultTRP(ownship_calcs, ownship_intent, intent, position_converter, m_horizontal_path, + traffic_reference_point, m_waypoint_x, m_waypoint_y, waypoint_index); LOG4CPLUS_DEBUG(m_logger, "Calculated TRP = (" << m_waypoint_x << "," << m_waypoint_y << ")"); // log the geographic coordinates EarthModel::LocalPositionEnu xy; @@ -78,7 +79,7 @@ AchievePointCalcs::AchievePointCalcs(const string &waypoint, const AircraftInten xy.y = m_waypoint_y; xy.z = Units::zero(); EarthModel::GeodeticPosition geo; - intent.GetTangentPlaneSequence()->convertLocalToGeodetic(xy, geo); + position_converter->ConvertLocalToGeodetic(xy, geo); LOG4CPLUS_DEBUG(m_logger, "(lat,lon) = (" << std::setprecision(10) << Units::DegreesAngle(geo.latitude) << "," << Units::DegreesAngle(geo.longitude) << ")"); } else { @@ -87,8 +88,6 @@ AchievePointCalcs::AchievePointCalcs(const string &waypoint, const AircraftInten ComputeEndValues(vpath); } -AchievePointCalcs::~AchievePointCalcs() = default; - void AchievePointCalcs::Clear() { m_waypoint_name = ""; @@ -107,6 +106,7 @@ void AchievePointCalcs::Clear() { void AchievePointCalcs::ComputeDefaultTRP(const AchievePointCalcs &ownship_calcs, const AircraftIntent &ownship_intent, const AircraftIntent &target_intent, + const std::shared_ptr &position_converter, const vector &target_horizontal_path, Waypoint &traffic_reference_point, Units::Length &waypoint_x, Units::Length &waypoint_y, size_t &waypoint_index_in_target_intent) { @@ -154,7 +154,7 @@ void AchievePointCalcs::ComputeDefaultTRP(const AchievePointCalcs &ownship_calcs // Search horizontal path for an intersecting segment HorizontalTurnPath trp_turn; bool first(true), crossed(false), end_is_usable(false); - double x0, y0, d0; + double x0{}, y0{}, d0{}; vector::const_iterator hpi; for (hpi = target_horizontal_path.begin(); hpi != target_horizontal_path.end(); ++hpi) { double x1 = hpi->GetXPositionMeters(); @@ -343,7 +343,7 @@ void AchievePointCalcs::ComputeDefaultTRP(const AchievePointCalcs &ownship_calcs xy.y = waypoint_y; xy.z = Units::zero(); EarthModel::GeodeticPosition geo; - target_intent.GetTangentPlaneSequence()->convertLocalToGeodetic(xy, geo); + position_converter->ConvertLocalToGeodetic(xy, geo); // populate traffic_reference_point traffic_reference_point.SetName("CALCULATED_TRP"); @@ -353,7 +353,7 @@ void AchievePointCalcs::ComputeDefaultTRP(const AchievePointCalcs &ownship_calcs if (trp_turn.radius > Units::zero()) { xy.x = Units::MetersLength(trp_turn.x_position_meters); xy.y = Units::MetersLength(trp_turn.y_position_meters); - target_intent.GetTangentPlaneSequence()->convertLocalToGeodetic(xy, geo); + position_converter->ConvertLocalToGeodetic(xy, geo); traffic_reference_point.SetRfTurnCenterLatitude(geo.latitude); traffic_reference_point.SetRfTurnCenterLongitude(geo.longitude); } else { diff --git a/IntervalManagement/AircraftState.cpp b/IntervalManagement/AircraftState.cpp index 1529e96..532a9e0 100644 --- a/IntervalManagement/AircraftState.cpp +++ b/IntervalManagement/AircraftState.cpp @@ -22,12 +22,10 @@ #include "utility/CustomUnits.h" #include "imalgs/IMUtils.h" -using namespace interval_management::open_source; - log4cplus::Logger interval_management::open_source::AircraftState::m_logger = log4cplus::Logger::getInstance(LOG4CPLUS_TEXT("AircraftState")); -AircraftState::AircraftState() +interval_management::open_source::AircraftState::AircraftState() : m_x(0), m_y(0), m_z(0), @@ -45,17 +43,18 @@ AircraftState::AircraftState() m_gamma(Units::ZERO_ANGLE), m_sensed_temperature(Units::negInfinity()) {} -const Units::UnsignedRadiansAngle AircraftState::GetHeadingCcwFromEastRadians() const { +const Units::UnsignedRadiansAngle interval_management::open_source::AircraftState::GetHeadingCcwFromEastRadians() + const { double result = atan3(m_yd, m_xd); return Units::UnsignedRadiansAngle(result); } -const Units::Speed AircraftState::GetGroundSpeed() const { +const Units::Speed interval_management::open_source::AircraftState::GetGroundSpeed() const { return Units::FeetPerSecondSpeed(sqrt(pow(m_xd, 2) + pow(m_yd, 2))); } -AircraftState &AircraftState::Interpolate(const AircraftState &a, const AircraftState &b, - const Units::SecondsTime time) { +interval_management::open_source::AircraftState &interval_management::open_source::AircraftState::Interpolate( + const AircraftState &a, const AircraftState &b, const Units::SecondsTime time) { const double dt = Units::SecondsTime(b.GetTimeStamp() - a.GetTimeStamp()).value(); double weight_a, weight_b; @@ -80,7 +79,8 @@ AircraftState &AircraftState::Interpolate(const AircraftState &a, const Aircraft return *this; } -AircraftState &AircraftState::Extrapolate(const AircraftState &in, const Units::SecondsTime &time) { +interval_management::open_source::AircraftState &interval_management::open_source::AircraftState::Extrapolate( + const AircraftState &in, const Units::SecondsTime &time) { const double dt = time.value() - in.GetTimeStamp().value(); m_time = time; m_id = in.m_id; @@ -93,7 +93,7 @@ AircraftState &AircraftState::Extrapolate(const AircraftState &in, const Units:: return *this; } -Units::Speed AircraftState::GetTrueAirspeed() const { +Units::Speed interval_management::open_source::AircraftState::GetTrueAirspeed() const { Units::MetersPerSecondSpeed tas_x, tas_y; tas_x = Units::FeetPerSecondSpeed(m_xd) - m_sensed_wind_east_component; tas_y = Units::FeetPerSecondSpeed(m_yd) - m_sensed_wind_north_component; @@ -101,13 +101,12 @@ Units::Speed AircraftState::GetTrueAirspeed() const { return tas; } -AircraftState &AircraftState::Create(const int &id, const Units::Time &time, - const EarthModel::LocalPositionEnu &enu_position, const Units::Speed &xd, - const Units::Speed &yd, const Units::Speed &zd, const Units::Angle &gamma, - const Units::Speed &sensed_wind_east, const Units::Speed &sensed_wind_north, - const Units::Speed &sensed_wind_parallel, - const Units::Speed &sensed_wind_perpendicular, - const Units::Temperature &sensed_temperature, const Units::Angle &psi_enu) { +interval_management::open_source::AircraftState &interval_management::open_source::AircraftState::Create( + const int &id, const Units::Time &time, const EarthModel::LocalPositionEnu &enu_position, const Units::Speed &xd, + const Units::Speed &yd, const Units::Speed &zd, const Units::Angle &gamma, const Units::Speed &sensed_wind_east, + const Units::Speed &sensed_wind_north, const Units::Speed &sensed_wind_parallel, + const Units::Speed &sensed_wind_perpendicular, const Units::Temperature &sensed_temperature, + const Units::Angle &psi_enu) { this->m_id = id; this->m_time = time; diff --git a/IntervalManagement/CMakeLists.txt b/IntervalManagement/CMakeLists.txt index 63abe67..1f2b357 100644 --- a/IntervalManagement/CMakeLists.txt +++ b/IntervalManagement/CMakeLists.txt @@ -47,9 +47,5 @@ set(LIBRARY_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/lib) add_library(imalgs STATIC ${IMALGS_SOURCE_FILES} ${OBSERVER_SOURCE_FILES}) target_compile_options(imalgs PUBLIC "-Wno-inconsistent-missing-destructor-override" "-Wno-inconsistent-missing-override" "-Wno-overloaded-virtual") -target_include_directories(imalgs PUBLIC - ${aaesim_INCLUDE_DIRS}) -target_link_libraries(imalgs - pub) - - +target_include_directories(imalgs PUBLIC ${fmacm_INCLUDE_DIRS}) +target_link_libraries(imalgs pub) diff --git a/IntervalManagement/ClosestPointMetric.cpp b/IntervalManagement/ClosestPointMetric.cpp index ac77a74..c8ce503 100644 --- a/IntervalManagement/ClosestPointMetric.cpp +++ b/IntervalManagement/ClosestPointMetric.cpp @@ -22,12 +22,7 @@ using namespace interval_management::open_source; -ClosestPointMetric::ClosestPointMetric(void) { - m_im_ac_id = 0; - m_target_ac_id = 0; - m_report_metrics = false; - mMinDist = Units::infinity(); -} +ClosestPointMetric::ClosestPointMetric() = default; void ClosestPointMetric::update(double imx, double imy, double targx, double targy) { @@ -38,8 +33,8 @@ void ClosestPointMetric::update(double imx, double imy, double targx, double tar // 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)); + Units::Length dist = aaesim::open_source::AircraftCalculations::PtToPtDist( + Units::FeetLength(imx), Units::FeetLength(imy), Units::FeetLength(targx), Units::FeetLength(targy)); if (dist < mMinDist) { mMinDist = dist; diff --git a/IntervalManagement/FIMAlgorithmAdapter.cpp b/IntervalManagement/FIMAlgorithmAdapter.cpp index aaa6134..6c47c48 100644 --- a/IntervalManagement/FIMAlgorithmAdapter.cpp +++ b/IntervalManagement/FIMAlgorithmAdapter.cpp @@ -19,37 +19,40 @@ #include "imalgs/FIMAlgorithmAdapter.h" +#include + #include "imalgs/IMUtils.h" #include "imalgs/FIMAlgorithmInitializer.h" +#include "public/SingleTangentPlaneSequence.h" +#include "public/CoreUtils.h" using namespace interval_management::open_source; +using json = nlohmann::json; interval_management::open_source::FIMAlgorithmAdapter::FIMAlgorithmAdapter(std::shared_ptr im_algorithm, IMUtils::IMAlgorithmTypes algorithm_type) - : m_im_algorithm(nullptr), - m_im_algorithm_type(IMUtils::IMAlgorithmTypes::NONE), - m_assap(nullptr), - m_current_guidance_phase(aaesim::open_source::GuidanceFlightPhase::TAKEOFF_ROLL), - m_target_history(), - m_initialized(false) { - m_im_algorithm = im_algorithm; - m_im_algorithm_type = algorithm_type; -} + : m_im_algorithm(im_algorithm), m_im_algorithm_type(algorithm_type) {} void interval_management::open_source::FIMAlgorithmAdapter::Initialize( aaesim::open_source::FlightDeckApplicationInitializer &initializer_visitor) { auto ownship_intent_from_clearance = m_im_algorithm->GetClearance().GetOwnshipIntent(); if (ownship_intent_from_clearance.has_value()) { - initializer_visitor.fms_prediction_paramters.ownship_aircraft_intent = ownship_intent_from_clearance.value(); + initializer_visitor.fms_prediction_parameters.fms_intent = ownship_intent_from_clearance.value(); } - m_im_algorithm->ValidateClearance(initializer_visitor.fms_prediction_paramters.ownship_aircraft_intent, - m_im_algorithm_type); + m_im_algorithm->ValidateClearance(initializer_visitor.fms_prediction_parameters.fms_intent, m_im_algorithm_type); + initializer_visitor.fms_prediction_parameters.fms_intent = + AircraftIntent::CopyAndTrimAfterNamedWaypoint(initializer_visitor.fms_prediction_parameters.fms_intent, + m_im_algorithm->GetClearance().GetPlannedTerminationPoint()); + auto waypoints = + CoreUtils::ShortenLongLegs(initializer_visitor.fms_prediction_parameters.fms_intent.GetWaypointList()); + m_position_converter = std::make_unique(waypoints); + initializer_visitor.position_converter = m_position_converter; + try { interval_management::open_source::FIMAlgorithmInitializer initializer = static_cast(initializer_visitor); initializer.Initialize(this); m_assap = initializer.surveillance_processor; - m_initialized = true; } catch (std::exception &e) { static const std::string error_message( "Developer Error: The wrong concrete instance of FlightDeckApplicationInitializer was provided. Expecting " @@ -63,28 +66,39 @@ aaesim::open_source::Guidance interval_management::open_source::FIMAlgorithmAdap const aaesim::open_source::DynamicsState &dynamics_state, const aaesim::open_source::AircraftState &own_truth_state) { - m_current_guidance_phase = current_guidance.m_active_guidance_phase; - if (m_current_guidance_phase != aaesim::open_source::GuidanceFlightPhase::CRUISE_DESCENT) return current_guidance; - - UpdateTargetHistory(simtime); - aaesim::open_source::Guidance im_algorithm_guidance = current_guidance; im_algorithm_guidance.SetValid(false); + if (current_guidance.m_active_guidance_phase != aaesim::open_source::GuidanceFlightPhase::CRUISE_DESCENT) + return im_algorithm_guidance; + if (m_im_algorithm->IsImOperationComplete()) return im_algorithm_guidance; + UpdateTargetHistory(simtime); aaesim::open_source::AircraftState synced_target_state = m_assap->Update( own_truth_state, m_assap->GetAdsbReceiver()->GetCurrentADSBReport(GetImClearance().GetTargetId())); - if (m_initialized && !m_im_algorithm->IsImOperationComplete()) { - if (im_algorithm_guidance.GetSelectedSpeed().GetSpeedType() == UNSPECIFIED_SPEED) { - im_algorithm_guidance.SetSelectedSpeed( - aaesim::open_source::AircraftSpeed::OfIndicatedAirspeed(Units::KnotsSpeed(60))); - } - - return m_im_algorithm->Update( - im_algorithm_guidance, dynamics_state, IMUtils::ConvertToIntervalManagementAircraftState(own_truth_state), - IMUtils::ConvertToIntervalManagementAircraftState(synced_target_state), m_target_history); + if (im_algorithm_guidance.GetSelectedSpeed().GetSpeedType() == UNSPECIFIED_SPEED) { + im_algorithm_guidance.SetSelectedSpeed( + aaesim::open_source::AircraftSpeed::OfIndicatedAirspeed(Units::KnotsSpeed(60))); } - return im_algorithm_guidance; + auto ownship_im_state = ConvertAircraftState(own_truth_state); + auto target_im_state = ConvertAircraftState(synced_target_state); + return m_im_algorithm->Update(im_algorithm_guidance, dynamics_state, ownship_im_state, target_im_state, + m_target_history); +} + +interval_management::open_source::AircraftState + interval_management::open_source::FIMAlgorithmAdapter::ConvertAircraftState( + const aaesim::open_source::AircraftState &state) const { + if (state.GetTime().value() < 0 || state.GetUniqueId() == IMUtils::UNINITIALIZED_AIRCRAFT_ID) + return IMUtils::ConvertToIntervalManagementAircraftState(state); + + EarthModel::LocalPositionEnu enu_position; + m_position_converter->ConvertGeodeticToLocal( + EarthModel::GeodeticPosition::Of(state.GetLatitude(), state.GetLongitude()), enu_position); + auto updated_state = + aaesim::open_source::AircraftState::Builder(state).Position(enu_position.x, enu_position.y)->Build(); + LogAircraftState(updated_state); + return IMUtils::ConvertToIntervalManagementAircraftState(updated_state); } bool interval_management::open_source::FIMAlgorithmAdapter::IsActive() const { @@ -93,7 +107,7 @@ bool interval_management::open_source::FIMAlgorithmAdapter::IsActive() const { void interval_management::open_source::FIMAlgorithmAdapter::UpdateTargetHistory( const aaesim::open_source::SimulationTime &simtime) { - std::vector recent_reports = + std::vector recent_reports = m_assap->GetAdsbReceiver()->GetReportsReceivedByTime(simtime); if (recent_reports.empty()) { return; @@ -101,15 +115,27 @@ void interval_management::open_source::FIMAlgorithmAdapter::UpdateTargetHistory( for (const auto &adsb_sv_report : recent_reports) { if (adsb_sv_report.GetId() == GetImClearance().GetTargetId()) { - - const aaesim::open_source::AircraftState ads_b_state = - aaesim::open_source::AircraftState::CreateFromADSBReport(adsb_sv_report); - interval_management::open_source::AircraftState imstate = - IMUtils::ConvertToIntervalManagementAircraftState(ads_b_state); - if (imstate.GetId() > IMUtils::UNINITIALIZED_AIRCRAFT_ID && imstate.GetTimeStamp() >= Units::zero()) { + if (adsb_sv_report.GetTime() >= Units::zero()) { + const aaesim::open_source::AircraftState ads_b_state = + aaesim::open_source::AircraftState::FromAdsbReport(adsb_sv_report); + interval_management::open_source::AircraftState imstate = ConvertAircraftState(ads_b_state); imstate.m_distance_to_go_meters = Units::MetersLength(m_im_algorithm->GetTargetDtgToLastWaypoint()).value(); m_target_history.push_back(imstate); } } } } + +void FIMAlgorithmAdapter::LogAircraftState(const aaesim::open_source::AircraftState &state) { + if (m_logger.getLogLevel() == log4cplus::TRACE_LOG_LEVEL) { + json j; + j["acid"] = state.GetUniqueId(); + j["time_sec"] = state.GetTime().value(); + j["position_enu_x_m"] = Units::MetersLength(state.GetPositionEnuX()).value(); + j["position_enu_y_m"] = Units::MetersLength(state.GetPositionEnuY()).value(); + j["altitude_msl_m"] = Units::MetersLength(state.GetAltitudeMsl()).value(); + j["position_lat_deg"] = Units::DegreesAngle(state.GetLatitude()).value(); + j["position_lon_deg"] = Units::DegreesAngle(state.GetLongitude()).value(); + LOG4CPLUS_TRACE(m_logger, j.dump()); + } +} diff --git a/IntervalManagement/FIMAlgorithmDataWriter.cpp b/IntervalManagement/FIMAlgorithmDataWriter.cpp index 4ee1489..b62fbe8 100644 --- a/IntervalManagement/FIMAlgorithmDataWriter.cpp +++ b/IntervalManagement/FIMAlgorithmDataWriter.cpp @@ -28,7 +28,7 @@ log4cplus::Logger interval_management::open_source::FIMAlgorithmDataWriter::m_lo log4cplus::Logger::getInstance(LOG4CPLUS_TEXT("FIMAlgorithmDataWriter")); interval_management::open_source::FIMAlgorithmDataWriter::FIMAlgorithmDataWriter() - : OutputHandler("", "_IMAlgorithm.csv"), m_sim_data() { + : OutputHandler("", "_im_algorithm.csv"), m_sim_data() { m_sim_data.reserve(10000); } @@ -80,7 +80,6 @@ void interval_management::open_source::FIMAlgorithmDataWriter::Finish() { os << "current_imspeed_command_count"; os << NEWLINE; - os.flush(); // Important for outputting double data. os.set_precision(10); @@ -120,7 +119,6 @@ void interval_management::open_source::FIMAlgorithmDataWriter::Finish() { os << Units::MetersLength(ix.m_current_imspeed_count).value(); os << NEWLINE; - os.flush(); } os.close(); diff --git a/IntervalManagement/FIMAlgorithmInitializer.cpp b/IntervalManagement/FIMAlgorithmInitializer.cpp index 36e0d5b..f1ad80d 100644 --- a/IntervalManagement/FIMAlgorithmInitializer.cpp +++ b/IntervalManagement/FIMAlgorithmInitializer.cpp @@ -23,7 +23,7 @@ using namespace interval_management::open_source; interval_management::open_source::FIMAlgorithmInitializer::FIMAlgorithmInitializer( const FIMAlgorithmInitializer::Builder *builder) { - fms_prediction_paramters = builder->GetFmsPredictionParameters(); + fms_prediction_parameters = builder->GetFmsPredictionParameters(); performance_parameters = builder->GetPerformanceParameters(); surveillance_processor = builder->GetSurveillanceProcessor(); } @@ -50,29 +50,26 @@ void interval_management::open_source::FIMAlgorithmInitializer::Initialize( void interval_management::open_source::FIMAlgorithmInitializer::Initialize( interval_management::open_source::IMKinematicAchieve *kinematic_algorithm) { - kinematic_algorithm->Initialize(BuildOwnshipPredictionParameters(), fms_prediction_paramters.ownship_aircraft_intent, - fms_prediction_paramters.weather_prediction); + kinematic_algorithm->Initialize(BuildOwnshipPredictionParameters(), fms_prediction_parameters.fms_intent, + fms_prediction_parameters.weather_prediction, position_converter); } void interval_management::open_source::FIMAlgorithmInitializer::Initialize( interval_management::open_source::IMTimeBasedAchieveMutableASG *test_vector_algorithm) { - test_vector_algorithm->Initialize(BuildOwnshipPredictionParameters(), - fms_prediction_paramters.ownship_aircraft_intent, - fms_prediction_paramters.weather_prediction); + test_vector_algorithm->Initialize(BuildOwnshipPredictionParameters(), fms_prediction_parameters.fms_intent, + fms_prediction_parameters.weather_prediction); } void interval_management::open_source::FIMAlgorithmInitializer::Initialize( interval_management::open_source::IMTimeBasedAchieve *time_achieve_algorithm) { - time_achieve_algorithm->Initialize(BuildOwnshipPredictionParameters(), - fms_prediction_paramters.ownship_aircraft_intent, - fms_prediction_paramters.weather_prediction); + time_achieve_algorithm->Initialize(BuildOwnshipPredictionParameters(), fms_prediction_parameters.fms_intent, + fms_prediction_parameters.weather_prediction, position_converter); } void interval_management::open_source::FIMAlgorithmInitializer::Initialize( interval_management::open_source::IMDistBasedAchieve *dist_achieve_algorithm) { - dist_achieve_algorithm->Initialize(BuildOwnshipPredictionParameters(), - fms_prediction_paramters.ownship_aircraft_intent, - fms_prediction_paramters.weather_prediction); + dist_achieve_algorithm->Initialize(BuildOwnshipPredictionParameters(), fms_prediction_parameters.fms_intent, + fms_prediction_parameters.weather_prediction, position_converter); } IMAlgorithm::OwnshipPredictionParameters @@ -82,11 +79,11 @@ IMAlgorithm::OwnshipPredictionParameters ownship_prediction_parameters.flap_speeds = performance_parameters.flap_speeds; ownship_prediction_parameters.flight_envelope = performance_parameters.flight_envelope; ownship_prediction_parameters.mass_data = performance_parameters.mass_data; - ownship_prediction_parameters.expected_cruise_altitude = fms_prediction_paramters.expected_cruise_altitude; - ownship_prediction_parameters.maximum_allowable_bank_angle = fms_prediction_paramters.maximum_allowable_bank_angle; - ownship_prediction_parameters.transition_altitude = fms_prediction_paramters.transition_altitude; - ownship_prediction_parameters.transition_ias = fms_prediction_paramters.transition_ias; - ownship_prediction_parameters.transition_mach = fms_prediction_paramters.transition_mach; + ownship_prediction_parameters.expected_cruise_altitude = fms_prediction_parameters.expected_cruise_altitude; + ownship_prediction_parameters.maximum_allowable_bank_angle = fms_prediction_parameters.maximum_allowable_bank_angle; + ownship_prediction_parameters.transition_altitude = fms_prediction_parameters.transition_altitude; + ownship_prediction_parameters.transition_ias = fms_prediction_parameters.transition_ias; + ownship_prediction_parameters.transition_mach = fms_prediction_parameters.transition_mach; return ownship_prediction_parameters; } diff --git a/IntervalManagement/FIMConfiguration.cpp b/IntervalManagement/FIMConfiguration.cpp index 9ad8ca8..71b14b6 100644 --- a/IntervalManagement/FIMConfiguration.cpp +++ b/IntervalManagement/FIMConfiguration.cpp @@ -18,39 +18,10 @@ // **************************************************************************** #include "imalgs/FIMConfiguration.h" -#include "imalgs/IMUtils.h" using namespace interval_management::open_source; log4cplus::Logger FIMConfiguration::m_logger = log4cplus::Logger::getInstance(LOG4CPLUS_TEXT("FIMConfiguration")); -const FIMConfiguration DEFAULT_CONFIGURATION(); - -// Do not change these values without discussing with Lesley Weitz. -const Units::HertzFrequency FIMConfiguration::ACHIEVE_CONTROL_GAIN_DEFAULT(0.008); -const Units::HertzFrequency FIMConfiguration::MAINTAIN_CONTROL_GAIN_DEFAULT(0.008); -const Units::SecondsTime FIMConfiguration::TIME_THRESHOLD_DEFAULT(0); -const Units::SecondsPerNauticalMileInvertedSpeed FIMConfiguration::SLOPE_DEFAULT(0.25); -const Units::NauticalMilesLength FIMConfiguration::ERROR_DISTANCE_DEFAULT(0); -const bool FIMConfiguration::THRESHOLD_FLAG_DEFAULT(true); - -FIMConfiguration::FIMConfiguration() - : m_achieve_control_gain(ACHIEVE_CONTROL_GAIN_DEFAULT), - m_maintain_control_gain(MAINTAIN_CONTROL_GAIN_DEFAULT), - m_time_threshold(TIME_THRESHOLD_DEFAULT), - m_slope(SLOPE_DEFAULT), - m_error_distance(ERROR_DISTANCE_DEFAULT), - - m_use_speed_limiting(IMUtils::LIMIT_FLAG_DEFAULT), - m_threshold_flag(THRESHOLD_FLAG_DEFAULT), - m_use_speed_quantization(IMUtils::QUANTIZE_FLAG_DEFAULT), - - m_loaded_middle_to_final_quantize_transition_distance(IMUtils::DIST_QUANTIZE_1_DEFAULT), - m_loaded_first_to_middle_quantize_transition_distance(IMUtils::DIST_QUANTIZE_2_DEFAULT), - m_loaded_speed_quantize_final_phase(IMUtils::SPEED_QUANTIZE_1_DEFAULT_1_KNOT), - m_loaded_speed_quantize_middle_phase(IMUtils::SPEED_QUANTIZE_2_DEFAULT), - m_loaded_speed_quantize_first_phase(IMUtils::SPEED_QUANTIZE_3_DEFAULT) {} - -FIMConfiguration::~FIMConfiguration() {} bool FIMConfiguration::load(DecodedStream *input) { set_stream(input); @@ -59,6 +30,7 @@ bool FIMConfiguration::load(DecodedStream *input) { register_var("k_maintain", &m_maintain_control_gain); register_var("limit", &m_use_speed_limiting); + register_var("blend_wind", &m_use_wind_blending); // speed quantization values (have defaults if not loaded) register_var("dist_quantize_1", &m_loaded_middle_to_final_quantize_transition_distance); diff --git a/IntervalManagement/IMAchieve.cpp b/IntervalManagement/IMAchieve.cpp index 9eced5e..9544ff4 100644 --- a/IntervalManagement/IMAchieve.cpp +++ b/IntervalManagement/IMAchieve.cpp @@ -19,7 +19,6 @@ #include "imalgs/IMAchieve.h" -#include "public/AircraftCalculations.h" #include "public/Environment.h" #include "imalgs/MOPSPredictedWindEvaluatorVersion1.h" #include "imalgs/MOPSPredictedWindEvaluatorVersion2.h" diff --git a/IntervalManagement/IMAlgorithm.cpp b/IntervalManagement/IMAlgorithm.cpp index 4b9db05..a727b1c 100644 --- a/IntervalManagement/IMAlgorithm.cpp +++ b/IntervalManagement/IMAlgorithm.cpp @@ -21,7 +21,6 @@ #include #include "public/CustomMath.h" #include "public/CoreUtils.h" -#include "public/AircraftCalculations.h" #include "imalgs/IMAlgorithm.h" #include "imalgs/InternalObserver.h" @@ -77,6 +76,7 @@ IMAlgorithm::IMAlgorithm() m_loaded_speed_quantize_middle_phase(IMUtils::SPEED_QUANTIZE_2_DEFAULT), m_loaded_speed_quantize_first_phase(IMUtils::SPEED_QUANTIZE_3_DEFAULT), m_loaded_use_speed_limiting(IMUtils::LIMIT_FLAG_DEFAULT), + m_loaded_use_wind_blending(IMUtils::WIND_BLENDING_FLAG_DEFAULT), m_loaded_use_speed_quantization(IMUtils::QUANTIZE_FLAG_DEFAULT) { IterClearIMAlg(); } @@ -151,6 +151,7 @@ void IMAlgorithm::Copy(const IMAlgorithm &obj) { m_loaded_speed_quantize_middle_phase = obj.m_loaded_speed_quantize_middle_phase; m_loaded_speed_quantize_first_phase = obj.m_loaded_speed_quantize_first_phase; m_loaded_use_speed_limiting = obj.m_loaded_use_speed_limiting; + m_loaded_use_wind_blending = obj.m_loaded_use_wind_blending; m_loaded_use_speed_quantization = obj.m_loaded_use_speed_quantization; } @@ -161,6 +162,7 @@ void IMAlgorithm::CopyParametersFromConfiguration() { m_slope = m_configuration.m_slope; m_error_distance = m_configuration.m_error_distance; m_loaded_use_speed_limiting = m_configuration.m_use_speed_limiting; + m_loaded_use_wind_blending = m_configuration.m_use_wind_blending; m_threshold_flag = m_configuration.m_threshold_flag; m_loaded_use_speed_quantization = m_configuration.m_use_speed_quantization; m_loaded_middle_to_final_quantize_transition_distance = @@ -205,10 +207,8 @@ aaesim::open_source::Guidance IMAlgorithm::Update( return prevguidance; } -void IMAlgorithm::SetPilotDelay(const bool pilot_delay_on, const Units::Time pilot_delay_mean, - const Units::Time pilot_delay_standard_deviation) { - m_pilot_delay.SetUsePilotDelay(pilot_delay_on); - m_pilot_delay.SetPilotDelayParameters(pilot_delay_mean, pilot_delay_standard_deviation); +void IMAlgorithm::SetPilotDelay(aaesim::open_source::StatisticalPilotDelay &pilot_delay) { + m_pilot_delay = std::move(pilot_delay); } void IMAlgorithm::IterClearIMAlg() { @@ -282,7 +282,5 @@ void IMAlgorithm::DumpParameters(const std::string ¶meters_to_print) { LOG4CPLUS_DEBUG(IMAlgorithm::m_logger, "error distance " << Units::NauticalMilesLength(m_error_distance).value() << std::endl); - m_pilot_delay.DumpParameters("mPilotDelay"); - LOG4CPLUS_DEBUG(IMAlgorithm::m_logger, "++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++" << std::endl); } diff --git a/IntervalManagement/IMClearance.cpp b/IntervalManagement/IMClearance.cpp index 993b858..6c030f7 100644 --- a/IntervalManagement/IMClearance.cpp +++ b/IntervalManagement/IMClearance.cpp @@ -18,33 +18,19 @@ // **************************************************************************** #include "imalgs/IMClearance.h" + +#include + #include "imalgs/IMClearanceLoader.h" -#include "utility/CustomUnits.h" -#include "loader/LoadError.h" #include "imalgs/IMUtils.h" -#include +#include "loader/LoadError.h" +#include "utility/CustomUnits.h" using namespace std; using namespace interval_management::open_source; log4cplus::Logger IMClearance::m_logger = log4cplus::Logger::getInstance(LOG4CPLUS_TEXT("IMClearance")); -IMClearance::IMClearance() - : m_clearance_type(CUSTOM), - m_assigned_spacing_goal_type(TIME), - m_target_aircraft_intent(), - m_ownship_intent(), - m_final_approach_spacing_merge_angle_mean(0), - m_final_approach_spacing_merge_angle_std(0), - m_achieve_by_point(), - m_planned_termination_point(), - m_traffic_reference_point(), - m_assigned_spacing_goal(-INFINITY), - m_target_id(IMUtils::UNINITIALIZED_AIRCRAFT_ID), - m_is_vector_aircraft(false), - m_is_coincident_route_pairing(false), - m_valid(false) {} - IMClearance::IMClearance(const IMClearance::Builder *builder) { m_clearance_type = builder->GetClearanceType(); m_assigned_spacing_goal_type = builder->GetSpacingGoalType(); @@ -61,42 +47,14 @@ IMClearance::IMClearance(const IMClearance::Builder *builder) { m_is_coincident_route_pairing = false; } -IMClearance::IMClearance(const IMClearance &obj) { *this = obj; } - -bool IMClearance::operator==(const IMClearance &obj) const { - return ((m_valid == obj.m_valid) && (m_clearance_type == obj.m_clearance_type) && (m_target_id == obj.m_target_id) && - (m_traffic_reference_point == obj.m_traffic_reference_point) && - (m_achieve_by_point == obj.m_achieve_by_point) && - (m_assigned_spacing_goal_type == obj.m_assigned_spacing_goal_type) && - (m_assigned_spacing_goal == obj.m_assigned_spacing_goal) && - (m_planned_termination_point == obj.m_planned_termination_point) && - (m_final_approach_spacing_merge_angle_mean == obj.m_final_approach_spacing_merge_angle_mean) && - (m_final_approach_spacing_merge_angle_std == obj.m_final_approach_spacing_merge_angle_std) && - (m_is_vector_aircraft == obj.m_is_vector_aircraft) && - (m_is_coincident_route_pairing == obj.m_is_coincident_route_pairing) && - (m_target_aircraft_intent == obj.m_target_aircraft_intent) && (m_ownship_intent == obj.m_ownship_intent)); -} - -bool IMClearance::operator!=(const IMClearance &obj) const { return (!(operator==(obj))); } - bool IMClearance::Validate(const AircraftIntent &ownship_aircraft_intent, const IMUtils::IMAlgorithmTypes im_algorithm_type) { if (im_algorithm_type == IMUtils::IMAlgorithmTypes::TESTSPEEDCONTROL || - im_algorithm_type == IMUtils::IMAlgorithmTypes::RTA || - im_algorithm_type == IMUtils::IMAlgorithmTypes::RTA_TOAC_NOT_IMALGORITHM || im_algorithm_type == IMUtils::IMAlgorithmTypes::NONE) { m_valid = true; return m_valid; } - static const IMClearance empty; - if (*this == empty) { - m_valid = false; - std::string msg = "The clearance object is null. Something in the input is wrong."; - LOG4CPLUS_FATAL(m_logger, msg); - throw LoadError(msg); - } - if (im_algorithm_type == IMUtils::IMAlgorithmTypes::NONE && m_clearance_type == ClearanceType::NONE) { m_valid = true; return m_valid; @@ -150,7 +108,7 @@ IMClearance::SpacingGoalType IMClearance::GetAssignedSpacingGoalType() const { r Units::Time IMClearance::GetAssignedTimeSpacingGoal() const { if (m_assigned_spacing_goal_type != SpacingGoalType::TIME) { char msg[100]; - sprintf(msg, "Spacing goal type is %d", m_assigned_spacing_goal_type); + snprintf(msg, sizeof(msg), "Spacing goal type is %d", m_assigned_spacing_goal_type); LOG4CPLUS_FATAL(m_logger, msg); throw logic_error(msg); } @@ -164,30 +122,13 @@ const string &IMClearance::GetTrafficReferencePoint() const { return m_traffic_r Units::Length IMClearance::GetAssignedDistanceSpacingGoal() const { if (m_assigned_spacing_goal_type != SpacingGoalType::DIST) { char msg[100]; - sprintf(msg, "Spacing goal type is %d", m_assigned_spacing_goal_type); + snprintf(msg, sizeof(msg), "Spacing goal type is %d", m_assigned_spacing_goal_type); LOG4CPLUS_FATAL(m_logger, msg); throw logic_error(msg); } return Units::NauticalMilesLength(m_assigned_spacing_goal); } -void IMClearance::dump(string hdr) const { - string clearstr = IMClearanceLoader::GetClearanceString(m_clearance_type); - string spacingstr = IMClearanceLoader::GetAssignedSpacingTypeString(m_assigned_spacing_goal_type); - - LOG4CPLUS_DEBUG(IMClearance::m_logger, hdr); - LOG4CPLUS_DEBUG(IMClearance::m_logger, "Clearance type " << clearstr.c_str()); - LOG4CPLUS_DEBUG(IMClearance::m_logger, "Target id " << m_target_id); - LOG4CPLUS_DEBUG(IMClearance::m_logger, "Target reference pt " << m_traffic_reference_point.c_str()); - LOG4CPLUS_DEBUG(IMClearance::m_logger, "Achieve Pt " << m_achieve_by_point.c_str()); - LOG4CPLUS_DEBUG(IMClearance::m_logger, "Planned Termination Pt " << m_planned_termination_point.c_str()); - LOG4CPLUS_DEBUG(IMClearance::m_logger, "Spacing goal type " << spacingstr.c_str()); - LOG4CPLUS_DEBUG(IMClearance::m_logger, "Spacing goal " << m_assigned_spacing_goal); - LOG4CPLUS_DEBUG( - IMClearance::m_logger, - "Final Approach merge angle" << Units::DegreesAngle(m_final_approach_spacing_merge_angle_mean).value()); -} - const bool IMClearance::IsCoincidentRoutePairing() const { return m_is_coincident_route_pairing; } const Units::Angle IMClearance::GetFinalApproachSpacingMergeAngleMean() const { @@ -274,8 +215,7 @@ bool IMClearance::ValidateBasicInputs(const AircraftIntent &ownship_aircraft_int /* * NOTE: All statements in here should throw when a check fails. */ - if (im_algorithm_type == IMUtils::IMAlgorithmTypes::RTA || - im_algorithm_type == IMUtils::IMAlgorithmTypes::TESTSPEEDCONTROL || + if (im_algorithm_type == IMUtils::IMAlgorithmTypes::TESTSPEEDCONTROL || im_algorithm_type == IMUtils::IMAlgorithmTypes::NONE) return true; @@ -326,7 +266,6 @@ void IMClearance::SetFinalApproachSpacingMergeAngleStd(Units::Angle final_approa bool IMClearance::ValidatePlannedTerminationPoint(const AircraftIntent &ownship_aircraft_intent, const IMUtils::IMAlgorithmTypes im_algorithm_type) const { - if (ownship_aircraft_intent.GetWaypointIndexByName(m_planned_termination_point) < 0) { const string msg = "The planned termination point, " + m_planned_termination_point + ", was not found in the ownship intent."; diff --git a/IntervalManagement/IMClearanceLoader.cpp b/IntervalManagement/IMClearanceLoader.cpp index 4304d05..713d63e 100644 --- a/IntervalManagement/IMClearanceLoader.cpp +++ b/IntervalManagement/IMClearanceLoader.cpp @@ -126,7 +126,11 @@ const IMClearance IMClearanceLoader::BuildClearance(void) { IMClearance::Builder builder(m_clearance_type, m_target_id, m_target_aircraft_intent, m_achieve_by_point, m_planned_termination_point, m_assigned_spacing_goal_type, m_assigned_spacing_goal); builder.TrafficReferencePoint(m_traffic_reference_point); - if (m_ownship_aircraft_intent.IsLoaded()) builder.OwnshipIntent(m_ownship_aircraft_intent); + + if (m_ownship_aircraft_intent.IsLoaded()) { + builder.OwnshipIntent(m_ownship_aircraft_intent); + } + if (m_clearance_type == IMClearance::ClearanceType::FAS) { builder.FinalApproachParameters(m_final_approach_spacing_merge_angle_mean, m_final_approach_spacing_merge_angle_std, m_is_vector_aircraft); diff --git a/IntervalManagement/IMDistBasedAchieve.cpp b/IntervalManagement/IMDistBasedAchieve.cpp index f2922b2..01c9d50 100644 --- a/IntervalManagement/IMDistBasedAchieve.cpp +++ b/IntervalManagement/IMDistBasedAchieve.cpp @@ -22,9 +22,9 @@ #include "public/SimulationTime.h" #include "public/CustomMath.h" #include "public/CoreUtils.h" -#include "public/AircraftCalculations.h" using namespace interval_management::open_source; +using namespace aaesim::open_source; log4cplus::Logger IMDistBasedAchieve::m_logger = log4cplus::Logger::getInstance(LOG4CPLUS_TEXT("IMDistBasedAchieve")); const Units::Length IMDistBasedAchieve::DEFAULT_DISTANCE_BASED_ASSIGNED_SPACING_GOAL = Units::NegInfinity(); @@ -50,6 +50,14 @@ void IMDistBasedAchieve::Copy(const IMDistBasedAchieve &obj) { IMDistBasedAchieve::~IMDistBasedAchieve() = default; +void IMDistBasedAchieve::Initialize(const OwnshipPredictionParameters &ownship_prediction_parameters, + const AircraftIntent &ownship_aircraft_intent, + aaesim::open_source::WeatherPrediction &weather_prediction, + std::shared_ptr &position_converter) { + SetTangentPlaneSequence(position_converter); + Initialize(ownship_prediction_parameters, ownship_aircraft_intent, weather_prediction); +} + void IMDistBasedAchieve::Initialize(const OwnshipPredictionParameters &ownship_prediction_parameters, const AircraftIntent &ownship_aircraft_intent, aaesim::open_source::WeatherPrediction &weather_prediction) { @@ -387,7 +395,7 @@ aaesim::open_source::Guidance IMDistBasedAchieve::Update( // Get target's along path position on ownship's path as an AircraftState object const Units::Length target_distance_with_asg = target_distance_along_ownships_path + m_assigned_spacing_goal; - Units::FeetLength target_projected_position_with_asg_x, target_projected_position_with_asg_y; + Units::FeetLength target_projected_position_with_asg_x(0), target_projected_position_with_asg_y(0); Units::UnsignedAngle course; m_position_calculator_target_on_ownship_hpath.CalculatePositionFromAlongPathDistance( target_distance_with_asg, target_projected_position_with_asg_x, target_projected_position_with_asg_y, @@ -402,6 +410,20 @@ aaesim::open_source::Guidance IMDistBasedAchieve::Update( m_target_state_projected_on_ownships_path_at_adjusted_distance.m_zd = current_target_state.m_zd; if (!m_transitioned_to_maintain) { + // Copy Atmosphere to maintain algorithm if it doesn't have one + if (!m_im_kinematic_dist_based_maintain->GetAtmosphere()) { + m_im_kinematic_dist_based_maintain->SetAtmosphere( + std::shared_ptr(GetAtmosphere()->Clone())); + } + if (m_im_kinematic_dist_based_maintain->GetAtmosphere()->GetTemperatureOffset() != + GetAtmosphere()->GetTemperatureOffset()) { + LOG4CPLUS_WARN(m_logger, + "Different atmospheric temperature offsets in Achieve (" + << GetAtmosphere()->GetTemperatureOffset() << ") vs Maintain (" + << m_im_kinematic_dist_based_maintain->GetAtmosphere()->GetTemperatureOffset() + << ")."); + } + m_im_kinematic_dist_based_maintain->Prepare( m_previous_reference_im_speed_command_tas, m_previous_im_speed_command_ias, m_speed_limiter, m_previous_reference_im_speed_command_mach, m_ownship_kinematic_trajectory_predictor, diff --git a/IntervalManagement/IMKinematicAchieve.cpp b/IntervalManagement/IMKinematicAchieve.cpp index 894a3bd..bdff8ff 100644 --- a/IntervalManagement/IMKinematicAchieve.cpp +++ b/IntervalManagement/IMKinematicAchieve.cpp @@ -22,23 +22,22 @@ #include #include "MiniCSV/minicsv.h" +#include "public/Atmosphere.h" +#include "imalgs/IMClearanceLoader.h" #include "public/CustomMath.h" #include "public/AircraftCalculations.h" #include "public/CoreUtils.h" #include "public/Environment.h" #include "public/Wind.h" -#include "public/StandardAtmosphere.h" #include "public/ScenarioUtils.h" -#include "imalgs/IMClearanceLoader.h" -using namespace interval_management; using namespace interval_management::open_source; +using namespace aaesim::open_source; log4cplus::Logger IMKinematicAchieve::logger = log4cplus::Logger::getInstance(LOG4CPLUS_TEXT("IMKinematicAchieve")); const int IMKinematicAchieve::MINIMUM_FAS_TRACK_COUNT(5); const Units::FeetLength IMKinematicAchieve::TARGET_ALTITUDE_TOLERANCE(3000); const Units::SecondsTime IMKinematicAchieve::TRACK_ANGLE_TAU(3.0); -const bool IMKinematicAchieve::BLEND_WIND_DEFAULT(true); IMKinematicAchieve::IMKinematicAchieve() : m_ownship_kinematic_trajectory_predictor(), @@ -61,7 +60,6 @@ IMKinematicAchieve::IMKinematicAchieve() m_target_history_exists(false), m_is_target_aligned(false), m_new_trajectory_prediction_available(), - m_blend_wind(IMKinematicAchieve::BLEND_WIND_DEFAULT), m_tangent_plane_sequence(nullptr) { IterClearIMKinAch(); } @@ -98,7 +96,6 @@ void IMKinematicAchieve::Initialize(const OwnshipPredictionParameters &ownship_p aaesim::open_source::WeatherPrediction &weather_prediction) { IMAchieve::Initialize(ownship_prediction_parameters, ownship_aircraft_intent, weather_prediction); - SetTangentPlaneSequence(std::move(ownship_aircraft_intent.GetTangentPlaneSequence())); m_ownship_track_angle_history.clear(); m_target_track_angle_history.clear(); m_fas_intent_valid = false; @@ -113,9 +110,7 @@ void IMKinematicAchieve::Initialize(const OwnshipPredictionParameters &ownship_p ownship_prediction_parameters.aerodynamics, quantizer); // --- Prepare Ownship objects ------------- - // Strip any ownship waypoints after the PTP m_ownship_aircraft_intent = ownship_aircraft_intent; - TrimAircraftIntentAfterWaypoint(m_ownship_aircraft_intent, m_im_clearance.GetPlannedTerminationPoint()); // Create the kinematic trajectory predictors using ownship assumptions m_ownship_kinematic_trajectory_predictor = aaesim::open_source::KinematicTrajectoryPredictor( @@ -152,42 +147,14 @@ void IMKinematicAchieve::Initialize(const OwnshipPredictionParameters &ownship_p } void IMKinematicAchieve::ResetDefaults() { - if (m_blend_wind != IMKinematicAchieve::BLEND_WIND_DEFAULT) { - LOG4CPLUS_WARN(logger, "mBlendWind reset to " << IMKinematicAchieve::BLEND_WIND_DEFAULT << RESET_MSG); - m_blend_wind = IMKinematicAchieve::BLEND_WIND_DEFAULT; + if (m_loaded_use_wind_blending != IMUtils::WIND_BLENDING_FLAG_DEFAULT) { + LOG4CPLUS_WARN(logger, "mBlendWind reset to " << IMUtils::WIND_BLENDING_FLAG_DEFAULT << RESET_MSG); + m_loaded_use_wind_blending = IMUtils::WIND_BLENDING_FLAG_DEFAULT; } IMAchieve::ResetDefaults(); } -void IMKinematicAchieve::TrimAircraftIntentAfterWaypoint(AircraftIntent &aircraft_intent, - const std::string &waypoint_name) { - - if (waypoint_name.empty()) return; // can only trim a real waypoint - - int index = 0; - const int maxIndex = aircraft_intent.GetNumberOfWaypoints(); - - LOG4CPLUS_DEBUG(logger, "Trim AircraftIntent to " << waypoint_name); - LOG4CPLUS_TRACE(logger, "Intent before trim:" << std::endl << aircraft_intent); - - for (index = 0; index < maxIndex; index++) { - if (aircraft_intent.GetWaypointName(index) == waypoint_name) { - aircraft_intent.SetNumberOfWaypoints(index + 1); - break; - } - } - - LOG4CPLUS_TRACE(logger, "Intent after trim:" << std::endl << aircraft_intent); - - if (index >= maxIndex) { - // NOTE: This is here for completeness. IMClearance::validate() should ensure this cannot occur. - std::string msg = "ERROR: Could not find trim point (" + waypoint_name + ") in list of Waypoints"; - LOG4CPLUS_ERROR(IMKinematicAchieve::logger, msg); - throw std::logic_error(msg); - } -} - bool IMKinematicAchieve::load(DecodedStream *input) { set_stream(input); @@ -268,7 +235,7 @@ void IMKinematicAchieve::HandleTrajectoryPrediction( m_compute_ownship_kinematic_trajectory || m_compute_target_kinematic_trajectory; if (m_compute_ownship_kinematic_trajectory) { - LOG4CPLUS_DEBUG(logger, "compute ownship kinematic trajectory: " << owntruthstate.GetId()); + LOG4CPLUS_TRACE(logger, "compute ownship kinematic trajectory: " << owntruthstate.GetId()); m_ownship_kinematic_trajectory_predictor.CalculateWaypoints(m_ownship_aircraft_intent, m_weather_prediction); std::vector ownship_horizontal_path = @@ -282,13 +249,14 @@ void IMKinematicAchieve::HandleTrajectoryPrediction( Units::RadiansAngle dummy_course = Units::RadiansAngle(0); std::vector::size_type dummy_index = 0; if (!AircraftCalculations::CalculateDistanceAlongPathFromPosition( - Units::FeetLength(owntruthstate.m_x), Units::FeetLength(owntruthstate.m_y), ownship_horizontal_path, 0, + owntruthstate.GetPositionX(), owntruthstate.GetPositionY(), ownship_horizontal_path, 0, ownship_distance_to_go, dummy_course, dummy_index)) { ownship_distance_to_go = Units::MetersLength(Units::infinity()); } - m_ownship_kinematic_trajectory_predictor.BuildTrajectoryPrediction( - m_weather_prediction, Units::FeetLength(owntruthstate.m_z), ownship_distance_to_go); + m_ownship_kinematic_trajectory_predictor.BuildTrajectoryPrediction(m_weather_prediction, m_tangent_plane_sequence, + Units::FeetLength(owntruthstate.m_z), + ownship_distance_to_go); std::shared_ptr kinematic_descent_4d_predictor = m_ownship_kinematic_trajectory_predictor.GetKinematicDescent4dPredictor(); @@ -315,9 +283,9 @@ void IMKinematicAchieve::HandleTrajectoryPrediction( if (m_target_aircraft_exists && m_target_history_exists && m_compute_target_kinematic_trajectory) { if (!InAchieveStage()) { - LOG4CPLUS_DEBUG(logger, "Skipping target prediction for ac " << owntruthstate.GetId()); + LOG4CPLUS_TRACE(logger, "Skipping target prediction for ac " << owntruthstate.GetId()); } else { - LOG4CPLUS_DEBUG(logger, "ac " << owntruthstate.GetId() + LOG4CPLUS_TRACE(logger, "ac " << owntruthstate.GetId() << " compute target kinematic trajectory: " << targetsyncstate.GetId()); const Units::Speed targetgroundspeed = Units::FeetPerSecondSpeed(sqrt(pow(targetsyncstate.m_xd, 2) + pow(targetsyncstate.m_yd, 2))); @@ -326,10 +294,10 @@ void IMKinematicAchieve::HandleTrajectoryPrediction( // Get Winds at Target Aircraft's current altitude Units::MetersPerSecondSpeed Vwx, Vwy; Units::HertzFrequency dVwx_dh, dVwy_dh; - m_weather_prediction.getAtmosphere()->CalculateWindGradientAtAltitude( - Units::FeetLength(targetsyncstate.m_z), m_weather_prediction.east_west, Vwx, dVwx_dh); - m_weather_prediction.getAtmosphere()->CalculateWindGradientAtAltitude( - Units::FeetLength(targetsyncstate.m_z), m_weather_prediction.north_south, Vwy, dVwy_dh); + m_weather_prediction.east_west().CalculateWindGradientAtAltitude(Units::FeetLength(targetsyncstate.m_z), Vwx, + dVwx_dh); + m_weather_prediction.north_south().CalculateWindGradientAtAltitude(Units::FeetLength(targetsyncstate.m_z), Vwy, + dVwy_dh); // calculate wind parallel and perpendicular to the ground track Units::Speed Vw_para = Vwx * cos(targetgroundtrack) + Vwy * sin(targetgroundtrack); @@ -395,8 +363,8 @@ void IMKinematicAchieve::HandleTrajectoryPrediction( if (!IsTargetPassedTrp()) { if (!AircraftCalculations::CalculateDistanceAlongPathFromPosition( - Units::FeetLength(targetsyncstate.m_x), Units::FeetLength(targetsyncstate.m_y), - target_horizontal_path, 0, target_distance_to_go, dummy_course, dummy_index)) { + targetsyncstate.GetPositionX(), targetsyncstate.GetPositionY(), target_horizontal_path, 0, + target_distance_to_go, dummy_course, dummy_index)) { target_distance_to_go = Units::MetersLength(Units::infinity()); } } @@ -408,7 +376,7 @@ void IMKinematicAchieve::HandleTrajectoryPrediction( size_t index; interval_management::open_source::AchievePointCalcs::ComputeDefaultTRP( m_ownship_kinematic_achieve_by_calcs, m_ownship_aircraft_intent, m_target_aircraft_intent, - target_horizontal_path, m_traffic_reference_point, x, y, index); + m_tangent_plane_sequence, target_horizontal_path, m_traffic_reference_point, x, y, index); m_target_aircraft_intent.InsertWaypointAtIndex(m_traffic_reference_point, index); } else { // retrieve TRP from target intent @@ -416,13 +384,14 @@ void IMKinematicAchieve::HandleTrajectoryPrediction( m_traffic_reference_point = m_target_aircraft_intent.GetWaypoint(trp_index); } - TrimAircraftIntentAfterWaypoint(m_target_aircraft_intent, m_traffic_reference_point.GetName()); + m_target_aircraft_intent = AircraftIntent::CopyAndTrimAfterNamedWaypoint(m_target_aircraft_intent, + m_traffic_reference_point.GetName()); SetTrafficReferencePointConstraints(owntruthstate, targetsyncstate); m_target_kinematic_trajectory_predictor.CalculateWaypoints(m_target_aircraft_intent, m_weather_prediction); m_target_kinematic_trajectory_predictor.BuildTrajectoryPrediction( - m_weather_prediction, target_start_altitude_msl, target_distance_to_go); + m_weather_prediction, m_tangent_plane_sequence, target_start_altitude_msl, target_distance_to_go); m_target_kinematic_traffic_reference_point_calcs = interval_management::open_source::AchievePointCalcs( m_im_clearance.GetTrafficReferencePoint(), m_target_aircraft_intent, m_target_kinematic_trajectory_predictor.GetKinematicDescent4dPredictor()->GetVerticalPath(), @@ -474,18 +443,19 @@ void IMKinematicAchieve::CheckPredictionAccuracy( m_ownship_kinematic_trajectory_predictor.GetVerticalPathAltitudes())); } - std::shared_ptr sensed_atmosphere(StandardAtmosphere::MakeInstance( - owntruthstate.GetSensedTemperature(), Units::FeetLength(owntruthstate.m_z))); + std::shared_ptr sensed_atmosphere(GetAtmosphere()->Clone()); + sensed_atmosphere->CalibrateTemperatureAtAltitude(owntruthstate.GetSensedTemperature(), + Units::FeetLength(owntruthstate.m_z)); - if (!m_predicted_wind_evaluator->ArePredictedWindsAccurate( + if (!m_predicted_wind_evaluator->ArePredictedWindsAccurate( // IMUtils::ConvertToAaesimAircraftState(owntruthstate), m_weather_prediction, m_ownship_reference_cas, - m_ownship_reference_altitude, sensed_atmosphere.get())) { + m_ownship_reference_altitude, sensed_atmosphere)) { - Wind::UpdatePredictedWindsAtAltitudeFromSensedWind(IMUtils::ConvertToAaesimAircraftState(owntruthstate), - m_weather_prediction); + m_wind_blender->BlendSensedWithPredicted(IMUtils::ConvertToAaesimAircraftState(owntruthstate), + m_weather_prediction); m_compute_ownship_kinematic_trajectory = true; m_weather_prediction.SetAtmosphere(sensed_atmosphere); - LOG4CPLUS_DEBUG(logger, "Bad wind prediction at t=" + LOG4CPLUS_TRACE(logger, "Bad wind prediction at t=" << owntruthstate.GetTimeStamp().value() << ", target ac exists=" << m_target_aircraft_exists << ", ref_cas=" << m_ownship_reference_cas << ", ref_alt=" << Units::FeetLength(m_ownship_reference_altitude) @@ -522,7 +492,7 @@ void IMKinematicAchieve::CheckPredictionAccuracy( if (++m_target_altitude_failure_count >= 2) { m_compute_target_kinematic_trajectory = true; } - LOG4CPLUS_DEBUG(logger, "Target altitude ref=" << Units::FeetLength(m_target_reference_altitude) + LOG4CPLUS_TRACE(logger, "Target altitude ref=" << Units::FeetLength(m_target_reference_altitude) << ", true=" << Units::FeetLength(targettruthstate.m_z) << ", " << m_target_altitude_failure_count << " iterations at t=" << targettruthstate.GetTimeStamp().value()); @@ -535,17 +505,16 @@ void IMKinematicAchieve::CheckPredictionAccuracy( void IMKinematicAchieve::CalculateOwnshipDtgToPlannedTerminationPoint( const interval_management::open_source::AircraftState ¤t_ownship_state) { - m_ownship_distance_calculator.CalculateAlongPathDistanceFromPosition(Units::FeetLength(current_ownship_state.m_x), - Units::FeetLength(current_ownship_state.m_y), - m_ownship_kinematic_dtg_to_ptp); + m_ownship_distance_calculator.CalculateAlongPathDistanceFromPosition( + current_ownship_state.GetPositionX(), current_ownship_state.GetPositionY(), m_ownship_kinematic_dtg_to_ptp); } void IMKinematicAchieve::CalculateTargetDtgToImPoints( const interval_management::open_source::AircraftState ¤t_lead_state) { if (m_target_aircraft_exists) { if (!IsTargetPassedLastWaypoint()) { - m_target_distance_calculator.CalculateAlongPathDistanceFromPosition(Units::FeetLength(current_lead_state.m_x), - Units::FeetLength(current_lead_state.m_y), + m_target_distance_calculator.CalculateAlongPathDistanceFromPosition(current_lead_state.GetPositionX(), + current_lead_state.GetPositionY(), m_target_kinematic_dtg_to_last_waypoint); if (!IsTargetPassedTrp()) { m_target_kinematic_dtg_to_trp = m_target_kinematic_dtg_to_last_waypoint - @@ -824,7 +793,7 @@ void IMKinematicAchieve::ComputeFASTrajectories( using namespace std; - LOG4CPLUS_DEBUG(logger, "Calculating FAS trajectories on AC " << owntruthstate.GetId() << " at " + LOG4CPLUS_TRACE(logger, "Calculating FAS trajectories on AC " << owntruthstate.GetId() << " at " << owntruthstate.GetTimeStamp().value()); // find merge point @@ -835,7 +804,7 @@ void IMKinematicAchieve::ComputeFASTrajectories( size_t target_end_waypoint_ix = m_target_aircraft_intent.GetNumberOfWaypoints() - 1; Waypoint target_end_waypoint{m_target_aircraft_intent.GetWaypoint(target_end_waypoint_ix)}; - LOG4CPLUS_DEBUG(logger, "Achieve by = " << achieve_by_waypoint_name); + LOG4CPLUS_TRACE(logger, "Achieve by = " << achieve_by_waypoint_name); Units::Angle merge_angle_mean; Units::MetersLength delta_y, delta_x; @@ -843,23 +812,21 @@ void IMKinematicAchieve::ComputeFASTrajectories( merge_angle_mean = IMUtils::CalculateTrackAngle(m_ownship_track_angle_history); x2 = m_ownship_aircraft_intent.GetWaypointX(achieve_by_waypoint_index); y2 = m_ownship_aircraft_intent.GetWaypointY(achieve_by_waypoint_index); - x3 = Units::FeetLength(owntruthstate.m_x); - y3 = Units::FeetLength(owntruthstate.m_y); - delta_x = Units::FeetLength(targettruthstate.m_x) - m_target_aircraft_intent.GetWaypointX(target_end_waypoint_ix); - delta_y = Units::FeetLength(targettruthstate.m_y) - m_target_aircraft_intent.GetWaypointY(target_end_waypoint_ix); + x3 = owntruthstate.GetPositionX(); + y3 = owntruthstate.GetPositionY(); + delta_x = targettruthstate.GetPositionX() - m_target_aircraft_intent.GetWaypointX(target_end_waypoint_ix); + delta_y = targettruthstate.GetPositionY() - m_target_aircraft_intent.GetWaypointY(target_end_waypoint_ix); } else { merge_angle_mean = IMUtils::CalculateTrackAngle(m_target_track_angle_history); x2 = m_target_aircraft_intent.GetWaypointX(target_end_waypoint_ix); y2 = m_target_aircraft_intent.GetWaypointY(target_end_waypoint_ix); - x3 = Units::FeetLength(targettruthstate.m_x); - y3 = Units::FeetLength(targettruthstate.m_y); - delta_x = - Units::FeetLength(owntruthstate.m_x) - m_ownship_aircraft_intent.GetWaypointX(achieve_by_waypoint_index); - delta_y = - Units::FeetLength(owntruthstate.m_y) - m_ownship_aircraft_intent.GetWaypointY(achieve_by_waypoint_index); + x3 = targettruthstate.GetPositionX(); + y3 = targettruthstate.GetPositionY(); + delta_x = owntruthstate.GetPositionX() - m_ownship_aircraft_intent.GetWaypointX(achieve_by_waypoint_index); + delta_y = owntruthstate.GetPositionY() - m_ownship_aircraft_intent.GetWaypointY(achieve_by_waypoint_index); } Units::DegreesAngle reverse_final_approach_angle = Units::arctan2(delta_y.value(), delta_x.value()); - LOG4CPLUS_DEBUG(logger, reverse_final_approach_angle); + LOG4CPLUS_TRACE(logger, reverse_final_approach_angle); x1 = x2 + Units::NauticalMilesLength(50) * Units::cos(reverse_final_approach_angle); y1 = y2 + Units::NauticalMilesLength(50) * Units::sin(reverse_final_approach_angle); @@ -868,14 +835,14 @@ void IMKinematicAchieve::ComputeFASTrajectories( Units::Angle merge_angle = aaesim::open_source::ScenarioUtils::RANDOM_NUMBER_GENERATOR.GaussianSample(merge_angle_mean, merge_angle_std); Units::DegreesAngle final_approach_angle = reverse_final_approach_angle + Units::PI_RADIANS_ANGLE; - LOG4CPLUS_DEBUG(logger, "Average track angle = " << Units::DegreesAngle(merge_angle_mean) + LOG4CPLUS_TRACE(logger, "Average track angle = " << Units::DegreesAngle(merge_angle_mean) << ", randomized merge angle = " << Units::DegreesAngle(merge_angle) << ", final_approach_angle = " << final_approach_angle); Units::MetersLength xMerge, yMerge; IMUtils::CalculateMergePoint(x1, y1, x2, y2, x3, y3, xMerge, yMerge, merge_angle); - LOG4CPLUS_DEBUG(logger, "Merge point is (" << xMerge << "," << yMerge << ")"); + LOG4CPLUS_TRACE(logger, "Merge point is (" << xMerge << "," << yMerge << ")"); // construct intents // final AC: current_pos last_wp @@ -900,15 +867,15 @@ void IMKinematicAchieve::ComputeFASTrajectories( CoreUtils::CalculateEuclideanDistance(current_coords, final_wpt_coords); Units::MetersLength current_to_merge_distance = CoreUtils::CalculateEuclideanDistance(current_coords, merge_coords); - LOG4CPLUS_DEBUG(logger, "current = (" << Units::MetersLength(current_coords.first) << "," + LOG4CPLUS_TRACE(logger, "current = (" << Units::MetersLength(current_coords.first) << "," << Units::MetersLength(current_coords.second) << ")"); - LOG4CPLUS_DEBUG(logger, "merge = (" << Units::MetersLength(merge_coords.first) << "," + LOG4CPLUS_TRACE(logger, "merge = (" << Units::MetersLength(merge_coords.first) << "," << Units::MetersLength(merge_coords.second) << ")"); - LOG4CPLUS_DEBUG(logger, "last wpt = (" << Units::MetersLength(final_wpt_coords.first) << "," + LOG4CPLUS_TRACE(logger, "last wpt = (" << Units::MetersLength(final_wpt_coords.first) << "," << Units::MetersLength(final_wpt_coords.second) << ")"); - LOG4CPLUS_DEBUG(logger, "Current to merge = " << current_to_merge_distance); - LOG4CPLUS_DEBUG(logger, "Current to lastwpt = " << current_to_lastwpt_distance); - LOG4CPLUS_DEBUG(logger, "Merge to lastwpt = " << merge_to_lastwpt_distance); + LOG4CPLUS_TRACE(logger, "Current to merge = " << current_to_merge_distance); + LOG4CPLUS_TRACE(logger, "Current to lastwpt = " << current_to_lastwpt_distance); + LOG4CPLUS_TRACE(logger, "Merge to lastwpt = " << merge_to_lastwpt_distance); // convert states to waypoints, using ownship's sensed winds own_intent.InsertWaypointAtIndex(MakeWaypointFromState(owntruthstate, owntruthstate.GetSensedWindEastComponent(), @@ -918,10 +885,10 @@ void IMKinematicAchieve::ComputeFASTrajectories( // Get predicted winds at target aircraft's current altitude & create waypoint Units::MetersPerSecondSpeed target_wind_x, target_wind_y; Units::HertzFrequency dVwx_dh, dVwy_dh; - m_weather_prediction.getAtmosphere()->CalculateWindGradientAtAltitude( - Units::FeetLength(targettruthstate.m_z), m_weather_prediction.east_west, target_wind_x, dVwx_dh); - m_weather_prediction.getAtmosphere()->CalculateWindGradientAtAltitude( - Units::FeetLength(targettruthstate.m_z), m_weather_prediction.north_south, target_wind_y, dVwy_dh); + m_weather_prediction.east_west().CalculateWindGradientAtAltitude(Units::FeetLength(targettruthstate.m_z), + target_wind_x, dVwx_dh); + m_weather_prediction.north_south().CalculateWindGradientAtAltitude(Units::FeetLength(targettruthstate.m_z), + target_wind_y, dVwy_dh); target_intent.InsertWaypointAtIndex(MakeWaypointFromState(targettruthstate, target_wind_x, target_wind_y), 0); const Units::DegreesAngle tolerance(10.0); @@ -955,18 +922,19 @@ void IMKinematicAchieve::ComputeFASTrajectories( m_fas_intent_valid = true; } -Waypoint IMKinematicAchieve::MakeWaypointFromState(const interval_management::open_source::AircraftState aircraft_state, - Units::Speed wind_x, Units::Speed wind_y) const { +Waypoint IMKinematicAchieve::MakeWaypointFromState( + const interval_management::open_source::AircraftState &aircraft_state, Units::Speed wind_x, + Units::Speed wind_y) const { // calculate fields needed for a Waypoint object // lat, lon EarthModel::LocalPositionEnu local_pos; - local_pos.x = Units::FeetLength(aircraft_state.m_x); - local_pos.y = Units::FeetLength(aircraft_state.m_y); - local_pos.z = Units::FeetLength(aircraft_state.m_z); + local_pos.x = aircraft_state.GetPositionX(); + local_pos.y = aircraft_state.GetPositionY(); + local_pos.z = aircraft_state.GetPositionZ(); EarthModel::GeodeticPosition geo_pos; - m_tangent_plane_sequence->convertLocalToGeodetic(local_pos, geo_pos); + m_tangent_plane_sequence->ConvertLocalToGeodetic(local_pos, geo_pos); // ias Units::Speed groundspeed_x = Units::FeetPerSecondSpeed(aircraft_state.m_xd) - wind_x; @@ -1016,7 +984,7 @@ void IMKinematicAchieve::SetTrafficReferencePointConstraints( // adjust TRP speed and altitude constraints - LOG4CPLUS_DEBUG(logger, "Old TRP constraints: " << m_traffic_reference_point); + LOG4CPLUS_TRACE(logger, "Old TRP constraints: " << m_traffic_reference_point); Units::KnotsSpeed target_trp_speed; Units::FeetLength target_trp_altitude; @@ -1079,12 +1047,10 @@ void IMKinematicAchieve::SetTrafficReferencePointConstraints( // assume CAS is "constant" in the intervening range: Speed case 1a Units::Speed estimated_wind_speed_x, estimated_wind_speed_y; Units::Frequency estimated_wind_gradient_x, estimated_wind_gradient_y; - m_weather_prediction.GetForecastAtmosphere()->CalculateWindGradientAtAltitude( - targetsyncstate.GetPositionZ(), m_weather_prediction.east_west, estimated_wind_speed_x, - estimated_wind_gradient_x); - m_weather_prediction.GetForecastAtmosphere()->CalculateWindGradientAtAltitude( - targetsyncstate.GetPositionZ(), m_weather_prediction.north_south, estimated_wind_speed_y, - estimated_wind_gradient_y); + m_weather_prediction.east_west().CalculateWindGradientAtAltitude( + targetsyncstate.GetPositionZ(), estimated_wind_speed_x, estimated_wind_gradient_x); + m_weather_prediction.north_south().CalculateWindGradientAtAltitude( + targetsyncstate.GetPositionZ(), estimated_wind_speed_y, estimated_wind_gradient_y); interval_management::open_source::AircraftState target_copy_state; target_copy_state.Create( @@ -1120,8 +1086,7 @@ void IMKinematicAchieve::SetTrafficReferencePointConstraints( m_traffic_reference_point.SetAltitudeConstraintHigh(target_trp_altitude); m_traffic_reference_point.SetAltitude(target_trp_altitude); } - m_traffic_reference_point.SetMach(0); - LOG4CPLUS_DEBUG(logger, "New TRP constraints: " << m_traffic_reference_point); + LOG4CPLUS_TRACE(logger, "New TRP constraints: " << m_traffic_reference_point); m_target_aircraft_intent.UpdateWaypoint(m_traffic_reference_point); } diff --git a/IntervalManagement/IMKinematicDistBasedMaintain.cpp b/IntervalManagement/IMKinematicDistBasedMaintain.cpp index 894c114..03f4926 100644 --- a/IntervalManagement/IMKinematicDistBasedMaintain.cpp +++ b/IntervalManagement/IMKinematicDistBasedMaintain.cpp @@ -20,7 +20,6 @@ #include "imalgs/IMKinematicDistBasedMaintain.h" #include "public/CustomMath.h" -#include "public/AircraftCalculations.h" #include "public/CoreUtils.h" #include "imalgs/InternalObserver.h" @@ -51,7 +50,7 @@ aaesim::open_source::Guidance IMKinematicDistBasedMaintain::Update( const std::vector &target_aircraft_state_history, const interval_management::open_source::AchievePointCalcs &ownship_achieve_point_calcs, const interval_management::open_source::AchievePointCalcs &traffic_reference_point_calcs, - PilotDelay &pilot_delay) { + aaesim::open_source::StatisticalPilotDelay &pilot_delay) { /* * Developer's note: In this level of the algorithm, all uses of the /target state/ @@ -62,8 +61,6 @@ aaesim::open_source::Guidance IMKinematicDistBasedMaintain::Update( aaesim::open_source::Guidance guidanceout = guidance_in; // target's along-path position on ownship's route (adjusted for ASG) - Units::Length target_projected_x(target_state_projected_on_ownships_path_at_adjusted_distance.GetPositionX()); - Units::Length target_projected_y(target_state_projected_on_ownships_path_at_adjusted_distance.GetPositionY()); Units::Length target_projected_dtg(target_dtg_along_ownships_path_at_adjusted_distance); Units::Length ownship_estimated_dtg; @@ -163,7 +160,7 @@ void IMKinematicDistBasedMaintain::CalculateIas( const Units::Length current_ownship_altitude, const Units::Length target_kinematic_dtg_to_end_of_route, const aaesim::open_source::DynamicsState &dynamics_state, const aaesim::open_source::KinematicTrajectoryPredictor &ownship_kinematic_trajectory_predictor, - PilotDelay &pilot_delay) { + aaesim::open_source::StatisticalPilotDelay &pilot_delay) { m_im_speed_command_ias = m_speed_limiter.LimitSpeedCommand( m_previous_im_speed_command_ias, m_im_speed_command_ias, ownship_kinematic_trajectory_predictor.GetVerticalPathCasByIndex(m_ownship_reference_lookup_index), @@ -187,7 +184,7 @@ void IMKinematicDistBasedMaintain::CalculateMach( const Units::Length current_ownship_altitude, const Units::Length target_kinematic_dtg_to_end_of_route, const Units::Speed true_airspeed_command, const aaesim::open_source::KinematicTrajectoryPredictor &ownship_kinematic_trajectory_predictor, - PilotDelay &pilot_delay, const Units::Mass current_mass) { + aaesim::open_source::StatisticalPilotDelay &pilot_delay, const Units::Mass current_mass) { Units::Speed nominal_profile_ias = Units::MetersPerSecondSpeed( ownship_kinematic_trajectory_predictor.GetVerticalPathVelocityByIndex(m_ownship_reference_lookup_index)); diff --git a/IntervalManagement/IMKinematicTimeBasedMaintain.cpp b/IntervalManagement/IMKinematicTimeBasedMaintain.cpp index 0edceda..99c8186 100644 --- a/IntervalManagement/IMKinematicTimeBasedMaintain.cpp +++ b/IntervalManagement/IMKinematicTimeBasedMaintain.cpp @@ -23,7 +23,6 @@ #include "public/CustomMath.h" #include "public/SimulationTime.h" -#include "public/AircraftCalculations.h" #include "public/CoreUtils.h" #include "imalgs/InternalObserver.h" @@ -45,7 +44,8 @@ aaesim::open_source::Guidance IMKinematicTimeBasedMaintain::Update( const std::vector &target_aircraft_state_history, const interval_management::open_source::AchievePointCalcs &ownship_achieve_point_calcs, const interval_management::open_source::AchievePointCalcs &traffic_reference_point_calcs, - PilotDelay &pilot_delay_model, const Units::Length &target_kinematic_dtg_to_end_of_route) { + aaesim::open_source::StatisticalPilotDelay &pilot_delay_model, + const Units::Length &target_kinematic_dtg_to_end_of_route) { /* * Developer's note: In this level of the algorithm, all uses of the /target state/ * must be projected onto ownship's route prior to use. This includes all items @@ -255,7 +255,7 @@ aaesim::open_source::Guidance IMKinematicTimeBasedMaintain::Update( void IMKinematicTimeBasedMaintain::CalculateIas( const Units::Length current_ownship_altitude, const aaesim::open_source::DynamicsState &dynamics_state, const aaesim::open_source::KinematicTrajectoryPredictor &ownship_kinematic_trajectory_predictor, - PilotDelay &pilot_delay) { + aaesim::open_source::StatisticalPilotDelay &pilot_delay) { m_im_speed_command_ias = m_speed_limiter.LimitSpeedCommand( m_previous_im_speed_command_ias, m_im_speed_command_ias, ownship_kinematic_trajectory_predictor.GetVerticalPathCasByIndex(m_ownship_reference_lookup_index), @@ -278,7 +278,7 @@ void IMKinematicTimeBasedMaintain::CalculateIas( void IMKinematicTimeBasedMaintain::CalculateMach( const Units::Length current_ownship_altitude, const Units::Speed true_airspeed_command, const aaesim::open_source::KinematicTrajectoryPredictor &ownship_kinematic_trajectory_predictor, - PilotDelay &pilot_delay, const Units::Mass current_mass) { + aaesim::open_source::StatisticalPilotDelay &pilot_delay, const Units::Mass current_mass) { // Make sure velocity is within nominal limits (AAES-694) Units::Speed nominal_profile_ias = Units::MetersPerSecondSpeed( diff --git a/IntervalManagement/IMMaintain.cpp b/IntervalManagement/IMMaintain.cpp index a2f1a37..5fca8dd 100644 --- a/IntervalManagement/IMMaintain.cpp +++ b/IntervalManagement/IMMaintain.cpp @@ -55,7 +55,7 @@ void IMMaintain::InitializeScenario(IMAchieve *obj, const Units::Frequency maint void IMMaintain::Prepare(Units::Speed previous_im_speed_command, Units::Speed previous_ias_command, interval_management::open_source::FIMSpeedLimiter speed_limiter, double previous_mach_command, const aaesim::open_source::EuclideanTrajectoryPredictor &ownship_trajectory_predictor, - const AlongPathDistanceCalculator &im_distance_calculator, + const aaesim::open_source::AlongPathDistanceCalculator &im_distance_calculator, const std::vector &target_adsb_track_history, const IMClearance &im_clearance, const std::vector &rf_limits) { @@ -63,8 +63,9 @@ void IMMaintain::Prepare(Units::Speed previous_im_speed_command, Units::Speed pr m_previous_reference_im_speed_command_tas = previous_im_speed_command; m_previous_im_speed_command_ias = previous_ias_command; m_previous_reference_im_speed_command_mach = previous_mach_command; - m_ownship_decrementing_distance_calculator = AlongPathDistanceCalculator( - ownship_trajectory_predictor.GetHorizontalPath(), TrajectoryIndexProgressionDirection::DECREMENTING); + m_ownship_decrementing_distance_calculator = aaesim::open_source::AlongPathDistanceCalculator( + ownship_trajectory_predictor.GetHorizontalPath(), + aaesim::open_source::TrajectoryIndexProgressionDirection::DECREMENTING); m_ownship_distance_calculator = im_distance_calculator; m_speed_limiter = speed_limiter; diff --git a/IntervalManagement/IMTimeBasedAchieve.cpp b/IntervalManagement/IMTimeBasedAchieve.cpp index 0525f97..b150fec 100644 --- a/IntervalManagement/IMTimeBasedAchieve.cpp +++ b/IntervalManagement/IMTimeBasedAchieve.cpp @@ -22,7 +22,6 @@ #include #include "public/CustomMath.h" -#include "public/AircraftCalculations.h" #include "public/CoreUtils.h" using namespace std; @@ -463,6 +462,18 @@ aaesim::open_source::Guidance IMTimeBasedAchieve::HandleMaintainStage( if (built) { if (!m_transitioned_to_maintain) { + if (!m_im_kinematic_time_based_maintain->GetAtmosphere()) { + m_im_kinematic_time_based_maintain->SetAtmosphere(std::shared_ptr(GetAtmosphere()->Clone())); + } + if (m_im_kinematic_time_based_maintain->GetAtmosphere()->GetTemperatureOffset() != + GetAtmosphere()->GetTemperatureOffset()) { + LOG4CPLUS_WARN(m_logger, + "Different atmospheric temperature offsets in Achieve (" + << GetAtmosphere()->GetTemperatureOffset() << ") vs Maintain (" + << m_im_kinematic_time_based_maintain->GetAtmosphere()->GetTemperatureOffset() + << ")."); + } + m_im_kinematic_time_based_maintain->Prepare( m_previous_reference_im_speed_command_tas, m_previous_im_speed_command_ias, m_speed_limiter, m_previous_reference_im_speed_command_mach, m_ownship_kinematic_trajectory_predictor, @@ -684,3 +695,11 @@ const Units::Speed IMTimeBasedAchieve::GetImSpeedCommandIas() const { return m_im_kinematic_time_based_maintain->GetImSpeedCommandIas(); } } + +void IMTimeBasedAchieve::Initialize(const OwnshipPredictionParameters &ownship_prediction_parameters, + const AircraftIntent &ownship_aircraft_intent, + aaesim::open_source::WeatherPrediction &weather_prediction, + std::shared_ptr &position_converter) { + SetTangentPlaneSequence(position_converter); + IMKinematicAchieve::Initialize(ownship_prediction_parameters, ownship_aircraft_intent, weather_prediction); +} diff --git a/IntervalManagement/IMTimeBasedAchieveMutableASG.cpp b/IntervalManagement/IMTimeBasedAchieveMutableASG.cpp index c500ab0..12f9159 100644 --- a/IntervalManagement/IMTimeBasedAchieveMutableASG.cpp +++ b/IntervalManagement/IMTimeBasedAchieveMutableASG.cpp @@ -21,7 +21,6 @@ #include #include "public/CustomMath.h" -#include "public/AircraftCalculations.h" #include "public/CoreUtils.h" #include "public/SimulationTime.h" @@ -67,6 +66,14 @@ bool IMTimeBasedAchieveMutableASG::load(DecodedStream *input) { return mLoaded; } +void IMTimeBasedAchieveMutableASG::Initialize(const OwnshipPredictionParameters &ownship_prediction_parameters, + const AircraftIntent &ownship_aircraft_intent, + aaesim::open_source::WeatherPrediction &weather_prediction, + std::shared_ptr &position_converter) { + SetTangentPlaneSequence(nullptr); + Initialize(ownship_prediction_parameters, ownship_aircraft_intent, weather_prediction); +} + void IMTimeBasedAchieveMutableASG::Initialize(const OwnshipPredictionParameters &ownship_prediction_parameters, const AircraftIntent &ownship_aircraft_intent, aaesim::open_source::WeatherPrediction &weather_prediction) { diff --git a/IntervalManagement/IMUtils.cpp b/IntervalManagement/IMUtils.cpp index 2db582b..637410a 100644 --- a/IntervalManagement/IMUtils.cpp +++ b/IntervalManagement/IMUtils.cpp @@ -17,11 +17,13 @@ // 2023 The MITRE Corporation. All Rights Reserved. // **************************************************************************** -#include -#include #include "imalgs/IMUtils.h" -#include "public/ScenarioUtils.h" + +#include +#include + #include "public/AircraftCalculations.h" +#include "public/ScenarioUtils.h" #include "public/SimulationTime.h" using namespace std; @@ -37,6 +39,7 @@ const Units::KnotsSpeed IMUtils::SPEED_QUANTIZE_1_DEFAULT_5_KNOTS(5); const Units::KnotsSpeed IMUtils::SPEED_QUANTIZE_2_DEFAULT(5); const Units::KnotsSpeed IMUtils::SPEED_QUANTIZE_3_DEFAULT(10); const bool IMUtils::LIMIT_FLAG_DEFAULT(true); +const bool IMUtils::WIND_BLENDING_FLAG_DEFAULT(true); const bool IMUtils::QUANTIZE_FLAG_DEFAULT(true); std::map IMUtils::algorithm_type_dictionary = { @@ -85,7 +88,6 @@ bool IMUtils::GetCrossingTime( void IMUtils::CalculateMergePoint(const Units::Length x1, const Units::Length y1, const Units::Length x2, const Units::Length y2, const Units::Length x3, const Units::Length y3, Units::Length &x_merge, Units::Length &y_merge, const Units::Angle theta_merge) { - LOG4CPLUS_TRACE(m_logger, "Pos of AC on final: (" << Units::MetersLength(x1) << "," << Units::MetersLength(y1) << ")"); LOG4CPLUS_TRACE(m_logger, @@ -722,8 +724,6 @@ bool IMUtils::CalculateTargetStateAtTime(const interval_management::open_source: const std::vector &ownship_horizontal_traj, const Units::Time extrapolation_time, const Units::Angle ownship_true_heading, interval_management::open_source::AircraftState &extrapolation_state) { - - interval_management::open_source::AircraftState projstate(target_state), stateattime; Units::FeetLength projx, projy, projdtg; const bool b0 = IMUtils::ProjectTargetPosition(Units::FeetLength(target_state.m_x), Units::FeetLength(target_state.m_y), @@ -764,7 +764,6 @@ bool IMUtils::CalculateTargetStateAtTime(const interval_management::open_source: bool IMUtils::GetPathLengthFromPosition(const Units::Length x, const Units::Length y, const std::vector &horizontal_path, Units::Length &distance_to_go, Units::Angle &track) { - try { /* * Note: Do not attempt to replace this call with an instance of AlongPathDistanceCalculator. The caller's @@ -882,8 +881,6 @@ void IMUtils::CalculateTargetStateAtDistance(const interval_management::open_sou const Units::Length extrapolation_distance, const Units::Angle ownship_true_heading, interval_management::open_source::AircraftState &extrapstate) { - - interval_management::open_source::AircraftState projstate(target_state); interval_management::open_source::AircraftState stateattime; Units::FeetLength projx, projy, projdtg; const bool b0 = @@ -1111,8 +1108,7 @@ interval_management::open_source::AircraftState IMUtils::GetProjectedTargetState return target_state; } -Units::SignedAngle IMUtils::CalculateTrackAngle(const std::list angle_history) { - +Units::SignedAngle IMUtils::CalculateTrackAngle(const std::list &angle_history) { // Average the track angles // The tricky part is avoiding a wrap effect. // Compute total using both signed and unsigned, @@ -1154,33 +1150,27 @@ interval_management::open_source::AircraftState IMUtils::ConvertToIntervalManage const aaesim::open_source::AircraftState &aircraft_state) { interval_management::open_source::AircraftState new_state; return new_state.Create( - aircraft_state.m_id, Units::SecondsTime(aircraft_state.m_time), - EarthModel::LocalPositionEnu::Of(aircraft_state.GetPositionX(), aircraft_state.GetPositionY(), - aircraft_state.GetPositionZ()), - aircraft_state.GetSpeedXd(), aircraft_state.GetSpeedYd(), aircraft_state.GetSpeedZd(), - Units::RadiansAngle(aircraft_state.m_gamma), Units::MetersPerSecondSpeed(aircraft_state.m_Vwx), - Units::MetersPerSecondSpeed(aircraft_state.m_Vwy), Units::MetersPerSecondSpeed(aircraft_state.m_Vw_para), - Units::MetersPerSecondSpeed(aircraft_state.m_Vw_perp), aircraft_state.m_sensed_temperature, - aircraft_state.m_psi); + aircraft_state.GetUniqueId(), aircraft_state.GetTime(), + EarthModel::LocalPositionEnu::Of(aircraft_state.GetPositionEnuX(), aircraft_state.GetPositionEnuY(), + aircraft_state.GetAltitudeMsl()), + aircraft_state.GetSpeedEnuX(), aircraft_state.GetSpeedEnuY(), aircraft_state.GetVerticalSpeed(), + aircraft_state.GetFlightPathAngle(), aircraft_state.GetSensedWindEast(), aircraft_state.GetSensedWindNorth(), + aircraft_state.GetSensedWindParallel(), aircraft_state.GetSensedWindPerpendicular(), + aircraft_state.GetSensedTemperature(), aircraft_state.GetPsi()); } aaesim::open_source::AircraftState IMUtils::ConvertToAaesimAircraftState( - const interval_management::open_source::AircraftState &aircraft_state) { - aaesim::open_source::AircraftState return_state; - return_state.m_x = Units::FeetLength(aircraft_state.GetPositionX()).value(); - return_state.m_y = Units::FeetLength(aircraft_state.GetPositionY()).value(); - return_state.m_z = Units::FeetLength(aircraft_state.GetPositionZ()).value(); - return_state.m_xd = Units::FeetPerSecondSpeed(aircraft_state.m_xd).value(); - return_state.m_yd = Units::FeetPerSecondSpeed(aircraft_state.m_yd).value(); - return_state.m_zd = Units::FeetPerSecondSpeed(aircraft_state.m_zd).value(); - return_state.m_id = aircraft_state.GetId(); - return_state.m_time = aircraft_state.GetTimeStamp().value(); - return_state.m_psi = aircraft_state.GetPsi(); - return_state.m_gamma = aircraft_state.GetGamma().value(); - return_state.m_Vwx = Units::MetersPerSecondSpeed(aircraft_state.GetSensedWindEastComponent()).value(); - return_state.m_Vwy = Units::MetersPerSecondSpeed(aircraft_state.GetSensedWindNorthComponent()).value(); - return_state.m_Vw_para = Units::MetersPerSecondSpeed(aircraft_state.GetSensedWindParallelComponent()).value(); - return_state.m_Vw_perp = Units::MetersPerSecondSpeed(aircraft_state.GetSensedWindPerpendicularComponent()).value(); - return_state.m_sensed_temperature = aircraft_state.GetSensedTemperature(); - return return_state; -} \ No newline at end of file + const interval_management::open_source::AircraftState &im_state) { + return aaesim::open_source::AircraftState::Builder(im_state.GetId(), im_state.GetTimeStamp()) + .Position(im_state.GetPositionX(), im_state.GetPositionY()) + ->AltitudeMsl(im_state.GetPositionZ()) + ->GroundSpeed(im_state.GetSpeedXd(), im_state.GetSpeedYd()) + ->AltitudeRate(im_state.GetSpeedZd()) + ->Psi(im_state.GetPsi()) + ->FlightPathAngle(im_state.GetGamma()) + ->SensedWindComponents(im_state.GetSensedWindEastComponent(), im_state.GetSensedWindNorthComponent()) + ->SensedWindsParallel(im_state.GetSensedWindParallelComponent()) + ->SensedWindsPerpendicular(im_state.GetSensedWindPerpendicularComponent()) + ->SensedTemperature(im_state.GetSensedTemperature()) + ->Build(); +} diff --git a/IntervalManagement/InternalObserver.cpp b/IntervalManagement/InternalObserver.cpp index ecfa511..3ea8518 100644 --- a/IntervalManagement/InternalObserver.cpp +++ b/IntervalManagement/InternalObserver.cpp @@ -24,90 +24,78 @@ #include "public/StereographicProjection.h" using namespace std; +using namespace aaesim::open_source::constants; using namespace interval_management::open_source; -InternalObserver *InternalObserver::mInstance = NULL; +std::unique_ptr InternalObserver::m_instance = nullptr; + log4cplus::Logger InternalObserver::logger = log4cplus::Logger::getInstance(LOG4CPLUS_TEXT("InternalObserver")); InternalObserver *InternalObserver::getInstance() { - if (mInstance == NULL) { - mInstance = new InternalObserver(); + if (m_instance == NULL) { + m_instance = std::unique_ptr(new InternalObserver()); } - return mInstance; + return m_instance.get(); } -void InternalObserver::clearInstance() { - if (mInstance != NULL) { - delete mInstance; - mInstance = NULL; // blow away the instance - } -} - -InternalObserver::InternalObserver() { - m_save_maintain_metrics = true; - m_scenario_iter = 0; - outputNMFiles = true; -} +InternalObserver::InternalObserver() = default; -void InternalObserver::process(void) { +void InternalObserver::process() { dumpPredictedWind(); process_NM_stats(); processMaintainMetrics(); processFinalGS(); processMergePointMetric(); processClosestPointMetric(); - process_ptis_b_reports(); dumpAchieveList(); } -void InternalObserver::set_scenario_name(string in) { scenario_name = in; } +void InternalObserver::set_scenario_name(const std::string &in) { scenario_name = in; } MergePointMetric &InternalObserver::GetMergePointMetric(int id) { - return m_aircraft_iteration_stats[id].m_merge_point_metric; + auto return_val = m_aircraft_iteration_stats.insert(std::make_pair(id, AircraftIterationStats())); + return return_val.first->second.m_merge_point_metric; } -MaintainMetric &InternalObserver::GetMaintainMetric(int id) { return m_aircraft_iteration_stats[id].m_maintain_metric; } +MaintainMetric &InternalObserver::GetMaintainMetric(int id) { + auto return_val = m_aircraft_iteration_stats.insert(std::make_pair(id, AircraftIterationStats())); + return return_val.first->second.m_maintain_metric; +} ClosestPointMetric &InternalObserver::GetClosestPointMetric(int id) { - return m_aircraft_iteration_stats[id].m_closest_point_metric; + auto return_val = m_aircraft_iteration_stats.insert(std::make_pair(id, AircraftIterationStats())); + return return_val.first->second.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; } +NMObserver &InternalObserver::GetNMObserver(int id) { + auto return_val = m_aircraft_scenario_stats.insert(std::make_pair(id, AircraftScenarioStats())); + return return_val.first->second.m_nm_observer; +} void InternalObserver::SetScenarioIter(int scenario_iter) { this->m_scenario_iter = scenario_iter; } -// 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); + snprintf(temp, 10, "%d", ix->first); - string output_file_name = scenario_name + "_AC" + temp + "-NM-output.csv"; + string output_file_name = scenario_name + "_aircraft_" + 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(" @@ -115,11 +103,9 @@ void InternalObserver::process_NM_aircraft() { << endl; } - nm_observer.initialize_stats(); // initialize the statistics to the size of the entry list + nm_observer.initialize_stats(); - // 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 << ","; @@ -133,7 +119,6 @@ void InternalObserver::process_NM_aircraft() { 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] = @@ -151,7 +136,7 @@ void InternalObserver::process_NM_aircraft() { } nm_observer.entry_list.clear(); - nm_observer.curr_NM = -2; // resets the current NM value + nm_observer.curr_NM = -2; out.close(); } } @@ -160,20 +145,16 @@ void InternalObserver::process_NM_aircraft() { } 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); + snprintf(temp, 10, "%d", ix->first); - string output_file_name = scenario_name + "_AC" + temp + "-stats-NM-output.csv"; + string output_file_name = scenario_name + "_aircraft_" + temp + "_stats_nm_output.csv"; delete[] temp; @@ -181,13 +162,11 @@ void InternalObserver::process_NM_stats() { 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] << ","; @@ -216,42 +195,35 @@ void InternalObserver::initializeIteration() { } 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) { + if (maintainOutput.empty()) { 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); + snprintf(bfr, sizeof(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 + snprintf(bfr, sizeof(bfr), "%d", ((int)maintainOutput.size() - 1)); 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()); + snprintf(bfr, sizeof(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,,,,"); + snprintf(bfr, sizeof(bfr), ",No samples,,,,"); } body = body + bfr; @@ -261,10 +233,7 @@ void InternalObserver::outputMaintainMetrics() { } void InternalObserver::processMaintainMetrics() { - - // Output maintain metrics .csv file. - - string output_file_name = scenario_name + "-Maintain-Metrics.csv"; + string output_file_name = scenario_name + "_maintain_metrics.csv"; ofstream out; out.open(output_file_name.c_str()); @@ -276,66 +245,44 @@ void InternalObserver::processMaintainMetrics() { 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; + auto return_val = m_aircraft_iteration_stats.insert(std::make_pair(id, AircraftIterationStats())); + return_val.first->second.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) { + if (finalGSOutput.empty()) { body = "Iteration"; for (auto ix = m_aircraft_iteration_stats.begin(); ix != m_aircraft_iteration_stats.end(); ++ix) { - sprintf(bfr, ",ac %d-gs", ix->first); + snprintf(bfr, sizeof(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. + snprintf(bfr, sizeof(bfr), "%d", ((int)finalGSOutput.size() - 1)); body = bfr; for (auto ix = m_aircraft_iteration_stats.begin(); ix != m_aircraft_iteration_stats.end(); ++ix) { - sprintf(bfr, ",%f", ix->second.finalGS); + snprintf(bfr, sizeof(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"; + string output_file_name = scenario_name + "_final_groundspeed.csv"; ofstream out; out.open(output_file_name.c_str()); @@ -347,23 +294,14 @@ void InternalObserver::processFinalGS() { 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) { + if (mergePointOutput.empty()) { body = "Iteration"; for (auto ix = m_aircraft_iteration_stats.begin(); ix != m_aircraft_iteration_stats.end(); ++ix) { @@ -371,7 +309,7 @@ void InternalObserver::outputMergePointMetric() { 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); + snprintf(bfr, sizeof(bfr), ",ac %d-mergePt,ac %d-distTo ac %d", id1, id1, id0); body = body + bfr; } } @@ -379,15 +317,14 @@ void InternalObserver::outputMergePointMetric() { mergePointOutput.push_back(body); } - // Add body. - sprintf(bfr, "%d", ((int)mergePointOutput.size() - 1)); // Iteration + snprintf(bfr, sizeof(bfr), "%d", ((int)mergePointOutput.size() - 1)); 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()); + snprintf(bfr, sizeof(bfr), ",%s,%f", merge_point_metric.getMergePoint().c_str(), + Units::NauticalMilesLength(merge_point_metric.getDist()).value()); body = body + bfr; } } @@ -396,10 +333,7 @@ void InternalObserver::outputMergePointMetric() { } void InternalObserver::processMergePointMetric() { - - // Output merge point metric to a .csv file. - - string output_file_name = scenario_name + "-Merge-Point-Metric.csv"; + string output_file_name = scenario_name + "_merge_point_metric.csv"; ofstream out; out.open(output_file_name.c_str()); @@ -417,24 +351,16 @@ void InternalObserver::processMergePointMetric() { } 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) { + if (closestPointOutput.empty()) { 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()); + snprintf(bfr, sizeof(bfr), ",ac %d-smallestDistTo ac %d", closest_point_metric.GetImAcId(), + closest_point_metric.GetTargetAcId()); body = body + bfr; } } @@ -442,14 +368,13 @@ void InternalObserver::outputClosestPointMetric() { closestPointOutput.push_back(body); } - // Add body. - sprintf(bfr, "%d", ((int)closestPointOutput.size() - 1)); // Iteration + snprintf(bfr, sizeof(bfr), "%d", ((int)closestPointOutput.size() - 1)); 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()); + snprintf(bfr, sizeof(bfr), ",%f", Units::NauticalMilesLength(closest_point_metric.getMinDist()).value()); body = body + bfr; } } @@ -458,10 +383,7 @@ void InternalObserver::outputClosestPointMetric() { } void InternalObserver::processClosestPointMetric() { - - // Output closest point metric to a .csv file. - - string output_file_name = scenario_name + "-Closest-Point-Metric.csv"; + string output_file_name = scenario_name + "_closest_point_metric.csv"; ofstream out; out.open(output_file_name.c_str()); @@ -473,113 +395,23 @@ void InternalObserver::processClosestPointMetric() { 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 aaesim::open_source::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())); + if (predWinds.empty()) { + 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(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 += ","; } @@ -588,41 +420,22 @@ string InternalObserver::predWindsHeading(int numVals) { } string InternalObserver::predWindsData(int id, int col, string field, const aaesim::open_source::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); + snprintf(txt, 301, "%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()); + snprintf(txt, 301, ",%lf", mat.GetAltitude(i).value()); break; case 2: - sprintf(txt, ",%lf", mat.GetSpeed(i).value()); + snprintf(txt, 301, ",%lf", mat.GetSpeed(i).value()); } str += txt; } @@ -634,38 +447,21 @@ string InternalObserver::predWindsData(int id, int col, string field, const aaes string InternalObserver::predTempData(int id, string field, const aaesim::open_source::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); + snprintf(txt, 31, "%d", id); str = txt; - // Field - str += ","; str += field.c_str(); - // Data line-all in meters, meters/second. - const aaesim::open_source::WindStack &mat(weatherPrediction.east_west); + const aaesim::open_source::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); + snprintf(txt, 31, ",%lf", temperature.value() - 273.15); str += txt; } @@ -675,9 +471,7 @@ string InternalObserver::predTempData(int id, string field, } void InternalObserver::dumpPredictedWind() { - // Outputs predicted winds .csv file. - - string fileName = scenario_name + "-Predicted-Winds.csv"; + string fileName = scenario_name + "_predicted_winds.csv"; ofstream out; out.open(fileName.c_str()); @@ -689,30 +483,18 @@ void InternalObserver::dumpPredictedWind() { 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"; + string fileName = scenario_name + "_time_to_go.csv"; ofstream out; bool needHdr = true; @@ -722,9 +504,8 @@ void InternalObserver::dumpAchieveList() { if (achieve_list.empty()) { continue; - } // Nada for this ac + } - // Header if (needHdr) { out.open(fileName.c_str()); if (!out.is_open()) { @@ -742,29 +523,10 @@ void InternalObserver::dumpAchieveList() { 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. +void InternalObserver::setNMOutput(bool NMflag) { this->outputNMFiles = NMflag; } - return this->outputNMFiles; -} +bool InternalObserver::outputNM() { 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/IntervalManagement/InternalObserverScenarioWriter.cpp b/IntervalManagement/InternalObserverScenarioWriter.cpp index d641cf1..a93bcb2 100644 --- a/IntervalManagement/InternalObserverScenarioWriter.cpp +++ b/IntervalManagement/InternalObserverScenarioWriter.cpp @@ -40,5 +40,4 @@ void interval_management::open_source::InternalObserverScenarioWriter::ScenarioB void interval_management::open_source::InternalObserverScenarioWriter::ScenarioEnd(const std::string &scenario_name) { InternalObserver::getInstance()->process(); - InternalObserver::clearInstance(); } diff --git a/IntervalManagement/KinematicDescent4DPredictor.cpp b/IntervalManagement/KinematicDescent4DPredictor.cpp deleted file mode 100644 index 3d2188d..0000000 --- a/IntervalManagement/KinematicDescent4DPredictor.cpp +++ /dev/null @@ -1,1551 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy 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/Waypoint.h" -#include "imalgs/KinematicDescent4DPredictor.h" -#include - -using namespace std; - -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_DEBUG(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; -} - -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) { - 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.altitude_rate_mps.push_back(0); - trajTemp.true_airspeed.push_back(Units::ZERO_SPEED); - 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; - - VerticalPath last_state = m_vertical_path; - VerticalPath last_waypoint_state = m_vertical_path; - VerticalPath low_tolerance_state; // earliesr waypoint in prediction that can do FPA to current aircraft position - bool low_tolerance_state_found = false; - - 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; - } - - LOG4CPLUS_TRACE(m_logger, "begin constrainedVerticalPath, start/cruise/transition altitude: " - << m_start_altitude_msl.value() << "/" << Units::MetersLength(m_cruise_altitude_msl).value() - << "/" << Units::MetersLength(m_transition_altitude_msl).value()); - // CAS segment - // Need to plan to end of segment, at least. Cannot stop when reaching start altitude. - // Start altitude can cause a switch in type of descent part way along a segment. - // The test for start altitude is placed after the segment is complete: see AAES-756. - while (m_vertical_path.altitude_m.back() < alt1) { - LOG4CPLUS_TRACE(m_logger, " before constantCASVerticalPath, x,h,cas,gs is: " - << m_vertical_path.along_path_distance_m.back() << "," - << m_vertical_path.altitude_m.back() << "," - << m_vertical_path.cas_mps.back() << "," - << m_vertical_path.gs_mps.back()); - if (m_vertical_path.along_path_distance_m.back() > horizontal_path.back().m_path_length_cumulative_meters) { - LOG4CPLUS_WARN(m_logger, - "KinematicDescent4DPredictor cannot create a trajectory that reaches " - << alt1 << "m in the distance required."); - 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); - } - LOG4CPLUS_TRACE(m_logger, " after constantCASVerticalPath, x/h/v is: " - << m_vertical_path.along_path_distance_m.back() << "/" << m_vertical_path.altitude_m.back() << "/" - << m_vertical_path.cas_mps.back() << " active_flag is: " - << static_cast(m_precalculated_constraints.active_flag)); - // special case if on first segment - if (Units::MetersLength(aircraft_distance_to_go).value() <= - Units::MetersLength(precalc_waypoints[0].m_precalc_constraints.constraint_along_path_distance).value()) { - low_tolerance_state = last_waypoint_state; - low_tolerance_state_found = true; - } - - 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(), - Units::MetersPerSecondSpeed(m_precalculated_constraints.constraint_speedHi).value(), FPA, - horizontal_path, - precalc_waypoints, - weather_prediction, - aircraft_distance_to_go); - - LOG4CPLUS_TRACE(m_logger, " after constantFPADecelVerticalPath, x/h/v is: " - << m_vertical_path.along_path_distance_m.back() << "/" << m_vertical_path.altitude_m.back() << "/" - << m_vertical_path.cas_mps.back()); - - 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); - - LOG4CPLUS_TRACE(m_logger, " after constantGeoFPAVerticalPath, x/h/v is: " - << m_vertical_path.along_path_distance_m.back() << "/" << m_vertical_path.altitude_m.back() << "/" - << m_vertical_path.cas_mps.back()); - - - } 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); - LOG4CPLUS_TRACE(m_logger, " after constantGeoFPAVerticalPath, x/h/v is: " - << m_vertical_path.along_path_distance_m.back() << "/" << m_vertical_path.altitude_m.back() << "/" - << m_vertical_path.cas_mps.back()); - - 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); - LOG4CPLUS_TRACE(m_logger, " after levelVerticalPath, x/h/v is: " - << m_vertical_path.along_path_distance_m.back() << "/" << m_vertical_path.altitude_m.back() << "/" - << m_vertical_path.cas_mps.back()); - - } 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); - LOG4CPLUS_TRACE(m_logger, " after constantDecelVerticalPath, x/h/v is: " - << m_vertical_path.along_path_distance_m.back() << "/" << m_vertical_path.altitude_m.back() << "/" - << m_vertical_path.cas_mps.back()); - } - - 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(m_descent_angle_max).value()) - LOG4CPLUS_WARN(m_logger, "prediction FPA is " << uFPA.value() << " which is greater than " - << m_descent_angle_warning.value()); - if (uFPA < m_descent_angle_max) { - m_vertical_path = ConstantFpaDecelerationVerticalPath(last_state, - Units::MetersLength(m_precalculated_constraints.constraint_altLow).value(), - Units::MetersPerSecondSpeed(m_precalculated_constraints.constraint_speedHi).value(), - FPA, horizontal_path, precalc_waypoints, - weather_prediction, - aircraft_distance_to_go); - LOG4CPLUS_TRACE(m_logger, " after constantFPADecelVerticalPath, x/h/v is: " - << m_vertical_path.along_path_distance_m.back() << "/" << m_vertical_path.altitude_m.back() << "/" - << m_vertical_path.cas_mps.back()); - } - } - } 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); - LOG4CPLUS_TRACE(m_logger, " after levelDecelVerticalPath, x/h/v is: " - << m_vertical_path.along_path_distance_m.back() << "/" << m_vertical_path.altitude_m.back() << "/" - << m_vertical_path.cas_mps.back()); - } - } - - if (m_prediction_too_low || m_prediction_too_high) { - LOG4CPLUS_TRACE(m_logger, "Constrained Vertical Path is out of tolerance at aircraft position; alt: " - << Units::MetersLength(m_start_altitude_msl).value() << " prediction alt: " - << m_vertical_path.altitude_m.back()); - break; - } - } // end if violation_flag - - - last_state = m_vertical_path; - if ((aircraft_distance_to_go != Units::MetersLength(Units::infinity())) && !low_tolerance_state_found) { - if ((m_vertical_path.altitude_m.back() > Units::MetersLength(m_start_altitude_msl).value()) - || (m_vertical_path.along_path_distance_m.back() > Units::MetersLength(aircraft_distance_to_go).value())) { - low_tolerance_state_found = true; - low_tolerance_state = last_waypoint_state; - } - } - 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"); - return; - } - - if (aircraft_distance_to_go != Units::MetersLength(Units::infinity())) { - // find if FPA from end of prediction to current position is valid - double distance_to_current_position = - Units::MetersLength(aircraft_distance_to_go).value() - m_vertical_path.along_path_distance_m.back(); - if (!low_tolerance_state_found) { - //if (distance_to_current_position <= 0) { - // low_tolerance_state_found = true; - // low_tolerance_state = last_waypoint_state; - //} else { - double alt_per_dist = (Units::MetersLength(m_start_altitude_msl).value() - m_vertical_path.altitude_m.back()) - / distance_to_current_position; - for (unsigned int loop = m_precalculated_constraints.index; loop < precalc_waypoints.size(); loop++) { - // end of prediction should be on next segment - if (m_vertical_path.along_path_distance_m.back() > Units::MetersLength(m_precalculated_constraints.constraint_along_path_distance).value()) { - continue; - } - if (precalc_waypoints[loop].m_precalc_constraints.constraint_along_path_distance > - aircraft_distance_to_go) { - low_tolerance_state_found = true; - low_tolerance_state = m_vertical_path; - break; - } - - double x_dist = - Units::MetersLength(precalc_waypoints[loop].m_precalc_constraints.constraint_along_path_distance).value() - m_vertical_path.along_path_distance_m.back(); - double y_alt = m_vertical_path.altitude_m.back() + alt_per_dist * x_dist; - - if (y_alt > (Units::MetersLength(precalc_waypoints[loop].m_precalc_constraints.constraint_altHi).value() + 10) - || y_alt < (Units::MetersLength(precalc_waypoints[loop].m_precalc_constraints.constraint_altLow).value() - 10)) { - break; - } - } - //} - } - } - - LOG4CPLUS_TRACE(m_logger, "last_waypoint_state reset to " << m_precalculated_constraints.index); - last_waypoint_state = m_vertical_path; - - if (m_vertical_path.altitude_m.back() > m_start_altitude_msl.value()) { - break; - } - } // END if x < constraint_along_path_distance - - - } // END while h < alt1 - - // Constant Mach segment - last_state = m_vertical_path; - - - if (m_prediction_too_low) - LOG4CPLUS_DEBUG(m_logger, "Constrained prediction too low. Replanning with FPA."); - if (m_prediction_too_high) - LOG4CPLUS_DEBUG(m_logger, "Constrained prediction too high. Replanning with FPA."); - if (m_prediction_too_low || m_prediction_too_high) { - if (low_tolerance_state_found) { - m_vertical_path = ConstantFpaToCurrentPositionVerticalPath(low_tolerance_state, horizontal_path, - precalc_waypoints, const_gamma_mach, - weather_prediction, aircraft_distance_to_go); - return; - } else { - // This should never happen. Wore case is vertical path to waypoint beginning this segment. - LOG4CPLUS_WARN(m_logger, - "low_tolerance_state vertical prediction not found for replan in ConstrainedVerticalPath"); - } - } - - 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_DEBUG(m_logger, "Constrained prediction too low. Replanning with FPA."); - if (m_prediction_too_high) - LOG4CPLUS_DEBUG(m_logger, "Constrained prediction too high. Replanning with FPA."); - if (m_prediction_too_low || m_prediction_too_high) { - if (low_tolerance_state_found) { - m_vertical_path = ConstantFpaToCurrentPositionVerticalPath(low_tolerance_state, horizontal_path, - precalc_waypoints, const_gamma_mach, - weather_prediction, aircraft_distance_to_go); - return; - } else { - // This should never happen. Wore case is vertical path to waypoint beginning this segment. - LOG4CPLUS_WARN(m_logger, - "low_tolerance_state vertical prediction not found for replan in ConstrainedVerticalPath"); - } - } - - // 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); - - // Do not need to check prediction too high or too low for level (start altitude) - -} - -VerticalPath KinematicDescent4DPredictor::ConstantCasVerticalPath(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; - - double delta_t = -0.5; - 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; - - result.along_path_distance_m.push_back(dist_new); - result.cas_mps.push_back(v_cas_new); - 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_DEBUG(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_DEBUG(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_DEBUG(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(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; - - double delta_t = -0.5; - 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; - - result.along_path_distance_m.push_back(dist_new); - result.cas_mps.push_back(v_cas_new); - 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_DEBUG(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_DEBUG(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_DEBUG(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(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; - - double delta_t = -0.5; - 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); - - result.along_path_distance_m.push_back(dist_new); - result.cas_mps.push_back(v_cas_new); - 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_DEBUG(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_DEBUG(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_DEBUG(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(VerticalPath vertical_path, - double altitude_at_end, - 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; - - double delta_t = -0.5; - 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); - - // TODO:turn the 0.3 into a constant, (maybe decel?). - - // acceleration rate - dv_dt = -0.3 * KNOTS_TO_METERS_PER_SECOND; - - 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); - - result.along_path_distance_m.push_back(dist_new); - result.cas_mps.push_back(v_cas_new); - 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_DEBUG(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_DEBUG(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_DEBUG(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( - 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; - - double delta_t = -0.5; - 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; - - result.along_path_distance_m.push_back(dist_new); - result.cas_mps.push_back(v_cas_new); - 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_DEBUG(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( - 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; - - double delta_t = -0.5; - 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; - - result.along_path_distance_m.push_back(dist_new); - result.cas_mps.push_back(v_cas_new); - 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_DEBUG(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(VerticalPath vertical_path, - double x_end, - vector &horizontal_path, - const WeatherPrediction &weather_prediction, - const Units::Length &aircraft_distance_to_go) { - VerticalPath result; - - double delta_t = -0.5; - 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; - - result.along_path_distance_m.push_back(dist_new); - result.cas_mps.push_back(v_cas_new); - 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_DEBUG(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(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; - - double delta_t = -0.5; - 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; - - result.along_path_distance_m.push_back(dist_new); - result.cas_mps.push_back(v_cas_new); - 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_DEBUG(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_DEBUG(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_DEBUG(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(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; - PrecalcConstraint constraints; - - 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 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()) { - constraints = FindActiveConstraint(dist, precalc_waypoints); - 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(); - } - 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_DEBUG(m_logger, "Aircraft position too close to waypoint for adequate FPA. See Issue AAES-1025"); - result = LevelVerticalPath(result, Units::MetersLength(aircraft_distance_to_go).value(), horizontal_path, - weather_prediction, aircraft_distance_to_go); - 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()) { - result = ConstantFpaDecelerationVerticalPath(result, altitude_at_end, - 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); -}; - diff --git a/IntervalManagement/KinematicTrajectoryPredictor.cpp b/IntervalManagement/KinematicTrajectoryPredictor.cpp deleted file mode 100644 index 29234f6..0000000 --- a/IntervalManagement/KinematicTrajectoryPredictor.cpp +++ /dev/null @@ -1,102 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy 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/AircraftCalculations.h" -#include "imalgs/KinematicTrajectoryPredictor.h" - -using namespace std; - -log4cplus::Logger KinematicTrajectoryPredictor::m_logger = log4cplus::Logger::getInstance( - LOG4CPLUS_TEXT("KinematicTrajectoryPredictor")); - -KinematicTrajectoryPredictor::KinematicTrajectoryPredictor() { - m_vertical_predictor = std::shared_ptr(new KinematicDescent4DPredictor()); -} - -KinematicTrajectoryPredictor::KinematicTrajectoryPredictor(std::shared_ptr bada_calculator, - 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); - kinematic_descent_predictor->m_bada_calculator = std::move(bada_calculator); - 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 ias_at_faf; - Units::Speed nominal_ias_at_faf = Units::FeetPerSecondSpeed( - aircraft_intent.GetRouteData().m_nominal_ias[aircraft_intent.GetNumberOfWaypoints() - 1]); - // Get the Mach from GetFms(). If it is zero, then use Fms.m_nominal_ias. - double mach_at_faf = aircraft_intent.GetRouteData().m_mach[aircraft_intent.GetNumberOfWaypoints() - 1]; - if (mach_at_faf == 0) - ias_at_faf = nominal_ias_at_faf; - else { // else calculate ias from mach and set ias_at_faf to the calculated ias. - ias_at_faf = weather_prediction.MachToCAS(mach_at_faf, altitude_at_faf); - // Compare with the nominal_ias and if higher, print a warning. - if (ias_at_faf > nominal_ias_at_faf) { - LOG4CPLUS_WARN(m_logger, "Aircraft Intent may be malformed. Last waypoint has non-zero mach and faf altitude is below transition altitude"); - } - } - GetKinematicDescent4dPredictor()->SetConditionsAtEndOfRoute(altitude_at_faf, 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; -} - -bool KinematicTrajectoryPredictor::operator==(const KinematicTrajectoryPredictor &obj) const { - bool match = EuclideanTrajectoryPredictor::operator==(obj); - - match = match && (*GetKinematicDescent4dPredictor() == *obj.GetKinematicDescent4dPredictor()); - - return match; -} - -bool KinematicTrajectoryPredictor::operator!=(const KinematicTrajectoryPredictor &obj) const { - return !operator==(obj); -} - -std::shared_ptr KinematicTrajectoryPredictor::GetKinematicDescent4dPredictor() const { - return std::shared_ptr( - static_pointer_cast(m_vertical_predictor)); -} - diff --git a/IntervalManagement/MOPSPredictedWindEvaluatorVersion1.cpp b/IntervalManagement/MOPSPredictedWindEvaluatorVersion1.cpp index 42d1370..10d0854 100644 --- a/IntervalManagement/MOPSPredictedWindEvaluatorVersion1.cpp +++ b/IntervalManagement/MOPSPredictedWindEvaluatorVersion1.cpp @@ -50,19 +50,17 @@ MOPSPredictedWindEvaluatorVersion1::~MOPSPredictedWindEvaluatorVersion1() = defa bool MOPSPredictedWindEvaluatorVersion1::ArePredictedWindsAccurate( const aaesim::open_source::AircraftState &state, const aaesim::open_source::WeatherPrediction &weatherPrediction, const Units::Speed reference_cas, const Units::Length reference_altitude, - const Atmosphere *sensed_atmosphere) const { + const std::shared_ptr &sensed_atmosphere) const { Units::MetersPerSecondSpeed windeastcomp, windnorthcomp; Units::Frequency NOT_USED; - weatherPrediction.getAtmosphere()->CalculateWindGradientAtAltitude( - Units::FeetLength(state.m_z), weatherPrediction.east_west, windeastcomp, NOT_USED); - weatherPrediction.getAtmosphere()->CalculateWindGradientAtAltitude( - Units::FeetLength(state.m_z), weatherPrediction.north_south, windnorthcomp, NOT_USED); + weatherPrediction.east_west().CalculateWindGradientAtAltitude(state.GetAltitudeMsl(), windeastcomp, NOT_USED); + weatherPrediction.north_south().CalculateWindGradientAtAltitude(state.GetAltitudeMsl(), windnorthcomp, NOT_USED); // Speed Units::Speed predicted_spd = sqrt(Units::sqr(windeastcomp) + Units::sqr(windnorthcomp)); - Units::Speed Vwx_sensed = Units::MetersPerSecondSpeed(state.m_Vwx); - Units::Speed Vwy_sensed = Units::MetersPerSecondSpeed(state.m_Vwy); + auto Vwx_sensed = Units::MetersPerSecondSpeed(state.GetSensedWindEast()); + auto Vwy_sensed = Units::MetersPerSecondSpeed(state.GetSensedWindNorth()); Units::Speed sensed_spd = sqrt((Vwx_sensed * Vwx_sensed) + (Vwy_sensed * Vwy_sensed)); bool isWindSpdAccurate = abs(abs(sensed_spd) - abs(predicted_spd)) <= toleranceSpeed; @@ -74,7 +72,7 @@ bool MOPSPredictedWindEvaluatorVersion1::ArePredictedWindsAccurate( // Direction Units::Angle predicted_dir = Units::arctan2(windnorthcomp.value(), windeastcomp.value()); // ENU, so east is the x-component - Units::Angle measured_dir = Units::arctan2(state.m_Vwy, state.m_Vwx); + Units::Angle measured_dir = Units::arctan2(Vwy_sensed.value(), Vwx_sensed.value()); Units::SignedDegreesAngle difference = measured_dir - predicted_dir; bool isWindDirAccurate = abs(difference) <= toleranceAngle; diff --git a/IntervalManagement/MOPSPredictedWindEvaluatorVersion2.cpp b/IntervalManagement/MOPSPredictedWindEvaluatorVersion2.cpp index 4ace35d..ffac99c 100644 --- a/IntervalManagement/MOPSPredictedWindEvaluatorVersion2.cpp +++ b/IntervalManagement/MOPSPredictedWindEvaluatorVersion2.cpp @@ -44,9 +44,9 @@ MOPSPredictedWindEvaluatorVersion2::~MOPSPredictedWindEvaluatorVersion2() = defa bool MOPSPredictedWindEvaluatorVersion2::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 Atmosphere *sensed_atmosphere) const { + const std::shared_ptr &sensed_atmosphere) const { - Units::FeetLength true_altitude(state.m_z); + Units::FeetLength true_altitude{state.GetAltitudeMsl()}; Units::KnotsSpeed tas1 = weather_prediction.CAS2TAS(reference_cas, reference_altitude); Units::KnotsSpeed tas2 = sensed_atmosphere->CAS2TAS(reference_cas, true_altitude); @@ -57,19 +57,17 @@ bool MOPSPredictedWindEvaluatorVersion2::ArePredictedWindsAccurate( Units::HertzFrequency dVwx_dh; Units::HertzFrequency dVwy_dh; - weather_prediction.GetForecastAtmosphere()->CalculateWindGradientAtAltitude( - reference_altitude, weather_prediction.east_west, predicted_wind_x, dVwx_dh); - weather_prediction.GetForecastAtmosphere()->CalculateWindGradientAtAltitude( - reference_altitude, weather_prediction.north_south, predicted_wind_y, dVwy_dh); + weather_prediction.east_west().CalculateWindGradientAtAltitude(reference_altitude, predicted_wind_x, dVwx_dh); + weather_prediction.north_south().CalculateWindGradientAtAltitude(reference_altitude, predicted_wind_y, dVwy_dh); - double sin_heading(sin(state.m_psi)); - double cos_heading(cos(state.m_psi)); + double sin_heading(sin(state.GetPsi())); + double cos_heading(cos(state.GetPsi())); // calculate predicted tailwind and crosswind based on true course Units::Speed vw_para1(predicted_wind_x * cos_heading + predicted_wind_y * sin_heading); Units::Speed vw_perp1(-predicted_wind_x * sin_heading + predicted_wind_y * cos_heading); - Units::MetersPerSecondSpeed vw_para2(state.m_Vw_para); - Units::MetersPerSecondSpeed vw_perp2(state.m_Vw_perp); + Units::MetersPerSecondSpeed vw_para2{state.GetSensedWindParallel()}; + Units::MetersPerSecondSpeed vw_perp2{state.GetSensedWindPerpendicular()}; // Pythagorean-subtract crosswind Units::KnotsSpeed tas_para1 = Units::sqrt(Units::sqr(tas1) - Units::sqr(vw_perp1)); @@ -114,17 +112,18 @@ void MOPSPredictedWindEvaluatorVersion2::LogWindDisagreeMetaData( << "||time||source||altitude(ft)||temperature(C)||pressure(Pa)||density(kg/" "m^3)||wind_ew(kts)||wind_ns(kts)" << "||CAS(kts)||TAS(kts)||GS(kts)||" << std::endl - << "|" << state.m_time << "|predicted|" << Units::FeetLength(reference_altitude).value() - << "|" << (predicted_temperature.value() - 273.15) << "|" << predicted_pressure.value() - << "|" << predicted_density.value() << "|" << Units::KnotsSpeed(predicted_wind_x).value() - << "|" << Units::KnotsSpeed(predicted_wind_y).value() << "|" + << "|" << state.GetTime().value() << "|predicted|" + << Units::FeetLength(reference_altitude).value() << "|" + << (predicted_temperature.value() - 273.15) << "|" << predicted_pressure.value() << "|" + << predicted_density.value() << "|" << Units::KnotsSpeed(predicted_wind_x).value() << "|" + << Units::KnotsSpeed(predicted_wind_y).value() << "|" << Units::KnotsSpeed(reference_cas).value() << "|" << Units::KnotsSpeed(tas1).value() << "|" << Units::KnotsSpeed(gs1).value() << "|" << std::endl - << "|" << state.m_time << "| true |" << true_altitude.value() << "|" + << "|" << state.GetTime().value() << "| true |" << true_altitude.value() << "|" << (true_temperature.value() - 273.15) << "|" << true_pressure.value() << "|" << true_density.value() << "|" - << Units::KnotsSpeed(Units::MetersPerSecondSpeed(state.m_Vwx)).value() << "|" - << Units::KnotsSpeed(Units::MetersPerSecondSpeed(state.m_Vwy)).value() << "|" - << Units::KnotsSpeed(reference_cas).value() << "|" << Units::KnotsSpeed(tas2).value() - << "|" << Units::KnotsSpeed(gs2).value() << "|"); -} \ No newline at end of file + << Units::KnotsSpeed(Units::MetersPerSecondSpeed(state.GetSensedWindEast())).value() << "|" + << Units::KnotsSpeed(Units::MetersPerSecondSpeed(state.GetSensedWindNorth())).value() + << "|" << Units::KnotsSpeed(reference_cas).value() << "|" + << Units::KnotsSpeed(tas2).value() << "|" << Units::KnotsSpeed(gs2).value() << "|"); +} diff --git a/IntervalManagement/MaintainMetric.cpp b/IntervalManagement/MaintainMetric.cpp index 2932464..047eaa2 100644 --- a/IntervalManagement/MaintainMetric.cpp +++ b/IntervalManagement/MaintainMetric.cpp @@ -22,12 +22,7 @@ using namespace interval_management::open_source; -MaintainMetric::MaintainMetric(void) { - achieveByTime = -1.0; - totalMaintainTime = 0.0; - numCyclesOutsideThreshold = 0; - m_output_enabled = false; -} +MaintainMetric::MaintainMetric() = default; void MaintainMetric::AddSpacingErrorSec(double err) { diff --git a/IntervalManagement/MergePointMetric.cpp b/IntervalManagement/MergePointMetric.cpp index 1077000..01e410a 100644 --- a/IntervalManagement/MergePointMetric.cpp +++ b/IntervalManagement/MergePointMetric.cpp @@ -25,17 +25,9 @@ using namespace std; using namespace interval_management::open_source; 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() = default; -MergePointMetric::~MergePointMetric(void) {} +MergePointMetric::~MergePointMetric() = default; void MergePointMetric::determineMergePoint(const AircraftIntent &imintent, const AircraftIntent &targintent) { @@ -87,15 +79,16 @@ void MergePointMetric::update(double imXNew, double imYNew, double targXNew, dou 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)); + m_im_x_ft = imXNew; + m_im_y_ft = imYNew; + mIMDist = aaesim::open_source::AircraftCalculations::PtToPtDist( + mMergePointX, mMergePointY, Units::FeetLength(m_im_x_ft), Units::FeetLength(m_im_y_ft)); - mTargX = targXNew; - mTargY = targYNew; - mMergeDist = AircraftCalculations::PtToPtDist(Units::FeetLength(mIMX), Units::FeetLength(mIMY), - Units::FeetLength(mTargX), Units::FeetLength(mTargY)); + m_targ_x_ft = targXNew; + m_targ_y_ft = targYNew; + mMergeDist = aaesim::open_source::AircraftCalculations::PtToPtDist( + Units::FeetLength(m_im_x_ft), Units::FeetLength(m_im_y_ft), Units::FeetLength(m_targ_x_ft), + Units::FeetLength(m_targ_y_ft)); } } @@ -126,8 +119,8 @@ bool MergePointMetric::newPointCloser(double x, double y) { // returns true if the new closer to the merge point. // else false. - return (AircraftCalculations::PtToPtDist(mMergePointX, mMergePointY, Units::FeetLength(x), Units::FeetLength(y)) < - mIMDist); + return (aaesim::open_source::AircraftCalculations::PtToPtDist(mMergePointX, mMergePointY, Units::FeetLength(x), + Units::FeetLength(y)) < mIMDist); } bool MergePointMetric::mergePointFound() { diff --git a/IntervalManagement/NMObserver.cpp b/IntervalManagement/NMObserver.cpp index 099f101..7999118 100644 --- a/IntervalManagement/NMObserver.cpp +++ b/IntervalManagement/NMObserver.cpp @@ -21,26 +21,9 @@ using namespace interval_management::open_source; -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. - +void NMObserver::output_NM_values(const double predictedDistance, const double trueDistance, const double time, + const double currIAS, const double currGS, const double targetGS, const double minIAS, + const double maxIAS, const double minTAS, const double maxTAS) { NMObserverEntry new_entry; new_entry.predictedDistance = predictedDistance; @@ -58,7 +41,6 @@ void NMObserver::output_NM_values(double predictedDistance, double trueDistance, } void NMObserver::initialize_stats() { - // sets the stats size while (predictedDistance.size() < entry_list.size()) { Statistics temp_value; predictedDistance.push_back(0.0); diff --git a/IntervalManagement/NMObserverEntry.cpp b/IntervalManagement/NMObserverEntry.cpp index 5572b67..c33a0cd 100644 --- a/IntervalManagement/NMObserverEntry.cpp +++ b/IntervalManagement/NMObserverEntry.cpp @@ -21,17 +21,6 @@ using namespace interval_management::open_source; -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() = default; -NMObserverEntry::~NMObserverEntry(void) {} +NMObserverEntry::~NMObserverEntry() = default; diff --git a/IntervalManagement/PredictionFileKinematic.cpp b/IntervalManagement/PredictionFileKinematic.cpp index 0a5839e..6cf6a2f 100644 --- a/IntervalManagement/PredictionFileKinematic.cpp +++ b/IntervalManagement/PredictionFileKinematic.cpp @@ -30,7 +30,7 @@ log4cplus::Logger PredictionFileKinematic::logger = log4cplus::Logger::getInstance(LOG4CPLUS_TEXT("PredictionFileKinematic")); PredictionFileKinematic::PredictionFileKinematic() - : OutputHandler("", "-Predicted_Kinematic_Trajectory.csv"), + : OutputHandler("", "_predicted_kinematic_trajectory.csv"), algorithm_prediction_ownship(), algorithm_prediction_target() {} @@ -61,60 +61,31 @@ void PredictionFileKinematic::Finish() { os << "vwn_mps"; os << "algorithm"; os << "flap_setting"; - os << NEWLINE; - os.flush(); - // Important for outputting double data. os.set_precision(10); - for (auto id = algorithm_prediction_ownship.cbegin(); id != algorithm_prediction_ownship.cend(); ++id) { - if (id->second.size() == 0) { - LOG4CPLUS_DEBUG(logger, - "No predicted data will be reported for acid " << id->first << ". No algorithm available."); - } - for (auto ix = 0; ix < id->second.size(); ++ix) { - os << id->second[ix].iteration_number; - os << id->second[ix].acid; - os << id->second[ix].source; - os << Units::SecondsTime(id->second[ix].simulation_time).value(); - os << Units::MetersLength(id->second[ix].altitude).value(); - os << Units::MetersPerSecondSpeed(id->second[ix].IAS).value(); - os << Units::SecondsTime(id->second[ix].time_to_go).value(); - os << Units::MetersLength(id->second[ix].distance_to_go).value(); - os << Units::MetersPerSecondSpeed(id->second[ix].GS).value(); - os << Units::MetersPerSecondSpeed(id->second[ix].TAS).value(); - os << id->second[ix].VwePred.value(); - os << id->second[ix].VwnPred.value(); - os << id->second[ix].algorithm; - os << id->second[ix].flap_setting; - + auto data_writer = [this](const std::pair> &data) { + for (auto ix = 0; ix < data.second.size(); ++ix) { + os << data.second[ix].iteration_number; + os << data.second[ix].acid; + os << data.second[ix].source; + os << Units::SecondsTime(data.second[ix].simulation_time).value(); + os << Units::MetersLength(data.second[ix].altitude).value(); + os << Units::MetersPerSecondSpeed(data.second[ix].IAS).value(); + os << Units::SecondsTime(data.second[ix].time_to_go).value(); + os << Units::MetersLength(data.second[ix].distance_to_go).value(); + os << Units::MetersPerSecondSpeed(data.second[ix].GS).value(); + os << Units::MetersPerSecondSpeed(data.second[ix].TAS).value(); + os << data.second[ix].VwePred.value(); + os << data.second[ix].VwnPred.value(); + os << data.second[ix].algorithm; + os << data.second[ix].flap_setting; os << NEWLINE; - os.flush(); } - } - - for (auto id = algorithm_prediction_target.cbegin(); id != algorithm_prediction_target.cend(); ++id) { - for (auto ix = 0; ix < id->second.size(); ++ix) { - os << id->second[ix].iteration_number; - os << id->second[ix].acid; - os << id->second[ix].source; - os << Units::SecondsTime(id->second[ix].simulation_time).value(); - os << Units::MetersLength(id->second[ix].altitude).value(); - os << Units::MetersPerSecondSpeed(id->second[ix].IAS).value(); - os << Units::SecondsTime(id->second[ix].time_to_go).value(); - os << Units::MetersLength(id->second[ix].distance_to_go).value(); - os << Units::MetersPerSecondSpeed(id->second[ix].GS).value(); - os << Units::MetersPerSecondSpeed(id->second[ix].TAS).value(); - os << id->second[ix].VwePred.value(); - os << id->second[ix].VwnPred.value(); - os << id->second[ix].algorithm; - os << id->second[ix].flap_setting; - - os << NEWLINE; - os.flush(); - } - } + }; + std::for_each(algorithm_prediction_ownship.cbegin(), algorithm_prediction_ownship.cend(), data_writer); + std::for_each(algorithm_prediction_target.cbegin(), algorithm_prediction_target.cend(), data_writer); os.close(); } @@ -137,8 +108,8 @@ void PredictionFileKinematic::Gather( std::dynamic_pointer_cast(im_algorithm_adapter->GetImAlgorithm()); IMUtils::IMAlgorithmTypes im_algorithm_type = im_algorithm_adapter->GetImAlgorithmType(); - const VerticalPath *ownship_vert_path; - const VerticalPath *target_vert_path; + const VerticalPath *ownship_vert_path{}; + const VerticalPath *target_vert_path{}; switch (im_algorithm_type) { case IMUtils::IMAlgorithmTypes::TIMEBASEDACHIEVE: @@ -149,15 +120,10 @@ void PredictionFileKinematic::Gather( target_vert_path = &im_kinematic_achieve->GetTargetKinematicPredictor().GetVerticalPredictor()->GetVerticalPath(); break; - case IMUtils::IMAlgorithmTypes::RTA: - ownship_vert_path = - &im_kinematic_achieve->GetOwnshipKinematicPredictor().GetVerticalPredictor()->GetVerticalPath(); - break; case IMUtils::IMAlgorithmTypes::NONE: case IMUtils::IMAlgorithmTypes::TESTSPEEDCONTROL: case IMUtils::IMAlgorithmTypes::KINETICACHIEVE: case IMUtils::IMAlgorithmTypes::KINETICTARGETACHIEVE: - case IMUtils::IMAlgorithmTypes::RTA_TOAC_NOT_IMALGORITHM: break; default: const std::string msg = @@ -171,46 +137,34 @@ void PredictionFileKinematic::Gather( (flight_stage == IMAlgorithm::FlightStage::ACHIEVE || flight_stage == IMAlgorithm::FlightStage::MAINTAIN) && flightdeck_application->IsActive(); - if (im_operation_is_active) { + if (im_operation_is_active && im_kinematic_achieve->IsNewTrajectoryPredictionAvailable()) { std::vector &ownship_prediction_data = algorithm_prediction_ownship[aircraft_id]; - if (TrajectoryWasRegenerated(ownship_prediction_data, *ownship_vert_path, iteration, PredictionData::DataSource::IM_ALGO_OWNSHIP)) { + auto prediction_data_vector = + ExtractPredictionDataFromVerticalPath(iteration, simulation_time, aircraft_id, *ownship_vert_path, + PredictionData::DataSource::IM_ALGO_OWNSHIP); + ownship_prediction_data.insert(ownship_prediction_data.end(), + std::make_move_iterator(prediction_data_vector.begin()), + std::make_move_iterator(prediction_data_vector.end())); + } - if (im_algorithm_type != IMUtils::IMAlgorithmTypes::RTA) { - std::vector &target_prediction_data = algorithm_prediction_target[aircraft_id]; - if (TrajectoryWasRegenerated(target_prediction_data, *target_vert_path, iteration, - PredictionData::DataSource::IM_ALGO_TARGET)) { - - auto prediction_data_vector = ExtractPredictionDataFromVerticalPath( - iteration, simulation_time, aircraft_id, *ownship_vert_path, - PredictionData::DataSource::IM_ALGO_OWNSHIP); - ownship_prediction_data.insert(ownship_prediction_data.end(), - std::make_move_iterator(prediction_data_vector.begin()), - std::make_move_iterator(prediction_data_vector.end())); - prediction_data_vector.clear(); - prediction_data_vector = ExtractPredictionDataFromVerticalPath( - iteration, simulation_time, aircraft_id, *target_vert_path, - PredictionData::DataSource::IM_ALGO_TARGET); - target_prediction_data.insert(target_prediction_data.end(), - std::make_move_iterator(prediction_data_vector.begin()), - std::make_move_iterator(prediction_data_vector.end())); - } - } else { - auto prediction_data_vector = - ExtractPredictionDataFromVerticalPath(iteration, simulation_time, aircraft_id, *ownship_vert_path, - PredictionData::DataSource::IM_ALGO_OWNSHIP); - ownship_prediction_data.insert(ownship_prediction_data.end(), - std::make_move_iterator(prediction_data_vector.begin()), - std::make_move_iterator(prediction_data_vector.end())); - } + std::vector &target_prediction_data = algorithm_prediction_target[aircraft_id]; + if (TrajectoryWasRegenerated(target_prediction_data, *target_vert_path, iteration, + PredictionData::DataSource::IM_ALGO_TARGET)) { + auto prediction_data_vector = + ExtractPredictionDataFromVerticalPath(iteration, simulation_time, aircraft_id, *target_vert_path, + PredictionData::DataSource::IM_ALGO_TARGET); + target_prediction_data.insert(target_prediction_data.end(), + std::make_move_iterator(prediction_data_vector.begin()), + std::make_move_iterator(prediction_data_vector.end())); } } } } const bool PredictionFileKinematic::TrajectoryWasRegenerated( - const std::vector &prediction_data_single_acid, const VerticalPath vertical_path, + const std::vector &prediction_data_single_acid, const VerticalPath &vertical_path, const int iteration, const PredictionData::DataSource source) const { if (!prediction_data_single_acid.empty()) { @@ -229,4 +183,4 @@ const bool PredictionFileKinematic::TrajectoryWasRegenerated( return trajectory_was_regenerated; } return true; -} \ No newline at end of file +} diff --git a/IntervalManagement/Statistics.cpp b/IntervalManagement/Statistics.cpp index 7063cb0..6af4f79 100644 --- a/IntervalManagement/Statistics.cpp +++ b/IntervalManagement/Statistics.cpp @@ -23,16 +23,12 @@ using namespace interval_management::open_source; -Statistics::Statistics() { - m_sum_of_samples = 0; - m_max = 0.0; - m_min = 0.0; -} +Statistics::Statistics() = default; -Statistics::~Statistics() {} +Statistics::~Statistics() = default; void Statistics::Insert(double value) { - if (m_samples.size() == 0) { + if (m_samples.empty()) { m_max = value; m_min = value; } else { @@ -48,10 +44,10 @@ void Statistics::Insert(double value) { double Statistics::ComputeStandardDeviation() const { double sDev = -1.0; - if (m_samples.size() > 0) { + if (!m_samples.empty()) { double variance_sum = 0.0; - for (int loop = 0; loop < (int)m_samples.size(); loop++) { + for (int loop = 0; loop < static_cast(m_samples.size()); loop++) { if (loop != 0) { variance_sum += pow(m_samples[loop] - GetMean(), 2); } else { @@ -65,8 +61,6 @@ double Statistics::ComputeStandardDeviation() const { } double Statistics::GetPercentile(double percentage) { - int desired_sample_num; - double result; vector samples_sorted; for (unsigned int i = 0; i < m_samples.size(); i++) { @@ -76,7 +70,8 @@ double Statistics::GetPercentile(double percentage) { stable_sort(samples_sorted.begin(), samples_sorted.end()); - desired_sample_num = (int)(percentage * m_samples.size()); + double result; + const int desired_sample_num = static_cast(percentage * m_samples.size()); if (desired_sample_num == 0) { result = samples_sorted.at(0); @@ -84,13 +79,13 @@ double Statistics::GetPercentile(double percentage) { result = samples_sorted.at(desired_sample_num - 1); } - return (result); + return result; } double Statistics::Get95thBounds() { double bound95 = -1.0; - double n = (double)m_samples.size(); + const double n = static_cast(m_samples.size()); double prob = 0.0; double delta = 0.1; diff --git a/IntervalManagement/TestVectorSpeedControl.cpp b/IntervalManagement/TestVectorSpeedControl.cpp deleted file mode 100644 index d1a3129..0000000 --- a/IntervalManagement/TestVectorSpeedControl.cpp +++ /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. -// -// 2022 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#include "imalgs/TestVectorSpeedControl.h" -#include "public/ThreeDOFDynamics.h" -#include "public/StandardAtmosphere.h" -#include "public/KinematicTrajectoryPredictor.h" -#include "imalgs/InternalObserver.h" - -using namespace interval_management::open_source; - -unsigned int TestVectorSpeedControl::DEFAULT_DECELERATION_START_TIME_SEC = 60; -unsigned int TestVectorSpeedControl::DEFAULT_ACCELERATION_START_TIME_SEC = 240; -unsigned long TestVectorSpeedControl::DEFAULT_DECELERATION_DELTA_IAS = 30.0; -unsigned long TestVectorSpeedControl::DEFAULT_ACCELERATION_DELTA_IAS = 40.0; - -TestVectorSpeedControl::TestVectorSpeedControl() - : m_distance_calculator(), - m_acceleration_phase_target_ias(), - m_deceleration_phase_target_ias(), - m_acceleration_phase_hold_duration(), - m_acceleration_phase_count(), - m_acceleration_phase_delta_ias(), - m_deceleration_phase_delta_ias(), - m_deceleration_start_time_sec(), - m_acceleration_start_time_sec(), - m_acceleration_phase_complete(false), - m_acceleration_target_achieved(false), - m_pilot_delayed_speeds() { - m_target_kinetic_trajectory_predictor = nullptr; - m_ownship_kinetic_trajectory_predictor = nullptr; -} - -TestVectorSpeedControl::~TestVectorSpeedControl() = default; - -void TestVectorSpeedControl::InitializeFmsPredictors( - const Wgs84KineticDescentPredictor &ownship_kinetic_trajectory_predictor, - const Wgs84KineticDescentPredictor &target_kinetic_trajectory_predictor) { - if (!m_loaded) { - ResetDefaults(); - m_loaded = true; - } - - m_distance_calculator = Wgs84AlongPathDistanceCalculator(ownship_kinetic_trajectory_predictor.GetHorizontalPath(), - TrajectoryIndexProgressionDirection::DECREMENTING); - - const Units::Speed initial_speed = Units::MetersPerSecondSpeed( - ownship_kinetic_trajectory_predictor.GetVertPredictor()->GetVerticalPath().cas_mps.back()); - // For developing Wgs84 - // const Units::Speed initial_speed = Units::MetersPerSecondSpeed(170); - m_deceleration_phase_target_ias = initial_speed - m_deceleration_phase_delta_ias; - m_acceleration_phase_target_ias = m_deceleration_phase_target_ias + m_acceleration_phase_delta_ias; - m_acceleration_phase_complete = false; - m_acceleration_target_achieved = false; - m_acceleration_phase_hold_duration = 10; - m_acceleration_phase_count = 0; - m_pilot_delayed_speeds.clear(); - if (m_pilot_delay.IsPilotDelayOn()) { - const auto delay_params = m_pilot_delay.GetPilotDelayParameters(); - const auto delay_mean = Units::SecondsTime(delay_params.first).value() - 1.0; - for (int i = 0; i < delay_mean; ++i) { - m_pilot_delayed_speeds.push_back(Units::zero()); - } - m_pilot_delayed_speeds.push_back(initial_speed); - } else { - m_pilot_delayed_speeds.push_back(initial_speed); - } -} - -aaesim::open_source::Guidance TestVectorSpeedControl::Update( - const aaesim::open_source::Guidance &prevguidance, const aaesim::open_source::DynamicsState &dynamicsstate, - const interval_management::open_source::AircraftState &owntruthstate, - const interval_management::open_source::AircraftState &targettruthstate, - const std::vector &targethistory) { - - aaesim::open_source::Guidance return_guidance = prevguidance; - const double current_time = owntruthstate.GetTimeStamp().value(); - Units::Speed new_ias_command = prevguidance.m_ias_command; - Units::Speed new_delayed_ias_command = prevguidance.m_ias_command; - - if (current_time < m_deceleration_start_time_sec) { - return_guidance.SetValid(false); - } else if (current_time >= m_deceleration_start_time_sec && current_time <= m_acceleration_start_time_sec) { - new_ias_command = m_deceleration_phase_target_ias; - return_guidance.SetValid(true); - } else if (current_time > 240 and !m_acceleration_phase_complete) { - new_ias_command = m_acceleration_phase_target_ias; - if (!m_acceleration_target_achieved) { - const Units::Speed tolerance = Units::KnotsSpeed(1.0); - m_acceleration_target_achieved = - dynamicsstate.v_indicated_airspeed > (m_acceleration_phase_target_ias - tolerance); - } - m_acceleration_phase_complete = - m_acceleration_target_achieved && m_acceleration_phase_count > m_acceleration_phase_hold_duration; - if (m_acceleration_target_achieved) { - m_acceleration_phase_count++; - } - return_guidance.SetValid(true); - } else { - // operation complete - m_ownship_kinematic_dtg_to_ptp = Units::NegInfinity(); - return_guidance.SetValid(false); - } - - // Modify pilot delay queue - m_pilot_delayed_speeds.pop_front(); - m_pilot_delayed_speeds.push_back(new_ias_command); - new_delayed_ias_command = m_pilot_delayed_speeds.front(); - - if (return_guidance.IsValid()) { - return_guidance.m_ias_command = new_delayed_ias_command; - - // Output only - { - StandardAtmosphere m_standard_atmosphere = StandardAtmosphere(Units::CelsiusTemperature(0.)); - Units::Speed tas_command = - m_standard_atmosphere.CAS2TAS(new_ias_command, Units::FeetLength(owntruthstate.m_z)); - Units::Length ownship_true_dtg; - m_distance_calculator.CalculateAlongPathDistanceFromPosition( - Units::FeetLength(owntruthstate.m_x), Units::FeetLength(owntruthstate.m_y), ownship_true_dtg); - InternalObserver::getInstance()->IM_command_output( - owntruthstate.GetId(), owntruthstate.GetTimeStamp().value(), owntruthstate.m_z, - Units::MetersPerSecondSpeed(dynamicsstate.v_true_airspeed).value(), - Units::MetersPerSecondSpeed(owntruthstate.GetGroundSpeed()).value(), - Units::MetersPerSecondSpeed(new_ias_command).value(), - Units::MetersPerSecondSpeed(new_delayed_ias_command).value(), - Units::MetersPerSecondSpeed(tas_command).value(), 0.0, 0.0, - Units::MetersLength(-ownship_true_dtg).value(), Units::MetersLength(ownship_true_dtg).value()); - } - } - - return return_guidance; -} - -void TestVectorSpeedControl::SetAssignedSpacingGoal(const IMClearance &clearance) { - // leave blank -} - -const double TestVectorSpeedControl::GetSpacingError() const { return UNDEFINED_INTERVAL; } - -bool TestVectorSpeedControl::load(DecodedStream *input) { - set_stream(input); - - ResetDefaults(); - - register_var("deceleration_phase_delta_ias", &m_deceleration_phase_delta_ias, false); - register_var("acceleration_phase_delta_ias", &m_acceleration_phase_delta_ias, false); - register_var("deceleration_start_time_sec", &m_deceleration_start_time_sec, false); - register_var("acceleration_start_time_sec", &m_acceleration_start_time_sec, false); - - // Complete parameter loading. - m_loaded = complete(); - - return m_loaded; -} - -void TestVectorSpeedControl::ResetDefaults() { - m_deceleration_start_time_sec = DEFAULT_DECELERATION_START_TIME_SEC; - m_acceleration_start_time_sec = DEFAULT_ACCELERATION_START_TIME_SEC; - m_deceleration_phase_delta_ias = Units::KnotsSpeed(DEFAULT_DECELERATION_DELTA_IAS); - m_acceleration_phase_delta_ias = Units::KnotsSpeed(DEFAULT_ACCELERATION_DELTA_IAS); - m_target_kinetic_trajectory_predictor = nullptr; - m_ownship_kinetic_trajectory_predictor = nullptr; -} diff --git a/IntervalManagement/TrueDistances.cpp b/IntervalManagement/TrueDistances.cpp index 013e3fd..db6c5e7 100644 --- a/IntervalManagement/TrueDistances.cpp +++ b/IntervalManagement/TrueDistances.cpp @@ -70,20 +70,18 @@ const Units::Length TrueDistances::ComputeTrueDtgToPtpAtTime(const Units::Time & } } - if (ix >= 0) { - // We have found a bounding pair. - if (time_in == m_times[ix - 1]) { - return m_true_distances[ix - 1]; - } else if (time_in == m_times[ix]) { - return m_true_distances[ix]; - } else { - // Interpolate - double ratio = (time_in - m_times[ix - 1]) / (m_times[ix] - m_times[ix - 1]); - return ((1.0 - ratio) * m_true_distances[ix - 1]) + (ratio * m_true_distances[ix]); - } + if (ix < 0) return Units::NegInfinity(); + if (ix >= 1 && time_in == m_times[ix - 1]) { + return m_true_distances[ix - 1]; + } + if (ix >= 0 && time_in == m_times[ix]) { + return m_true_distances[ix]; } + if (ix == 0) return Units::NegInfinity(); - return Units::NegInfinity(); + // Otherwise, interpolate... + double ratio = (time_in - m_times[ix - 1]) / (m_times[ix] - m_times[ix - 1]); + return ((1.0 - ratio) * m_true_distances[ix - 1]) + (ratio * m_true_distances[ix]); } void TrueDistances::Clear() { diff --git a/README.md b/README.md index 8b1555b..fc7d655 100644 --- a/README.md +++ b/README.md @@ -1,23 +1,31 @@ # MITRE's Interval Management Sample Algorithm +![Apache 2.0](https://img.shields.io/badge/license-Apache%20License%202.0-blue?style=for-the-badge) + +![CMake](https://img.shields.io/badge/CMake-%23008FBA.svg?style=for-the-badge&logo=cmake&logoColor=white) +![C++](https://img.shields.io/badge/c++-%2300599C.svg?style=for-the-badge&logo=c%2B%2B&logoColor=white) + +![Rocky Linux](https://img.shields.io/badge/-Rocky%20Linux-%2310B981?style=for-the-badge&logo=rockylinux&logoColor=white) +![macOS](https://img.shields.io/badge/mac%20os-000000?style=for-the-badge&logo=macos&logoColor=F0F0F0) + ## 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 Management Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. +> 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 Management Office, 7515 Colshire Drive, McLean, VA 22102-7539, (703) 983-6000. -Copyright 2020 The MITRE Corporation. All Rights Reserved. +Copyright 2025 The MITRE Corporation. All Rights Reserved. Approved for Public Release; Distribution Unlimited. Public Release Case Number 19-3053 -This project contains content developed by The MITRE Corporation. If this code is used in a deployment or embedded within another project, it is requested that you send an email to opensource@mitre.org in order to let us know where this software is being used. +This project contains content developed by The MITRE Corporation. If this code is used in a deployment or embedded within another project, it is requested that you send an email to in order to let us know where this software is being used. ## Licensing [Apache 2.0](https://github.com/mitre/im_sample_algorithm/blob/master/LICENSE) -Any questions related to MITRE Open Source technologies may be emailed to opensource@mitre.org +Any questions related to MITRE Open Source technologies may be emailed to ## Documentation -Algorithm descriptions are available via [RTCA's DO-361A](https://my.rtca.org/nc__store?search=do-361) documentation (expected publication date Fall 2020). Please contact RTCA for more information. +Formal algorithm descriptions for the aviation industry are available in the [RTCA online store](https://my.rtca.org/): search for `DO-361A` standards documentation. Please contact [RTCA](https://www.rtca.org) for more information. -Code-level documentation is in /docs & posted to [the io page](https://mitre.github.io/im_sample_algorithm/). +Code-level documentation is in this repo in [/docs](./docs/README.md) & hosted [online](https://mitre.github.io/im_sample_algorithm/). diff --git a/docs/README.md b/docs/README.md index ebdf2da..cec7166 100644 --- a/docs/README.md +++ b/docs/README.md @@ -1,91 +1,99 @@ - - -- [MITRE's Interval Management Sample Algorithm](#mitre-s-interval-management-sample-algorithm) - * [TL;DR](#tl-dr) - * [The Legal Stuff (yay!)](#the-legal-stuff--yay--) - + [Notice](#notice) - + [Licensing](#licensing) - * [Published Documentation](#published-documentation) - * [Living Documentation](#living-documentation) - * [EUROCONTROL BADA Development Necessary](#eurocontrol-bada-development-necessary) - * [Related Project](#related-project) - # MITRE's Interval Management Sample Algorithm +- [MITRE's Interval Management Sample Algorithm](#mitres-interval-management-sample-algorithm) + - [TL;DR](#tldr) + - [The Legal Stuff](#the-legal-stuff) + - [Notice](#notice) + - [Licensing](#licensing) + - [Published Documentation](#published-documentation) + - [Living Documentation](#living-documentation) + - [EUROCONTROL BADA Development -- Necessary?](#eurocontrol-bada-development----necessary) + - [Related Project](#related-project) + ## TL;DR -You should be familiar with the [FAA's Flight-deck Interval Management](https://www.faa.gov/about/office_org/headquarters_offices/ang/offices/tc/library/storyboard/detailedwebpages/im.html) concept. To understand any of this content at a techincal level, you need to also have [RTCA's DO-361A, Appendix C](https://my.rtca.org/nc__store?search=do-361). +![Apache 2.0](https://img.shields.io/badge/license-Apache%20License%202.0-blue?style=for-the-badge) -The code is in [this git repo](https://github.com/mitre/im_sample_algorithm). It doesn't compile; that's intentional. We are considering providing some unit tests. +![CMake](https://img.shields.io/badge/CMake-%23008FBA.svg?style=for-the-badge&logo=cmake&logoColor=white) +![C++](https://img.shields.io/badge/c++-%2300599C.svg?style=for-the-badge&logo=c%2B%2B&logoColor=white) -MITRE is happy to answer questions; please [post your questions publicly](https://github.com/mitre/im_sample_algorithm/issues) and we'll gladly respond. +![Rocky Linux](https://img.shields.io/badge/-Rocky%20Linux-%2310B981?style=for-the-badge&logo=rockylinux&logoColor=white) +![macOS](https://img.shields.io/badge/mac%20os-000000?style=for-the-badge&logo=macos&logoColor=F0F0F0) -Good luck. :crossed_fingers: :four_leaf_clover: +This [repository](https://github.com/mitre/im_sample_algorithm) contains a C++ library with some tests. +_It is not an application._ +GitHub [CI](../.github/workflows/ci.yml) is used to build & run tests. -## The Legal Stuff (yay!) +You should be familiar with the [FAA's Flight-deck Interval Management](https://www.faa.gov/about/office_org/headquarters_offices/ang/offices/tc/library/storyboard/detailedwebpages/im.html) concept. To understand any of this content at a techincal level, you need to also have [RTCA's DO-361A, Appendix C](https://my.rtca.org/). -### Notice +For an application that can use this library, see the [FIM MOPS Aircraft & Control Model](https://mitre.github.io/FMACM/) (FMACM). + +MITRE is happy to answer questions; please [post your questions publicly](https://github.com/mitre/im_sample_algorithm/issues) and we'll do our best to respond. + +Good luck. :crossed_fingers: :four_leaf_clover: -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. +## The Legal Stuff -Copyright 2020 The MITRE Corporation. All Rights Reserved. -Approved for Public Release; Distribution Unlimited. 15-1482 +### Notice -This project contains content developed by The MITRE Corporation. If this code is used in a deployment or embedded within another project, it is requested that you send an email to opensource@mitre.org in order to let us know where this software is being used. +> 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. +> Approved for Public Release; Distribution Unlimited. 15-1482 + +This project contains content developed by The MITRE Corporation. If this code is used in a deployment or embedded within another project, it is requested that you send an email to in order to let us know where this software is being used. ### Licensing [Apache 2.0](https://github.com/mitre/im_sample_algorithm/blob/master/LICENSE) -Any questions related to MITRE Open Source technologies may be emailed to opensource@mitre.org +Any questions related to MITRE Open Source technologies may be emailed to ## Published Documentation -Official algorithm descriptions are available via [RTCA's DO-361A](https://my.rtca.org/nc__store?search=do-361) documentation (expected publication date Fall 2020). Please contact RTCA for more information. +Official algorithm descriptions are available via [RTCA's DO-361A](https://my.rtca.org/nc__store?search=do-361) documentation. Please contact RTCA for more information. ## Living Documentation Living, developer-level documentation is provided on this GitHub site. Here the goal is to provide deeper detail regarding how our code works and how others might use this code to inform their own implementations of DO-361A. All topics assume the reader has access to DO-361A, specifically Appendix C. -* **Big Picture** of this codebase -- read [the context](context.md) overview - -* **Developer Talk**: [Does it compile & pass tests?](dev_talk.md) -- hint: nope - -* **Modeling Topics** - * Mapping DO-361A [Appendix C to published code](appendix_url_mapping.md) - * Interval Management [clearance types](im_clearance_types.md) - * [Navigation Database/ARINC-424](navdb.md) discussion - * [Runtime Frequency](traffic_data.md) considerations - * How does the [kinematic trajectory prediction](kinematic_prediction.md) work? -- [Coming Soon](coming_soon.md) - * [Coordinate systems](coordinate_systems.md) used in the code -- [Coming Soon](coming_soon.md) - * EUROCONTROL BADA v3.7. Do you need it? -- Review [the usage guide](bada_usage_guide.md) - -* **Code Topics** - * Who you gonna call? Read [code entry points](entry_points.md) - * How does an algorithm receive IM Clearance details? Read [code entry points](entry_points.md) - * How does an algorithm receive a new aircraft state? Read [code entry points](entry_points.md) and the [traffic data discussion](traffic_data.md) - * Find your own (shorter) path: [IM Turn Implementation](imturn.md) - * What are the ADS-B Data expectations? -- Reveiw the [traffic data discussion](traffic_data.md) - * Where is the [IFPI defined](ifpi.md)? -- [Coming Soon](coming_soon.md) - * Uh...Which algorithm is running? -- Review [the algorithm stages](which_algorithm.md) discussion - * Are we missing some code? Review the [missing code table](missing_code.md) - +- **Big Picture** of this codebase -- read [the context](context.md) overview + +- **Modeling Topics** + + - Mapping DO-361A [Appendix C to published code](appendix_url_mapping.md) + - Interval Management [clearance types](im_clearance_types.md) + - [Navigation Database/ARINC-424](navdb.md) discussion + - [Runtime Frequency](traffic_data.md) considerations + - How does the [kinematic trajectory prediction](kinematic_prediction.md) work? -- [Coming Soon](coming_soon.md) + - [Coordinate systems](coordinate_systems.md) used in the code -- [Coming Soon](coming_soon.md) + - EUROCONTROL BADA v3.7. Do you need it? -- Review [the usage guide](bada_usage_guide.md) + +- **Code Topics** + + - Who you gonna call? :ghosts: Read [code entry points](entry_points.md) + - How does an algorithm receive IM Clearance details? Read [code entry points](entry_points.md) + - How does an algorithm receive a new aircraft state? Read [code entry points](entry_points.md) and the [traffic data discussion](traffic_data.md) + - Find your own (shorter) path: [IM Turn Implementation](imturn.md) + - What are the ADS-B Data expectations? -- Reveiw the [traffic data discussion](traffic_data.md) + - Where is the [IFPI defined](ifpi.md)? -- [Coming Soon](coming_soon.md) + - Uh...Which algorithm is running? -- Review [the algorithm stages](which_algorithm.md) discussion + - Are we missing some code? Review the [missing code table](missing_code.md) + Not seeing what you need to know about? [Post an issue](https://github.com/mitre/im_sample_algorithm/issues). We gladly prioritize specific requests. Thanks! ## EUROCONTROL BADA Development -- Necessary? -As discussed in the [DevTalk](dev_talk.md) documentation, this code uses [EUROCONTROL's BADA](https://eurocontrol.int/services/bada) for aircraft performance data. However, our BADA functionality and code cannot be provided due to licensing restrictions imposed by EUROCONTROL. - -Do you need BADA implemented to use this code base? **Nope**. Take a look at [the BADA usage guide](bada_usage_guide.md) to get more details. +Do you need a full BADA implementation to use this code base? **Nope**. Take a look at [the BADA usage guide](bada_usage_guide.md) to get more details. ## Related Project diff --git a/docs/_data/navigation.yml b/docs/_data/navigation.yml index 93316fa..275e177 100644 --- a/docs/_data/navigation.yml +++ b/docs/_data/navigation.yml @@ -1,5 +1,3 @@ main: - title: "Main" url: / - - title: "Developer Talk" - url: /dev_talk.md diff --git a/docs/appendix_url_mapping.md b/docs/appendix_url_mapping.md index 0f500fc..38876eb 100644 --- a/docs/appendix_url_mapping.md +++ b/docs/appendix_url_mapping.md @@ -6,8 +6,8 @@ * Appendix C: SAMPLE ALGORITHM provides detailed technical documentation of a "sample" algorithm that meets the requirements in DO-361A. The software in this git repo is our own implementation of Appendix C. * Appendix H: MOPS Aircraft and Control Model is an overview of a 3-degree-of-freedom aircraft dynamics & control model which we've used for our research. - * Detailed documentation is found in [this published paper](https://www.mitre.org/publications/technical-papers/derivation-of-a-point-mass-aircraft-model-used-for-fast-time). - * An implementation of Appendix H is [also available](https://mitre.github.io/FMACM) + * Detailed documentation is found in [this published paper](https://www.mitre.org/publications/technical-papers/derivation-of-a-point-mass-aircraft-model-used-for-fast-time). + * An implementation of Appendix H is [also available](https://mitre.github.io/FMACM) These appendices have been written by [MITRE](https://www.mitre.org) and are the copyright of RTCA. In this git repo, and our related [FIM MOPS Aircraft & Control Model](https://mitre.github.io/FMACM), are provided our software implementations of the algorithms and models. Below is a general mapping between each section of DO-361A Appendix C/H and our open source code. diff --git a/docs/bada_usage_guide.md b/docs/bada_usage_guide.md index eccf6b4..02e0bdb 100644 --- a/docs/bada_usage_guide.md +++ b/docs/bada_usage_guide.md @@ -2,62 +2,23 @@ [Back to Landing Page](/README.md) -This Sample Algorithm code uses [EUROCONTROL's BADA](https://eurocontrol.int/services/bada) for aircraft performance data and some calculations. But, the use of BADA data is limited and you may not need to use the same approach that we did. +This Sample Algorithm code benefits from [EUROCONTROL's BADA](https://eurocontrol.int/services/bada) model for aircraft performance data and some calculations. +But the use of BADA data is intentionally limited in scope and you may not need to use the same approach that we did. +Note that the code compiles and passes tests without any BADA code/library provided. -For discussions of IM Speed Limiting, see also DO-361A Section 2.2.4.5.3, Appendix C.4.7, and Appendix K. +For formal documentation of IM Speed Limiting, see also DO-361A Section 2.2.4.5.3, Appendix C.4.7, and Appendix K. **NOTE**: The kinematic trajectory prediction algorithms (e.g. [KinematicDescent4DPredictor.cpp](https://github.com/mitre/im_sample_algorithm/blob/master/IntervalManagement/KinematicDescent4DPredictor.cpp)) do _not use BADA_ at all. -## Code Overview of BADA Usage - -As a minimum operational requirement, your algorithm implementation must limit the IM Speeds according to ownship's flight envelope. In our implementation, we chose to use BADA's flight envelope as a way to meet the miniumum requirements. It provided the data to help us ensure that the IM Speeds produced by the algorithm were within ownship's speed bounds. - -The significant portion of this implementation is found in [IMAlgorithm::LimitImSpeedCommand()](https://github.com/mitre/im_sample_algorithm/blob/968030837f662a197c0e2755280756b18ce9f5b6/IntervalManagement/IMAlgorithm.cpp#L397): - -```c++ -Units::Speed IMAlgorithm::LimitImSpeedCommand( - const Units::Speed im_speed_command_ias, - const double reference_velocity_mps, - const Units::Length distance_to_go_to_abp, - const BadaWithCalc &bada_with_calc, - const Units::Length ownship_altitude, - const int flap_configuration, - const Units::Speed rf_upper_limit) { - -... - // flight envelope protection - if (limitedspeed > bada_with_calc.flight_envelope.V_mo) { - limitedspeed = bada_with_calc.flight_envelope.V_mo; - if (m_quantize_flag) { - int ilimitspeed = Units::KnotsSpeed(limitedspeed).value(); - int iqthreshold = Units::KnotsSpeed(qThreshold).value(); - limitedspeed = Units::KnotsSpeed(ilimitspeed - (ilimitspeed % iqthreshold)); - } - m_active_filter_flag |= 64; - } - -... - // flaps deployed protection - if (flap_configuration > 0) { - if (flap_configuration == 1 && limitedspeed > bada_with_calc.mFlapSpeeds.VappMax) { - limitedspeed = bada_with_calc.mFlapSpeeds.VappMax; - m_active_filter_flag |= 512; - } else if (flap_configuration == 2 && limitedspeed > bada_with_calc.mFlapSpeeds.VlndMax) { - limitedspeed = bada_with_calc.mFlapSpeeds.VlndMax; - m_active_filter_flag |= 512; - } else if (flap_configuration == 3 && limitedspeed > bada_with_calc.mFlapSpeeds.VgearMax) { - limitedspeed = bada_with_calc.mFlapSpeeds.VgearMax; - m_active_filter_flag |= 512; - } - } - -... - - } -``` +## MITRE's BADA Usage -## Using this Code without BADA +As a minimum operational requirement, a FIM algorithm implementation must limit the IM Speeds according to ownship's flight envelope. In our implementation, we chose to use BADA's flight envelope as a way to meet the miniumum requirements. It provided the data to help us ensure that the IM Speeds produced by the algorithm were within ownship's speed bounds. + +The significant portion of this implementation is found in `FIMSpeedLimiter`. -External users of this code may prefer not to rely upon the BADA model for aircraft performance data. It is possible to fully remove BADA references from this code base and still get desired performance. To do so, supply an alternate data source for ownship's flight envelope (e.g. min/max speeds under various operating conditions) and provide those instead. Pay particular attention to populating the code in [IMAlgorithm::LimitImSpeedCommand()](https://github.com/mitre/im_sample_algorithm/blob/968030837f662a197c0e2755280756b18ce9f5b6/IntervalManagement/IMAlgorithm.cpp#L397). +## Using this Code without BADA -As an alternative to BADA, one may consider using the open-source [WRAP](https://github.com/junzis/wrap) aviation data product. However no attempt has been made by MITRE to use that project. Let us know how it goes! :four_leaf_clover: \ No newline at end of file +External users of this code may prefer not to rely upon the BADA model for aircraft performance data. +As an alternative to BADA, one may consider using the open-source [WRAP aviation data](https://github.com/junzis/wrap) product. +No attempt has been made by MITRE to use that project. +If you try it, let us know how it goes! :four_leaf_clover: \ No newline at end of file diff --git a/docs/context.md b/docs/context.md index 9817046..6610804 100644 --- a/docs/context.md +++ b/docs/context.md @@ -2,7 +2,7 @@ [Back to Landing Page](/README.md) -In order to understand this code, what it can (or can't) do, and why it has been publicly released there needs to be a discussion of the context in which this code came about. +In order to understand this code, what it can (or can't) do, and why it has been publicly released there needs to be a discussion of the context in which this code came about. ## What Is This Code? @@ -28,7 +28,7 @@ The first paradigm is a fast-time, parametric simulation environment developed t ![cartoon graphic of parametric simulation architecture](images/parametric_simulation_cartoon.png) -A core part of this fast-time simulation has also been publicly released as the [FIM MOPS Aircraft Dynamics & Control Model](https://mitre.github.io/FMACM). Review the [dev-talk discussion](dev_talk.md) for more information. +A core part of this fast-time simulation has also been publicly released as the [FIM MOPS Aircraft Dynamics & Control Model](https://mitre.github.io/FMACM). ### Human-In-The-Loop Simulations diff --git a/docs/dev_talk.md b/docs/dev_talk.md deleted file mode 100644 index d08abd1..0000000 --- a/docs/dev_talk.md +++ /dev/null @@ -1,44 +0,0 @@ -# Developer Talk - -[Back to Landing Page](/README.md) - -## Build Dependencies - -This code does not compile. We provide responses & hints related to [missing code here](missing_code.md). At the same time, most of the header include statments can be taken care of by bringing in some other projects to your dev environment. - - -### Dependency: FMACM - -This code is missing a lot of include files that come from a related MITRE project: [FIM MOPS Aircraft Dynamics & Control Model](https://mitre.github.io/FMACM). You should definitely get that project into your dev environment, make sure it's compiling and passing the unit tests, then connect it to this code base. - -### Dependency: EUROCONTROL BADA - -This code uses [EUROCONTROL's BADA v3.x](https://eurocontrol.int/services/bada) for aircraft performance data. However, our BADA functionality and code cannot be provided due to licensing restrictions imposed by EUROCONTROL. - -Do you need BADA implemented to use this code base? **Nope**. Take a look at [the BADA usage guide](bada_usage_guide.md) to get more details. - -As an alternative to BADA, one may consider using the open-source [WRAP](https://github.com/junzis/wrap) research product. However no attempt has been made by MITRE to use that project. Let us know how it goes! :four_leaf_clover: - -### Dependency: Log4Cplus - -Log4Cplus is a logging application used by this code base. It needs to be installed prior to building this code. You can download it from [their GitHub repo](https://github.com/log4cplus/log4cplus). - -### Dependency: Units of Measure Library - -[Units of Measure Library](http://sourceforge.net/projects/tuoml/) is an open source (LGPLv2), NIST-compliant, C++ library for handling scientific units of measure. It provides abstract object types, mathematical operations, and compile-time checking of uses. It is used extensively in this code base in order to remove ambiguity and reduce the potential of UoM-related bugs. - -## Continuous Integration & Testing - -MITRE takes quality seriously. But, this repository houses code that is not expected to compile. We will do our best to provide unit testing and CI via travis-ci. - -### Compile & Run - -Disappointment time...this code does not produce a self-contained binary executable nor does it necessarily compile. :disappointed: Rather this repository represents a collection of implemented algorithms provided as a supplement to DO-361A. No attempt has been made to ensure that this compiles. Where possible, examine the unit tests for an understanding of which sections of the code are fully operational. Otherwise, please rely upon the documentation and post issues with questions or conerns. - -No attempt has been made to ensure that this code will compile on a range of operating systems. For MITRE, this code compiles successfully on Linux machines, specifically [CentOs](https://www.centos.org/) 7 using gcc 4.8.5. For all other computing environments, YMMV. - -The [CMake](https://cmake.org/) utility is used to compile this code. If not already installed on the target environment, it is easily installed via `apt`. Please use version 3.0+. - -### Run Unit Tests - -We hope to provide these in time. diff --git a/docs/which_algorithm.md b/docs/which_algorithm.md index db3c694..21ca2a0 100644 --- a/docs/which_algorithm.md +++ b/docs/which_algorithm.md @@ -39,4 +39,4 @@ Guidance IMTimeBasedAchieve::HandleAchieveStage( ... } -``` \ No newline at end of file +``` diff --git a/include/imalgs/AchieveObserver.h b/include/imalgs/AchieveObserver.h index 7496ff7..a975f5f 100644 --- a/include/imalgs/AchieveObserver.h +++ b/include/imalgs/AchieveObserver.h @@ -19,13 +19,14 @@ #pragma once -#include #include #include +#include + namespace interval_management { namespace open_source { -class AchieveObserver { +class AchieveObserver final { public: AchieveObserver(); diff --git a/include/imalgs/AchievePointCalcs.h b/include/imalgs/AchievePointCalcs.h index ca5e655..eeba1f0 100644 --- a/include/imalgs/AchievePointCalcs.h +++ b/include/imalgs/AchievePointCalcs.h @@ -35,20 +35,21 @@ namespace interval_management { namespace open_source { -class AchievePointCalcs { +class AchievePointCalcs final { public: - AchievePointCalcs(void); + AchievePointCalcs(); AchievePointCalcs(const std::string &waypoint, const AircraftIntent &intent, const VerticalPath &vpath, - const std::vector &htraj); + const std::vector &htraj); /** Constructor for target, with enough info to calculate TRP */ AchievePointCalcs(const std::string &waypoint, const AircraftIntent &intent, const VerticalPath &vpath, - const std::vector &htraj, const AchievePointCalcs &ownship_calcs, - const AircraftIntent &ownship_intent); + const std::vector &htraj, + const AchievePointCalcs &ownship_calcs, const AircraftIntent &ownship_intent, + const std::shared_ptr &position_converter); - virtual ~AchievePointCalcs(void); + ~AchievePointCalcs() = default; /** * Compute waypoint distance from end of horizontal path (if there is an achieve waypoint). @@ -91,7 +92,8 @@ class AchievePointCalcs { static void ComputeDefaultTRP(const AchievePointCalcs &ownship_calcs, const AircraftIntent &ownship_intent, const AircraftIntent &target_intent, - const std::vector &target_horizontal_path, + const std::shared_ptr &position_converter, + const std::vector &target_horizontal_path, Waypoint &traffic_reference_point, Units::Length &waypoint_x, Units::Length &waypoint_y, size_t &waypoint_index_in_target_intent); @@ -112,12 +114,12 @@ class AchievePointCalcs { Units::Time m_time_to_go_to_waypoint; Units::Time m_crossing_time; bool m_waypoint_is_set; - std::vector m_horizontal_path; + std::vector m_horizontal_path; private: static log4cplus::Logger m_logger; - AlongPathDistanceCalculator m_distance_calculator; + aaesim::open_source::AlongPathDistanceCalculator m_distance_calculator; }; inline const bool AchievePointCalcs::HasWaypoint() const { return m_waypoint_is_set; } diff --git a/include/imalgs/AircraftState.h b/include/imalgs/AircraftState.h index 0639c1c..eb3497f 100644 --- a/include/imalgs/AircraftState.h +++ b/include/imalgs/AircraftState.h @@ -19,8 +19,6 @@ #pragma once -#include "public/Logging.h" -#include "public/EarthModel.h" #include #include #include @@ -28,9 +26,12 @@ #include #include +#include "public/EarthModel.h" +#include "public/Logging.h" + namespace interval_management { namespace open_source { -class AircraftState { +class AircraftState final { public: AircraftState(); diff --git a/include/imalgs/ClosestPointMetric.h b/include/imalgs/ClosestPointMetric.h index 9534f33..f415e2f 100644 --- a/include/imalgs/ClosestPointMetric.h +++ b/include/imalgs/ClosestPointMetric.h @@ -23,11 +23,11 @@ namespace interval_management { namespace open_source { -class ClosestPointMetric { +class ClosestPointMetric final { public: - ClosestPointMetric(void); + ClosestPointMetric(); - ~ClosestPointMetric(void) = default; + ~ClosestPointMetric() = default; // Computes distance for input position and updates minimum // position if less than minimum distance. @@ -41,12 +41,12 @@ class ClosestPointMetric { int GetTargetAcId() const; private: - int m_im_ac_id; - int m_target_ac_id; - bool m_report_metrics; + int m_im_ac_id{0}; + int m_target_ac_id{0}; + bool m_report_metrics{false}; // Minimum distance between IM and target aircraft. - Units::Length mMinDist; + Units::Length mMinDist{Units::Infinity()}; }; } // namespace open_source } // namespace interval_management \ No newline at end of file diff --git a/include/imalgs/CrossTrackObserver.h b/include/imalgs/CrossTrackObserver.h index db8e0f1..355c62a 100644 --- a/include/imalgs/CrossTrackObserver.h +++ b/include/imalgs/CrossTrackObserver.h @@ -21,7 +21,7 @@ namespace interval_management { namespace open_source { -class CrossTrackObserver { +class CrossTrackObserver final { public: CrossTrackObserver(void); diff --git a/include/imalgs/FIMAlgorithmAdapter.h b/include/imalgs/FIMAlgorithmAdapter.h index f79f656..ef5df30 100644 --- a/include/imalgs/FIMAlgorithmAdapter.h +++ b/include/imalgs/FIMAlgorithmAdapter.h @@ -20,12 +20,13 @@ #pragma once #include "imalgs/IMAlgorithm.h" -#include "public/FlightDeckApplication.h" #include "public/ASSAP.h" +#include "public/FlightDeckApplication.h" +#include "public/TangentPlaneSequence.h" namespace interval_management { namespace open_source { -class FIMAlgorithmAdapter : public aaesim::open_source::FlightDeckApplication { +class FIMAlgorithmAdapter final : public aaesim::open_source::FlightDeckApplication { public: FIMAlgorithmAdapter(std::shared_ptr im_algorithm, IMUtils::IMAlgorithmTypes algorithm_type); @@ -44,13 +45,16 @@ class FIMAlgorithmAdapter : public aaesim::open_source::FlightDeckApplication { } private: + inline static log4cplus::Logger m_logger{log4cplus::Logger::getInstance(LOG4CPLUS_TEXT("FIMAlgorithmAdapter"))}; + static void LogAircraftState(const aaesim::open_source::AircraftState &state); void UpdateTargetHistory(const aaesim::open_source::SimulationTime &simtime); - std::shared_ptr m_im_algorithm; - IMUtils::IMAlgorithmTypes m_im_algorithm_type; - std::shared_ptr m_assap; - aaesim::open_source::GuidanceFlightPhase m_current_guidance_phase; - std::vector m_target_history; - bool m_initialized; + interval_management::open_source::AircraftState ConvertAircraftState( + const aaesim::open_source::AircraftState &state) const; + std::shared_ptr m_im_algorithm{}; + IMUtils::IMAlgorithmTypes m_im_algorithm_type{IMUtils::IMAlgorithmTypes::NONE}; + std::shared_ptr m_assap{}; + std::vector m_target_history{}; + std::shared_ptr m_position_converter{}; }; inline std::shared_ptr @@ -59,5 +63,6 @@ inline std::shared_ptr } inline IMUtils::IMAlgorithmTypes FIMAlgorithmAdapter::GetImAlgorithmType() const { return m_im_algorithm_type; } + } // namespace open_source } // namespace interval_management \ No newline at end of file diff --git a/include/imalgs/FIMAlgorithmDataWriter.h b/include/imalgs/FIMAlgorithmDataWriter.h index 7b2aceb..7d58917 100644 --- a/include/imalgs/FIMAlgorithmDataWriter.h +++ b/include/imalgs/FIMAlgorithmDataWriter.h @@ -19,26 +19,26 @@ #pragma once -#include -#include +#include #include #include -#include #include +#include + +#include #include -#include "public/Logging.h" -#include "public/SimulationTime.h" -#include "public/OutputHandler.h" -#include "public/Guidance.h" -#include "public/FlightDeckApplication.h" #include "imalgs/AircraftState.h" #include "imalgs/IMAlgorithm.h" +#include "public/FlightDeckApplication.h" +#include "public/Guidance.h" +#include "public/Logging.h" +#include "public/OutputHandler.h" +#include "public/SimulationTime.h" namespace interval_management { namespace open_source { -class FIMAlgorithmDataWriter : public OutputHandler { - +class FIMAlgorithmDataWriter final : public OutputHandler { public: FIMAlgorithmDataWriter(); ~FIMAlgorithmDataWriter() = default; @@ -50,58 +50,36 @@ class FIMAlgorithmDataWriter : public OutputHandler { private: struct SimData { - SimData() { - iteration_number = -1; - acid = ""; - simulation_time = Units::SecondsTime(-1.0); - m_target_acid = -1; - - m_imspeed_ias = Units::MetersPerSecondSpeed(-1.0); - m_ownship_ttg_to_abp = Units::SecondsTime(-1.0); - m_ownship_dtg_to_abp = Units::MetersLength(-1.0); - m_flight_stage = interval_management::open_source::IMAlgorithm::FlightStage::UNSET; - m_measured_spacing_interval = -1.0; - m_predicted_spacing_interval = -1.0; - m_assigned_spacing_goal = -INFINITY; - m_im_speed_limit_flags = 0L; - m_ownship_dtg_to_ptp = Units::MetersLength(-1.0); - }; - - int iteration_number; - Units::Time simulation_time; - std::string acid; - - int m_target_acid; - - interval_management::open_source::IMAlgorithm::FlightStage m_flight_stage; - double m_assigned_spacing_goal; - unsigned long int m_current_imspeed_count; - - // algorithm intermediate calculations - double m_measured_spacing_interval; - double m_predicted_spacing_interval; - Units::Time m_ownship_ttg_to_abp; - Units::Time m_ownship_ttg_to_ptp; - Units::Length m_ownship_dtg_to_abp; - Units::Length m_ownship_dtg_to_ptp; - Units::Time m_target_ttg_to_trp; - Units::Length m_target_dtg_to_trp; - Units::Length m_target_projected_position_x; - Units::Length m_target_projected_position_y; - unsigned long int m_im_speed_limit_flags; - double m_target_is_aligned; // can be -infinity, 0, or 1 - - // algorithm output - Units::Speed m_imspeed_ias; - Units::Speed m_imspeed_delayed_ias; - - // reference trajectory parameters used by the algorithm - Units::Speed m_ownship_reference_ias; - Units::Speed m_target_reference_ias; - Units::Speed m_ownship_reference_groundspeed; - Units::Speed m_target_reference_groundspeed; - Units::Length m_ownship_reference_altitude; - Units::Length m_target_reference_altitude; + SimData() = default; + + int iteration_number{-1}; + Units::Time simulation_time{Units::SecondsTime(-1.0)}; + std::string acid{}; + int m_target_acid{-1}; + interval_management::open_source::IMAlgorithm::FlightStage m_flight_stage{ + interval_management::open_source::IMAlgorithm::FlightStage::UNSET}; + double m_assigned_spacing_goal{INT32_MIN}; + unsigned long int m_current_imspeed_count{0}; + double m_measured_spacing_interval{INT32_MIN}; + double m_predicted_spacing_interval{INT32_MIN}; + Units::Time m_ownship_ttg_to_abp{Units::negInfinity()}; + Units::Time m_ownship_ttg_to_ptp{Units::negInfinity()}; + Units::Length m_ownship_dtg_to_abp{Units::negInfinity()}; + Units::Length m_ownship_dtg_to_ptp{Units::negInfinity()}; + Units::Time m_target_ttg_to_trp{Units::negInfinity()}; + Units::Length m_target_dtg_to_trp{Units::negInfinity()}; + Units::Length m_target_projected_position_x{Units::negInfinity()}; + Units::Length m_target_projected_position_y{Units::negInfinity()}; + unsigned long int m_im_speed_limit_flags{0L}; + double m_target_is_aligned{-INFINITY}; // can be -infinity, 0, or 1 + Units::Speed m_imspeed_ias{Units::negInfinity()}; + Units::Speed m_imspeed_delayed_ias{Units::negInfinity()}; + Units::Speed m_ownship_reference_ias{Units::negInfinity()}; + Units::Speed m_target_reference_ias{Units::negInfinity()}; + Units::Speed m_ownship_reference_groundspeed{Units::negInfinity()}; + Units::Speed m_target_reference_groundspeed{Units::negInfinity()}; + Units::Length m_ownship_reference_altitude{Units::negInfinity()}; + Units::Length m_target_reference_altitude{Units::negInfinity()}; }; std::vector m_sim_data; @@ -109,4 +87,4 @@ class FIMAlgorithmDataWriter : public OutputHandler { static log4cplus::Logger m_logger; }; } // namespace open_source -} // namespace interval_management \ No newline at end of file +} // namespace interval_management diff --git a/include/imalgs/FIMAlgorithmInitializer.h b/include/imalgs/FIMAlgorithmInitializer.h index 7fac8cc..561c539 100644 --- a/include/imalgs/FIMAlgorithmInitializer.h +++ b/include/imalgs/FIMAlgorithmInitializer.h @@ -19,16 +19,16 @@ #pragma once -#include "public/FlightDeckApplication.h" #include "imalgs/FIMAlgorithmAdapter.h" +#include "imalgs/IMDistBasedAchieve.h" #include "imalgs/IMKinematicAchieve.h" -#include "imalgs/IMTimeBasedAchieveMutableASG.h" #include "imalgs/IMTimeBasedAchieve.h" -#include "imalgs/IMDistBasedAchieve.h" +#include "imalgs/IMTimeBasedAchieveMutableASG.h" +#include "public/FlightDeckApplication.h" #include "public/PassThroughAssap.h" namespace interval_management { namespace open_source { -class FIMAlgorithmInitializer : public aaesim::open_source::FlightDeckApplicationInitializer { +class FIMAlgorithmInitializer final : public aaesim::open_source::FlightDeckApplicationInitializer { public: FIMAlgorithmInitializer() = default; @@ -41,9 +41,10 @@ class FIMAlgorithmInitializer : public aaesim::open_source::FlightDeckApplicatio std::shared_ptr m_surveillance_processor; public: - Builder() : m_performance_parameters(), m_prediction_parameters() { - m_surveillance_processor = std::make_shared(); - }; + Builder() + : m_performance_parameters(), + m_prediction_parameters(), + m_surveillance_processor(std::make_shared()) {}; ~Builder() = default; const interval_management::open_source::FIMAlgorithmInitializer Build() const; Builder *AddOwnshipPerformanceParameters( @@ -73,4 +74,4 @@ class FIMAlgorithmInitializer : public aaesim::open_source::FlightDeckApplicatio }; } // namespace open_source -} // namespace interval_management \ No newline at end of file +} // namespace interval_management diff --git a/include/imalgs/FIMConfiguration.h b/include/imalgs/FIMConfiguration.h index 54c6bc5..115fa8d 100644 --- a/include/imalgs/FIMConfiguration.h +++ b/include/imalgs/FIMConfiguration.h @@ -19,51 +19,58 @@ #pragma once -#include "loader/DecodedStream.h" #include "loader/Loadable.h" + +#include "loader/DecodedStream.h" #include "public/Logging.h" +#include "imalgs/IMUtils.h" namespace interval_management { namespace open_source { -class FIMConfiguration : public Loadable { +class FIMConfiguration final : public Loadable { public: - FIMConfiguration(); - virtual ~FIMConfiguration(); + FIMConfiguration() = default; + virtual ~FIMConfiguration() = default; bool load(DecodedStream *input); - static const FIMConfiguration &GetDefaultConfiguration() { return DEFAULT_CONFIGURATION; } + static const FIMConfiguration &GetDefaultConfiguration(); private: + inline static FIMConfiguration DEFAULT_CONFIGURATION(); static log4cplus::Logger m_logger; - static const FIMConfiguration DEFAULT_CONFIGURATION; public: - static const Units::HertzFrequency ACHIEVE_CONTROL_GAIN_DEFAULT; - static const Units::HertzFrequency MAINTAIN_CONTROL_GAIN_DEFAULT; - static const Units::SecondsTime TIME_THRESHOLD_DEFAULT; - static const bool THRESHOLD_FLAG_DEFAULT; - static const Units::NauticalMilesLength ERROR_DISTANCE_DEFAULT; - static const Units::SecondsPerNauticalMileInvertedSpeed SLOPE_DEFAULT; + // Do not change these values without discussing with Lesley Weitz. + inline static const Units::HertzFrequency ACHIEVE_CONTROL_GAIN_DEFAULT{0.008}; + inline static const Units::HertzFrequency MAINTAIN_CONTROL_GAIN_DEFAULT{0.008}; + inline static const Units::SecondsTime TIME_THRESHOLD_DEFAULT{0}; + inline static const bool THRESHOLD_FLAG_DEFAULT{true}; + inline static const Units::NauticalMilesLength ERROR_DISTANCE_DEFAULT{0}; + inline static const Units::SecondsPerNauticalMileInvertedSpeed SLOPE_DEFAULT{0.25}; - Units::HertzFrequency m_achieve_control_gain; - Units::HertzFrequency m_maintain_control_gain; - Units::SecondsTime m_time_threshold; - Units::SecondsPerNauticalMileInvertedSpeed m_slope; - Units::NauticalMilesLength m_error_distance; + Units::HertzFrequency m_achieve_control_gain{ACHIEVE_CONTROL_GAIN_DEFAULT}; + Units::HertzFrequency m_maintain_control_gain{MAINTAIN_CONTROL_GAIN_DEFAULT}; + Units::SecondsTime m_time_threshold{TIME_THRESHOLD_DEFAULT}; + Units::SecondsPerNauticalMileInvertedSpeed m_slope{SLOPE_DEFAULT}; + Units::NauticalMilesLength m_error_distance{ERROR_DISTANCE_DEFAULT}; + bool m_use_speed_limiting{IMUtils::LIMIT_FLAG_DEFAULT}; + bool m_use_wind_blending{IMUtils::WIND_BLENDING_FLAG_DEFAULT}; + bool m_threshold_flag{THRESHOLD_FLAG_DEFAULT}; + bool m_use_speed_quantization{IMUtils::QUANTIZE_FLAG_DEFAULT}; + Units::NauticalMilesLength m_loaded_middle_to_final_quantize_transition_distance{IMUtils::DIST_QUANTIZE_1_DEFAULT}; + Units::NauticalMilesLength m_loaded_first_to_middle_quantize_transition_distance{IMUtils::DIST_QUANTIZE_2_DEFAULT}; + Units::KnotsSpeed m_loaded_speed_quantize_final_phase{IMUtils::SPEED_QUANTIZE_1_DEFAULT_1_KNOT}; + Units::KnotsSpeed m_loaded_speed_quantize_middle_phase{IMUtils::SPEED_QUANTIZE_2_DEFAULT}; + Units::KnotsSpeed m_loaded_speed_quantize_first_phase{IMUtils::SPEED_QUANTIZE_3_DEFAULT}; - bool m_use_speed_limiting; - bool m_threshold_flag; - bool m_use_speed_quantization; - - Units::NauticalMilesLength m_loaded_middle_to_final_quantize_transition_distance; - Units::NauticalMilesLength m_loaded_first_to_middle_quantize_transition_distance; - Units::KnotsSpeed m_loaded_speed_quantize_final_phase; - Units::KnotsSpeed m_loaded_speed_quantize_middle_phase; - Units::KnotsSpeed m_loaded_speed_quantize_first_phase; - - bool m_loaded; + bool m_loaded{false}; }; +inline const FIMConfiguration &interval_management::open_source::FIMConfiguration::GetDefaultConfiguration() { + static FIMConfiguration fim_configuration{}; + return fim_configuration; +} + } /* namespace open_source */ } /* namespace interval_management */ diff --git a/include/imalgs/FIMSpeedLimiter.h b/include/imalgs/FIMSpeedLimiter.h index a1b1e06..d8e07ea 100644 --- a/include/imalgs/FIMSpeedLimiter.h +++ b/include/imalgs/FIMSpeedLimiter.h @@ -17,17 +17,17 @@ // 2023 The MITRE Corporation. All Rights Reserved. // **************************************************************************** -#include "utility/BoundedValue.h" -#include "scalar/Speed.h" -#include "scalar/Length.h" -#include "public/WeatherPrediction.h" #include "imalgs/FIMSpeedQuantizer.h" #include "public/BadaUtils.h" #include "public/SpeedCommandLimiter.h" +#include "public/WeatherPrediction.h" +#include "scalar/Length.h" +#include "scalar/Speed.h" +#include "utility/BoundedValue.h" namespace interval_management { namespace open_source { -class FIMSpeedLimiter : public aaesim::open_source::SpeedCommandLimiter { +class FIMSpeedLimiter final : public aaesim::open_source::SpeedCommandLimiter { public: struct RfLegLimit { RfLegLimit(Units::Length distance_to_go, Units::Speed upper_ias_limit) diff --git a/include/imalgs/FIMSpeedQuantizer.h b/include/imalgs/FIMSpeedQuantizer.h index 08880ef..9bd65de 100644 --- a/include/imalgs/FIMSpeedQuantizer.h +++ b/include/imalgs/FIMSpeedQuantizer.h @@ -17,15 +17,15 @@ // 2023 The MITRE Corporation. All Rights Reserved. // **************************************************************************** -#include "utility/BoundedValue.h" #include "public/Logging.h" -#include "scalar/Speed.h" #include "scalar/Length.h" +#include "scalar/Speed.h" +#include "utility/BoundedValue.h" namespace interval_management { namespace open_source { -class FIMSpeedQuantizer { +class FIMSpeedQuantizer final { public: FIMSpeedQuantizer(); diff --git a/include/imalgs/IMAlgorithm.h b/include/imalgs/IMAlgorithm.h index be7d911..2b5467b 100644 --- a/include/imalgs/IMAlgorithm.h +++ b/include/imalgs/IMAlgorithm.h @@ -24,7 +24,7 @@ #include "public/AircraftIntent.h" #include "imalgs/AircraftState.h" #include "public/Logging.h" -#include "public/PilotDelay.h" +#include "public/StatisticalPilotDelay.h" #include "public/ThreeDOFDynamics.h" #include "public/TangentPlaneSequence.h" #include @@ -89,8 +89,7 @@ class IMAlgorithm { void UpdatePositionMetrics(const interval_management::open_source::AircraftState &ownship_aircraft_state, const interval_management::open_source::AircraftState &target_aircraft_state); - void SetPilotDelay(const bool pilot_delay_on, const Units::Time pilot_delay_mean, - const Units::Time pilot_delay_standard_deviation); + void SetPilotDelay(aaesim::open_source::StatisticalPilotDelay &pilot_delay); void DisablePilotDelayModel(); @@ -150,16 +149,16 @@ class IMAlgorithm { virtual const double GetSpacingInterval() const; + virtual bool IsBlendWind() const; + + virtual void SetBlendWind(bool wind_blending_enabled); + const Units::Speed GetUnmodifiedImSpeedCommandIas() const; const IMClearance &GetClearance() const; const unsigned long int GetActiveFilter() const; - virtual bool IsBlendWind() const; - - virtual void SetBlendWind(bool wind_blending_enabled) = 0; - Units::Length GetMiddleToFinalQuantizationTransitionDistance() const; Units::Length GetFirstToMiddleQuantizationTransitionDistance() const; @@ -200,6 +199,9 @@ class IMAlgorithm { void SetClearance(const IMClearance &im_clearance) { m_im_clearance = im_clearance; } + std::shared_ptr GetAtmosphere() { return m_weather_prediction.GetForecastAtmosphere(); } + void SetAtmosphere(std::shared_ptr atmosphere) { m_weather_prediction.SetAtmosphere(atmosphere); } + protected: void Copy(const IMAlgorithm &obj); @@ -212,7 +214,7 @@ class IMAlgorithm { AircraftIntent m_target_aircraft_intent; FlightStage m_stage_of_im_operation; IMClearance m_im_clearance; - PilotDelay m_pilot_delay; + aaesim::open_source::StatisticalPilotDelay m_pilot_delay; aaesim::open_source::WeatherPrediction m_weather_prediction; interval_management::open_source::FIMSpeedLimiter m_speed_limiter; @@ -265,6 +267,7 @@ class IMAlgorithm { Units::KnotsSpeed m_loaded_speed_quantize_middle_phase; Units::KnotsSpeed m_loaded_speed_quantize_first_phase; bool m_loaded_use_speed_limiting; + bool m_loaded_use_wind_blending; bool m_loaded_use_speed_quantization; Units::Frequency m_achieve_control_gain; @@ -299,6 +302,10 @@ inline void IMAlgorithm::EnableDefaultPilotDelayModel() { m_pilot_delay.SetUsePi inline void IMAlgorithm::SetLimitFlag(bool limit_flag) { m_speed_limiter.SetLimitFlag(limit_flag); } +inline void IMAlgorithm::SetBlendWind(bool wind_blending_enabled) { + m_loaded_use_wind_blending = wind_blending_enabled; +} + inline void IMAlgorithm::SetErrorDistance(Units::Length error_distance) { m_error_distance = error_distance; } inline void IMAlgorithm::SetSlope(Units::InvertedSpeed slope) { m_slope = slope; } @@ -344,11 +351,10 @@ inline int IMAlgorithm::GetSpeedChangeCount() const { return m_total_number_of_i inline const bool IMAlgorithm::IsLoaded() const { return m_loaded; } -inline bool IMAlgorithm::IsBlendWind() const { return false; } +inline bool IMAlgorithm::IsBlendWind() const { return m_loaded_use_wind_blending; } inline void IMAlgorithm::SetWeatherPrediction(const aaesim::open_source::WeatherPrediction &weather_prediction) { m_weather_prediction = weather_prediction; - m_pilot_delay.SetAtmosphere(weather_prediction.getAtmosphere()); } inline Units::Length IMAlgorithm::GetMiddleToFinalQuantizationTransitionDistance() const { @@ -397,4 +403,4 @@ inline const Units::Length IMAlgorithm::GetTargetReferenceAltitude() const { ret inline const Units::Length IMAlgorithm::GetTargetKinematicDtgToTrp() const { return m_target_kinematic_dtg_to_trp; } } // namespace open_source -} // namespace interval_management \ No newline at end of file +} // namespace interval_management diff --git a/include/imalgs/IMClearance.h b/include/imalgs/IMClearance.h index 596d561..921f336 100644 --- a/include/imalgs/IMClearance.h +++ b/include/imalgs/IMClearance.h @@ -19,25 +19,26 @@ #pragma once -#include -#include - -#include #include #include -#include "public/Logging.h" -#include "public/AircraftIntent.h" +#include + +#include +#include + #include "imalgs/IMUtils.h" +#include "public/AircraftIntent.h" +#include "public/Logging.h" namespace interval_management { namespace open_source { -class IMClearance { +class IMClearance final { public: enum ClearanceType { NONE = -1, CUSTOM = 0, CAPTURE, MAINTAIN, ACHIEVE, FAS }; enum SpacingGoalType { TIME = 0, DIST }; - IMClearance(); + IMClearance() = default; ~IMClearance() = default; @@ -60,26 +61,25 @@ class IMClearance { Builder(const ClearanceType &clearance_type, const int target_id, const AircraftIntent &target_intent, const std::string &achieve_by_point, const std::string &planned_termination_point, const SpacingGoalType &assigned_spacing_goal_type, const double assigned_spacing_goal) - : m_builder_ownship_intent(), + : m_builder_clearance_type(clearance_type), + m_builder_assigned_spacing_goal_type(assigned_spacing_goal_type), + m_builder_target_aircraft_intent(target_intent), + m_builder_ownship_intent(), m_builder_final_approach_spacing_merge_angle_mean(0), m_builder_final_approach_spacing_merge_angle_std(0), + m_builder_achieve_by_point(achieve_by_point), + m_builder_planned_termination_point(planned_termination_point), m_builder_traffic_reference_point(), - m_builder_is_vector_aircraft(false) { - m_builder_clearance_type = clearance_type; - m_builder_target_id = target_id; - m_builder_target_aircraft_intent = target_intent; - m_builder_achieve_by_point = achieve_by_point; - m_builder_planned_termination_point = planned_termination_point; - m_builder_assigned_spacing_goal_type = assigned_spacing_goal_type; - m_builder_assigned_spacing_goal = assigned_spacing_goal; - }; + m_builder_assigned_spacing_goal(assigned_spacing_goal), + m_builder_target_id(target_id), + m_builder_is_vector_aircraft(false) {}; ~Builder() = default; const IMClearance Build() const { return IMClearance(this); } Builder *OwnshipIntent(const AircraftIntent &ownship_intent) { m_builder_ownship_intent = ownship_intent; return this; }; - Builder *TrafficReferencePoint(const std::string traffic_reference_point) { + Builder *TrafficReferencePoint(const std::string &traffic_reference_point) { m_builder_traffic_reference_point = traffic_reference_point; return this; }; @@ -105,12 +105,6 @@ class IMClearance { bool IsVectorAircraft() const { return m_builder_is_vector_aircraft; }; }; - IMClearance(const IMClearance &obj); - - bool operator==(const IMClearance &obj) const; - - bool operator!=(const IMClearance &obj) const; - bool Validate(const AircraftIntent &ownship_aircraft_intent, const IMUtils::IMAlgorithmTypes im_algorithm_type); bool IsValid() const; @@ -139,8 +133,6 @@ class IMClearance { bool IsVectorAircraft() const; - void dump(std::string hdr) const; - Units::Angle GetFinalApproachSpacingMergeAngleStd() const; void SetFinalApproachSpacingMergeAngleStd(Units::Angle final_approach_spacing_merge_angle_std); @@ -170,24 +162,24 @@ class IMClearance { bool ValidatePlannedTerminationPoint(const AircraftIntent &ownship_aircraft_intent, const IMUtils::IMAlgorithmTypes im_algorithm_type) const; - ClearanceType m_clearance_type; - SpacingGoalType m_assigned_spacing_goal_type; - AircraftIntent m_target_aircraft_intent; - AircraftIntent m_ownship_intent; - Units::RadiansAngle m_final_approach_spacing_merge_angle_mean; - Units::RadiansAngle m_final_approach_spacing_merge_angle_std; - std::string m_achieve_by_point; - std::string m_planned_termination_point; - std::string m_traffic_reference_point; - double m_assigned_spacing_goal; - int m_target_id; - bool m_is_vector_aircraft; - bool m_is_coincident_route_pairing; - bool m_valid; + ClearanceType m_clearance_type{CUSTOM}; + SpacingGoalType m_assigned_spacing_goal_type{TIME}; + AircraftIntent m_target_aircraft_intent{}; + AircraftIntent m_ownship_intent{}; + Units::RadiansAngle m_final_approach_spacing_merge_angle_mean{0}; + Units::RadiansAngle m_final_approach_spacing_merge_angle_std{0}; + std::string m_achieve_by_point{}; + std::string m_planned_termination_point{}; + std::string m_traffic_reference_point{}; + double m_assigned_spacing_goal{-INFINITY}; + int m_target_id{IMUtils::UNINITIALIZED_AIRCRAFT_ID}; + bool m_is_vector_aircraft{false}; + bool m_is_coincident_route_pairing{false}; + bool m_valid{false}; }; inline const bool IMClearance::AbpAndPtpAreColocated() const { return m_achieve_by_point == m_planned_termination_point; } } // namespace open_source -} // namespace interval_management \ No newline at end of file +} // namespace interval_management diff --git a/include/imalgs/IMClearanceLoader.h b/include/imalgs/IMClearanceLoader.h index 48e099b..7f7739c 100644 --- a/include/imalgs/IMClearanceLoader.h +++ b/include/imalgs/IMClearanceLoader.h @@ -19,14 +19,15 @@ #pragma once -#include "loader/Loadable.h" +#include + #include "imalgs/IMClearance.h" +#include "loader/Loadable.h" #include "public/Logging.h" -#include namespace interval_management { -class IMClearanceLoader : public Loadable { +class IMClearanceLoader final : public Loadable { public: IMClearanceLoader(); diff --git a/include/imalgs/IMDistBasedAchieve.h b/include/imalgs/IMDistBasedAchieve.h index 11021a9..7ad566e 100644 --- a/include/imalgs/IMDistBasedAchieve.h +++ b/include/imalgs/IMDistBasedAchieve.h @@ -20,12 +20,12 @@ #pragma once #include "imalgs/IMKinematicAchieve.h" -#include "imalgs/TrueDistances.h" #include "imalgs/IMKinematicDistBasedMaintain.h" +#include "imalgs/TrueDistances.h" namespace interval_management { namespace open_source { -class IMDistBasedAchieve : public IMKinematicAchieve { +class IMDistBasedAchieve final : public IMKinematicAchieve { public: static const Units::Length DEFAULT_DISTANCE_BASED_ASSIGNED_SPACING_GOAL; @@ -35,7 +35,12 @@ class IMDistBasedAchieve : public IMKinematicAchieve { virtual ~IMDistBasedAchieve(); - virtual void IterationReset(); + virtual void IterationReset() override; + + void Initialize(const OwnshipPredictionParameters &ownship_prediction_parameters, + const AircraftIntent &ownship_aircraft_intent, + aaesim::open_source::WeatherPrediction &weather_prediction, + std::shared_ptr &position_converter) override; virtual void Initialize(const OwnshipPredictionParameters &ownship_prediction_parameters, const AircraftIntent &ownship_aircraft_intent, @@ -46,29 +51,29 @@ class IMDistBasedAchieve : public IMKinematicAchieve { const aaesim::open_source::DynamicsState &three_dof_dynamics_state, const interval_management::open_source::AircraftState ¤t_ownship_state, const interval_management::open_source::AircraftState ¤t_target_state, - const std::vector &target_adsb_history); + const std::vector &target_adsb_history) override; - virtual void DumpParameters(const std::string ¶meters_to_print); + virtual void DumpParameters(const std::string ¶meters_to_print) override; - virtual const double GetSpacingError() const; + virtual const double GetSpacingError() const override; - virtual void SetAssignedSpacingGoal(const IMClearance &clearance); + virtual void SetAssignedSpacingGoal(const IMClearance &clearance) override; - virtual const bool IsImOperationComplete() const; + virtual const bool IsImOperationComplete() const override; - virtual const bool InAchieveStage() const; + virtual const bool InAchieveStage() const override; - virtual int GetSpeedChangeCount() const; + virtual int GetSpeedChangeCount() const override; - virtual const Units::Speed GetImSpeedCommandIas() const; + virtual const Units::Speed GetImSpeedCommandIas() const override; - virtual const double GetSpacingInterval() const; + virtual const double GetSpacingInterval() const override; - virtual const double GetPsi() const; + virtual const double GetPsi() const override; - virtual const double GetMsi() const; + virtual const double GetMsi() const override; - virtual const double GetAssignedSpacingGoal() const; + virtual const double GetAssignedSpacingGoal() const override; const Units::Length GetTrueDtgToAchieveByPointUsingReferenceTime(const Units::Time reference_time); @@ -76,11 +81,11 @@ class IMDistBasedAchieve : public IMKinematicAchieve { const interval_management::open_source::AircraftState GetTargetStateProjectedAsgAdjusted() const override; - bool load(DecodedStream *input); + bool load(DecodedStream *input) override; protected: virtual void CalculateIas(const Units::Length current_ownship_altitude, - const aaesim::open_source::DynamicsState &three_dof_dynamics_state); + const aaesim::open_source::DynamicsState &three_dof_dynamics_state) override; virtual void CalculateMach(const Units::Time reference_ttg, const Units::Length current_ownship_altitude, const Units::Mass current_mass); @@ -111,8 +116,8 @@ class IMDistBasedAchieve : public IMKinematicAchieve { Units::Length m_predicted_spacing_interval; Units::Length m_measured_spacing_interval; Units::Length m_assigned_spacing_goal; - AlongPathDistanceCalculator m_distance_calculator_target_on_ownship_hpath; - PositionCalculator m_position_calculator_target_on_ownship_hpath; + aaesim::open_source::AlongPathDistanceCalculator m_distance_calculator_target_on_ownship_hpath; + aaesim::open_source::PositionCalculator m_position_calculator_target_on_ownship_hpath; static log4cplus::Logger m_logger; }; diff --git a/include/imalgs/IMKinematicAchieve.h b/include/imalgs/IMKinematicAchieve.h index 52678b2..516f59c 100644 --- a/include/imalgs/IMKinematicAchieve.h +++ b/include/imalgs/IMKinematicAchieve.h @@ -19,17 +19,18 @@ #pragma once +#include "imalgs/AchievePointCalcs.h" #include "imalgs/IMAchieve.h" #include "imalgs/IMKinematicTimeBasedMaintain.h" -#include "public/KinematicTrajectoryPredictor.h" -#include "imalgs/AchievePointCalcs.h" #include "imalgs/InternalObserver.h" +#include "public/BlendWindsVerticallyByAltitude.h" +#include "public/KinematicTrajectoryPredictor.h" +#include "public/WindBlendingAlgorithm.h" namespace interval_management { namespace open_source { class IMKinematicAchieve : public IMAchieve, public Loadable { - public: enum RFLegPhase { NON_RF_LEG, ON_RF_LEG, PRE_RF_LEG }; @@ -47,6 +48,11 @@ class IMKinematicAchieve : public IMAchieve, public Loadable { const AircraftIntent &ownship_aircraft_intent, aaesim::open_source::WeatherPrediction &weather_prediction) override; + virtual void Initialize(const OwnshipPredictionParameters &ownship_prediction_parameters, + const AircraftIntent &ownship_aircraft_intent, + aaesim::open_source::WeatherPrediction &weather_prediction, + std::shared_ptr &position_converter) = 0; + aaesim::open_source::Guidance Update( const aaesim::open_source::Guidance &prevguidance, const aaesim::open_source::DynamicsState &dynamicsstate, const interval_management::open_source::AircraftState &owntruthstate, @@ -61,10 +67,6 @@ class IMKinematicAchieve : public IMAchieve, public Loadable { virtual const interval_management::open_source::AircraftState GetTargetStateProjectedAsgAdjusted() const = 0; - bool IsBlendWind() const override; - - void SetBlendWind(bool wind_blending_enabled) override; - /* * API */ @@ -98,7 +100,7 @@ class IMKinematicAchieve : public IMAchieve, public Loadable { const aaesim::open_source::KinematicTrajectoryPredictor &GetTargetKinematicPredictor() const; - bool load(DecodedStream *input); + bool load(DecodedStream *input) override; const bool IsTargetAligned() const; @@ -107,15 +109,15 @@ class IMKinematicAchieve : public IMAchieve, public Loadable { const Waypoint &GetTrafficReferencePoint() const; protected: - virtual const bool IsOwnshipBelowTransitionAltitude(Units::Length current_ownship_altitude); + virtual const bool IsOwnshipBelowTransitionAltitude(Units::Length current_ownship_altitude) override; - Waypoint MakeWaypointFromState(const interval_management::open_source::AircraftState aircraft_state, + Waypoint MakeWaypointFromState(const interval_management::open_source::AircraftState &aircraft_state, Units::Speed wind_x, Units::Speed wind_y) const; void CalculateRFLegPhase(const std::vector &waypoints, const Units::Acceleration deceleration_rate_flight_path_angle, const VerticalPath &vertical_path, - const std::vector &horizontal_trajectory); + const std::vector &horizontal_trajectory); void ComputeFASTrajectories(const interval_management::open_source::AircraftState &owntruthstate, const interval_management::open_source::AircraftState &targettruthstate); @@ -135,9 +137,9 @@ class IMKinematicAchieve : public IMAchieve, public Loadable { AircraftIntent m_ownship_aircraft_intent; - AlongPathDistanceCalculator m_ownship_distance_calculator; - AlongPathDistanceCalculator m_target_distance_calculator; - AlongPathDistanceCalculator m_im_ownship_distance_calculator; + aaesim::open_source::AlongPathDistanceCalculator m_ownship_distance_calculator; + aaesim::open_source::AlongPathDistanceCalculator m_target_distance_calculator; + aaesim::open_source::AlongPathDistanceCalculator m_im_ownship_distance_calculator; std::list m_ownship_track_angle_history; std::list m_target_track_angle_history; @@ -154,8 +156,14 @@ class IMKinematicAchieve : public IMAchieve, public Loadable { bool m_is_target_aligned; bool m_new_trajectory_prediction_available; + std::shared_ptr m_wind_blender{ + std::make_shared()}; + static const Units::FeetLength TARGET_ALTITUDE_TOLERANCE; + protected: + void SetTangentPlaneSequence(std::shared_ptr tangent_plane_sequence); + private: void IterClearIMKinAch(); @@ -171,14 +179,8 @@ class IMKinematicAchieve : public IMAchieve, public Loadable { void CalculateTargetDtgToImPoints(const interval_management::open_source::AircraftState ¤t_lead_state); - void TrimAircraftIntentAfterWaypoint(AircraftIntent &aircraft_intent, const std::string &waypoint_name); - - void SetTangentPlaneSequence(std::shared_ptr tangent_plane_sequence); - static log4cplus::Logger logger; - bool m_blend_wind; - std::shared_ptr m_tangent_plane_sequence; }; @@ -225,10 +227,6 @@ inline const aaesim::open_source::KinematicTrajectoryPredictor &IMKinematicAchie return m_target_kinematic_trajectory_predictor; } -inline bool IMKinematicAchieve::IsBlendWind() const { return m_blend_wind; } - -inline void IMKinematicAchieve::SetBlendWind(bool wind_blending_enabled) { m_blend_wind = wind_blending_enabled; } - inline void IMKinematicAchieve::SetRecordMaintainMetrics(bool new_value) { InternalObserver::getInstance()->SetRecordMaintainMetrics(new_value); } diff --git a/include/imalgs/IMKinematicDistBasedMaintain.h b/include/imalgs/IMKinematicDistBasedMaintain.h index 32d8f0f..f85838e 100644 --- a/include/imalgs/IMKinematicDistBasedMaintain.h +++ b/include/imalgs/IMKinematicDistBasedMaintain.h @@ -19,14 +19,14 @@ #pragma once +#include "imalgs/AchievePointCalcs.h" #include "imalgs/IMMaintain.h" -#include "public/KinematicTrajectoryPredictor.h" #include "imalgs/TrueDistances.h" -#include "imalgs/AchievePointCalcs.h" +#include "public/KinematicTrajectoryPredictor.h" namespace interval_management { namespace open_source { -class IMKinematicDistBasedMaintain : public IMMaintain { +class IMKinematicDistBasedMaintain final : public IMMaintain { public: IMKinematicDistBasedMaintain(); @@ -47,7 +47,7 @@ class IMKinematicDistBasedMaintain : public IMMaintain { const std::vector &target_aircraft_state_history, const interval_management::open_source::AchievePointCalcs &ownship_achieve_point_calcs, const interval_management::open_source::AchievePointCalcs &traffic_reference_point_calcs, - PilotDelay &pilot_delay); + aaesim::open_source::StatisticalPilotDelay &pilot_delay); virtual const double GetMsi() const; @@ -60,13 +60,13 @@ class IMKinematicDistBasedMaintain : public IMMaintain { const Units::Length target_kinematic_dtg_to_end_of_route, const aaesim::open_source::DynamicsState &dynamics_state, const aaesim::open_source::KinematicTrajectoryPredictor &ownship_kinematic_trajectory_predictor, - PilotDelay &pilot_delay); + aaesim::open_source::StatisticalPilotDelay &pilot_delay); void CalculateMach(const Units::Length current_ownship_altitude, const Units::Length target_kinematic_dtg_to_end_of_route, const Units::Speed true_airspeed_command, const aaesim::open_source::KinematicTrajectoryPredictor &ownship_kinematic_trajectory_predictor, - PilotDelay &pilot_delay, const Units::Mass current_mass); + aaesim::open_source::StatisticalPilotDelay &pilot_delay, const Units::Mass current_mass); void RecordInternalObserverData( const interval_management::open_source::AircraftState &ownship_aircraft_state, diff --git a/include/imalgs/IMKinematicTimeBasedMaintain.h b/include/imalgs/IMKinematicTimeBasedMaintain.h index 5411cf4..c0b6948 100644 --- a/include/imalgs/IMKinematicTimeBasedMaintain.h +++ b/include/imalgs/IMKinematicTimeBasedMaintain.h @@ -46,7 +46,8 @@ class IMKinematicTimeBasedMaintain final : public IMMaintain { const std::vector &target_aircraft_state_history, const interval_management::open_source::AchievePointCalcs &ownship_achieve_point_calcs, const interval_management::open_source::AchievePointCalcs &traffic_reference_point_calcs, - PilotDelay &pilot_delay_model, const Units::Length &target_kinematic_dtg_to_end_of_route); + aaesim::open_source::StatisticalPilotDelay &pilot_delay_model, + const Units::Length &target_kinematic_dtg_to_end_of_route); virtual const double GetMsi() const; @@ -65,11 +66,11 @@ class IMKinematicTimeBasedMaintain final : public IMMaintain { void CalculateIas(const Units::Length current_ownship_altitude, const aaesim::open_source::DynamicsState &dynamics_state, const aaesim::open_source::KinematicTrajectoryPredictor &ownship_kinematic_trajectory_predictor, - PilotDelay &pilot_delay); + aaesim::open_source::StatisticalPilotDelay &pilot_delay); void CalculateMach(const Units::Length current_ownship_altitude, const Units::Speed true_airspeed_command, const aaesim::open_source::KinematicTrajectoryPredictor &ownship_kinematic_trajectory_predictor, - PilotDelay &pilot_delay, const Units::Mass current_mass); + aaesim::open_source::StatisticalPilotDelay &pilot_delay, const Units::Mass current_mass); const bool IsOwnshipBelowTransitionAltitude( Units::Length current_ownship_altitude, @@ -98,7 +99,7 @@ inline void IMKinematicTimeBasedMaintain::DoAlgorithmLogging( using json = nlohmann::json; if (m_logger.getLogLevel() == log4cplus::TRACE_LOG_LEVEL) { json j; - auto state_to_json = [&j](std::string prefix, const AircraftState &state) { + auto state_to_json = [&j](const std::string &prefix, const AircraftState &state) { j[prefix + ".id"] = state.GetId(); j[prefix + ".timestamp_sec"] = Units::SecondsTime(state.GetTimeStamp()).value(); j[prefix + ".groundspeed_mps"] = Units::MetersPerSecondSpeed(state.GetGroundSpeed()).value(); @@ -131,4 +132,4 @@ inline void IMKinematicTimeBasedMaintain::DoAlgorithmLogging( } } } // namespace open_source -} // namespace interval_management \ No newline at end of file +} // namespace interval_management diff --git a/include/imalgs/IMMaintain.h b/include/imalgs/IMMaintain.h index f2facaa..6ed2487 100644 --- a/include/imalgs/IMMaintain.h +++ b/include/imalgs/IMMaintain.h @@ -19,9 +19,10 @@ #pragma once -#include "imalgs/IMAlgorithm.h" -#include "imalgs/IMAchieve.h" #include + +#include "imalgs/IMAchieve.h" +#include "imalgs/IMAlgorithm.h" #include "public/EuclideanTrajectoryPredictor.h" namespace interval_management { namespace open_source { @@ -36,41 +37,40 @@ class IMMaintain : public IMAlgorithm { virtual ~IMMaintain() = default; - virtual void IterationReset(); + virtual void IterationReset() override; - virtual void ResetDefaults(); + virtual void ResetDefaults() override; void InitializeScenario(IMAchieve *obj, const Units::Frequency maintain_control_gain); virtual void Prepare(Units::Speed previous_im_speed_command, Units::Speed previous_ias_command, interval_management::open_source::FIMSpeedLimiter speed_limiter, double previous_mach_command, const aaesim::open_source::EuclideanTrajectoryPredictor &ownship_trajectory_predictor, - const AlongPathDistanceCalculator &im_distance_calculator, + const aaesim::open_source::AlongPathDistanceCalculator &im_distance_calculator, const std::vector &target_adsb_track_history, const IMClearance &im_clearance, const std::vector &rf_limits); Units::Frequency GetMaintainControlGain() const; - virtual const double GetSpacingError() const; + virtual const double GetSpacingError() const override; - virtual void SetAssignedSpacingGoal(const IMClearance &clearance); + virtual void SetAssignedSpacingGoal(const IMClearance &clearance) override; - virtual void DumpParameters(const std::string ¶meters_to_print); + virtual void DumpParameters(const std::string ¶meters_to_print) override; void SetBlendWind(bool wind_blending_enabled) override; protected: void Copy(const IMMaintain &obj); - AlongPathDistanceCalculator m_ownship_decrementing_distance_calculator; - AlongPathDistanceCalculator m_ownship_distance_calculator; + aaesim::open_source::AlongPathDistanceCalculator m_ownship_decrementing_distance_calculator; + aaesim::open_source::AlongPathDistanceCalculator m_ownship_distance_calculator; private: static log4cplus::Logger m_logger; }; -inline void IMMaintain::SetBlendWind(bool wind_blending_enabled) { /* required by the interface, but not used */ -} +inline void IMMaintain::SetBlendWind(bool wind_blending_enabled) { /* required by the interface, but not used */ } } // namespace open_source } // namespace interval_management \ No newline at end of file diff --git a/include/imalgs/IMTimeBasedAchieve.h b/include/imalgs/IMTimeBasedAchieve.h index c799375..ecd9220 100644 --- a/include/imalgs/IMTimeBasedAchieve.h +++ b/include/imalgs/IMTimeBasedAchieve.h @@ -33,40 +33,45 @@ class IMTimeBasedAchieve : public IMKinematicAchieve { IMTimeBasedAchieve &operator=(const IMTimeBasedAchieve &obj); - virtual void IterationReset(); + virtual void IterationReset() override; virtual aaesim::open_source::Guidance Update( const aaesim::open_source::Guidance &previous_im_guidance, const aaesim::open_source::DynamicsState &three_dof_dynamics_state, const interval_management::open_source::AircraftState ¤t_ownship_state, const interval_management::open_source::AircraftState ¤t_target_state, - const std::vector &target_adsb_history); + const std::vector &target_adsb_history) override; - virtual const bool IsImOperationComplete() const; + virtual const bool IsImOperationComplete() const override; - virtual const double GetAssignedSpacingGoal() const; + virtual const double GetAssignedSpacingGoal() const override; - virtual const Units::Speed GetImSpeedCommandIas() const; + virtual const Units::Speed GetImSpeedCommandIas() const override; - virtual const double GetSpacingInterval() const; + virtual const double GetSpacingInterval() const override; - virtual const double GetPsi() const; + virtual const double GetPsi() const override; - virtual const double GetMsi() const; + virtual const double GetMsi() const override; - virtual const double GetSpacingError() const; + virtual const double GetSpacingError() const override; - virtual int GetSpeedChangeCount() const; + virtual int GetSpeedChangeCount() const override; - virtual void DumpParameters(const std::string ¶meters_to_print); + virtual void DumpParameters(const std::string ¶meters_to_print) override; const interval_management::open_source::AircraftState GetTargetStateProjectedAsgAdjusted() const override; - bool load(DecodedStream *input); + bool load(DecodedStream *input) override; + + void Initialize(const OwnshipPredictionParameters &ownship_prediction_parameters, + const AircraftIntent &ownship_aircraft_intent, + aaesim::open_source::WeatherPrediction &weather_prediction, + std::shared_ptr &position_converter) override; protected: virtual void CalculateIas(const Units::Length current_ownship_altitude, - const aaesim::open_source::DynamicsState &three_dof_dynamics_state); + const aaesim::open_source::DynamicsState &three_dof_dynamics_state) override; virtual void CalculateMach(const Units::Time reference_ttg, const Units::Length current_ownship_altitude, const Units::Mass current_mass); @@ -76,15 +81,16 @@ class IMTimeBasedAchieve : public IMKinematicAchieve { const interval_management::open_source::AircraftState ¤t_target_state, const aaesim::open_source::DynamicsState &dynamics_state, const Units::Speed unmodified_ias, const Units::Speed tas_command, const Units::Speed reference_velocity, const Units::Length reference_distance, - const aaesim::open_source::Guidance &guidance); + const aaesim::open_source::Guidance &guidance) override; void Copy(const IMTimeBasedAchieve &obj); - virtual void SetAssignedSpacingGoal(const IMClearance &clearance); + virtual void SetAssignedSpacingGoal(const IMClearance &clearance) override; std::shared_ptr m_im_kinematic_time_based_maintain; Units::SecondsTime m_assigned_spacing_goal; + Units::Time m_predicted_spacing_interval; private: aaesim::open_source::Guidance HandleAchieveStage( @@ -130,7 +136,6 @@ class IMTimeBasedAchieve : public IMKinematicAchieve { Units::Time m_time_since_traffic_alignment; Units::Time m_time_at_traffic_alignment; - Units::Time m_predicted_spacing_interval; Units::Time m_measured_spacing_interval; interval_management::open_source::AircraftState m_target_state_projected_asg_adjusted; @@ -202,7 +207,7 @@ inline void IMTimeBasedAchieve::DoAlgorithmLogging( if (m_logger.getLogLevel() == log4cplus::TRACE_LOG_LEVEL) { using json = nlohmann::json; json j; - auto state_to_json = [&j](std::string prefix, const AircraftState &state) { + auto state_to_json = [&j](const std::string &prefix, const AircraftState &state) { j[prefix + ".id"] = state.GetId(); j[prefix + ".timestamp_sec"] = Units::SecondsTime(state.GetTimeStamp()).value(); j[prefix + ".groundspeed_mps"] = Units::MetersPerSecondSpeed(state.GetGroundSpeed()).value(); @@ -237,4 +242,4 @@ inline void IMTimeBasedAchieve::DoAlgorithmLogging( } } } // namespace open_source -} // namespace interval_management \ No newline at end of file +} // namespace interval_management diff --git a/include/imalgs/IMTimeBasedAchieveMutableASG.h b/include/imalgs/IMTimeBasedAchieveMutableASG.h index 3be23d1..b051749 100644 --- a/include/imalgs/IMTimeBasedAchieveMutableASG.h +++ b/include/imalgs/IMTimeBasedAchieveMutableASG.h @@ -31,7 +31,7 @@ namespace interval_management { namespace open_source { -class IMTimeBasedAchieveMutableASG : public IMTimeBasedAchieve { +class IMTimeBasedAchieveMutableASG final : public IMTimeBasedAchieve { public: IMTimeBasedAchieveMutableASG(); @@ -41,22 +41,27 @@ class IMTimeBasedAchieveMutableASG : public IMTimeBasedAchieve { IMTimeBasedAchieveMutableASG &operator=(const IMTimeBasedAchieveMutableASG &obj); - virtual void IterationReset(); + virtual void IterationReset() override; virtual aaesim::open_source::Guidance Update( const aaesim::open_source::Guidance &previous_im_guidance, const aaesim::open_source::DynamicsState &three_dof_dynamics_state, const interval_management::open_source::AircraftState ¤t_ownship_state, const interval_management::open_source::AircraftState ¤t_target_state, - const std::vector &target_adsb_history); + const std::vector &target_adsb_history) override; - virtual void DumpParameters(const std::string ¶meters_to_print); + virtual void DumpParameters(const std::string ¶meters_to_print) override; virtual void Initialize(const OwnshipPredictionParameters &ownship_prediction_parameters, const AircraftIntent &ownship_aircraft_intent, aaesim::open_source::WeatherPrediction &weather_prediction) override; - bool load(DecodedStream *input); + bool load(DecodedStream *input) override; + + void Initialize(const OwnshipPredictionParameters &ownship_prediction_parameters, + const AircraftIntent &ownship_aircraft_intent, + aaesim::open_source::WeatherPrediction &weather_prediction, + std::shared_ptr &position_converter) override; protected: void Copy(const IMTimeBasedAchieveMutableASG &obj); diff --git a/include/imalgs/IMUtils.h b/include/imalgs/IMUtils.h index 17e8ac6..c4eb627 100644 --- a/include/imalgs/IMUtils.h +++ b/include/imalgs/IMUtils.h @@ -19,24 +19,27 @@ #pragma once +#include +#include + #include #include -#include -#include -#include "public/Logging.h" -#include "public/AircraftState.h" + #include "imalgs/AircraftState.h" -#include "public/HorizontalPath.h" #include "public/AircraftIntent.h" +#include "public/AircraftState.h" #include "public/AlongPathDistanceCalculator.h" +#include "public/HorizontalPath.h" +#include "public/Logging.h" #include "utility/BoundedValue.h" -class IMUtils { +class IMUtils final { public: static const int UNINITIALIZED_AIRCRAFT_ID; static const Units::Length BEYOND_END_OF_ROUTE_TOL; static const bool QUANTIZE_FLAG_DEFAULT; static const bool LIMIT_FLAG_DEFAULT; + static const bool WIND_BLENDING_FLAG_DEFAULT; static const Units::NauticalMilesLength DIST_QUANTIZE_1_DEFAULT; static const Units::NauticalMilesLength DIST_QUANTIZE_2_DEFAULT; static const Units::KnotsSpeed SPEED_QUANTIZE_1_DEFAULT_1_KNOT; @@ -50,8 +53,6 @@ class IMUtils { KINETICACHIEVE, KINETICTARGETACHIEVE, TIMEBASEDACHIEVEMUTABLEASG, - RTA, - RTA_TOAC_NOT_IMALGORITHM, TESTSPEEDCONTROL, NONE, }; @@ -63,67 +64,69 @@ class IMUtils { static bool GetCrossingTime( const Units::Length current_dtg, const std::vector &aircraft_state_history, - const std::vector &horizontal_path, Units::Time &crossing_time); + const std::vector &horizontal_path, Units::Time &crossing_time); static bool GetCrossingTime( const Units::Length current_dtg, const std::vector &aircraft_state_history, - const std::vector &horizontal_path, Units::Time &crossing_time, Units::Length &projected_x, - Units::Length &projected_y); + const std::vector &horizontal_path, Units::Time &crossing_time, + Units::Length &projected_x, Units::Length &projected_y); static bool GetCrossingTime( const Units::Length current_dtg, const std::vector &aircraft_state_history, - AlongPathDistanceCalculator &distance_calculator, Units::Time &crossing_time, Units::Length &projected_x, - Units::Length &projected_y); + aaesim::open_source::AlongPathDistanceCalculator &distance_calculator, Units::Time &crossing_time, + Units::Length &projected_x, Units::Length &projected_y); static void CalculateMergePoint(const Units::Length x1, const Units::Length y1, const Units::Length x2, const Units::Length y2, const Units::Length x3, const Units::Length y3, Units::Length &x_merge, Units::Length &y_merge, const Units::Angle theta_merge); - static void CalculateTimeBasedExtrapolate(const Units::Length &ownship_dtg, - const interval_management::open_source::AircraftState &oldest_target_state, - const std::vector &ownship_horizontal_traj, - Units::Time &extrapolated_target_time, Units::Length &projected_x, - Units::Length &projected_y, Units::Length &projected_distance_to_go); + static void CalculateTimeBasedExtrapolate( + const Units::Length &ownship_dtg, const interval_management::open_source::AircraftState &oldest_target_state, + const std::vector &ownship_horizontal_traj, + Units::Time &extrapolated_target_time, Units::Length &projected_x, Units::Length &projected_y, + Units::Length &projected_distance_to_go); static void CalculateTimeBasedExtrapolate(const Units::Length &ownship_dtg, const interval_management::open_source::AircraftState &oldest_target_state, - AlongPathDistanceCalculator &distance_calculator, + aaesim::open_source::AlongPathDistanceCalculator &distance_calculator, Units::Time &extrapolated_target_time, Units::Length &projected_x, Units::Length &projected_y, Units::Length &projected_distance_to_go); - static bool CalculateTargetStateAtTime(const interval_management::open_source::AircraftState &target_state, - const std::vector &ownship_horizontal_traj, - const Units::Time extrapolation_time, const Units::Angle ownship_true_heading, - interval_management::open_source::AircraftState &extrapolation_state); + static bool CalculateTargetStateAtTime( + const interval_management::open_source::AircraftState &target_state, + const std::vector &ownship_horizontal_traj, + const Units::Time extrapolation_time, const Units::Angle ownship_true_heading, + interval_management::open_source::AircraftState &extrapolation_state); static void GetTimeBasedExtrapolateState( const interval_management::open_source::AircraftState ¤t_ownship_state, const interval_management::open_source::AircraftState &oldest_target_state, - const std::vector &ownship_horizontal_traj, - const std::vector &target_horizontal_traj, Units::Time &measured_spacing_interval, + const std::vector &ownship_horizontal_traj, + const std::vector &target_horizontal_traj, + Units::Time &measured_spacing_interval, interval_management::open_source::AircraftState &aircraft_state_to_return); static void GetPositionFromPathLength(const Units::Length distance_to_go_in, - const std::vector &horizontal_path_in, + const std::vector &horizontal_path_in, const Units::Angle ownship_true_heading, Units::Length &x_out, Units::Length &y_out, Units::UnsignedAngle &course_out, int &horizontal_path_index); static bool GetPathLengthFromPosition(const Units::Length x, const Units::Length y, - const std::vector &horizontal_path, + const std::vector &horizontal_path, Units::Length &distance_to_go, Units::Angle &track); // method to project target aircraft position onto ownship horizontal trajectory static bool ProjectTargetPosition(const Units::Length x_target, const Units::Length y_target, - const std::vector &ownship_horizontal_path, + const std::vector &ownship_horizontal_path, Units::Length &x_projected, Units::Length &y_projected, Units::Length &dtg); // method to get target aircraft position on ownship horizontal trajectory based upon distance to go - static bool ProjectTargetPositionFromDistance(const Units::Length dtg, - const std::vector &ownship_horizontal_path, - Units::Length &x_projected, Units::Length &y_projected); + static bool ProjectTargetPositionFromDistance( + const Units::Length dtg, const std::vector &ownship_horizontal_path, + Units::Length &x_projected, Units::Length &y_projected); /** * @deprecated AAES-994 make this method unused. It will be removed. @@ -133,11 +136,11 @@ class IMUtils { * @param ownship_true_heading * @param extrapstate */ - static void CalculateTargetStateAtDistance(const interval_management::open_source::AircraftState &target_state, - const std::vector &ownship_horizontal_traj, - const Units::Length extrapolation_distance, - const Units::Angle ownship_true_heading, - interval_management::open_source::AircraftState &extrapstate); + static void CalculateTargetStateAtDistance( + const interval_management::open_source::AircraftState &target_state, + const std::vector &ownship_horizontal_traj, + const Units::Length extrapolation_distance, const Units::Angle ownship_true_heading, + interval_management::open_source::AircraftState &extrapstate); /** * @deprecated AAES-994 has made this method unnecessary. It will be removed. @@ -149,8 +152,8 @@ class IMUtils { */ static interval_management::open_source::AircraftState GetTargetStateOnOwnshipPathForDtg( const std::vector &target_adsb_history, - const std::vector &ownship_horizontal_path, const Units::Length target_distance, - const Units::Angle ownship_true_heading); + const std::vector &ownship_horizontal_path, + const Units::Length target_distance, const Units::Angle ownship_true_heading); static Units::Time InterpolateTimeAtDtg(const Units::Length target_dtg, const Units::Time time1, const Units::Time time2, const Units::Length dtg1, const Units::Length dtg2); @@ -170,16 +173,16 @@ class IMUtils { */ static interval_management::open_source::AircraftState GetProjectedTargetState( const std::vector &target_state_history, - const std::vector &ownship_horizontal_traj, const Units::Time target_time, + const std::vector &ownship_horizontal_traj, const Units::Time target_time, const Units::Angle ownship_true_heading, bool &target_state_is_valid); static interval_management::open_source::AircraftState GetProjectedTargetState( - AlongPathDistanceCalculator &distance_calculator, + aaesim::open_source::AlongPathDistanceCalculator &distance_calculator, const std::vector &target_state_history, - const std::vector &ownship_horizontal_traj, const Units::Time target_time, + const std::vector &ownship_horizontal_traj, const Units::Time target_time, const Units::Angle ownship_true_heading, bool &target_state_is_valid); - static Units::SignedAngle CalculateTrackAngle(const std::list angle_history); + static Units::SignedAngle CalculateTrackAngle(const std::list &angle_history); /** * This map is a simple container that provides retrieval of a string that represents an enum values. diff --git a/include/imalgs/InternalObserver.h b/include/imalgs/InternalObserver.h index 982ef3d..a1b4d82 100644 --- a/include/imalgs/InternalObserver.h +++ b/include/imalgs/InternalObserver.h @@ -35,17 +35,15 @@ namespace interval_management { namespace open_source { -class InternalObserver { +class InternalObserver final { public: static void FatalError(const char *str) { LOG4CPLUS_FATAL(logger, str); throw std::logic_error(str); } static InternalObserver *getInstance(); - static void clearInstance(); void process(); - void collect_ptis_b_report(Sensor::ADSB::ADSBSVReport adsb_sv_report); void process_NM_aircraft(); void outputMaintainMetrics(); void updateFinalGS(int id, double gs); @@ -64,63 +62,55 @@ class InternalObserver { MaintainMetric &GetMaintainMetric(int id); MergePointMetric &GetMergePointMetric(int id); ClosestPointMetric &GetClosestPointMetric(int id); - void set_scenario_name(std::string in); + void set_scenario_name(const std::string &in); void initializeIteration(); void setNMOutput(bool NMflag); - bool outputNM(void); + bool outputNM(); void SetRecordMaintainMetrics(bool new_value); const bool GetRecordMaintainMetrics() const; - int GetScenarioIter() const; + void SetScenarioIter(int scenario_iter); - CrossTrackObserver &GetCrossEntry(); + + ~InternalObserver() = default; private: - static InternalObserver *mInstance; + static std::unique_ptr m_instance; static log4cplus::Logger logger; - class AircraftIterationStats { - public: - MergePointMetric m_merge_point_metric; - MaintainMetric m_maintain_metric; - ClosestPointMetric m_closest_point_metric; - double finalGS; - AircraftIterationStats(); + struct AircraftIterationStats { + MergePointMetric m_merge_point_metric{}; + MaintainMetric m_maintain_metric{}; + ClosestPointMetric m_closest_point_metric{}; + double finalGS{-1.0}; }; - class AircraftScenarioStats { - public: - NMObserver m_nm_observer; - std::vector m_achieve_list; + struct AircraftScenarioStats { + NMObserver m_nm_observer{}; + std::vector m_achieve_list{}; }; InternalObserver(); - ~InternalObserver() = default; - void process_ptis_b_reports(); + void dumpPredictedWind(); void process_NM_stats(); void processMaintainMetrics(); void processClosestPointMetric(); void dumpAchieveList(); - bool outputNMFiles; - bool m_save_maintain_metrics; - 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 + bool outputNMFiles{true}; + bool m_save_maintain_metrics{true}; + std::string scenario_name{}; + int m_scenario_iter{0}; + CrossTrackObserver m_cross_entry{}; + std::vector predWinds{}; - // output data vectors - std::vector ptis_b_report_list; + std::map m_aircraft_iteration_stats{}; + std::map m_aircraft_scenario_stats{}; - // string vectors for file output - std::vector maintainOutput; - std::vector finalGSOutput; - std::vector mergePointOutput; - std::vector closestPointOutput; + std::vector maintainOutput{}; + std::vector finalGSOutput{}; + std::vector mergePointOutput{}; + std::vector closestPointOutput{}; }; } // namespace open_source } // namespace interval_management diff --git a/include/imalgs/InternalObserverScenarioWriter.h b/include/imalgs/InternalObserverScenarioWriter.h index 888c69e..83ea4c2 100644 --- a/include/imalgs/InternalObserverScenarioWriter.h +++ b/include/imalgs/InternalObserverScenarioWriter.h @@ -22,7 +22,7 @@ #include "public/ScenarioEventNotifier.h" namespace interval_management::open_source { -class InternalObserverScenarioWriter : public aaesim::open_source::ScenarioEventNotifier { +class InternalObserverScenarioWriter final : public aaesim::open_source::ScenarioEventNotifier { public: InternalObserverScenarioWriter() = default; ~InternalObserverScenarioWriter() = default; @@ -30,5 +30,7 @@ class InternalObserverScenarioWriter : public aaesim::open_source::ScenarioEvent void IterationEnd(const int &iteration_number) override; void ScenarioBegin(const std::string &scenario_name) override; void ScenarioEnd(const std::string &scenario_name) override; + void ErrorOccurred(const int &iteration_number, + const std::exception &exception_object) override { /*nothing to do*/ }; }; } // namespace interval_management::open_source diff --git a/include/imalgs/KinematicDescent4DPredictor.h b/include/imalgs/KinematicDescent4DPredictor.h deleted file mode 100644 index ea7f16b..0000000 --- a/include/imalgs/KinematicDescent4DPredictor.h +++ /dev/null @@ -1,181 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy 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 "aaesim/VerticalPredictor.h" - -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(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(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(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(VerticalPath vertical_path, - double altitude_at_end, - 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(VerticalPath vertical_path, - double x_end, - std::vector &horizontal_path, - const WeatherPrediction &weather_prediction, - const Units::Length &aircraft_distance_to_go); - - VerticalPath ConstantDecelerationVerticalPath(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(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(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(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); - - 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; - - static const Units::Length m_vertical_tolerance_distance; - - bool m_prediction_too_low; - bool m_prediction_too_high; - - static log4cplus::Logger m_logger; -}; - -inline KinematicDescent4DPredictor::KinematicDescentType KinematicDescent4DPredictor::GetDescentType() const { - return m_kinematic_descent_type; -} - -inline const Units::Length KinematicDescent4DPredictor::GetAltitudeAtEndOfRoute() const { - return m_altitude_at_end_of_route; -} - -inline void 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 KinematicDescent4DPredictor::GetDecelerationRateFPA() const { - return m_deceleration_fpa_mps; -} \ No newline at end of file diff --git a/include/imalgs/KinematicTrajectoryPredictor.h b/include/imalgs/KinematicTrajectoryPredictor.h deleted file mode 100644 index a7118b6..0000000 --- a/include/imalgs/KinematicTrajectoryPredictor.h +++ /dev/null @@ -1,120 +0,0 @@ -// **************************************************************************** -// NOTICE -// -// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 -// and is subject to Federal Aviation Administration Acquisition Management System -// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). -// -// The contents of this document reflect the views of the author and The MITRE -// Corporation and do not necessarily reflect the views of the Federal Aviation -// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA -// nor the DOT makes any warranty or guarantee, expressed or implied, concerning -// the content or accuracy 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 "utility/Logging.h" -#include "aaesim/EuclideanTrajectoryPredictor.h" -#include "imalgs/KinematicDescent4DPredictor.h" -#include "public/PrecalcWaypoint.h" -#include "public/HorizontalPath.h" -#include "public/AircraftIntent.h" -#include "public/Guidance.h" -#include "imalgs/AircraftState.h" - - -class KinematicTrajectoryPredictor : public EuclideanTrajectoryPredictor -{ -public: - - KinematicTrajectoryPredictor(); - - KinematicTrajectoryPredictor( - std::shared_ptr bada_calculator, - 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); - - bool operator==(const KinematicTrajectoryPredictor &obj) const; - - bool operator!=(const KinematicTrajectoryPredictor &boj) const; - - 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; - - const double GetVerticalPathVelocityByIndex(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; -}; - - -inline const std::vector &KinematicTrajectoryPredictor::GetVerticalPathDistances() const { - return m_vertical_predictor->GetVerticalPath().along_path_distance_m; -} - -inline const double KinematicTrajectoryPredictor::GetVerticalPathDistanceByIndex(int index) const { - return m_vertical_predictor->GetVerticalPath().along_path_distance_m[index]; -} - -inline const std::vector &KinematicTrajectoryPredictor::GetVerticalPathTimes() const { - return m_vertical_predictor->GetVerticalPath().time_to_go_sec; -} - -inline const double KinematicTrajectoryPredictor::GetVerticalPathTimeByIndex(int index) const { - return m_vertical_predictor->GetVerticalPath().time_to_go_sec[index]; -} - -inline const std::vector &KinematicTrajectoryPredictor::GetVerticalPathGroundspeeds() const { - return m_vertical_predictor->GetVerticalPath().gs_mps; -} - -inline const std::vector &KinematicTrajectoryPredictor::GetVerticalPathVelocities() const { - return m_vertical_predictor->GetVerticalPath().cas_mps; -} - -inline const double KinematicTrajectoryPredictor::GetVerticalPathVelocityByIndex(int index) const { - return m_vertical_predictor->GetVerticalPath().cas_mps[index]; -} - -inline const std::vector &KinematicTrajectoryPredictor::GetVerticalPathAltitudes() const { - return m_vertical_predictor->GetVerticalPath().altitude_m; -} - -inline const double KinematicTrajectoryPredictor::GetVerticalPathAltitudeByIndex(const int index) const { - return m_vertical_predictor->GetVerticalPath().altitude_m[index]; -} - diff --git a/include/imalgs/MOPSPredictedWindEvaluatorVersion1.h b/include/imalgs/MOPSPredictedWindEvaluatorVersion1.h index 27b9eb5..850c3ef 100644 --- a/include/imalgs/MOPSPredictedWindEvaluatorVersion1.h +++ b/include/imalgs/MOPSPredictedWindEvaluatorVersion1.h @@ -23,7 +23,7 @@ namespace interval_management { namespace open_source { -class MOPSPredictedWindEvaluatorVersion1 : public aaesim::open_source::PredictedWindEvaluator { +class MOPSPredictedWindEvaluatorVersion1 final : public aaesim::open_source::PredictedWindEvaluator { public: const static std::shared_ptr &GetInstance(); @@ -44,7 +44,7 @@ class MOPSPredictedWindEvaluatorVersion1 : public aaesim::open_source::Predicted virtual bool ArePredictedWindsAccurate(const aaesim::open_source::AircraftState &state, const aaesim::open_source::WeatherPrediction &weatherPrediction, const Units::Speed reference_cas, const Units::Length reference_altitude, - const Atmosphere *sensed_atmosphere) const; + const std::shared_ptr &sensed_atmosphere) const; private: const static Units::Angle toleranceAngle; @@ -55,4 +55,4 @@ class MOPSPredictedWindEvaluatorVersion1 : public aaesim::open_source::Predicted MOPSPredictedWindEvaluatorVersion1(); }; } // namespace open_source -} // namespace interval_management \ No newline at end of file +} // namespace interval_management diff --git a/include/imalgs/MOPSPredictedWindEvaluatorVersion2.h b/include/imalgs/MOPSPredictedWindEvaluatorVersion2.h index c29e8c9..73cdd1f 100644 --- a/include/imalgs/MOPSPredictedWindEvaluatorVersion2.h +++ b/include/imalgs/MOPSPredictedWindEvaluatorVersion2.h @@ -23,7 +23,7 @@ namespace interval_management { namespace open_source { -class MOPSPredictedWindEvaluatorVersion2 : public aaesim::open_source::PredictedWindEvaluator { +class MOPSPredictedWindEvaluatorVersion2 final : public aaesim::open_source::PredictedWindEvaluator { public: static Units::KnotsSpeed MAX_PERMITTED_GROUNDSPEED_ERROR; @@ -42,7 +42,7 @@ class MOPSPredictedWindEvaluatorVersion2 : public aaesim::open_source::Predicted virtual bool ArePredictedWindsAccurate(const aaesim::open_source::AircraftState &state, const aaesim::open_source::WeatherPrediction &weatherPrediction, const Units::Speed reference_cas, const Units::Length reference_altitude, - const Atmosphere *sensed_atmosphere) const; + const std::shared_ptr &sensed_atmosphere) const; private: static log4cplus::Logger m_logger; @@ -61,4 +61,4 @@ class MOPSPredictedWindEvaluatorVersion2 : public aaesim::open_source::Predicted MOPSPredictedWindEvaluatorVersion2(); }; } // namespace open_source -} // namespace interval_management \ No newline at end of file +} // namespace interval_management diff --git a/include/imalgs/MaintainMetric.h b/include/imalgs/MaintainMetric.h index 2432c14..b64b4da 100644 --- a/include/imalgs/MaintainMetric.h +++ b/include/imalgs/MaintainMetric.h @@ -27,11 +27,11 @@ namespace interval_management { namespace open_source { -class MaintainMetric { +class MaintainMetric final { public: - MaintainMetric(void); + MaintainMetric(); - ~MaintainMetric(void) = default; + ~MaintainMetric() = default; // Adds data to be added for each pass through an IM::update method. void AddSpacingErrorSec(double err); @@ -68,19 +68,19 @@ class MaintainMetric { private: // Running sum of time spacing errors between IM and target ac. - Statistics spacingError; + Statistics spacingError{}; // Time went by achieve by point. - double achieveByTime; + double achieveByTime{-1.0}; // Time spent in maintain stage, (current time - achieve by time) - double totalMaintainTime; + double totalMaintainTime{0.0}; // Number of cycles with a spacing error > 10 secs. - int numCyclesOutsideThreshold; + int numCyclesOutsideThreshold{0}; /** Output should be enabled for IM aircraft */ - bool m_output_enabled; + bool m_output_enabled{false}; }; } // namespace open_source } // namespace interval_management \ No newline at end of file diff --git a/include/imalgs/MergePointMetric.h b/include/imalgs/MergePointMetric.h index 0cff385..364a73e 100644 --- a/include/imalgs/MergePointMetric.h +++ b/include/imalgs/MergePointMetric.h @@ -19,18 +19,18 @@ #pragma once -#include "public/AircraftIntent.h" #include +#include "public/AircraftIntent.h" + namespace interval_management { namespace open_source { -class MergePointMetric { - +class MergePointMetric final { public: - MergePointMetric(void); + MergePointMetric(); - ~MergePointMetric(void); + ~MergePointMetric(); // Determines and stores the merge point. void determineMergePoint(const AircraftIntent &IMIntent, const AircraftIntent &targIntent); @@ -57,24 +57,24 @@ class MergePointMetric { // 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; + int m_im_ac_id{0}; + int m_target_ac_id{0}; - std::string mMergePointName; - Units::Length mMergePointX; - Units::Length mMergePointY; + std::string mMergePointName{}; + Units::Length mMergePointX{Units::ZERO_LENGTH}; + Units::Length mMergePointY{Units::ZERO_LENGTH}; - double mIMX; // ft - double mIMY; // ft + double m_im_x_ft{0}; + double m_im_y_ft{0}; - Units::Length mIMDist; + Units::Length mIMDist{Units::infinity()}; - double mTargX; // ft - double mTargY; // ft + double m_targ_x_ft{0}; + double m_targ_y_ft{0}; - Units::Length mMergeDist; + Units::Length mMergeDist{Units::infinity()}; - bool mReportMetrics; + bool mReportMetrics{false}; }; } // namespace open_source diff --git a/include/imalgs/NMObserver.h b/include/imalgs/NMObserver.h index b878cd7..b3a6f08 100644 --- a/include/imalgs/NMObserver.h +++ b/include/imalgs/NMObserver.h @@ -20,34 +20,35 @@ #pragma once #include + #include "imalgs/NMObserverEntry.h" #include "imalgs/Statistics.h" namespace interval_management { namespace open_source { -class NMObserver { +class NMObserver final { public: - NMObserver(void); + NMObserver() = default; - ~NMObserver(void); + ~NMObserver() = default; // 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 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; + 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; + int curr_NM{-2}; void initialize_stats(); }; diff --git a/include/imalgs/NMObserverEntry.h b/include/imalgs/NMObserverEntry.h index f42ff6d..413f69b 100644 --- a/include/imalgs/NMObserverEntry.h +++ b/include/imalgs/NMObserverEntry.h @@ -22,22 +22,22 @@ namespace interval_management { namespace open_source { -class NMObserverEntry { +class NMObserverEntry final { public: - NMObserverEntry(void); + NMObserverEntry(); - ~NMObserverEntry(void); + ~NMObserverEntry(); - double predictedDistance; - double trueDistance; - double time; - double acIAS; - double acGS; - double targetGS; - double minIAS; - double maxIAS; - double minTAS; - double maxTAS; + double predictedDistance{0.0}; + double trueDistance{0.0}; + double time{0.0}; + double acIAS{0.0}; + double acGS{0.0}; + double targetGS{0.0}; + double minIAS{0.0}; + double maxIAS{0.0}; + double minTAS{0.0}; + double maxTAS{0.0}; }; } // namespace open_source diff --git a/include/imalgs/PredictionFileKinematic.h b/include/imalgs/PredictionFileKinematic.h index 0ca3258..0a1f427 100644 --- a/include/imalgs/PredictionFileKinematic.h +++ b/include/imalgs/PredictionFileKinematic.h @@ -34,7 +34,7 @@ class PredictionFileKinematic final : private PredictionFileBase, public OutputH private: const bool TrajectoryWasRegenerated(const std::vector &prediction_data_single_acid, - const VerticalPath vertical_path, const int iteration, + const VerticalPath &vertical_path, const int iteration, const PredictionData::DataSource source) const; std::map > algorithm_prediction_ownship; diff --git a/include/imalgs/Statistics.h b/include/imalgs/Statistics.h index 667ca91..8e3b513 100644 --- a/include/imalgs/Statistics.h +++ b/include/imalgs/Statistics.h @@ -26,7 +26,7 @@ using std::vector; namespace interval_management { namespace open_source { -class Statistics { +class Statistics final { public: Statistics(); @@ -49,10 +49,10 @@ class Statistics { double Get95thBounds(); private: - double m_sum_of_samples; - vector m_samples; - double m_max; - double m_min; + double m_sum_of_samples{0}; + vector m_samples{}; + double m_max{0}; + double m_min{0}; }; } // namespace open_source } // namespace interval_management \ No newline at end of file diff --git a/include/imalgs/TestVectorSpeedControl.h b/include/imalgs/TestVectorSpeedControl.h deleted file mode 100644 index b011c9f..0000000 --- a/include/imalgs/TestVectorSpeedControl.h +++ /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. -// -// 2022 The MITRE Corporation. All Rights Reserved. -// **************************************************************************** - -#pragma once - -#include -#include -#include "IMAlgorithm.h" -#include "KineticAlgorithm.h" - -namespace interval_management { -namespace open_source { -class TestVectorSpeedControl : public IMAlgorithm, public KineticAlgorithm, Loadable { - public: - TestVectorSpeedControl(); - virtual ~TestVectorSpeedControl(); - - virtual aaesim::open_source::Guidance Update( - const aaesim::open_source::Guidance &prevguidance, const aaesim::open_source::DynamicsState &dynamicsstate, - const interval_management::open_source::AircraftState &owntruthstate, - const interval_management::open_source::AircraftState &targettruthstate, - const std::vector &targethistory); - - bool load(DecodedStream *input) override; - - void ResetDefaults() override; - - void InitializeFmsPredictors(const Wgs84KineticDescentPredictor &ownship_kinetic_trajectory_predictor, - const Wgs84KineticDescentPredictor &target_kinetic_trajectory_predictor) override; - - void SetBlendWind(bool wind_blending_enabled) override; - - protected: - virtual void SetAssignedSpacingGoal(const IMClearance &clearance); - - virtual const double GetSpacingError() const; - - private: - static unsigned int DEFAULT_DECELERATION_START_TIME_SEC, DEFAULT_ACCELERATION_START_TIME_SEC; - static unsigned long DEFAULT_DECELERATION_DELTA_IAS, DEFAULT_ACCELERATION_DELTA_IAS; - - Wgs84AlongPathDistanceCalculator m_distance_calculator; - Units::KnotsSpeed m_acceleration_phase_target_ias; - Units::KnotsSpeed m_deceleration_phase_target_ias; - unsigned long m_acceleration_phase_hold_duration; - unsigned long m_acceleration_phase_count; - Units::KnotsSpeed m_acceleration_phase_delta_ias; - Units::KnotsSpeed m_deceleration_phase_delta_ias; - unsigned int m_deceleration_start_time_sec, m_acceleration_start_time_sec; - bool m_acceleration_phase_complete; - bool m_acceleration_target_achieved; - std::deque m_pilot_delayed_speeds; - const aaesim::Wgs84KineticDescentPredictor *m_target_kinetic_trajectory_predictor; - const aaesim::Wgs84KineticDescentPredictor *m_ownship_kinetic_trajectory_predictor; -}; - -inline void TestVectorSpeedControl::SetBlendWind(bool wind_blending_enabled) { /* required by the interface, but not - used */ -} -} // namespace open_source -} // namespace interval_management \ No newline at end of file diff --git a/include/imalgs/TrueDistances.h b/include/imalgs/TrueDistances.h index 5c2a7be..ce1b370 100644 --- a/include/imalgs/TrueDistances.h +++ b/include/imalgs/TrueDistances.h @@ -19,14 +19,14 @@ #pragma once -#include -#include #include +#include + +#include namespace interval_management { namespace open_source { -class TrueDistances { - +class TrueDistances final { public: static const Units::Time END_OF_ROUTE_CROSSING_TIME_TOLERANCE; diff --git a/unittest/CMakeLists.txt b/unittest/CMakeLists.txt new file mode 100644 index 0000000..eaf054c --- /dev/null +++ b/unittest/CMakeLists.txt @@ -0,0 +1,16 @@ +cmake_minimum_required(VERSION 3.26...4.1.2) + +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -fno-strict-aliasing -Wunused-result") +set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -Wall -Wno-sign-compare -O1 -g3 -Wunused-result") + +CPMAddPackage( + NAME googletest + GITHUB_REPOSITORY google/googletest + VERSION 1.15.2 + OPTIONS + "INSTALL_GTEST OFF" + "GTEST_FORCE_SHARED_CRT ON" + "BUILD_GMOCK ON" +) + +set(LIBRARY_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/lib) \ No newline at end of file diff --git a/unittest/src/IntervalManagement/clearance_tests.cpp b/unittest/src/IntervalManagement/clearance_tests.cpp index 12c16ce..24a9e37 100644 --- a/unittest/src/IntervalManagement/clearance_tests.cpp +++ b/unittest/src/IntervalManagement/clearance_tests.cpp @@ -138,30 +138,6 @@ TEST(IMClearance, basic) { } catch (logic_error &e) { // expected } - - // 4) Comparator Operators - auto builder3 = IMClearance::Builder(IMClearance::ClearanceType::CAPTURE, 4, AircraftIntent(), "YOKXO", "DERVL", - IMClearance::SpacingGoalType::TIME, 120); - IMClearance c2 = builder3.Build(); - auto builder4 = IMClearance::Builder(IMClearance::ClearanceType::ACHIEVE, 4, AircraftIntent(), "YOKXO", "DERVL", - IMClearance::SpacingGoalType::TIME, 120); - IMClearance c3 = builder4.Build(); - - // == - if (!(c1 == c2)) { - FAIL(); - } - - // != - if (!(c1 != c3)) { - FAIL(); - } - - // 5) Operator = (copy) - IMClearance c4 = c2; - if (!(c4 == c2)) { - FAIL(); - } } TEST(IMClearance, load_build) { diff --git a/unittest/src/IntervalManagement/imalgo_tests.cpp b/unittest/src/IntervalManagement/imalgo_tests.cpp index b685fa1..c2bacf3 100644 --- a/unittest/src/IntervalManagement/imalgo_tests.cpp +++ b/unittest/src/IntervalManagement/imalgo_tests.cpp @@ -25,6 +25,7 @@ using namespace std; using namespace aaesim::open_source; +using namespace aaesim::open_source::constants; using namespace interval_management::open_source; namespace aaesim { @@ -89,7 +90,8 @@ TEST(IMAlgorithm, API) { im_time_based_achieve->SetSlope(Units::KnotsInvertedSpeed(0)); im_time_based_achieve->SetErrorDistance(Units::MetersLength(0)); im_time_based_achieve->SetBlendWind(true); - im_time_based_achieve->SetPilotDelay(true, Units::SecondsTime(0), Units::SecondsTime(0)); + auto no_delay = aaesim::open_source::StatisticalPilotDelay::NoDelay(); + im_time_based_achieve->SetPilotDelay(no_delay); im_time_based_achieve->GetTargetTrpCrossingTime(); im_time_based_achieve->GetOwnshipTtgtoAbp(); @@ -775,8 +777,8 @@ TEST(IMUtils, GetTargetStateAtGivenDtgOnOwnshipsPath) { result = IMUtils::GetTargetStateOnOwnshipPathForDtg( aircraft_state_history, ownship_horizontal_traj, Units::MetersLength(aircraft_state_history[0].m_distance_to_go_meters - 0.1), Units::Angle()); - EXPECT_DOUBLE_EQ(Units::SecondsTime(aircraft_state_history[0].GetTimeStamp().value() + 0.5).value(), - result.GetTimeStamp().value()); + EXPECT_NEAR(Units::SecondsTime(aircraft_state_history[0].GetTimeStamp().value() + 0.5).value(), + result.GetTimeStamp().value(), 1e-8); EXPECT_LT(result.m_x, Units::FeetLength(xboundupper).value()); EXPECT_GT(result.m_x, Units::FeetLength(xboundlower).value()); EXPECT_DOUBLE_EQ(Units::FeetLength(expected_y_value).value(), Units::FeetLength(result.m_y).value()); @@ -818,73 +820,78 @@ TEST(AircraftState, Create) { auto actual_gamma = Units::DegreesAngle(2); auto actual_time = Units::MinutesTime(1); - interval_management::open_source::AircraftState *test_state = new interval_management::open_source::AircraftState(); - test_state->Create(actual_id, actual_time, actual_position, actual_xd, actual_yd, actual_zd, actual_gamma, - Units::zero(), Units::zero(), Units::zero(), Units::zero(), Units::zero(), Units::zero()); + interval_management::open_source::AircraftState test_state = interval_management::open_source::AircraftState(); + test_state.Create(actual_id, actual_time, actual_position, actual_xd, actual_yd, actual_zd, actual_gamma, + Units::zero(), Units::zero(), Units::zero(), Units::zero(), Units::zero(), Units::zero()); - EXPECT_EQ(actual_id, test_state->GetId()); - EXPECT_DOUBLE_EQ(Units::SecondsTime(actual_time).value(), test_state->GetTimeStamp().value()); - EXPECT_DOUBLE_EQ(Units::MetersLength(actual_x).value(), Units::MetersLength(test_state->GetPositionX()).value()); - EXPECT_DOUBLE_EQ(Units::MetersLength(actual_y).value(), Units::MetersLength(test_state->GetPositionY()).value()); - EXPECT_DOUBLE_EQ(Units::MetersLength(actual_z).value(), Units::MetersLength(test_state->GetPositionZ()).value()); + EXPECT_EQ(actual_id, test_state.GetId()); + EXPECT_DOUBLE_EQ(Units::SecondsTime(actual_time).value(), test_state.GetTimeStamp().value()); + EXPECT_DOUBLE_EQ(Units::MetersLength(actual_x).value(), Units::MetersLength(test_state.GetPositionX()).value()); + EXPECT_DOUBLE_EQ(Units::MetersLength(actual_y).value(), Units::MetersLength(test_state.GetPositionY()).value()); + EXPECT_DOUBLE_EQ(Units::MetersLength(actual_z).value(), Units::MetersLength(test_state.GetPositionZ()).value()); EXPECT_DOUBLE_EQ(Units::MetersPerSecondSpeed(actual_xd).value(), - Units::MetersPerSecondSpeed(test_state->GetSpeedXd()).value()); + Units::MetersPerSecondSpeed(test_state.GetSpeedXd()).value()); EXPECT_DOUBLE_EQ(Units::MetersPerSecondSpeed(actual_yd).value(), - Units::MetersPerSecondSpeed(test_state->GetSpeedYd()).value()); + Units::MetersPerSecondSpeed(test_state.GetSpeedYd()).value()); EXPECT_DOUBLE_EQ(Units::MetersPerSecondSpeed(actual_zd).value(), - Units::MetersPerSecondSpeed(test_state->GetSpeedZd()).value()); - EXPECT_DOUBLE_EQ(Units::RadiansAngle(actual_gamma).value(), Units::RadiansAngle(test_state->GetGamma()).value()); + Units::MetersPerSecondSpeed(test_state.GetSpeedZd()).value()); + EXPECT_DOUBLE_EQ(Units::RadiansAngle(actual_gamma).value(), Units::RadiansAngle(test_state.GetGamma()).value()); } TEST(IMUtils, ConvertAircraftStates) { - auto actual_id = 100; - auto actual_x = Units::MetersLength(10); - auto actual_y = Units::FeetLength(20); - auto actual_z = Units::NauticalMilesLength(1); - auto actual_position = EarthModel::LocalPositionEnu::Of(actual_x, actual_y, actual_z); - auto actual_xd = Units::MetersPerSecondSpeed(10); - auto actual_yd = Units::FeetPerSecondSpeed(20); - auto actual_zd = Units::KnotsSpeed(1); - auto actual_gamma = Units::DegreesAngle(2); - auto actual_time = Units::MinutesTime(1); - auto actual_psi = Units::SignedRadiansAngle(Units::arctan2(actual_xd.value(), actual_yd.value())); - auto actual_sensed_veast = Units::MetersPerSecondSpeed(10); - auto actual_sensed_vnorth = Units::MetersPerSecondSpeed(5); - auto actual_sensed_wind_parallel = Units::MetersPerSecondSpeed(3); - auto actual_sensed_wind_perpendicular = Units::MetersPerSecondSpeed(1); - auto actual_sensed_temperature = Units::CelsiusTemperature(22); - interval_management::open_source::AircraftState *test_state = new interval_management::open_source::AircraftState(); - test_state->Create(actual_id, actual_time, actual_position, actual_xd, actual_yd, actual_zd, actual_gamma, - actual_sensed_veast, actual_sensed_vnorth, actual_sensed_wind_parallel, - actual_sensed_wind_perpendicular, actual_sensed_temperature, actual_psi); + const auto actual_id = 100; + const auto actual_x = Units::MetersLength(10); + const auto actual_y = Units::FeetLength(20); + const auto actual_z = Units::NauticalMilesLength(1); + const auto actual_position = EarthModel::LocalPositionEnu::Of(actual_x, actual_y, actual_z); + const auto actual_xd = Units::MetersPerSecondSpeed(10); + const auto actual_yd = Units::FeetPerSecondSpeed(20); + const auto actual_zd = Units::KnotsSpeed(1); + const auto actual_gamma = Units::DegreesAngle(2); + const auto actual_time = Units::MinutesTime(1); + const auto actual_psi = Units::SignedRadiansAngle(Units::arctan2(actual_xd.value(), actual_yd.value())); + const auto actual_sensed_veast = Units::MetersPerSecondSpeed(10); + const auto actual_sensed_vnorth = Units::MetersPerSecondSpeed(5); + const auto actual_sensed_wind_parallel = Units::MetersPerSecondSpeed(3); + const auto actual_sensed_wind_perpendicular = Units::MetersPerSecondSpeed(1); + const auto actual_sensed_temperature = Units::CelsiusTemperature(22); + interval_management::open_source::AircraftState test_state = interval_management::open_source::AircraftState(); + test_state.Create(actual_id, actual_time, actual_position, actual_xd, actual_yd, actual_zd, actual_gamma, + actual_sensed_veast, actual_sensed_vnorth, actual_sensed_wind_parallel, + actual_sensed_wind_perpendicular, actual_sensed_temperature, actual_psi); // This is the tested method // convert to aaesim::open_source::AircraftState - const aaesim::open_source::AircraftState converted_state_1 = IMUtils::ConvertToAaesimAircraftState(*test_state); + const aaesim::open_source::AircraftState converted_state_1 = IMUtils::ConvertToAaesimAircraftState(test_state); // assert data transferred - EXPECT_EQ(actual_id, converted_state_1.m_id); - EXPECT_DOUBLE_EQ(Units::SecondsTime(actual_time).value(), converted_state_1.m_time); + EXPECT_EQ(actual_id, converted_state_1.GetUniqueId()); + EXPECT_DOUBLE_EQ(Units::SecondsTime(actual_time).value(), converted_state_1.GetTime().value()); EXPECT_DOUBLE_EQ(Units::MetersLength(actual_x).value(), - Units::MetersLength(converted_state_1.GetPositionX()).value()); + Units::MetersLength(converted_state_1.GetPositionEnuX()).value()); EXPECT_DOUBLE_EQ(Units::MetersLength(actual_y).value(), - Units::MetersLength(converted_state_1.GetPositionY()).value()); + Units::MetersLength(converted_state_1.GetPositionEnuY()).value()); EXPECT_DOUBLE_EQ(Units::MetersLength(actual_z).value(), - Units::MetersLength(converted_state_1.GetPositionZ()).value()); + Units::MetersLength(converted_state_1.GetAltitudeMsl()).value()); EXPECT_DOUBLE_EQ(Units::MetersPerSecondSpeed(actual_xd).value(), - Units::MetersPerSecondSpeed(converted_state_1.GetSpeedXd()).value()); + Units::MetersPerSecondSpeed(converted_state_1.GetSpeedEnuX()).value()); EXPECT_DOUBLE_EQ(Units::MetersPerSecondSpeed(actual_yd).value(), - Units::MetersPerSecondSpeed(converted_state_1.GetSpeedYd()).value()); + Units::MetersPerSecondSpeed(converted_state_1.GetSpeedEnuY()).value()); EXPECT_DOUBLE_EQ(Units::MetersPerSecondSpeed(actual_zd).value(), - Units::MetersPerSecondSpeed(converted_state_1.GetSpeedZd()).value()); - EXPECT_DOUBLE_EQ(Units::RadiansAngle(actual_gamma).value(), converted_state_1.m_gamma); - EXPECT_DOUBLE_EQ(Units::RadiansAngle(actual_psi).value(), Units::RadiansAngle(converted_state_1.m_psi).value()); + Units::MetersPerSecondSpeed(converted_state_1.GetVerticalSpeed()).value()); + EXPECT_DOUBLE_EQ(Units::RadiansAngle(actual_gamma).value(), + Units::RadiansAngle(converted_state_1.GetFlightPathAngle()).value()); + EXPECT_DOUBLE_EQ(Units::RadiansAngle(actual_psi).value(), Units::RadiansAngle(converted_state_1.GetPsi()).value()); EXPECT_DOUBLE_EQ(Units::CelsiusTemperature(actual_sensed_temperature).value(), - Units::CelsiusTemperature(converted_state_1.m_sensed_temperature).value()); - EXPECT_DOUBLE_EQ(Units::MetersPerSecondSpeed(actual_sensed_veast).value(), converted_state_1.m_Vwx); - EXPECT_DOUBLE_EQ(Units::MetersPerSecondSpeed(actual_sensed_vnorth).value(), converted_state_1.m_Vwy); - EXPECT_DOUBLE_EQ(Units::MetersPerSecondSpeed(actual_sensed_wind_parallel).value(), converted_state_1.m_Vw_para); - EXPECT_DOUBLE_EQ(Units::MetersPerSecondSpeed(actual_sensed_wind_perpendicular).value(), converted_state_1.m_Vw_perp); + Units::CelsiusTemperature(converted_state_1.GetSensedTemperature()).value()); + EXPECT_DOUBLE_EQ(Units::MetersPerSecondSpeed(actual_sensed_veast).value(), + Units::MetersPerSecondSpeed(converted_state_1.GetSensedWindEast()).value()); + EXPECT_DOUBLE_EQ(Units::MetersPerSecondSpeed(actual_sensed_vnorth).value(), + Units::MetersPerSecondSpeed(converted_state_1.GetSensedWindNorth()).value()); + EXPECT_DOUBLE_EQ(Units::MetersPerSecondSpeed(actual_sensed_wind_parallel).value(), + Units::MetersPerSecondSpeed(converted_state_1.GetSensedWindParallel()).value()); + EXPECT_DOUBLE_EQ(Units::MetersPerSecondSpeed(actual_sensed_wind_perpendicular).value(), + Units::MetersPerSecondSpeed(converted_state_1.GetSensedWindPerpendicular()).value()); // This is the tested method // convert to interval_management::open_source::AircraftState diff --git a/unittest/src/IntervalManagement/interval_management.cmake b/unittest/src/IntervalManagement/interval_management.cmake index 867651e..886c37f 100644 --- a/unittest/src/IntervalManagement/interval_management.cmake +++ b/unittest/src/IntervalManagement/interval_management.cmake @@ -1,17 +1,14 @@ -cmake_minimum_required(VERSION 3.14) - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fPIC") set(IMALGORITHM_TEST_SOURCE ${UNITTEST_DIR}/src/IntervalManagement/imalgo_tests.cpp ${UNITTEST_DIR}/src/IntervalManagement/predicted_wind_evaluator_tests.cpp - ${UNITTEST_DIR}/src/IntervalManagement/clearance_tests.cpp + # ${UNITTEST_DIR}/src/IntervalManagement/clearance_tests.cpp ) add_executable(imalgs_test ${IMALGORITHM_TEST_SOURCE} - ${PUBLIC_TEST_SUPPORT_SOURCE} ${UNITTEST_DIR}/src/main.cpp ) target_link_libraries(imalgs_test @@ -19,7 +16,8 @@ target_link_libraries(imalgs_test imalgs ) target_include_directories(imalgs_test PUBLIC - ${aaesim_INCLUDE_DIRS} + ${fmacm_INCLUDE_DIRS} + ${fmacm_SOURCE_DIR}/unittest/src ${UNITTEST_DIR}/src ${geolib_idealab_INCLUDE_DIRS}) set_target_properties(imalgs_test PROPERTIES diff --git a/unittest/src/IntervalManagement/predicted_wind_evaluator_tests.cpp b/unittest/src/IntervalManagement/predicted_wind_evaluator_tests.cpp index a760b95..2383685 100644 --- a/unittest/src/IntervalManagement/predicted_wind_evaluator_tests.cpp +++ b/unittest/src/IntervalManagement/predicted_wind_evaluator_tests.cpp @@ -17,9 +17,11 @@ // 2023 The MITRE Corporation. All Rights Reserved. // **************************************************************************** +#include "public/Atmosphere.h" #include "imalgs/MOPSPredictedWindEvaluatorVersion1.h" #include "imalgs/MOPSPredictedWindEvaluatorVersion2.h" -#include "public/StandardAtmosphere.h" +#include "public/NullAtmosphere.h" +#include "public/USStandardAtmosphere1976.h" #include "public/Wind.h" #include "gtest/gtest.h" @@ -34,16 +36,16 @@ class PredictedWindEvaluatorTest : public ::testing::Test { protected: PredictedWindEvaluatorTest() : aircraft_state(), - weather_prediction(Wind::CreateZeroWindPrediction()), reference_cas(Units::ZERO_SPEED), reference_altitude(Units::ZERO_LENGTH), - sensed_atmosphere(StandardAtmosphere::MakeInstanceFromTemperatureOffset(Units::CelsiusTemperature(0))) {} + sensed_atmosphere(new USStandardAtmosphere1976()), + weather_prediction(WeatherPrediction::CreateZeroWindPrediction(sensed_atmosphere)) {} - aaesim::open_source::AircraftState aircraft_state; - WeatherPrediction weather_prediction; + 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, MOPSPredictedWindEvaluatorVersion1) { @@ -52,12 +54,13 @@ TEST_F(PredictedWindEvaluatorTest, MOPSPredictedWindEvaluatorVersion1) { ASSERT_TRUE(mops_predicted_wind_evaluator_v1->ArePredictedWindsAccurate( aircraft_state, weather_prediction, Units::ZERO_SPEED, Units::ZERO_LENGTH, nullptr)); - aircraft_state.m_Vwx = 10.0; + aircraft_state = + AircraftState::Builder(0, 0).SensedWindComponents(Units::MetersPerSecondSpeed(10), Units::ZERO_SPEED)->Build(); ASSERT_FALSE(mops_predicted_wind_evaluator_v1->ArePredictedWindsAccurate( aircraft_state, weather_prediction, Units::ZERO_SPEED, Units::ZERO_LENGTH, nullptr)); - aircraft_state.Clear(); - aircraft_state.m_Vwy = 1.0; + aircraft_state = + AircraftState::Builder(0, 0).SensedWindComponents(Units::ZERO_SPEED, Units::MetersPerSecondSpeed(1))->Build(); ASSERT_FALSE(mops_predicted_wind_evaluator_v1->ArePredictedWindsAccurate( aircraft_state, weather_prediction, Units::ZERO_SPEED, Units::ZERO_LENGTH, nullptr)); } @@ -66,13 +69,14 @@ TEST_F(PredictedWindEvaluatorTest, MOPSPredictedWindEvaluatorVersion2) { const std::shared_ptr mops_predicted_wind_evaluator_v2 = MOPSPredictedWindEvaluatorVersion2::GetInstance(); ASSERT_TRUE(mops_predicted_wind_evaluator_v2->ArePredictedWindsAccurate( - aircraft_state, weather_prediction, reference_cas, reference_altitude, sensed_atmosphere.get())); + aircraft_state, weather_prediction, reference_cas, reference_altitude, sensed_atmosphere)); reference_altitude = Units::FeetLength(10000); reference_cas = Units::KnotsSpeed(100); ASSERT_FALSE(mops_predicted_wind_evaluator_v2->ArePredictedWindsAccurate( - aircraft_state, weather_prediction, reference_cas, reference_altitude, sensed_atmosphere.get())); + aircraft_state, weather_prediction, reference_cas, reference_altitude, sensed_atmosphere)); } + } // namespace imalgo } // namespace test -} // namespace aaesim \ No newline at end of file +} // namespace aaesim diff --git a/unittest/src/main.cpp b/unittest/src/main.cpp new file mode 100644 index 0000000..26b5213 --- /dev/null +++ b/unittest/src/main.cpp @@ -0,0 +1,32 @@ +// **************************************************************************** +// NOTICE +// +// This work was produced for the U.S. Government under Contract 693KA8-22-C-00001 +// and is subject to Federal Aviation Administration Acquisition Management System +// Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV (Oct. 1996). +// +// The contents of this document reflect the views of the author and The MITRE +// Corporation and do not necessarily reflect the views of the Federal Aviation +// Administration (FAA) or the Department of Transportation (DOT). Neither the FAA +// nor the DOT makes any warranty or guarantee, expressed or implied, concerning +// the content or accuracy 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 "gtest/gtest.h" +#include "public/Logging.h" +#include + +GTEST_API_ int main(int argc, char **argv) { + log4cplus::Initializer initializer; + LoadLoggerProperties(); + log4cplus::Logger logger = log4cplus::Logger::getInstance(LOG4CPLUS_TEXT("main")); + LOG4CPLUS_INFO(logger, "Running main()"); + testing::InitGoogleTest(&argc, argv); + (void)!RUN_ALL_TESTS(); // we ignore the return status here. + return 0; +} diff --git a/unittest/unittest.cmake b/unittest/unittest.cmake new file mode 100644 index 0000000..5d5dce8 --- /dev/null +++ b/unittest/unittest.cmake @@ -0,0 +1,7 @@ +if(${BUILD_TESTING}) + # *************************** UNIT TESTS ******************************** # + # Link all the actual test code along with main.cpp to the executable, + # so as much of test infrastructure is built into + # the /unittest library as possible. + include(${UNITTEST_DIR}/src/IntervalManagement/interval_management.cmake) +endif()