From a16daa67163368910194a7964779d74f9d7109ae Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 29 May 2019 18:02:23 -0400 Subject: [PATCH] WIP: commander re-evaluate RC mode switch when local position becomes valid --- src/modules/commander/Commander.cpp | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 9ae8fe253fdb..f45eefd51f8e 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -160,6 +160,7 @@ static uint64_t rc_signal_lost_timestamp; // Time at which the RC reception was static uint8_t arm_requirements = ARM_REQ_NONE; static bool _last_condition_local_altitude_valid = false; +static bool _last_condition_local_position_valid = false; static bool _last_condition_global_position_valid = false; static struct vehicle_land_detected_s land_detector = {}; @@ -2024,6 +2025,7 @@ Commander::run() /* store last position lock state */ _last_condition_local_altitude_valid = status_flags.condition_local_altitude_valid; + _last_condition_local_position_valid = status_flags.condition_local_position_valid; _last_condition_global_position_valid = status_flags.condition_global_position_valid; /* play tune on mode change only if armed, blink LED always */ @@ -2595,10 +2597,11 @@ Commander::set_main_state_rc(const vehicle_status_s &status_local, bool *changed // we want to allow rc mode change to take precidence. This is a safety // feature, just in case offboard control goes crazy. - const bool altitude_got_valid = !_last_condition_local_altitude_valid && status_flags.condition_local_altitude_valid; - const bool position_got_valid = !_last_condition_global_position_valid && status_flags.condition_global_position_valid; - const bool first_time_rc = _last_sp_man.timestamp == 0; - const bool rc_values_updated = _last_sp_man.timestamp != sp_man.timestamp; + const bool altitude_got_valid = (!_last_condition_local_altitude_valid && status_flags.condition_local_altitude_valid); + const bool lpos_got_valid = (!_last_condition_local_position_valid && status_flags.condition_local_position_valid); + const bool gpos_got_valid = (!_last_condition_global_position_valid && status_flags.condition_global_position_valid); + const bool first_time_rc = (_last_sp_man.timestamp == 0); + const bool rc_values_updated = (_last_sp_man.timestamp != sp_man.timestamp); const bool some_switch_changed = (_last_sp_man.offboard_switch != sp_man.offboard_switch) || (_last_sp_man.return_switch != sp_man.return_switch) @@ -2614,7 +2617,8 @@ Commander::set_main_state_rc(const vehicle_status_s &status_local, bool *changed // only switch mode based on RC switch if necessary to also allow mode switching via MAVLink const bool should_evaluate_rc_mode_switch = first_time_rc || altitude_got_valid - || position_got_valid + || lpos_got_valid + || gpos_got_valid || (rc_values_updated && some_switch_changed); if (!should_evaluate_rc_mode_switch) {