Merge branch 'master' of ssh://mustache.mitre.org:7999/aaes/staging-f…

…or-external-repo

.cmake/aaesim.cmake

@@ -267,6 +267,7 @@ SET(IM_BUILD_SRC
         ${IM_DIR}/IMAircraft.h
         ${IM_DIR}/IMAlgorithmLoader.cpp
         ${IM_DIR}/IMAlgorithmLoader.h
+        ${IM_DIR}/IMAlgorithmFile.h
         ${IM_DIR}/IMClearanceLoader.cpp
         ${IM_DIR}/IMClearanceLoader.h
         ${IM_DIR}/IMScenario.cpp

CMakeLists.txt

@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.0)
-project(aaesim VERSION 3.4.0.0) # the nano value is a boolean. 1 == SNAPSHOT, 0 == release
+project(aaesim VERSION 3.4.1.0) # the nano value is a boolean. 1 == SNAPSHOT, 0 == release
 
 set (CMAKE_CXX_STANDARD 11)
 

Public/AircraftCalculations.cpp

@@ -490,8 +490,8 @@ bool AircraftCalculations::CalculateDistanceAlongPathFromPosition(const Units::L
        * must allow it to throw. Some callers will catch and handle the situation because
        * it is sometimes normal. See AAES-382, AAES-633
        */
-      char msg[150];
-      sprintf(msg, "Trajectory point with acceptable cross track error not found %lf nmi", cte.value());
+      char msg[500];
+      sprintf(msg, "Trajectory point with acceptable cross track error not found %lf nmi\nThis can occur for two reasons:\n\t1. Position cannot be projected onto a route segment (before beginning or after end),\n\t2. Position is farther than horizontal tolerance from horizontal trajectory.\nThe second can occur for clearance type of CAPTURE or MAINTAIN (see Issue AAES-1037)", cte.value());
       throw logic_error(msg);
    }
 

Public/AircraftIntent.cpp

