fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-03 00:00:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

commander: do not check global position when using flow in POSCTL

Browse Source
This commit is contained in:
Paul Riseborough
2018-05-11 10:13:00 +10:00
committed by Lorenz Meier
parent 2c325414f9
commit 9fb674929c
1 changed files with 11 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+11 -6
View File
@@ -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);
}