Added position lock check

2 files changed, 23 insertions, 2 deletions

diff --git a/apps/commander/commander.c b/apps/commander/commander.c
index b5df8a8b3..e0ee500dc 100644
--- a/apps/commander/commander.c
+++ b/apps/commander/commander.c
@@ -1589,6 +1589,26 @@ int commander_thread_main(int argc, char *argv[])
 			state_changed = true;
 		}
 
+		/*
+		 * Mark the position of the first position lock as return to launch (RTL)
+		 * position. The MAV will return here on command or emergency.
+		 *
+		 * Conditions:
+		 * 
+		 * 	1) The system aquired position lock just now
+		 *	2) The system has not aquired position lock before
+		 *	3) The system is not armed (on the ground)
+		 */
+		if (!current_status.flag_valid_launch_position &&
+			!vector_flight_mode_ok && current_status.flag_vector_flight_mode_ok &&
+			!current_status.flag_system_armed) {
+			/* first time a valid position, store it and emit it */
+
+			// XXX implement storage and publication of RTL position
+			current_status.flag_valid_launch_position = true;
+			current_status.flag_auto_flight_mode_ok = true;
+			state_changed = true;
+		}
 
 
 		/* Check if last transition deserved an audio event */
diff --git a/apps/uORB/topics/vehicle_status.h b/apps/uORB/topics/vehicle_status.h
index ed3fed1ab..230521f53 100644
--- a/apps/uORB/topics/vehicle_status.h
+++ b/apps/uORB/topics/vehicle_status.h
@@ -170,9 +170,10 @@ struct vehicle_status_s
 	uint16_t errors_count3;
 	uint16_t errors_count4;
 
-	bool flag_global_position_valid;     /**< set to true by the commander app if the quality of the gps signal is good enough to use it in the position estimator */
+	bool flag_global_position_valid;		/**< set to true by the commander app if the quality of the gps signal is good enough to use it in the position estimator */
 	bool flag_local_position_valid;
-	bool flag_vector_flight_mode_ok;			/**< position estimation, battery voltage and other critical subsystems are good for autonomous flight */
+	bool flag_vector_flight_mode_ok;		/**< position estimation, battery voltage and other critical subsystems are good for autonomous flight */
+	bool flag_auto_flight_mode_ok;			/**< conditions of vector flight mode apply plus a valid takeoff position lock has been aquired */
 	bool flag_external_manual_override_ok;		/**< external override non-fatal for system. Only true for fixed wing */
 	bool flag_valid_launch_position;		/**< indicates a valid launch position */
 };