From 76ee48bc441565850b45460be73f3ac5168674dd Mon Sep 17 00:00:00 2001
From: Corvus Corax <corvuscorax@cybertrench.com>
Date: Sun, 12 Jan 2014 21:37:40 +0100
Subject: [PATCH] OP-1122 safer sanity checks and arming prevention in case of
 invalid flight plan

---
 flight/libraries/sanitycheck.c           | 13 ++++++++++---
 flight/modules/PathPlanner/pathplanner.c | 22 ++++++++++++++++------
 2 files changed, 26 insertions(+), 9 deletions(-)

diff --git a/flight/libraries/sanitycheck.c b/flight/libraries/sanitycheck.c
index 51442f48e..7efaa057b 100644
--- a/flight/libraries/sanitycheck.c
+++ b/flight/libraries/sanitycheck.c
@@ -35,6 +35,7 @@
 // UAVOs
 #include <manualcontrolsettings.h>
 #include <systemsettings.h>
+#include <systemalarms.h>
 #include <taskinfo.h>
 
 /****************************
@@ -151,9 +152,15 @@ int32_t configuration_check()
         case MANUALCONTROLSETTINGS_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 {
+                // Revo supports PathPlanner and that must be OK or we are not sane
+                // PathPlan alarm is uninitialized if not running
+                // PathPlan alarm is warning or error if the flightplan is invalid
+                SystemAlarmsAlarmData alarms;
+                SystemAlarmsAlarmGet(&alarms);
+                if (alarms.PathPlan != SYSTEMALARMS_ALARM_OK) {
+                    severity = SYSTEMALARMS_ALARM_ERROR;
+                }
             }
             break;
         case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_RETURNTOBASE:
diff --git a/flight/modules/PathPlanner/pathplanner.c b/flight/modules/PathPlanner/pathplanner.c
index 0572f86ed..25bc3d97e 100644
--- a/flight/modules/PathPlanner/pathplanner.c
+++ b/flight/modules/PathPlanner/pathplanner.c
@@ -135,16 +135,20 @@ static void pathPlannerTask()
     // check flight plan validity early to raise alarm
     // even if not in guided mode
     uint8_t validFlightPlan = checkFlightPlan();
-    if (!validFlightPlan) {
-        AlarmsSet(SYSTEMALARMS_ALARM_PATHPLAN, SYSTEMALARMS_ALARM_ERROR);
-    } else {
-        AlarmsClear(SYSTEMALARMS_ALARM_PATHPLAN);
-    }
 
     FlightStatusData flightStatus;
     FlightStatusGet(&flightStatus);
     if (flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) {
         pathplanner_active = false;
+        if (!validFlightPlan) {
+            // unverified flight plans are only a warning while we are not in pathplanner mode
+            // so it does not prevent arming. However manualcontrols safety check
+            // shall test for this warning when pathplan is on the flight mode selector
+            // thus a valid flight plan is a prerequirement for arming
+            AlarmsSet(SYSTEMALARMS_ALARM_PATHPLAN, SYSTEMALARMS_ALARM_WARNING);
+        } else {
+            AlarmsClear(SYSTEMALARMS_ALARM_PATHPLAN);
+        }
 
         return;
     }
@@ -156,7 +160,9 @@ static void pathPlannerTask()
     if (!validFlightPlan) {
         // At this point the craft is in PathPlanner mode, so pilot has no manual control capability.
         // Failsafe: behave as if in return to base mode
-        // what to do in this case is debatable, it might be better to just make a forced disarming but rth has a higher chance of survival when in flight.
+        // what to do in this case is debatable, it might be better to just
+        // make a forced disarming but rth has a higher chance of survival when
+        // in flight.
         pathplanner_active = false;
 
         if (!failsafeRTHset) {
@@ -178,8 +184,12 @@ static void pathPlannerTask()
             pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
             PathDesiredSet(&pathDesired);
         }
+        AlarmsSet(SYSTEMALARMS_ALARM_PATHPLAN, SYSTEMALARMS_ALARM_ERROR);
+
+        return;
     }
     failsafeRTHset = 0;
+    AlarmsClear(SYSTEMALARMS_ALARM_PATHPLAN);
 
     WaypointActiveGet(&waypointActive);