HAL_SITL: fixed handling of SIM_FLOAT_EXCEPT in main thread

libraries/AP_HAL_AVR_SITL/HAL_AVR_SITL_Class.cpp

@@ -23,9 +23,9 @@
 
 using namespace AVR_SITL;
 
-static SITLScheduler sitlScheduler;
 static SITLEEPROMStorage sitlEEPROMStorage;
 static SITL_State sitlState;
+static SITLScheduler sitlScheduler(&sitlState);
 static SITLRCInput  sitlRCInput(&sitlState);
 static SITLRCOutput sitlRCOutput(&sitlState);
 static SITLAnalogIn sitlAnalogIn(&sitlState);

libraries/AP_HAL_AVR_SITL/SITL_State.cpp

@@ -79,7 +79,7 @@ bool SITL_State::new_rc_input;
 // catch floating point exceptions
 void SITL_State::_sig_fpe(int signum)
 {
-	fprintf(stderr, "ERROR: Floating point exception\n");
+	fprintf(stderr, "ERROR: Floating point exception - aborting\n");
     abort();
 }
 

libraries/AP_HAL_AVR_SITL/SITL_State.h

@@ -24,6 +24,7 @@
 class HAL_AVR_SITL;
 
 class AVR_SITL::SITL_State {
+    friend class AVR_SITL::SITLScheduler;
 public:
     void init(int argc, char * const argv[]);
 

libraries/AP_HAL_AVR_SITL/Scheduler.cpp

@@ -7,6 +7,7 @@
 #include "Scheduler.h"
 #include <sys/time.h>
 #include <unistd.h>
+#include <fenv.h>
 
 #ifdef __CYGWIN__
 #include <stdio.h> 
@@ -43,7 +44,8 @@ double SITLScheduler::_cyg_freq = 0;
 long SITLScheduler::_cyg_start = 0;
 #endif
 
-SITLScheduler::SITLScheduler()
+SITLScheduler::SITLScheduler(SITL_State *sitlState) :
+    _sitlState(sitlState)
 {}
 
 void SITLScheduler::init(void *unused) 
@@ -207,6 +209,11 @@ void SITLScheduler::system_initialized() {
         panic(
             PSTR("PANIC: scheduler system initialized called more than once"));
     }
+    if (_sitlState->_sitl->float_exception) {
+        feenableexcept(FE_INVALID | FE_OVERFLOW | FE_DIVBYZERO);
+    } else {
+        feclearexcept(FE_INVALID | FE_OVERFLOW | FE_DIVBYZERO);
+    }
     _initialized = true;
 }
 

libraries/AP_HAL_AVR_SITL/Scheduler.h

@@ -12,7 +12,7 @@
 /* Scheduler implementation: */
 class AVR_SITL::SITLScheduler : public AP_HAL::Scheduler {
 public:
-    SITLScheduler();
+    SITLScheduler(SITL_State *sitlState);
     /* AP_HAL::Scheduler methods */
 
     void     init(void *unused);
@@ -49,6 +49,7 @@ public:
     static void timer_event() { _run_timer_procs(true); _run_io_procs(true); }
 
 private:
+    SITL_State *_sitlState;
     uint8_t _nested_atomic_ctr;
     AP_HAL::Proc _delay_cb;
     uint16_t _min_delay_cb_ms;

libraries/AP_HAL_AVR_SITL/sitl_ins.cpp

@@ -128,12 +128,6 @@ void SITL_State::_update_ins(float roll, 	float pitch, 	float yaw,		// Relative
 		return;
 	}
 
-        if (_sitl->float_exception) {
-            feenableexcept(FE_INVALID | FE_OVERFLOW);
-        } else {
-            feclearexcept(FE_INVALID | FE_OVERFLOW);
-        }
-
 	SITL::convert_body_frame(roll, pitch,
 				 rollRate, pitchRate, yawRate,
 				 &p, &q, &r);