CopterExpress/clover
1
0
Fork 0
mirror of https://github.com/CopterExpress/clover.git synced 2026-05-26 21:19:35 +00:00
Code Issues Packages Projects Releases Wiki Activity

selfcheck.py: support PX4 v1.14 EKF2 aiding parameters change

Browse Source 
EKF2_AID_MASK has been split (EKF2_EV_CTRL, EKF2_GPS_CTRL, EKF2_OF_CTRL)
EKF2_HGT_MODE renamed to EKF2_HGT_REF
EKF2_RNG_AID is removed
This commit is contained in:
Oleg Kalachev
2023-10-10 08:25:27 +03:00
parent 7ca0ede1d7
commit 29f01c25e0
1 changed files with 44 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files

62
clover/src/selfcheck.py
View File

@@ -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')