Update to v3.5.3

CMakeLists.txt

@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.0)
-project(aaesim VERSION 3.5.0.0) # the nano value is a boolean. 1 == SNAPSHOT, 0 == release
+project(aaesim VERSION 3.5.3.0) # the nano value is a boolean. 1 == SNAPSHOT, 0 == release
 
 set (CMAKE_CXX_STANDARD 11)
 
@@ -56,6 +56,7 @@ set (GTEST_DIR    ${CMAKE_CURRENT_SOURCE_DIR}/unittest/gtest-1.7.0/)
 set (FRAMEWORK_DIR ${CMAKE_CURRENT_SOURCE_DIR}/AircraftDynamicsTestFramework)
 set (aaesim_INCLUDE_DIRS
         ${CMAKE_CURRENT_SOURCE_DIR}/include
+        ${CMAKE_CURRENT_SOURCE_DIR}/unitsLib/units-2.1/
         ${CMAKE_CURRENT_SOURCE_DIR}/unitsLib/units-2.1/scalar/)
 
 add_subdirectory(${UNITSLIB_DIR})

Public/AircraftIntent.cpp

@@ -69,7 +69,7 @@ void AircraftIntent::Initialize() {
       m_waypoint_descent_angle[c] = Units::ZERO_ANGLE;
       m_waypoint_nominal_ias[c] = Units::ZERO_SPEED;
       m_waypoint_mach[c] = 0;
-      m_waypoint_descent_rate[c] = Units::Acceleration(0);
+      m_waypoint_descent_rate[c] = Units::MetersSecondAcceleration(0);
 
       m_fms.m_name[c] = "";
       m_fms.m_altitude[c] = Units::ZERO_LENGTH;
@@ -101,7 +101,7 @@ AircraftIntent &AircraftIntent::operator=(const AircraftIntent &in) {
    return *this;
 }
 
-void AircraftIntent::LoadWaypointsFromList(std::list<Waypoint> &waypoint_list) {
+void AircraftIntent::ResetWaypoints(std::list<Waypoint> &waypoint_list) {
    int c = 0;
    auto i = waypoint_list.begin();
    auto e = waypoint_list.end();
@@ -141,14 +141,14 @@ void AircraftIntent::LoadWaypointsFromList(std::list<Waypoint> &waypoint_list) {
          m_fms.m_nominal_ias[c] = m_mach_transition_cas;
          m_fms.m_mach[c] = m_waypoint_mach[c];
       } else if (c >= 1 && m_fms.m_mach[c - 1] != 0
-                 && ias.value() == 0) {
+         && ias.value() == 0) {
          m_fms.m_nominal_ias[c] = m_mach_transition_cas;
          m_fms.m_mach[c] = m_fms.m_mach[c - 1];
       } else if (c >= 1 && ias.value() != 0) {
          m_fms.m_nominal_ias[c] = ias;
          m_fms.m_mach[c] = 0;
       } else if (c >= 1 && m_fms.m_nominal_ias[c - 1].value() != 0
-                 && ias.value() == 0) {
+         && ias.value() == 0) {
          m_fms.m_nominal_ias[c] = m_fms.m_nominal_ias[c - 1];
          m_fms.m_mach[c] = 0;
       } else {
@@ -162,7 +162,10 @@ void AircraftIntent::LoadWaypointsFromList(std::list<Waypoint> &waypoint_list) {
 
    m_number_of_waypoints = static_cast<unsigned int>(waypoint_list.size());
    m_waypoints = waypoint_list;
+}
 
+void AircraftIntent::LoadWaypointsFromList(std::list<Waypoint> &waypoint_list) {
+   ResetWaypoints(waypoint_list);
    m_tangent_plane_sequence = std::shared_ptr<TangentPlaneSequence>(new SingleTangentPlaneSequence(waypoint_list));
 }
 
@@ -429,7 +432,7 @@ void AircraftIntent::InsertWaypointAtIndex(const Waypoint &wp, int index) {
    auto itr = std::next(m_waypoints.begin(), index);
    m_waypoints.insert(itr, wp);
 
-   LoadWaypointsFromList(m_waypoints);
+   ResetWaypoints(m_waypoints);
    UpdateXYZFromLatLonWgs84();
 }
 
@@ -445,7 +448,7 @@ void AircraftIntent::UpdateWaypoint(const Waypoint &waypoint) {
       LOG4CPLUS_FATAL(logger, update_count << " waypoints updated.");
       throw std::runtime_error("Wrong number of waypoints matched.");
    }
-   LoadWaypointsFromList(m_waypoints);
+   ResetWaypoints(m_waypoints);
    UpdateXYZFromLatLonWgs84();
 }
 
@@ -454,7 +457,7 @@ void AircraftIntent::ClearWaypoints() {
    m_waypoints.clear();
 
    // Re-initialize this object with no waypoints
-   LoadWaypointsFromList(m_waypoints);
+   ResetWaypoints(m_waypoints);
    UpdateXYZFromLatLonWgs84();
 }
 

Public/AircraftSpeed.cpp

@@ -25,7 +25,6 @@ speed_type AircraftSpeed::GetSpeedType() const {
 }
 
 void AircraftSpeed::SetSpeed(const speed_type type, const double value) {
-   // TODO range sanity checks -- perhaps allow MACH 0.1 to 10.0, IAS 20 to 2000
    m_speed_type = type;
    m_value = value;
 }

Public/AircraftState.cpp

@@ -15,6 +15,7 @@
 // Copyright 2020 The MITRE Corporation. All Rights Reserved.
 // ****************************************************************************
 
+#include <iomanip>
 #include "public/AircraftCalculations.h"
 #include "math/CustomMath.h"
 
@@ -130,18 +131,13 @@ AircraftState AircraftState::CreateFromADSBReport(const Sensor::ADSB::ADSBSVRepo
 }
 
 void AircraftState::DumpParms(std::string str) const {
-   LOG4CPLUS_DEBUG(AircraftState::logger, std::endl << "(Subset) Aircraft state parms for " << str.c_str());
-
-   LOG4CPLUS_DEBUG(AircraftState::logger,
-                   std::endl << "time " << m_time << "  id " << m_id << "  distToGo " << m_distance_to_go_meters);
-   LOG4CPLUS_DEBUG(AircraftState::logger, std::endl << "position   x " << m_x << "  y " << m_y << "  z " << m_z);
-   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, "aircraft state for " << str.c_str() << ":");
+   LOG4CPLUS_DEBUG(AircraftState::logger, std::setprecision(12) << "time " << m_time << ", id " << m_id << ", distToGo (m) " << m_distance_to_go_meters);
+   LOG4CPLUS_DEBUG(AircraftState::logger, std::setprecision(12) << "position (ft): x " << m_x << ", y " << m_y << ", z " << m_z);
+   LOG4CPLUS_DEBUG(AircraftState::logger, std::setprecision(12) << "speed (ft/s): x " << m_xd << ", y " << m_yd << ", z " << m_zd);
+   LOG4CPLUS_DEBUG(AircraftState::logger, std::setprecision(12) << "Wind Horiz Components (m/s): vwpara " << m_Vw_para << ", vwperp " << m_Vw_perp << ", Vwx " << m_Vwx << ", Vwy " << m_Vwy);
+   LOG4CPLUS_DEBUG(AircraftState::logger, std::setprecision(12) << "Wind Vert Components (1/s): vwx_dh " << Units::HertzFrequency(m_Vwx_dh) << ", vwy_dh " << Units::HertzFrequency(m_Vwy_dh));
+   LOG4CPLUS_DEBUG(AircraftState::logger, std::setprecision(12) << "psi_enu (deg): " << Units::SignedDegreesAngle (m_psi) << ", gamma (deg): " << Units::DegreesAngle(Units::RadiansAngle(m_gamma)) );
 }
 
 void AircraftState::CsvDataDump(std::string str) const {
@@ -246,7 +242,7 @@ void AircraftState::SetZd(double zd) {
 Units::Speed AircraftState::GetTrueAirspeed() const {
    Units::MetersPerSecondSpeed tas_x, tas_y;
    tas_x = Units::FeetPerSecondSpeed(m_xd) - Units::MetersPerSecondSpeed(m_Vwx);
-   tas_y = Units::FeetPerSecondSpeed(m_xd) - Units::MetersPerSecondSpeed(m_Vwy);
-   Units::MetersPerSecondSpeed tas(hypot(tas_x.value(), tas_y.value()));
+   tas_y = Units::FeetPerSecondSpeed(m_yd) - Units::MetersPerSecondSpeed(m_Vwy);
+   Units::MetersPerSecondSpeed tas = Units::sqrt(Units::sqr(tas_x) + Units::sqr(tas_y));
    return tas;
 }

Public/Guidance.cpp

@@ -26,6 +26,7 @@ Guidance::Guidance() {
    m_cross_track_error = Units::ZERO_LENGTH;
    m_track_angle = Units::ZERO_ANGLE;
    m_reference_bank_angle = Units::ZERO_ANGLE;
+   m_mach_command = 0;
 
    m_use_cross_track = false;
    m_valid = false;

Public/HorizontalPathTracker.cpp

@@ -1,25 +1,25 @@
-// ****************************************************************************
-// NOTICE
-//
-// This is the copyright work of The MITRE Corporation, and was produced
-// for the U. S. Government under Contract Number DTFAWA-10-C-00080, and
-// is subject to Federal Aviation Administration Acquisition Management
-// System Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV
-// (Oct. 1996).  No other use other than that granted to the U. S.
-// Government, or to those acting on behalf of the U. S. Government,
-// under that Clause is authorized without the express written
-// permission of The MITRE Corporation. For further information, please
-// contact The MITRE Corporation, Contracts Office, 7515 Colshire Drive,
-// McLean, VA  22102-7539, (703) 983-6000. 
-//
-// Copyright 2020 The MITRE Corporation. All Rights Reserved.
-// ****************************************************************************
-
+// ****************************************************************************
+// NOTICE
+//
+// This is the copyright work of The MITRE Corporation, and was produced
+// for the U. S. Government under Contract Number DTFAWA-10-C-00080, and
+// is subject to Federal Aviation Administration Acquisition Management
+// System Clause 3.5-13, Rights In Data-General, Alt. III and Alt. IV
+// (Oct. 1996).  No other use other than that granted to the U. S.
+// Government, or to those acting on behalf of the U. S. Government,
+// under that Clause is authorized without the express written
+// permission of The MITRE Corporation. For further information, please
+// contact The MITRE Corporation, Contracts Office, 7515 Colshire Drive,
+// McLean, VA  22102-7539, (703) 983-6000. 
+//
+// Copyright 2020 The MITRE Corporation. All Rights Reserved.
+// ****************************************************************************
+
 #include <public/HorizontalPathTracker.h>
 #include <public/AircraftCalculations.h>
 
 log4cplus::Logger HorizontalPathTracker::m_logger = log4cplus::Logger::getInstance("HorizontalPathTracker");
-const Units::Length HorizontalPathTracker::EXTENSION_LENGTH = Units::NauticalMilesLength(1.0);
+const Units::Length HorizontalPathTracker::EXTENSION_LENGTH = Units::NauticalMilesLength(10.0);
 const Units::MetersLength HorizontalPathTracker::ON_NODE_TOLERANCE = Units::MetersLength(1e-10);
 
 HorizontalPathTracker::HorizontalPathTracker(const std::vector<HorizontalPath> &horizontal_trajectory, TrajectoryIndexProgressionDirection expected_index_progression) {

Public/PilotDelay.cpp

@@ -29,12 +29,14 @@ PilotDelay::PilotDelay()
         m_pilot_delay_is_on(true),
         m_delay_count(0),
         m_delay_sum(0),
-        m_delay_square_sum(0) {
+        m_delay_square_sum(0),
+        m_current_speed_command_mach_as_ias(Units::negInfinity()),
+        m_next_speed_command_mach_as_ias(Units::negInfinity()){
    IterationReset();
 }
 
 PilotDelay::~PilotDelay() {
-   if (m_delay_count > 0 && m_logger.getLogLevel() <= log4cplus::TRACE_LOG_LEVEL) {
+   if (m_delay_count > 0 && m_logger.isEnabledFor(log4cplus::TRACE_LOG_LEVEL)) {
       DumpStatistics();
    }
 }
@@ -47,80 +49,92 @@ void PilotDelay::IterationReset() {
 
 /**
  * Recomputes and updates mach as necessary according to the time delay
- * and reccomputes ias accordingly.  Manages time to the next speed change.
+ * and recomputes ias accordingly.  Manages time to the next speed change.
  *
- * @param previous_im_speed_command_mach mach command from previous time.
- * @param input_im_speed_command_mach mach computed from calling method.
+ * @param previous_im_speed_command_as_mach mach command from previous time.
+ * @param im_speed_command_as_mach mach computed from calling method.
  * @param current_altitude altitude flight is at.
  * @param altitude_at_end_of_route altitude at the FAF or last point.
  *
  * @return recomputed guidance ias speed.
  */
-Units::Speed PilotDelay::UpdateMach(double previous_im_speed_command_mach,
-                                    double input_im_speed_command_mach,
+Units::Speed PilotDelay::UpdateMach(double previous_im_speed_command_as_mach,
+                                    double im_speed_command_as_mach,
                                     Units::Length current_altitude,
                                     Units::Length altitude_at_end_of_route) {
-   if ((input_im_speed_command_mach != previous_im_speed_command_mach) &&
+   if ((im_speed_command_as_mach != previous_im_speed_command_as_mach) &&
          (m_time_to_next_speed_change < Units::zero())) {
 
       // Reset time delay counter and set m_guidance_mach to previous speed.
       m_time_to_next_speed_change = ComputeTimeToSpeedChange(current_altitude, altitude_at_end_of_route);
-      m_guidance_mach = previous_im_speed_command_mach;
+      m_guidance_mach = previous_im_speed_command_as_mach;
+   }
+   else if (m_guidance_ias == Units::zero()) {
+      // ias has not changed and guidance ias not set. Compute guidance ias
+      // from guidance mach.
+      m_guidance_mach = previous_im_speed_command_as_mach;
+      SetInitialIAS(current_altitude, m_atmosphere->MachToIAS(previous_im_speed_command_as_mach, current_altitude));
+
+      if (m_time_to_next_speed_change < Units::zero()) {
+         // past delay time-recompute new delay time and update guidance ias.
+         m_time_to_next_speed_change = ComputeTimeToSpeedChange(current_altitude, altitude_at_end_of_route);
+      }
    }
 
    if (m_time_to_next_speed_change == Units::zero()) {
       // At time to change speed-set to input mach.
-      m_guidance_mach = input_im_speed_command_mach;
+      m_guidance_mach = im_speed_command_as_mach;
    }
 
    // Update time delay counter and return guidance speed in ias.
    m_time_to_next_speed_change -= Units::SecondsTime(1.0);
 
+   // Return the mach converted to IAS using the current conditions
    return m_atmosphere->MachToIAS(m_guidance_mach, current_altitude);
 }
 
 /**
  * Recomputes and updates ias as necessary.  The pilot delay time is also updated.
  *
- * @param previous_im_speed_command_ias Computed ias from the last time.
- * @param input_im_speed_command_ias computed ias from calling method.
+ * @param previous_im_speed_command_as_ias Computed ias from the previous call
+ * @param im_speed_command_as_ias computed ias from calling method.
  * @param current_altitude aircraft altitude.
  * @param altitude_at_end_of_route altitude at the FAF or last point.
  *
  * @return guidance ias speed.
  */
-Units::Speed PilotDelay::UpdateIAS(Units::Speed previous_im_speed_command_ias,
-                                   Units::Speed input_im_speed_command_ias,
+Units::Speed PilotDelay::UpdateIAS(Units::Speed previous_im_speed_command_as_ias,
+                                   Units::Speed im_speed_command_as_ias,
                                    Units::Length current_altitude,
                                    Units::Length altitude_at_end_of_route) {
 
-   if (input_im_speed_command_ias != previous_im_speed_command_ias) {
+   if (im_speed_command_as_ias != previous_im_speed_command_as_ias) {
       // ias has changed.
       if (m_time_to_next_speed_change < Units::zero()) {
          // past delay time-recompute new delay time and update guidance ias.
          m_time_to_next_speed_change = ComputeTimeToSpeedChange(current_altitude, altitude_at_end_of_route);
 
          if (m_guidance_ias == Units::zero()) {
             // compute first guidance ias
-            SetInitialIAS(current_altitude, previous_im_speed_command_ias);
+            SetInitialIAS(current_altitude, previous_im_speed_command_as_ias);
          } else {
             // set guidance ias from previous ias.
-            m_guidance_ias = previous_im_speed_command_ias;
+            m_guidance_ias = previous_im_speed_command_as_ias;
          }
       } else if ((m_time_to_next_speed_change > Units::zero()) && (m_guidance_ias == Units::zero())) {
          // not at delay time yet and guidance ias not set-compute guidance ias
          // from guidance mach.
-         SetInitialIAS(current_altitude, previous_im_speed_command_ias);
+         SetInitialIAS(current_altitude, previous_im_speed_command_as_ias);
       }
    } else if (m_guidance_ias == Units::zero()) {
       // ias has not changed and guidance ias not set-compute guidnace ias
       // from guidance mach.
-      SetInitialIAS(current_altitude, previous_im_speed_command_ias);
+      SetInitialIAS(current_altitude, previous_im_speed_command_as_ias);
    }
 
    if (m_time_to_next_speed_change == Units::zero()) {
       // time to process delay-set guidance ias from input ias.
-      m_guidance_ias = input_im_speed_command_ias;
+      m_guidance_ias = im_speed_command_as_ias;
    }
 
    // update delay time and return guidance ias.

Public/StandardAtmosphere.cpp

@@ -34,9 +34,8 @@ StandardAtmosphere* StandardAtmosphere::MakeInstance(
       const Units::KelvinTemperature temperature,
       const Units::Length altitude) {
    if (temperature < T_TROP) {
-      LOG4CPLUS_FATAL(m_logger, "Unable to create StandardAtmosphere with temperature " << temperature
+      LOG4CPLUS_DEBUG(m_logger, "Creating non-standard StandardAtmosphere with temperature " << temperature
             << " at " << Units::FeetLength(altitude) << ".  Tropopause temperature is " << T_TROP);
-      throw std::runtime_error("Invalid temperature");
    }
    if (altitude < Units::zero()) {
       LOG4CPLUS_FATAL(m_logger, "Specified altitude is below sea level:  " << Units::FeetLength(altitude));
@@ -54,6 +53,10 @@ StandardAtmosphere* StandardAtmosphere::MakeInstance(
    return new StandardAtmosphere(offset);
 }
 
+StandardAtmosphere* StandardAtmosphere::MakeInstanceFromTemperatureOffset(Units::CelsiusTemperature temperature_offset) {
+   return new StandardAtmosphere(temperature_offset);
+}
+
 StandardAtmosphere::StandardAtmosphere(const Units::Temperature temperatureOffset) {
    SetTemperatureOffset(temperatureOffset);
 }

Public/Waypoint.cpp

@@ -33,7 +33,7 @@ Waypoint::Waypoint() {
    m_altitude = Units::ZERO_LENGTH;
    m_nominal_ias = Units::ZERO_SPEED;
    m_mach = 0;
-   m_descent_rate = Units::Acceleration(0);
+   m_descent_rate = Units::MetersSecondAcceleration(0);
 
    m_altitude_constraint_high = Units::ZERO_LENGTH;
    m_altitude_constraint_low = Units::ZERO_LENGTH;
@@ -79,7 +79,7 @@ Waypoint::Waypoint(const std::string &name_in,
    m_rf_turn_arc_radius = Units::ZERO_LENGTH;
 
    m_mach = 0;
-   m_descent_rate = Units::Acceleration(0);
+   m_descent_rate = Units::MetersSecondAcceleration(0);
    m_descent_angle = Units::ZERO_ANGLE;
 }
 

Public/WeatherPrediction.cpp

@@ -20,6 +20,8 @@
 
 const PredictedWindOption WeatherPrediction::PWOValues[3] = {SINGLE_DTG, MULTIPLE_DTG_LEGACY, MULTIPLE_DTG_ALONG_ROUTE};
 
+log4cplus::Logger WeatherPrediction::m_logger = log4cplus::Logger::getInstance(LOG4CPLUS_TEXT("WeatherPrediction"));
+
 WeatherPrediction::WeatherPrediction()
       : WeatherEstimate(),
         m_predicted_wind_option(SINGLE_DTG),
@@ -40,11 +42,15 @@ WeatherPrediction::WeatherPrediction(PredictedWindOption option,
 WeatherPrediction::~WeatherPrediction() = default;
 
 const void WeatherPrediction::Dump() const {
+   LOG4CPLUS_DEBUG(m_logger, "m_predicted_wind_option: " << m_predicted_wind_option);
+   LOG4CPLUS_DEBUG(m_logger, "m_update_count: " << m_update_count );
+   LOG4CPLUS_DEBUG(m_logger, "m_temperature_checked: " << m_temperature_checked);
+   LOG4CPLUS_DEBUG(m_logger, "m_temperature_available: " << m_temperature_available);
+   LOG4CPLUS_DEBUG(m_logger, "altitude, eastvel, northvel");
    for (int iAlt = east_west.GetMinRow(); iAlt <= east_west.GetMaxRow(); iAlt++) {
-      std::cout << iAlt << ":  " <<
+      LOG4CPLUS_DEBUG(m_logger, iAlt << ":  " <<
                 east_west.GetAltitude(iAlt) << " " <<
                 east_west.GetSpeed(iAlt) << " " <<
-                north_south.GetSpeed(iAlt) << std::endl;
+                north_south.GetSpeed(iAlt));
    }
-   std::cout << std::endl;
 }

include/public/AchieveObserver.h

@@ -45,7 +45,7 @@ class AchieveObserver
 private:
    int m_iteration;
    int m_id; // aircraft id
-   Units::Time m_time;
+   Units::SecondsTime m_time;
    Units::Time m_targ_ttg_to_ach;
    Units::Time m_own_ttg_to_ach;
    Units::Length m_curr_dist;