AP_HAL_ChibiOS: raise internal error on FPE

libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp

@@ -27,6 +27,7 @@
 #include "hwdef/common/usbcfg.h"
 #include "hwdef/common/stm32_util.h"
 #include "hwdef/common/watchdog.h"
+#include "hwdef/common/fpe.h"
 #include <AP_BoardConfig/AP_BoardConfig.h>
 #include <AP_InternalError/AP_InternalError.h>
 #ifndef HAL_BOOTLOADER_BUILD
@@ -265,6 +266,11 @@ static void main_loop()
         }
 #endif
         schedulerInstance.watchdog_pat();
+#if !defined(HAL_BOOTLOADER_BUILD) && !defined(IOMCU_FW)
+        if (stm32_fpe_flags() != 0) {
+            AP::internalerror().error(AP_InternalError::error_t::fpe);
+        }
+#endif
     }
     thread_running = false;
 }

libraries/AP_HAL_ChibiOS/hwdef/common/chibios_board.mk

@@ -124,6 +124,7 @@ CSRC += $(HWDEF)/common/stubs.c \
 	   $(HWDEF)/common/hrt.c \
        $(HWDEF)/common/stm32_util.c \
        $(HWDEF)/common/bouncebuffer.c \
+       $(HWDEF)/common/fpe.c \
        $(HWDEF)/common/watchdog.c
 
 #	   $(TESTSRC) \

libraries/AP_HAL_ChibiOS/hwdef/common/fpe.c

@@ -0,0 +1,26 @@
+/*
+  FPE detection support
+ */
+
+#include "hal.h"
+#include "fpe.h"
+
+uint32_t stm32_fpe_flags(void)
+{
+    // Flush to zero (IDC)
+    // Inexact result (IXC)
+    // Underflow (UFC)
+    // Overflow (OFC)
+    // Division by zero (DZC)
+    // Invalid operation (IOC)
+
+    // see page 14/31:
+    const uint32_t IOC = 1U << 0;
+    const uint32_t DZC = 1U << 1;
+    const uint32_t OFC = 1U << 2;
+    // const uint32_t UFC = 1U << 3;
+    // const uint32_t IXC = 1U << 4;
+    const uint32_t IDC = 1U << 7;
+
+    return __get_FPSCR() & (IOC|DZC|OFC|IDC);
+}

libraries/AP_HAL_ChibiOS/hwdef/common/fpe.h

@@ -0,0 +1,9 @@
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+uint32_t stm32_fpe_flags(void);
+
+#ifdef __cplusplus
+}
+#endif