Merge remote-tracking branch 'origin/amorale/OP-1704_sanity_check_hooks' into next

flight/libraries/inc/sanitycheck.h

@@ -32,6 +32,9 @@
 
 #include <systemalarms.h>
 
+#include <stdint.h>
+#include <utlist.h>
+
 typedef enum {
     FRAME_TYPE_MULTIROTOR,
     FRAME_TYPE_HELI,
@@ -40,6 +43,8 @@ typedef enum {
     FRAME_TYPE_CUSTOM,
 } FrameType_t;
 
+typedef SystemAlarmsExtendedAlarmStatusOptions (SANITYCHECK_CustomHook_function)();
+
 #define SANITYCHECK_STATUS_ERROR_NONE         SYSTEMALARMS_EXTENDEDALARMSTATUS_NONE
 #define SANITYCHECK_STATUS_ERROR_FLIGHTMODE   SYSTEMALARMS_EXTENDEDALARMSTATUS_FLIGHTMODE
 
@@ -55,4 +60,16 @@ extern int32_t configuration_check();
 
 extern FrameType_t GetCurrentFrameType();
 
+/**
+ * Attach a custom hook to the sanity check process
+ * @param hook a custom hook function
+ */
+extern void SANITYCHECK_AttachHook(SANITYCHECK_CustomHook_function *hook);
+
+/**
+ * Detach a custom hook to the sanity check process
+ * @param hook a custom hook function
+ */
+extern void SANITYCHECK_DetachHook(SANITYCHECK_CustomHook_function *hook);
+
 #endif /* SANITYCHECK_H */

flight/libraries/sanitycheck.c

@@ -45,10 +45,18 @@
 // a number of useful macros
 #define ADDSEVERITY(check) severity = (severity != SYSTEMALARMS_ALARM_OK ? severity : ((check) ? SYSTEMALARMS_ALARM_OK : SYSTEMALARMS_ALARM_CRITICAL))
 
+// private types
+typedef struct SANITYCHECK_CustomHookInstance {
+    SANITYCHECK_CustomHook_function *hook;
+    struct SANITYCHECK_CustomHookInstance *next;
+    bool enabled;
+} SANITYCHECK_CustomHookInstance;
 
 // ! Check a stabilization mode switch position for safety
 static bool check_stabilization_settings(int index, bool multirotor, bool coptercontrol, bool gpsassisted);
 
+SANITYCHECK_CustomHookInstance *hooks = 0;
+
 /**
  * Run a preflight check over the hardware configuration
  * and currently active modules
@@ -176,6 +184,20 @@ int32_t configuration_check()
         severity = SYSTEMALARMS_ALARM_WARNING;
     }
 
+    // query sanity check hooks
+    if (severity == SYSTEMALARMS_ALARM_OK) {
+        SANITYCHECK_CustomHookInstance *instance = NULL;
+        LL_FOREACH(hooks, instance) {
+            if (instance->enabled) {
+                alarmstatus = instance->hook();
+                if (alarmstatus != SYSTEMALARMS_EXTENDEDALARMSTATUS_NONE) {
+                    severity = SYSTEMALARMS_ALARM_WARNING;
+                    break;
+                }
+            }
+        }
+    }
+
     if (severity != SYSTEMALARMS_ALARM_OK) {
         ExtendedAlarmsSet(SYSTEMALARMS_ALARM_SYSTEMCONFIGURATION, severity, alarmstatus, alarmsubstatus);
     } else {
@@ -270,6 +292,7 @@ static bool check_stabilization_settings(int index, bool multirotor, bool copter
     // and is the same for STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL
     // (this is checked at compile time by static constraint manualcontrol.h)
 
+
     return true;
 }
 
@@ -314,3 +337,39 @@ FrameType_t GetCurrentFrameType()
     // anyway it should not reach here
     return FRAME_TYPE_CUSTOM;
 }
+
+void SANITYCHECK_AttachHook(SANITYCHECK_CustomHook_function *hook)
+{
+    PIOS_Assert(hook);
+    SANITYCHECK_CustomHookInstance *instance = NULL;
+
+    // Check whether there is an existing instance and enable it
+    LL_FOREACH(hooks, instance) {
+        if (instance->hook == hook) {
+            instance->enabled = true;
+            return;
+        }
+    }
+
+    // No existing instance found, attach this new one
+    instance = (SANITYCHECK_CustomHookInstance *)pios_malloc(sizeof(SANITYCHECK_CustomHookInstance));
+    PIOS_Assert(instance);
+    instance->hook    = hook;
+    instance->next    = NULL;
+    instance->enabled = true;
+    LL_APPEND(hooks, instance);
+}
+
+void SANITYCHECK_DetachHook(SANITYCHECK_CustomHook_function *hook)
+{
+    if (!hooks) {
+        return;
+    }
+    SANITYCHECK_CustomHookInstance *instance = NULL;
+    LL_FOREACH(hooks, instance) {
+        if (instance->hook == hook) {
+            instance->enabled = false;
+            return;
+        }
+    }
+}