Sub: add support for visual odometry

ArduSub/system.cpp

@@ -247,11 +247,24 @@ bool Sub::ekf_position_ok()
 // optflow_position_ok - returns true if optical flow based position estimate is ok
 bool Sub::optflow_position_ok()
 {
-#if OPTFLOW != ENABLED
-    return false;
-#else
-    // return immediately if optflow is not enabled or EKF not used
-    if (!optflow.enabled() || !ahrs.have_inertial_nav()) {
+    // return immediately if EKF not used
+    if (!ahrs.have_inertial_nav()) {
+        return false;
+    }
+
+    // return immediately if neither optflow nor visual odometry is enabled
+    bool enabled = false;
+#if OPTFLOW == ENABLED
+    if (optflow.enabled()) {
+        enabled = true;
+    }
+#endif
+#if HAL_VISUALODOM_ENABLED
+    if (visual_odom.enabled()) {
+        enabled = true;
+    }
+#endif
+    if (!enabled) {
         return false;
     }
 
@@ -263,7 +276,6 @@ bool Sub::optflow_position_ok()
         return (filt_status.flags.pred_horiz_pos_rel);
     }
     return (filt_status.flags.horiz_pos_rel && !filt_status.flags.const_pos_mode);
-#endif
 }
 
 /*