From 63b08ec129312d94324215b9b55c77c4b9b40b3f Mon Sep 17 00:00:00 2001
From: Corvus Corax <corvuscorax@cybertrench.com>
Date: Sun, 1 Jun 2014 12:17:42 +0200
Subject: [PATCH] OP-1363 check for navigation enabled (outdoor) fusion
 algorithm if any flightmode is configured to use a path follower

---
 flight/libraries/sanitycheck.c | 40 +++++++++++++++++++++++++++++++++-
 1 file changed, 39 insertions(+), 1 deletion(-)

diff --git a/flight/libraries/sanitycheck.c b/flight/libraries/sanitycheck.c
index 03ab299df..36edd7358 100644
--- a/flight/libraries/sanitycheck.c
+++ b/flight/libraries/sanitycheck.c
@@ -37,6 +37,7 @@
 #include <flightmodesettings.h>
 #include <systemsettings.h>
 #include <systemalarms.h>
+#include <revosettings.h>
 #include <taskinfo.h>
 
 /****************************
@@ -61,8 +62,27 @@ int32_t configuration_check()
     const struct pios_board_info *bdinfo = &pios_board_info_blob;
     bool coptercontrol     = bdinfo->board_type == 0x04;
 
+    // Classify navigation capability
+#ifdef REVOLUTION
+    RevoSettingsInitialize();
+    uint8_t revoFusion;
+    RevoSettingsFusionAlgorithmGet(&revoFusion);
+    bool navCapableFusion;
+    switch (revoFusion) {
+    case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARYMAGGPSOUTDOOR:
+    case REVOSETTINGS_FUSIONALGORITHM_INS13GPSOUTDOOR:
+        navCapableFusion = true;
+        break;
+    default:
+        navCapableFusion = false;
+    }
+#else
+    const bool navCapableFusion = false;
+#endif
+
+
     // Classify airframe type
-    bool multirotor = true;
+    bool multirotor;
     uint8_t airframe_type;
 
     SystemSettingsAirframeTypeGet(&airframe_type);
@@ -126,6 +146,9 @@ int32_t configuration_check()
             } else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) {
                 // Revo supports Position Hold
                 severity = SYSTEMALARMS_ALARM_ERROR;
+            } else if (!navCapableFusion) {
+                // we need nav capable fusion algorithm
+                severity = SYSTEMALARMS_ALARM_ERROR;
             }
             break;
         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_LAND:
@@ -134,6 +157,9 @@ int32_t configuration_check()
             } else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) {
                 // Revo supports AutoLand Mode
                 severity = SYSTEMALARMS_ALARM_ERROR;
+            } else if (!navCapableFusion) {
+                // we need nav capable fusion algorithm
+                severity = SYSTEMALARMS_ALARM_ERROR;
             }
             break;
         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POI:
@@ -142,11 +168,20 @@ int32_t configuration_check()
             } else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) {
                 // Revo supports POI Mode
                 severity = SYSTEMALARMS_ALARM_ERROR;
+            } else if (!navCapableFusion) {
+                // we need nav capable fusion algorithm
+                severity = SYSTEMALARMS_ALARM_ERROR;
             }
             break;
         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_PATHPLANNER:
             if (coptercontrol) {
                 severity = SYSTEMALARMS_ALARM_ERROR;
+            } else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) {
+                // Revo supports PathPlanner
+                severity = SYSTEMALARMS_ALARM_ERROR;
+            } else if (!navCapableFusion) {
+                // we need nav capable fusion algorithm
+                severity = SYSTEMALARMS_ALARM_ERROR;
             } else {
                 // Revo supports PathPlanner and that must be OK or we are not sane
                 // PathPlan alarm is uninitialized if not running
@@ -164,6 +199,9 @@ int32_t configuration_check()
             } else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) {
                 // Revo supports ReturnToBase
                 severity = SYSTEMALARMS_ALARM_ERROR;
+            } else if (!navCapableFusion) {
+                // we need nav capable fusion algorithm
+                severity = SYSTEMALARMS_ALARM_ERROR;
             }
             break;
         default: