From 91f8f255ee159e3c5e27b882a15a5f591ba2d512 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Fri, 16 Aug 2024 08:31:14 +1000
Subject: [PATCH] AP_NavEKF2: added EK2_OPTIONS

this allows for disabling of external navigation data (such as visual
odomotry) in EKF2. This is very useful when using EKF2 as a fallback
and truth reference when testing visual odomotry on EKF3
---
 libraries/AP_NavEKF2/AP_NavEKF2.cpp | 13 +++++++++----
 libraries/AP_NavEKF2/AP_NavEKF2.h   | 11 +++++++++++
 2 files changed, 20 insertions(+), 4 deletions(-)

diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.cpp b/libraries/AP_NavEKF2/AP_NavEKF2.cpp
index bc958e012aef6..05c3f41b9820a 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2.cpp
+++ b/libraries/AP_NavEKF2/AP_NavEKF2.cpp
@@ -598,6 +598,13 @@ const AP_Param::GroupInfo NavEKF2::var_info[] = {
     // @User: Advanced
     // @RebootRequired: True
     AP_GROUPINFO("GSF_RST_MAX", 57, NavEKF2, _gsfResetMaxCount, 2),
+
+    // @Param: OPTIONS
+    // @DisplayName: Optional EKF behaviour
+    // @Description: optional EKF2 behaviour. Disabling external navigation prevents use of external vision data in the EKF2 solution
+    // @Bitmask: 0:DisableExternalNavigation
+    // @User: Advanced
+    AP_GROUPINFO("OPTIONS",  58, NavEKF2, _options, 0),
     
     AP_GROUPEND
 };
@@ -1514,8 +1521,7 @@ void NavEKF2::updateLaneSwitchPosDownResetData(uint8_t new_primary, uint8_t old_
 void NavEKF2::writeExtNavData(const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint16_t delay_ms, uint32_t resetTime_ms)
 {
     AP::dal().writeExtNavData(pos, quat, posErr, angErr, timeStamp_ms, delay_ms, resetTime_ms);
-
-    if (core) {
+    if (!option_is_set(Option::DisableExternalNav) && core) {
         for (uint8_t i=0; i<num_cores; i++) {
             core[i].writeExtNavData(pos, quat, posErr, angErr, timeStamp_ms, delay_ms, resetTime_ms);
         }
@@ -1568,8 +1574,7 @@ void NavEKF2::writeDefaultAirSpeed(float airspeed)
 void NavEKF2::writeExtNavVelData(const Vector3f &vel, float err, uint32_t timeStamp_ms, uint16_t delay_ms)
 {
     AP::dal().writeExtNavVelData(vel, err, timeStamp_ms, delay_ms);
-
-    if (core) {
+    if (!option_is_set(Option::DisableExternalNav) && core) {
         for (uint8_t i=0; i<num_cores; i++) {
             core[i].writeExtNavVelData(vel, err, timeStamp_ms, delay_ms);
         }
diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.h b/libraries/AP_NavEKF2/AP_NavEKF2.h
index f15726f7355b0..dbf1a51dbc037 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2.h
+++ b/libraries/AP_NavEKF2/AP_NavEKF2.h
@@ -364,7 +364,18 @@ class NavEKF2 {
     AP_Int8 _gsfRunMask;            // mask controlling which EKF2 instances run a separate EKF-GSF yaw estimator
     AP_Int8 _gsfUseMask;            // mask controlling which EKF2 instances will use EKF-GSF yaw estimator data to assit with yaw resets
     AP_Int8 _gsfResetMaxCount;      // maximum number of times the EKF2 is allowed to reset it's yaw to the EKF-GSF estimate
+    AP_Int32 _options;              // optional behaviour bitmask
 
+    // enum for processing options
+    enum class Option {
+        DisableExternalNav     = (1U<<0),
+    };
+
+    // return true if an option is set
+    bool option_is_set(Option option) const {
+        return (uint32_t(option) & uint32_t(_options)) != 0;
+    }
+    
 // Possible values for _flowUse
 #define FLOW_USE_NONE    0
 #define FLOW_USE_NAV     1