mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-17 00:03:34 +08:00
commander: extend local position 'relaxed' validity
Relaxed position is valid as long as a velocity aiding source is active (e.g.: optical flow or airspeed+sideslip)
This commit is contained in:
committed by
Mathieu Bresciani
parent
f04aa2494b
commit
8b12b7c641
@@ -533,19 +533,6 @@ void EstimatorChecks::checkEstimatorStatusFlags(const Context &context, Report &
|
||||
estimator_status_flags_s estimator_status_flags;
|
||||
|
||||
if (_estimator_status_flags_sub.copy(&estimator_status_flags)) {
|
||||
|
||||
bool dead_reckoning = estimator_status_flags.cs_wind_dead_reckoning
|
||||
|| estimator_status_flags.cs_inertial_dead_reckoning;
|
||||
|
||||
if (!dead_reckoning) {
|
||||
// position requirements (update if not dead reckoning)
|
||||
bool gps = estimator_status_flags.cs_gps;
|
||||
bool optical_flow = estimator_status_flags.cs_opt_flow;
|
||||
bool vision_position = estimator_status_flags.cs_ev_pos;
|
||||
|
||||
_position_reliant_on_optical_flow = !gps && optical_flow && !vision_position;
|
||||
}
|
||||
|
||||
// Check for a magnetometer fault and notify the user
|
||||
if (estimator_status_flags.cs_mag_fault) {
|
||||
/* EVENT
|
||||
@@ -722,15 +709,6 @@ void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_f
|
||||
// Check if quality checking of position accuracy and consistency is to be performed
|
||||
const float lpos_eph_threshold = (_param_com_pos_fs_eph.get() < 0) ? INFINITY : _param_com_pos_fs_eph.get();
|
||||
|
||||
float lpos_eph_threshold_relaxed = lpos_eph_threshold;
|
||||
|
||||
// Set the allowable position uncertainty based on combination of flight and estimator state
|
||||
// When we are in a operator demanded position control mode and are solely reliant on optical flow,
|
||||
// do not check position error because it will gradually increase throughout flight and the operator will compensate for the drift
|
||||
if (_position_reliant_on_optical_flow) {
|
||||
lpos_eph_threshold_relaxed = INFINITY;
|
||||
}
|
||||
|
||||
bool xy_valid = lpos.xy_valid && !_nav_test_failed;
|
||||
bool v_xy_valid = lpos.v_xy_valid && !_nav_test_failed;
|
||||
|
||||
@@ -793,6 +771,10 @@ void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_f
|
||||
!checkPosVelValidity(now, xy_valid, lpos.eph, lpos_eph_threshold, lpos.timestamp,
|
||||
_last_lpos_fail_time_us, !failsafe_flags.local_position_invalid);
|
||||
|
||||
|
||||
// In some modes we assume that the operator will compensate for the drift so we do not need to check the position error
|
||||
const float lpos_eph_threshold_relaxed = INFINITY;
|
||||
|
||||
failsafe_flags.local_position_invalid_relaxed =
|
||||
!checkPosVelValidity(now, xy_valid, lpos.eph, lpos_eph_threshold_relaxed, lpos.timestamp,
|
||||
_last_lpos_relaxed_fail_time_us, !failsafe_flags.local_position_invalid_relaxed);
|
||||
|
||||
@@ -100,8 +100,6 @@ private:
|
||||
bool _nav_test_passed{false}; ///< true if the post takeoff navigation test has passed
|
||||
bool _nav_test_failed{false}; ///< true if the post takeoff navigation test has failed
|
||||
|
||||
bool _position_reliant_on_optical_flow{false};
|
||||
|
||||
bool _gps_was_fused{false};
|
||||
bool _gnss_spoofed{false};
|
||||
|
||||
|
||||
Reference in New Issue
Block a user