diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 7ad664ec8a..5ec49c26f4 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -187,6 +187,7 @@ static bool _last_condition_global_position_valid = false; static struct vehicle_land_detected_s land_detector = {}; static float _eph_threshold_adj = INFINITY; ///< maximum allowable horizontal position uncertainty after adjustment for flight condition +static bool _skip_pos_accuracy_check = false; /** * The daemon app only briefly exists to start @@ -1759,8 +1760,8 @@ Commander::run() && !(estimator_status.control_mode_flags & (1 << estimator_status_s::CS_GPS)) && !(estimator_status.control_mode_flags & (1 << estimator_status_s::CS_EV_POS))); bool operator_controlled_position = (internal_state.main_state == commander_state_s::MAIN_STATE_POSCTL); - bool skip_pos_accuracy_check = reliant_on_opt_flow && operator_controlled_position; - if (skip_pos_accuracy_check) { + _skip_pos_accuracy_check = reliant_on_opt_flow && operator_controlled_position; + if (_skip_pos_accuracy_check) { _eph_threshold_adj = INFINITY; } else { _eph_threshold_adj = _eph_threshold.get(); @@ -1831,9 +1832,11 @@ Commander::run() status_flags.condition_local_velocity_valid = false; } else { - // use global position message to determine validity - const vehicle_global_position_s&global_position = _global_position_sub.get(); - check_posvel_validity(true, global_position.eph, _eph_threshold_adj, global_position.timestamp, &_last_gpos_fail_time_us, &_gpos_probation_time_us, &status_flags.condition_global_position_valid, &status_changed); + if (!_skip_pos_accuracy_check) { + // use global position message to determine validity + const vehicle_global_position_s&global_position = _global_position_sub.get(); + check_posvel_validity(true, global_position.eph, _eph_threshold_adj, global_position.timestamp, &_last_gpos_fail_time_us, &_gpos_probation_time_us, &status_flags.condition_global_position_valid, &status_changed); + } // use local position message to determine validity const vehicle_local_position_s &local_position = _local_position_sub.get(); @@ -3352,7 +3355,9 @@ Commander::reset_posvel_validity(bool *changed) const vehicle_global_position_s &global_position = _global_position_sub.get(); // recheck validity - check_posvel_validity(true, global_position.eph, _eph_threshold_adj, global_position.timestamp, &_last_gpos_fail_time_us, &_gpos_probation_time_us, &status_flags.condition_global_position_valid, changed); + if (!_skip_pos_accuracy_check) { + check_posvel_validity(true, global_position.eph, _eph_threshold_adj, global_position.timestamp, &_last_gpos_fail_time_us, &_gpos_probation_time_us, &status_flags.condition_global_position_valid, changed); + } check_posvel_validity(local_position.xy_valid, local_position.eph, _eph_threshold_adj, local_position.timestamp, &_last_lpos_fail_time_us, &_lpos_probation_time_us, &status_flags.condition_local_position_valid, changed); check_posvel_validity(local_position.v_xy_valid, local_position.evh, _evh_threshold.get(), local_position.timestamp, &_last_lvel_fail_time_us, &_lvel_probation_time_us, &status_flags.condition_local_velocity_valid, changed); }