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

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

clover/src/selfcheck.py

@@ -107,15 +107,16 @@ 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:
         failure('%s: %s', name, str(e))
         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')