From fe6496a04dd0a232bb530f57031cfb4f6e65bb44 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 19 Dec 2012 14:20:40 +0100 Subject: Correctly do position lock led signalling on IO and position lock measurement on FMU, tested with HIL. --- apps/commander/commander.c | 28 ++++++++++++++++++++++++++-- 1 file changed, 26 insertions(+), 2 deletions(-) (limited to 'apps/commander/commander.c') diff --git a/apps/commander/commander.c b/apps/commander/commander.c index f61fd053c..5dfdf83ad 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -1476,21 +1476,45 @@ int commander_thread_main(int argc, char *argv[]) /* End battery voltage check */ + + /* + * Check for valid position information. + * + * If the system has a valid position source from an onboard + * position estimator, it is safe to operate it autonomously. + * The flag_vector_flight_mode_ok flag indicates that a minimum + * set of position measurements is available. + */ + + /* store current state to reason later about a state change */ bool vector_flight_mode_ok = current_status.flag_vector_flight_mode_ok; bool global_pos_valid = current_status.flag_global_position_valid; bool local_pos_valid = current_status.flag_local_position_valid; /* check for global or local position updates, set a timeout of 2s */ if (hrt_absolute_time() - last_global_position_time < 2000000) { - current_status.flag_vector_flight_mode_ok = true; current_status.flag_global_position_valid = true; // XXX check for controller status and home position as well + } else { + current_status.flag_global_position_valid = false; } if (hrt_absolute_time() - last_local_position_time < 2000000) { - current_status.flag_vector_flight_mode_ok = true; current_status.flag_local_position_valid = true; // XXX check for controller status and home position as well + } else { + current_status.flag_local_position_valid = false; + } + + /* + * Consolidate global position and local position valid flags + * for vector flight mode. + */ + if (current_status.flag_local_position_valid || + current_status.flag_global_position_valid) { + current_status.flag_vector_flight_mode_ok = true; + } else { + current_status.flag_vector_flight_mode_ok = false; } /* consolidate state change, flag as changed if required */ -- cgit v1.2.3