@@ -175,6 +175,7 @@ void AircraftIntent::UpdateXYZFromLatLonWgs84() {
       geoPosition.latitude = Units::RadiansAngle(m_fms.m_latitude[var]);
       geoPosition.longitude = Units::RadiansAngle(m_fms.m_longitude[var]);
       m_tangent_plane_sequence->convertGeodeticToLocal(geoPosition, xyPosition);
+      LOG4CPLUS_TRACE(logger, "Waypoint " << var << " at " << geoPosition << " is " << xyPosition);
       m_waypoint_x[var] = m_fms.m_x[var] = xyPosition.x; // yup, waypoint_x always holds the same value as fms.xWp
       m_waypoint_y[var] = m_fms.m_y[var] = xyPosition.y; // yup, waypoint_y always holds the same value as fms.yWp
       m_fms.m_z[var] = xyPosition.z;
@@ -410,6 +411,14 @@ void AircraftIntent::InsertPairAtIndex(const std::string &wpname,
    wp.SetWaypointLatLon(lat, lon);
    wp.SetName(wpname);
 
+   // copy constraints -- high from previous waypoint and low from next
+   auto wp2 = std::next(m_waypoints.begin(), index-1);
+   wp.SetAltitudeConstraintHigh(wp2->GetAltitudeConstraintHigh());
+   wp.SetSpeedConstraintHigh(wp2->GetSpeedConstraintHigh());
+   ++wp2;
+   wp.SetAltitudeConstraintLow(wp2->GetAltitudeConstraintLow());
+   wp.SetSpeedConstraintLow(wp2->GetSpeedConstraintLow());
+
    InsertWaypointAtIndex(wp, index);
 }
 

Public/AircraftState.cpp

@@ -29,6 +29,7 @@ AircraftState::AircraftState()
         m_z(0),
         m_xd(0),
         m_yd(0),
+        m_zd(0),
         m_xdd(0),
         m_ydd(0),
         m_zdd(0.0),
@@ -46,7 +47,32 @@ AircraftState::AircraftState()
 
 AircraftState::~AircraftState() = default;
 
-bool AircraftState::operator==(const AircraftState &in) const {
+void AircraftState::Clear() {
+   m_id = -1;
+   m_time = -1;
+   m_x = 0;
+   m_y = 0;
+   m_z = 0;
+   m_xd = 0;
+   m_yd = 0;
+   m_zd = 0;
+   m_xdd = 0;
+   m_ydd = 0;
+   m_zdd = 0;
+   m_gamma = 0;
+   m_Vwx = 0;
+   m_Vwy = 0;
+   m_Vw_para = 0;
+   m_Vw_perp = 0;
+   m_psi = Units::RadiansAngle(0);
+
+   SetZd(0);
+   m_Vwx_dh = Units::zero();
+   m_Vwy_dh = Units::zero();
+   m_distance_to_go_meters = -99999.99999;
+}
+
+bool AircraftState::operator==(const AircraftState& in) const {
 
    bool same = true;
 
@@ -63,22 +89,21 @@ bool AircraftState::operator==(const AircraftState &in) const {
    return same;
 }
 
-bool AircraftState::operator<(const AircraftState &in) const {
+bool AircraftState::operator<(const AircraftState& in) const {
    if (m_id < in.m_id || (m_id == in.m_id && m_time < in.m_time)) {
       return true;
    }
 
    return false;
 }
 
-const bool AircraftState::IsTurning() const
-{
-	//Determine if a turn is taking place: source nav_NSE.cpp of WinSS
+const bool AircraftState::IsTurning() const {
+   //Determine if a turn is taking place: source nav_NSE.cpp of WinSS
 
-	double spd = sqrt(std::pow(m_xd, 2) + std::pow(m_yd, 2));
-	double turn_rate = (m_xd * m_ydd - m_yd * m_xdd) / spd;
+   double spd = sqrt(std::pow(m_xd, 2) + std::pow(m_yd, 2));
+   double turn_rate = (m_xd * m_ydd - m_yd * m_xdd) / spd;
 
-	return std::fabs(turn_rate) > 1.5;
+   return std::fabs(turn_rate) > 1.5;
 }
 
 const Units::UnsignedRadiansAngle AircraftState::GetHeadingCcwFromEastRadians() const {
@@ -90,7 +115,7 @@ const Units::Speed AircraftState::GetGroundSpeed() const {
    return Units::FeetPerSecondSpeed(sqrt(pow(m_xd, 2) + pow(m_yd, 2)));
 }
 
-AircraftState AircraftState::CreateFromADSBReport(const Sensor::ADSB::ADSBSVReport &adsbsvReport) {
+AircraftState AircraftState::CreateFromADSBReport(const Sensor::ADSB::ADSBSVReport& adsbsvReport) {
    AircraftState result;
    result.m_id = adsbsvReport.GetId();
    result.m_time = adsbsvReport.GetTime().value();
@@ -113,8 +138,9 @@ void AircraftState::DumpParms(std::string str) const {
    LOG4CPLUS_DEBUG(AircraftState::logger, std::endl << "speed      x " << m_xd << "  y " << m_yd << "  z " << m_zd);
    LOG4CPLUS_DEBUG(AircraftState::logger, std::endl << "Vw_para      " << m_Vw_para << "  Vw_perp " << m_Vw_perp);
    LOG4CPLUS_DEBUG(AircraftState::logger, std::endl << "Vwx      " << m_Vwx << "  Vwy " << m_Vwy);
-   LOG4CPLUS_DEBUG(AircraftState::logger, std::endl << "Vwx_dh     " << Units::HertzFrequency(m_Vwx_dh) << "  Vwy_dh "
-                                                    << Units::HertzFrequency(m_Vwy_dh));
+   LOG4CPLUS_DEBUG(AircraftState::logger,
+                   std::endl << "Vwx_dh     " << Units::HertzFrequency(m_Vwx_dh) << "  Vwy_dh "
+                             << Units::HertzFrequency(m_Vwy_dh));
 
 }
 
@@ -148,8 +174,8 @@ void AircraftState::CsvHdrDump(std::string str) const {
  * Other fields, such as acceleration, orientation, and wind,
  * are left unchanged.
  */
-AircraftState &AircraftState::Interpolate(const AircraftState &a,
-                                          const AircraftState &b,
+AircraftState& AircraftState::Interpolate(const AircraftState& a,
+                                          const AircraftState& b,
                                           const double time) {
 
    if (a.m_id != b.m_id) {
@@ -186,7 +212,7 @@ AircraftState &AircraftState::Interpolate(const AircraftState &a,
  * Other fields, such as acceleration, orientation, and wind,
  * are left unchanged.
  */
-AircraftState &AircraftState::Extrapolate(const AircraftState &in,
+AircraftState& AircraftState::Extrapolate(const AircraftState& in,
                                           const double time_in) {
    double dt = time_in - in.m_time;
 
@@ -202,8 +228,8 @@ AircraftState &AircraftState::Extrapolate(const AircraftState &in,
    return *this;
 }
 
-AircraftState &AircraftState::Extrapolate(const AircraftState &in,
-                                          const Units::SecondsTime &time) {
+AircraftState& AircraftState::Extrapolate(const AircraftState& in,
+                                          const Units::SecondsTime& time) {
    Extrapolate(in, time.value());
    return *this;
 }

Public/AlongPathDistanceCalculator.cpp

@@ -22,7 +22,8 @@
 
 log4cplus::Logger AlongPathDistanceCalculator::m_logger = log4cplus::Logger::getInstance("AlongPathDistanceCalculator");
 Units::Length AlongPathDistanceCalculator::CROSS_TRACK_TOLERANCE = Units::NauticalMilesLength(2.5);
-Units::Length AlongPathDistanceCalculator::EXTENDED_CROSS_TRACK_TOLERANCE = Units::NauticalMilesLength(4.0);
+Units::Length AlongPathDistanceCalculator::EXTENDED_CROSS_TRACK_TOLERANCE = Units::NauticalMilesLength(20.0);
+Units::Length AlongPathDistanceCalculator::CAPTURE_CROSS_TRACK_TOLERANCE = Units::NauticalMilesLength(20.0);
 
 AlongPathDistanceCalculator::AlongPathDistanceCalculator(const std::vector<HorizontalPath> &horizontal_path,
                              TrajectoryIndexProgressionDirection expected_index_progression) : HorizontalPathTracker(horizontal_path, expected_index_progression) {
@@ -48,6 +49,11 @@ AlongPathDistanceCalculator::AlongPathDistanceCalculator(const std::vector<Horiz
    m_cross_track_tolerance = specified_cross_track_tolerance;
 }
 
+AlongPathDistanceCalculator AlongPathDistanceCalculator::CreateForCaptureClearance(const std::vector<HorizontalPath> &horizontal_path)   {
+   AlongPathDistanceCalculator result(horizontal_path, /*TrajectoryIndexProgressionDirection::*/UNDEFINED,
+           CAPTURE_CROSS_TRACK_TOLERANCE);
+   return result;
+}
 
 AlongPathDistanceCalculator::~AlongPathDistanceCalculator() = default;
 

include/public/AircraftState.h

@@ -39,6 +39,8 @@ class AircraftState
 
    virtual ~AircraftState();
 
+   void Clear();
+
    const Units::UnsignedRadiansAngle GetHeadingCcwFromEastRadians() const;
 
    void SetPsi(const Units::Angle psi_in);

include/public/AlongPathDistanceCalculator.h

@@ -28,6 +28,7 @@
 class AlongPathDistanceCalculator: public HorizontalPathTracker
 {
 public:
+   static AlongPathDistanceCalculator CreateForCaptureClearance(const std::vector<HorizontalPath> &horizontal_path);
    AlongPathDistanceCalculator();
    AlongPathDistanceCalculator(const std::vector<HorizontalPath> &horizontal_path,
                                TrajectoryIndexProgressionDirection expected_index_progression);
@@ -36,10 +37,6 @@ class AlongPathDistanceCalculator: public HorizontalPathTracker
                                TrajectoryIndexProgressionDirection expected_index_progression,
                                bool use_large_cross_track_tolerance);
 
-   AlongPathDistanceCalculator(const std::vector<HorizontalPath> &horizontal_path,
-                               TrajectoryIndexProgressionDirection expected_index_progression,
-                               Units::Length specified_cross_track_tolerance);
-
    virtual ~AlongPathDistanceCalculator();
 
    /**
@@ -71,9 +68,13 @@ class AlongPathDistanceCalculator: public HorizontalPathTracker
 
 private:
    static log4cplus::Logger m_logger;
-   static Units::Length CROSS_TRACK_TOLERANCE, EXTENDED_CROSS_TRACK_TOLERANCE;
+   static Units::Length CROSS_TRACK_TOLERANCE, EXTENDED_CROSS_TRACK_TOLERANCE, CAPTURE_CROSS_TRACK_TOLERANCE;
    bool m_is_first_call;
    Units::NauticalMilesLength m_cross_track_tolerance;
+   // private constructor used to construct an AlongPathDistanceCalculator when clearance is CAPTURE
+   AlongPathDistanceCalculator(const std::vector<HorizontalPath> &horizontal_path,
+                               TrajectoryIndexProgressionDirection expected_index_progression,
+                               Units::Length specified_cross_track_tolerance);
 };
 
 

include/public/HorizontalPathTracker.h

@@ -73,6 +73,11 @@ class HorizontalPathTracker
 
    void UpdateCurrentIndex(std::vector<HorizontalPath>::size_type new_index);
 
+   /**
+    * Logic to initialize the starting index.
+    */
+   void InitializeStartingIndex();
+
 protected:
    static const Units::Length EXTENSION_LENGTH;
    std::vector<HorizontalPath>::size_type m_current_index;
@@ -108,12 +113,6 @@ class HorizontalPathTracker
 private:
    static log4cplus::Logger m_logger;
    static const Units::MetersLength ON_NODE_TOLERANCE;
-
-   /**
-    * Logic to initialize the starting index.
-    */
-   void InitializeStartingIndex();
-
 };
 
 inline bool HorizontalPathTracker::IsPassedEndOfRoute() const {

unittest/src/framework_tests.cpp

@@ -11,7 +11,7 @@
 // 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.
 // ****************************************************************************