From a66b1b9d04c6c200879576e2f4d9609e229606c0 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Thu, 11 Jun 2015 17:02:11 +0200
Subject: [PATCH] Improve feedback when auto mode is rejected due to a non
 suitable mission

---
 src/modules/navigator/mission.cpp             |  4 +--
 .../navigator/mission_feasibility_checker.cpp | 25 +++++++++----------
 .../navigator/mission_feasibility_checker.h   |  2 +-
 src/modules/navigator/navigator_main.cpp      |  5 +++-
 4 files changed, 19 insertions(+), 17 deletions(-)

diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
index 4eaeca1d8e..13fcd5daca 100644
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -230,7 +230,7 @@ Mission::update_offboard_mission()
 		 * however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */
 		dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id);
 
-		failed = !_missionFeasiblityChecker.checkMissionFeasible(_navigator->get_vstatus()->is_rotary_wing,
+		failed = !_missionFeasiblityChecker.checkMissionFeasible(_navigator->get_mavlink_fd(), _navigator->get_vstatus()->is_rotary_wing,
 				dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(),
 				_navigator->get_home_position()->alt, _navigator->home_position_valid());
 
@@ -374,7 +374,7 @@ Mission::set_mission_items()
 	} else if (home_dist_ok && read_mission_item(false, true, &_mission_item)) {
 		/* if mission type changed, notify */
 		if (_mission_type != MISSION_TYPE_OFFBOARD) {
-			mavlink_log_critical(_navigator->get_mavlink_fd(), "offboard mission now running");
+			mavlink_log_info(_navigator->get_mavlink_fd(), "offboard mission now running");
 		}
 		_mission_type = MISSION_TYPE_OFFBOARD;
 	} else {
diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp
index 19baa79fcd..9d1dc7c7e6 100644
--- a/src/modules/navigator/mission_feasibility_checker.cpp
+++ b/src/modules/navigator/mission_feasibility_checker.cpp
@@ -63,26 +63,27 @@ MissionFeasibilityChecker::MissionFeasibilityChecker() : _mavlink_fd(-1), _capab
 }
 
 
-bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid)
+bool MissionFeasibilityChecker::checkMissionFeasible(int mavlink_fd, bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid)
 {
 	bool failed = false;
 	/* Init if not done yet */
 	init();
 
-	/* Open mavlink fd */
-	if (_mavlink_fd < 0) {
-		/* try to open the mavlink log device every once in a while */
-		_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
-	}
+	_mavlink_fd = mavlink_fd;
 
 	// check if all mission item commands are supported
 	failed |= !checkMissionItemValidity(dm_current, nMissionItems);
 
 
-	if (isRotarywing)
+	if (isRotarywing) {
 		failed |= !checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence, home_alt, home_valid);
-	else
+	} else {
 		failed |= !checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence, home_alt, home_valid);
+	}
+
+	if (!failed) {
+		mavlink_log_info(_mavlink_fd, "Mission checked and ready.");
+	}
 
 	return !failed;
 }
@@ -152,11 +153,10 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current,
 			}
 		}
 
+		/* always reject relative alt without home set */
 		if (missionitem.altitude_is_relative && !home_valid) {
-			if (throw_error) {
-				mavlink_log_critical(_mavlink_fd, "Rejecting Mission: No home pos, WP %d uses rel alt", i);
-				return false;
-			}
+			mavlink_log_critical(_mavlink_fd, "Rejecting Mission: No home pos, WP %d uses rel alt", i);
+			return false;
 		}
 
 		/* calculate the global waypoint altitude */
@@ -204,7 +204,6 @@ bool MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, s
 			return false;
 		}
 	}
-	mavlink_log_info(_mavlink_fd, "Mission ready.");
 	return true;
 }
 
diff --git a/src/modules/navigator/mission_feasibility_checker.h b/src/modules/navigator/mission_feasibility_checker.h
index 964bd6097d..9c9511be3d 100644
--- a/src/modules/navigator/mission_feasibility_checker.h
+++ b/src/modules/navigator/mission_feasibility_checker.h
@@ -79,7 +79,7 @@ public:
 	/*
 	 * Returns true if mission is feasible and false otherwise
 	 */
-	bool checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence,
+	bool checkMissionFeasible(int mavlink_fd, bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence,
 				  float home_alt, bool home_valid);
 
 };
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 97baa1235f..8cfce50879 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -209,7 +209,10 @@ Navigator::home_position_update()
 
 	if (updated) {
 		orb_copy(ORB_ID(home_position), _home_pos_sub, &_home_pos);
-		_home_position_set = true;
+
+		if (_home_pos.timestamp > 0) {
+			_home_position_set = true;
+		}
 	}
 }