From 0e545d711cd00e31d8ffd0acf4d4912094e93d4b Mon Sep 17 00:00:00 2001
From: James Turner <zakalawe@mac.com>
Date: Fri, 18 Dec 2020 13:58:33 +0000
Subject: [PATCH] Attempt to handle NaNs more elegantly

Check for NaNs after each FDM iteration, and freeze the sim if found.
Report this condition to the user and to the reporting backend, along
with the last valid position.

Probably needs some refinement, this is just a first guess.

Sentry-Id: FLIGHTGEAR-AM
---
 src/FDM/CMakeLists.txt         |  2 +-
 src/FDM/fdm_shell.cxx          | 49 +++++++++++++++++++++++++++++++---
 src/FDM/fdm_shell.hxx          | 10 ++++++-
 src/Main/sentryIntegration.cxx |  7 +++++
 4 files changed, 62 insertions(+), 6 deletions(-)

diff --git a/src/FDM/CMakeLists.txt b/src/FDM/CMakeLists.txt
index cfcb90798..ad36b22a3 100644
--- a/src/FDM/CMakeLists.txt
+++ b/src/FDM/CMakeLists.txt
@@ -173,7 +173,7 @@ if(ENABLE_LARCSIM)
 	endforeach()
 endif()
 
-flightgear_component(FDM "${SOURCES}")
+flightgear_component(FDM "${SOURCES}" "${HEADERS}")
 
 if(ENABLE_YASIM)
 	add_subdirectory(YASim)
diff --git a/src/FDM/fdm_shell.cxx b/src/FDM/fdm_shell.cxx
index b008694e9..e49b9014a 100644
--- a/src/FDM/fdm_shell.cxx
+++ b/src/FDM/fdm_shell.cxx
@@ -63,11 +63,12 @@
 #include <FDM/YASim/YASim.hxx>
 #endif
 
+#include <GUI/MessageBox.hxx>
+#include <Main/sentryIntegration.hxx>
+
 using std::string;
 
-FDMShell::FDMShell() :
-  _tankProperties( fgGetNode("/consumables/fuel", true) ),
-  _dataLogging(false)
+FDMShell::FDMShell() : _tankProperties(fgGetNode("/consumables/fuel", true))
 {
 }
 
@@ -94,6 +95,10 @@ void FDMShell::init()
   _max_radius_nm    = _props->getNode("fdm/ai-wake/max-radius-nm",          true);
   _ai_wake_enabled  = _props->getNode("fdm/ai-wake/enabled",                true);
 
+  _nanCheckFailed = false;
+  fgSetBool("/sim/fdm-nan-failure", false);
+  _lastValidPos = SGGeod::invalid();
+
   createImplementation();
 }
 
@@ -170,7 +175,11 @@ void FDMShell::update(double dt)
   if (!_impl) {
     return;
   }
-  
+
+  if (_nanCheckFailed) {
+      return;
+  }
+
   if (!_impl->get_inited()) {
     // Check for scenery around the aircraft.
     double lon = fgGetDouble("/sim/presets/longitude-deg");
@@ -263,6 +272,8 @@ void FDMShell::update(double dt)
           // replay is active
           break;
   }
+
+  validateOutputProperties();
 }
 
 FGInterface* FDMShell::getInterface() const
@@ -390,6 +401,36 @@ void FDMShell::createImplementation()
     }
 }
 
+void FDMShell::validateOutputProperties()
+{
+    if (_nanCheckFailed)
+        return;
+
+    const auto p = globals->get_aircraft_position();
+    if (SGMisc<double>::isNaN(p.getLatitudeDeg()) ||
+        SGMisc<double>::isNaN(p.getLongitudeDeg()) ||
+        SGMisc<double>::isNaN(p.getElevationFt())) {
+        SG_LOG(SG_FLIGHT, SG_ALERT, "FDM position became invalid. Last valid position was:" << _lastValidPos);
+        _nanCheckFailed = true;
+    } else {
+        _lastValidPos = p;
+    }
+
+    if (SGMisc<double>::isNaN(_impl->get_V_true_kts())) {
+        SG_LOG(SG_FLIGHT, SG_ALERT, "FDM velocity became invalid");
+        _nanCheckFailed = true;
+    }
+
+    if (_nanCheckFailed) {
+        flightgear::sentryReportException("FDM NaN check failed");
+
+        flightgear::modalMessageBox("Flight dynamics error",
+                                    "The flight dynamics model (FDM) has become invalid. The simulation will be stopped, so you can restart at a new location.");
+        fgSetBool("/sim/fdm-nan-failure", true);
+        fgSetBool("/sim/freeze/master", true);
+        fgSetBool("/sim/freeze/clock", true);
+    }
+}
 
 // Register the subsystem.
 SGSubsystemMgr::Registrant<FDMShell> registrantFDMShell(
diff --git a/src/FDM/fdm_shell.hxx b/src/FDM/fdm_shell.hxx
index a64bdd1a7..d01195e94 100644
--- a/src/FDM/fdm_shell.hxx
+++ b/src/FDM/fdm_shell.hxx
@@ -23,7 +23,9 @@
 #ifndef FG_FDM_SHELL_HXX
 #define FG_FDM_SHELL_HXX
 
+#include <simgear/math/SGGeod.hxx>
 #include <simgear/structure/subsystem_mgr.hxx>
+
 #include "TankProperties.hxx"
 
 // forward decls
@@ -61,10 +63,16 @@ public:
 private:
     void createImplementation();
 
+    void validateOutputProperties();
+
+private:
     TankPropertiesList _tankProperties;
     SGSharedPtr<FGInterface> _impl;
     SGPropertyNode_ptr _props; // root property tree for this FDM instance
-    bool _dataLogging;
+    bool _dataLogging = false;
+
+    bool _nanCheckFailed = false; ///< set when we detect NaN output from the impl
+    SGGeod _lastValidPos;
 
     SGPropertyNode_ptr _wind_north, _wind_east,_wind_down;
     SGPropertyNode_ptr _control_fdm_atmo,_temp_degc,_pressure_inhg;
diff --git a/src/Main/sentryIntegration.cxx b/src/Main/sentryIntegration.cxx
index f5250c361..4a004fb42 100644
--- a/src/Main/sentryIntegration.cxx
+++ b/src/Main/sentryIntegration.cxx
@@ -68,6 +68,9 @@ auto XML_messageWhitelist = {
      "syntax error (from 'SimGear XML Parser')"
 };
 
+auto exception_messageWhitelist = {
+    "position is invalid, NaNs"};
+
 // we don't want sentry enabled for the test suite
 #if defined(HAVE_SENTRY) && !defined(BUILDING_TESTSUITE)
 
@@ -94,6 +97,10 @@ void sentryTraceSimgearThrow(const std::string& msg, const std::string& origin,
         return;
     }
 
+    if (doesStringMatchPrefixes(msg, exception_messageWhitelist)) {
+        return;
+    }
+
     sentry_value_t exc = sentry_value_new_object();
     sentry_value_set_by_key(exc, "type", sentry_value_new_string("Exception"));