diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index f4c7a4091455abc6f2d845ff5651bf509b785a70..3993d5d8efff790b60262b339ede7f79ea6c7c09 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -165,6 +165,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_global_position_valid = false; static struct vehicle_land_detected_s land_detector = {}; @@ -2085,6 +2086,7 @@ Commander::run() transition_result_t main_res = set_main_state(status, &status_changed); /* store last position lock state */ + _last_condition_local_altitude_valid = status_flags.condition_local_altitude_valid; _last_condition_global_position_valid = status_flags.condition_global_position_valid; /* play tune on mode change only if armed, blink LED always */ @@ -2730,6 +2732,7 @@ 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; @@ -2747,6 +2750,7 @@ 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 || (rc_values_updated && some_switch_changed);