AP_Math: matrix_alg: disable FE_OVERFLOW in inverse4x4() for SITL

There are occasional overflows on the determinant calculation in inverse4x4()
when using calibration SITL model.

libraries/AP_Math/matrix_alg.cpp

@@ -18,9 +18,15 @@
  */
 #pragma GCC optimize("O3")
 
-#include <AP_Math/AP_Math.h>
 #include <AP_HAL/AP_HAL.h>
+
 #include <stdio.h>
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+#include <fenv.h>
+#endif
+
+#include <AP_Math/AP_Math.h>
+
 extern const AP_HAL::HAL& hal;
 
 //TODO: use higher precision datatypes to achieve more accuracy for matrix algebra operations
@@ -279,6 +285,13 @@ bool inverse4x4(float m[],float invOut[])
     float inv[16], det;
     uint8_t i;
 
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    int old = fedisableexcept(FE_OVERFLOW);
+    if (old < 0) {
+        hal.console->printf("inverse4x4(): warning: error on disabling FE_OVERFLOW floating point exception\n");
+    }
+#endif
+
     inv[0] = m[5]  * m[10] * m[15] -
     m[5]  * m[11] * m[14] -
     m[9]  * m[6]  * m[15] +
@@ -393,6 +406,12 @@ bool inverse4x4(float m[],float invOut[])
 
     det = m[0] * inv[0] + m[1] * inv[4] + m[2] * inv[8] + m[3] * inv[12];
 
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    if (old >= 0 && feenableexcept(old) < 0) {
+        hal.console->printf("inverse4x4(): warning: error on restoring floating exception mask\n");
+    }
+#endif
+
     if (is_zero(det) || isinf(det)){
         return false;
     }