mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-03 16:48:52 +08:00
ekf-checks: set min and max values for required/failsafe eph
This commit is contained in:
@@ -739,7 +739,9 @@ void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_f
|
||||
|
||||
// run position and velocity accuracy checks
|
||||
// Check if quality checking of position accuracy and consistency is to be performed
|
||||
float lpos_eph_threshold_relaxed = _param_com_pos_fs_eph.get();
|
||||
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,
|
||||
@@ -762,11 +764,11 @@ void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_f
|
||||
}
|
||||
|
||||
failsafe_flags.global_position_invalid =
|
||||
!checkPosVelValidity(now, xy_valid, gpos.eph, _param_com_pos_fs_eph.get(), gpos.timestamp,
|
||||
!checkPosVelValidity(now, xy_valid, gpos.eph, lpos_eph_threshold, gpos.timestamp,
|
||||
_last_gpos_fail_time_us, !failsafe_flags.global_position_invalid);
|
||||
|
||||
failsafe_flags.local_position_invalid =
|
||||
!checkPosVelValidity(now, xy_valid, lpos.eph, _param_com_pos_fs_eph.get(), lpos.timestamp,
|
||||
!checkPosVelValidity(now, xy_valid, lpos.eph, lpos_eph_threshold, lpos.timestamp,
|
||||
_last_lpos_fail_time_us, !failsafe_flags.local_position_invalid);
|
||||
|
||||
failsafe_flags.local_position_invalid_relaxed =
|
||||
|
||||
@@ -708,8 +708,11 @@ PARAM_DEFINE_INT32(COM_POS_FS_DELAY, 1);
|
||||
* If the previous position error was below this threshold, there is an additional
|
||||
* factor of 2.5 applied (threshold for invalidation 2.5 times the one for validation).
|
||||
*
|
||||
* Set to -1 to disable.
|
||||
*
|
||||
* @unit m
|
||||
* @min 0
|
||||
* @min -1
|
||||
* @max 400
|
||||
* @decimal 1
|
||||
* @group Commander
|
||||
*/
|
||||
@@ -1076,8 +1079,9 @@ PARAM_DEFINE_FLOAT(COM_WIND_MAX, -1.f);
|
||||
*
|
||||
* Set to -1 to disable.
|
||||
*
|
||||
* @group Commander
|
||||
* @min -1
|
||||
* @max 1000
|
||||
* @group Commander
|
||||
* @unit m
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(COM_POS_LOW_EPH, -1.0f);
|
||||
|
||||
Reference in New Issue
Block a user