ekf-checks: set min and max values for required/failsafe eph

This commit is contained in:
bresch
2023-07-24 15:10:07 -04:00
committed by Daniel Agar
parent 30bfe0d379
commit beb5fc5eb6
2 changed files with 11 additions and 5 deletions
@@ -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 =
+6 -2
View File
@@ -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);