diff --git a/clover/src/selfcheck.py b/clover/src/selfcheck.py index 5c1711960..274594b79 100755 --- a/clover/src/selfcheck.py +++ b/clover/src/selfcheck.py @@ -107,7 +107,7 @@ def ff(value, precision=2): param_get = rospy.ServiceProxy('mavros/param/get', ParamGet) -def get_param(name, default=None): +def get_param(name, default=None, strict=True): try: res = param_get(param_id=name) except rospy.ServiceException as e: @@ -115,7 +115,8 @@ def get_param(name, default=None): return None if not res.success: - failure('unable to retrieve PX4 parameter %s', name) + if strict: + failure('unable to retrieve PX4 parameter %s', name) return default else: if res.value.integer != 0: @@ -501,11 +502,22 @@ def check_vpe(): failure('LPE_VIS_DELAY = %s, but it should be zero', delay) info('LPE_VIS_XY = %s m, LPE_VIS_Z = %s m', get_paramf('LPE_VIS_XY'), get_paramf('LPE_VIS_Z')) elif est == 2: - fuse = int(get_param('EKF2_AID_MASK')) - if not fuse & (1 << 3): - failure('vision position fusion is disabled, change EKF2_AID_MASK parameter') - if not fuse & (1 << 4): - failure('vision yaw fusion is disabled, change EKF2_AID_MASK parameter') + ev_ctrl = get_param('EKF2_EV_CTRL', strict=False) + if ev_ctrl is not None: # PX4 after v1.14 + ev_ctrl = int(ev_ctrl) + if not ev_ctrl & (1 << 0): + failure('vision horizontal position fusion is disabled, change EKF2_EV_CTRL parameter') + if not ev_ctrl & (1 << 1): + failure('vision vertical position fusion is disabled, change EKF2_EV_CTRL parameter') + if not ev_ctrl & (1 << 3): + failure('vision yaw fusion is disabled, change EKF2_EV_CTRL parameter') + else: # PX4 before v1.14 + fuse = int(get_param('EKF2_AID_MASK')) + if not fuse & (1 << 3): + failure('vision position fusion is disabled, change EKF2_AID_MASK parameter') + if not fuse & (1 << 4): + failure('vision yaw fusion is disabled, change EKF2_AID_MASK parameter') + delay = get_param('EKF2_EV_DELAY') if delay != 0: failure('EKF2_EV_DELAY = %.2f, but it should be zero', delay) @@ -612,8 +624,14 @@ def check_global_position(): rospy.wait_for_message('mavros/global_position/global', NavSatFix, timeout=0.8) except rospy.ROSException: info('no global position') - if get_param('SYS_MC_EST_GROUP') == 2 and (int(get_param('EKF2_AID_MASK', 0)) & (1 << 0)): - failure('enabled GPS fusion may suppress vision position aiding') + if get_param('SYS_MC_EST_GROUP') == 2: + gps_ctrl = get_param('EKF2_GPS_CTRL', strict=False) + if gps_ctrl is not None: # PX4 after v1.14 + if int(gps_ctrl) & (1 << 0): + failure('GPS fusion enabled may suppress vision position aiding, change EKF2_GPS_CTRL') + else: # PX4 before v1.14 + if int(get_param('EKF2_AID_MASK', 0)) & (1 << 0): + failure('GPS fusion enabled may suppress vision position aiding, change EKF2_AID_MASK') @check('Optical flow') @@ -646,9 +664,14 @@ def check_optical_flow(): get_paramf('LPE_FLW_R', 4), get_paramf('LPE_FLW_RR', 4)) elif est == 2: - fuse = int(get_param('EKF2_AID_MASK', 0)) - if not fuse & (1 << 1): - failure('optical flow fusion is disabled, change EKF2_AID_MASK parameter') + of_ctrl = get_param('EKF2_OF_CTRL', strict=False) + if of_ctrl is not None: # PX4 after v1.14 + if of_ctrl == 0: + failure('optical flow fusion is disabled, change EKF2_OF_CTRL') + else: # PX4 before v1.14 + fuse = int(get_param('EKF2_AID_MASK', 0)) + if not fuse & (1 << 1): + failure('optical flow fusion is disabled, change EKF2_AID_MASK parameter') delay = get_param('EKF2_OF_DELAY', 0) if delay != 0: failure('EKF2_OF_DELAY = %.2f, but it should be zero', delay) @@ -697,16 +720,19 @@ def check_rangefinder(): info('"pub agl as lpos down" in LPE_FUSION is enabled, operating over flat surface') elif est == 2: - hgt = get_param('EKF2_HGT_MODE') + hgt = get_param('EKF2_HGT_REF', strict=False) + if hgt is None: # PX4 before v1.14 + hgt = get_param('EKF2_HGT_MODE') if hgt != 2: info('EKF2_HGT_MODE != Range sensor, NOT operating over flat surface') else: info('EKF2_HGT_MODE = Range sensor, operating over flat surface') - aid = get_param('EKF2_RNG_AID') - if aid != 1: - info('EKF2_RNG_AID != 1, range sensor aiding disabled') - else: - info('EKF2_RNG_AID = 1, range sensor aiding enabled') + aid = get_param('EKF2_RNG_AID', strict=False) + if aid is not None: # PX4 before v1.14 + if aid != 1: + info('EKF2_RNG_AID != 1, range sensor aiding disabled') + else: + info('EKF2_RNG_AID = 1, range sensor aiding enabled') @check('Boot duration')