From f033de21ea4a2c7bbd1f13c8f05ebf0fd4f36a47 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 16 Feb 2017 14:32:07 +0100 Subject: [PATCH] Commander: Fall back to GPS enabled modes once GPS becomes available --- src/modules/commander/commander.cpp | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index cd750783c0f0..b7c5976f8f1b 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -229,6 +229,7 @@ static uint64_t rc_signal_lost_timestamp; // Time at which the RC reception was static float avionics_power_rail_voltage; // voltage of the avionics power rail static bool can_arm_without_gps = false; +static bool _last_condition_global_position_valid = false; /** @@ -2706,6 +2707,9 @@ int commander_thread_main(int argc, char *argv[]) bool first_rc_eval = (_last_sp_man.timestamp == 0) && (sp_man.timestamp > 0); transition_result_t main_res = set_main_state_rc(&status); + /* store last position lock state */ + _last_condition_global_position_valid = status_flags.condition_global_position_valid; + /* play tune on mode change only if armed, blink LED always */ if (main_res == TRANSITION_CHANGED || first_rc_eval) { tune_positive(armed.armed); @@ -3267,7 +3271,9 @@ set_main_state_rc(struct vehicle_status_s *status_local) // feature, just in case offboard control goes crazy. /* manual setpoint has not updated, do not re-evaluate it */ - if (((_last_sp_man.timestamp != 0) && (_last_sp_man.timestamp == sp_man.timestamp)) || + if (!(!_last_condition_global_position_valid && + status_flags.condition_global_position_valid) + && (((_last_sp_man.timestamp != 0) && (_last_sp_man.timestamp == sp_man.timestamp)) || ((_last_sp_man.offboard_switch == sp_man.offboard_switch) && (_last_sp_man.return_switch == sp_man.return_switch) && (_last_sp_man.mode_switch == sp_man.mode_switch) && @@ -3275,7 +3281,7 @@ set_main_state_rc(struct vehicle_status_s *status_local) (_last_sp_man.rattitude_switch == sp_man.rattitude_switch) && (_last_sp_man.posctl_switch == sp_man.posctl_switch) && (_last_sp_man.loiter_switch == sp_man.loiter_switch) && - (_last_sp_man.mode_slot == sp_man.mode_slot))) { + (_last_sp_man.mode_slot == sp_man.mode_slot)))) { // update these fields for the geofence system