From 22237f2530ed3a4d6ca8ef6f26c6caa150498576 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Sun, 16 Nov 2014 11:04:55 +1100
Subject: [PATCH] HAL_SITL: fixed handling of SIM_FLOAT_EXCEPT in main thread

---
 libraries/AP_HAL_AVR_SITL/HAL_AVR_SITL_Class.cpp | 2 +-
 libraries/AP_HAL_AVR_SITL/SITL_State.cpp         | 2 +-
 libraries/AP_HAL_AVR_SITL/SITL_State.h           | 1 +
 libraries/AP_HAL_AVR_SITL/Scheduler.cpp          | 9 ++++++++-
 libraries/AP_HAL_AVR_SITL/Scheduler.h            | 3 ++-
 libraries/AP_HAL_AVR_SITL/sitl_ins.cpp           | 6 ------
 6 files changed, 13 insertions(+), 10 deletions(-)

diff --git a/libraries/AP_HAL_AVR_SITL/HAL_AVR_SITL_Class.cpp b/libraries/AP_HAL_AVR_SITL/HAL_AVR_SITL_Class.cpp
index ab9c8b11fe..299f00445b 100644
--- a/libraries/AP_HAL_AVR_SITL/HAL_AVR_SITL_Class.cpp
+++ b/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);
diff --git a/libraries/AP_HAL_AVR_SITL/SITL_State.cpp b/libraries/AP_HAL_AVR_SITL/SITL_State.cpp
index ae9c3cc6ad..3f0b83c3ca 100644
--- a/libraries/AP_HAL_AVR_SITL/SITL_State.cpp
+++ b/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();
 }
 
diff --git a/libraries/AP_HAL_AVR_SITL/SITL_State.h b/libraries/AP_HAL_AVR_SITL/SITL_State.h
index ca5c8e1ef2..533be7fc01 100644
--- a/libraries/AP_HAL_AVR_SITL/SITL_State.h
+++ b/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[]);
 
diff --git a/libraries/AP_HAL_AVR_SITL/Scheduler.cpp b/libraries/AP_HAL_AVR_SITL/Scheduler.cpp
index 6d7a8a2024..6bf0c94f82 100644
--- a/libraries/AP_HAL_AVR_SITL/Scheduler.cpp
+++ b/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;
 }
 
diff --git a/libraries/AP_HAL_AVR_SITL/Scheduler.h b/libraries/AP_HAL_AVR_SITL/Scheduler.h
index 8719e723e0..0894f7c992 100644
--- a/libraries/AP_HAL_AVR_SITL/Scheduler.h
+++ b/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;
diff --git a/libraries/AP_HAL_AVR_SITL/sitl_ins.cpp b/libraries/AP_HAL_AVR_SITL/sitl_ins.cpp
index 7dceb3dc26..f105e573ff 100644
--- a/libraries/AP_HAL_AVR_SITL/sitl_ins.cpp
+++ b/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);