From fbaece0f884883ab991eb1648448ba249a320f23 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Sun, 17 Mar 2019 00:36:12 +0300 Subject: [PATCH] selfcheck.py: check VPE settings for EKF2 --- clever/src/selfcheck.py | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/clever/src/selfcheck.py b/clever/src/selfcheck.py index a4ad916f..9c264b86 100755 --- a/clever/src/selfcheck.py +++ b/clever/src/selfcheck.py @@ -157,8 +157,17 @@ def check_vpe(): failure('LPE_VIS_DELAY parameter is %s, but it should be zero', delay) rospy.loginfo('LPE_VIS_XY is %.2f m, LPE_VIS_Z is %.2f m', get_param('LPE_VIS_XY'), get_param('LPE_VIS_Z')) elif est == 2: - # TODO - pass + fuse = get_param('EKF2_AID_MASK') + if not fuse & (1 << 3): + failure('vision position fusing is disabled, change EKF2_AID_MASK parameter') + if not fuse & (1 << 4): + failure('vision yaw fusing is disabled, change EKF2_AID_MASK parameter') + delay = get_param('EKF2_EV_DELAY') + if delay != 0: + failure('EKF2_EV_DELAY is %.2f, but it should be zero', delay) + rospy.loginfo('EKF2_EVA_NOISE is %.3f, EKF2_EVP_NOISE is %.3f', + get_param('EKF2_EVA_NOISE'), + get_param('EKF2_EVP_NOISE')) # check vision pose and estimated pose inconsistency try: