From e2196bca4f574c783f2b1883e7a9cec20e8283e1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 27 Dec 2012 18:36:37 +0100 Subject: [PATCH] Added position lock check --- apps/commander/commander.c | 20 ++++++++++++++++++++ apps/uORB/topics/vehicle_status.h | 5 +++-- 2 files changed, 23 insertions(+), 2 deletions(-) diff --git a/apps/commander/commander.c b/apps/commander/commander.c index b5df8a8b34..e0ee500dc2 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 ed3fed1abe..230521f536 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 */ };