HAL_SITL: enable use of SIM_FLOAT_EXCEPT parameter

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++;
 

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);