From 05bffb5915a28d5ab634f9f29f6f9387a40816c6 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Mon, 21 Apr 2014 15:25:07 +1000
Subject: [PATCH] HAL_SITL: enable use of SIM_FLOAT_EXCEPT parameter

---
 libraries/AP_HAL_AVR_SITL/SITL_State.cpp |  7 +++++++
 libraries/AP_HAL_AVR_SITL/sitl_ins.cpp   | 10 ++++++++++
 2 files changed, 17 insertions(+)

diff --git a/libraries/AP_HAL_AVR_SITL/SITL_State.cpp b/libraries/AP_HAL_AVR_SITL/SITL_State.cpp
index 643f26465f..f5b71de452 100644
--- a/libraries/AP_HAL_AVR_SITL/SITL_State.cpp
+++ b/libraries/AP_HAL_AVR_SITL/SITL_State.cpp
@@ -379,6 +379,13 @@ void SITL_State::_fdm_input(void)
 
         if (_sitl != NULL) {
             _sitl->state = d.fg_pkt;
+            // prevent bad inputs from SIM from corrupting our state
+            double *v = &_sitl->state.latitude;
+            for (uint8_t i=0; i<17; i++) {
+                if (isinf(v[i]) || isnan(v[i])) {
+                    v[i] = 0;
+                }
+            }
         }
 		_update_count++;
 
diff --git a/libraries/AP_HAL_AVR_SITL/sitl_ins.cpp b/libraries/AP_HAL_AVR_SITL/sitl_ins.cpp
index a4f2dc00f2..71a00ec0e3 100644
--- a/libraries/AP_HAL_AVR_SITL/sitl_ins.cpp
+++ b/libraries/AP_HAL_AVR_SITL/sitl_ins.cpp
@@ -23,6 +23,7 @@
 #include <AP_Math.h>
 #include "../AP_ADC/AP_ADC.h"
 #include <SITL_State.h>
+#include <fenv.h>
 
 
 using namespace AVR_SITL;
@@ -38,6 +39,9 @@ uint16_t SITL_State::_airspeed_sensor(float airspeed)
 
 	airspeed_pressure = (airspeed*airspeed) / airspeed_ratio;
 	airspeed_raw = airspeed_pressure + airspeed_offset;
+        if (airspeed_raw/4 > 0xFFFF) {
+            return 0xFFFF;
+        }
 	return airspeed_raw/4;
 }
 
@@ -120,6 +124,12 @@ 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);