From f172c2fd036be67b845d72bbdfbb1241732db95e Mon Sep 17 00:00:00 2001
From: rishabsingh3003 <rishabh.singh100358@gmail.com>
Date: Thu, 10 Apr 2025 17:10:39 -0400
Subject: [PATCH] AP_NavEKF: Allow EK3 cores to run different sources

---
 libraries/AP_NavEKF/AP_NavEKF_Source.cpp | 62 ++++++++++++------------
 libraries/AP_NavEKF/AP_NavEKF_Source.h   | 36 ++++++++++----
 2 files changed, 57 insertions(+), 41 deletions(-)

diff --git a/libraries/AP_NavEKF/AP_NavEKF_Source.cpp b/libraries/AP_NavEKF/AP_NavEKF_Source.cpp
index ececde2d870..2608a14bced 100644
--- a/libraries/AP_NavEKF/AP_NavEKF_Source.cpp
+++ b/libraries/AP_NavEKF/AP_NavEKF_Source.cpp
@@ -134,8 +134,8 @@ const AP_Param::GroupInfo AP_NavEKF_Source::var_info[] = {
 
     // @Param: _OPTIONS
     // @DisplayName: EKF Source Options
-    // @Description: EKF Source Options
-    // @Bitmask: 0:FuseAllVelocities, 1:AlignExtNavPosWhenUsingOptFlow
+    // @Description: EKF Source Options. Bit 0: Fuse all velocity sources present in EK3_SRCx_VEL_. Bit 1: Align external navigation position when using optical flow. Bit 3: Use SRC per core. By default, EKF source selection is controlled via the EK3_SRC parameters, allowing only one source to be active at a time across all cores (switchable via MAVLink, Lua, or RC). Enabling this bit maps EKF core 1 to SRC1, core 2 to SRC2, etc., allowing each core to run independently with a dedicated source.
+    // @Bitmask: 0:FuseAllVelocities, 1:AlignExtNavPosWhenUsingOptFlow, 3: UsePerCoreEKFSources
     // @User: Advanced
     AP_GROUPINFO("_OPTIONS", 16, AP_NavEKF_Source, _options, (int16_t)SourceOptions::FUSE_ALL_VELOCITIES),
 
@@ -165,14 +165,14 @@ void AP_NavEKF_Source::setPosVelYawSourceSet(AP_NavEKF_Source::SourceSetSelectio
 }
 
 // true/false of whether velocity source should be used
-bool AP_NavEKF_Source::useVelXYSource(SourceXY velxy_source) const
+bool AP_NavEKF_Source::useVelXYSource(SourceXY velxy_source, uint8_t core_index) const
 {
-    if (velxy_source == _source_set[active_source_set].velxy) {
+    if (velxy_source == _source_set[getActiveSourceSet(core_index)].velxy) {
         return true;
     }
 
     // check for fuse all velocities
-    if (_options.get() & (uint16_t)(SourceOptions::FUSE_ALL_VELOCITIES)) {
+    if (option_is_set(SourceOptions::FUSE_ALL_VELOCITIES)) {
         for (uint8_t i=0; i<AP_NAKEKF_SOURCE_SET_MAX; i++) {
             if (_source_set[i].velxy == velxy_source) {
                 return true;
@@ -184,14 +184,14 @@ bool AP_NavEKF_Source::useVelXYSource(SourceXY velxy_source) const
     return false;
 }
 
-bool AP_NavEKF_Source::useVelZSource(SourceZ velz_source) const
+bool AP_NavEKF_Source::useVelZSource(SourceZ velz_source, uint8_t core_index) const
 {
-    if (velz_source == _source_set[active_source_set].velz) {
+    if (velz_source == _source_set[getActiveSourceSet(core_index)].velz) {
         return true;
     }
 
     // check for fuse all velocities
-    if (_options.get() & (uint16_t)(SourceOptions::FUSE_ALL_VELOCITIES)) {
+    if (option_is_set(SourceOptions::FUSE_ALL_VELOCITIES)) {
         for (uint8_t i=0; i<AP_NAKEKF_SOURCE_SET_MAX; i++) {
             if (_source_set[i].velz == velz_source) {
                 return true;
@@ -204,14 +204,14 @@ bool AP_NavEKF_Source::useVelZSource(SourceZ velz_source) const
 }
 
 // true if a velocity source is configured
-bool AP_NavEKF_Source::haveVelZSource() const
+bool AP_NavEKF_Source::haveVelZSource(uint8_t core_index) const
 {
-    if (_source_set[active_source_set].velz != SourceZ::NONE) {
+    if (_source_set[getActiveSourceSet(core_index)].velz != SourceZ::NONE) {
         return true;
     }
 
     // check for fuse all velocities
-    if (_options.get() & (uint16_t)(SourceOptions::FUSE_ALL_VELOCITIES)) {
+    if (option_is_set(SourceOptions::FUSE_ALL_VELOCITIES)) {
         for (uint8_t i=0; i<AP_NAKEKF_SOURCE_SET_MAX; i++) {
             if (_source_set[i].velz != SourceZ::NONE) {
                 return true;
@@ -224,30 +224,30 @@ bool AP_NavEKF_Source::haveVelZSource() const
 }
 
 // get yaw source
-AP_NavEKF_Source::SourceYaw AP_NavEKF_Source::getYawSource() const
+AP_NavEKF_Source::SourceYaw AP_NavEKF_Source::getYawSource(uint8_t core_index) const
 {
     // check for special case of disabled compasses
-    if ((_source_set[active_source_set].yaw == SourceYaw::COMPASS) && (AP::dal().compass().get_num_enabled() == 0)) {
+    if ((_source_set[getActiveSourceSet(core_index)].yaw == SourceYaw::COMPASS) && (AP::dal().compass().get_num_enabled() == 0)) {
         return SourceYaw::NONE;
     }
 
-    return _source_set[active_source_set].yaw;
+    return _source_set[getActiveSourceSet(core_index)].yaw;
 }
 
 // get pos Z source
-AP_NavEKF_Source::SourceZ AP_NavEKF_Source::getPosZSource() const
+AP_NavEKF_Source::SourceZ AP_NavEKF_Source::getPosZSource(uint8_t core_index) const
 {
 #ifdef HAL_BARO_ALLOW_INIT_NO_BARO
     // check for special case of missing baro
-    if ((_source_set[active_source_set].posz == SourceZ::BARO) && (AP::dal().baro().num_instances() == 0)) {
+    if ((_source_set[getActiveSourceSet(core_index)].posz == SourceZ::BARO) && (AP::dal().baro().num_instances() == 0)) {
         return SourceZ::NONE;
     }
 #endif
-    return _source_set[active_source_set].posz;
+    return _source_set[getActiveSourceSet(core_index)].posz;
 }
 
 // align position of inactive sources to ahrs
-void AP_NavEKF_Source::align_inactive_sources()
+void AP_NavEKF_Source::align_inactive_sources(uint8_t core_number)
 {
     // align visual odometry
 #if HAL_VISUALODOM_ENABLED
@@ -259,9 +259,9 @@ void AP_NavEKF_Source::align_inactive_sources()
 
     // consider aligning ExtNav XY position:
     bool align_posxy = false;
-    if ((getPosXYSource() == SourceXY::GPS) ||
-        (getPosXYSource() == SourceXY::BEACON) ||
-        ((getVelXYSource() == SourceXY::OPTFLOW) && option_is_set(SourceOptions::ALIGN_EXTNAV_POS_WHEN_USING_OPTFLOW))) {
+    if ((getPosXYSource(core_number) == SourceXY::GPS) ||
+        (getPosXYSource(core_number) == SourceXY::BEACON) ||
+        ((getVelXYSource(core_number) == SourceXY::OPTFLOW) && option_is_set(SourceOptions::ALIGN_EXTNAV_POS_WHEN_USING_OPTFLOW))) {
         // align ExtNav position if active source is GPS, Beacon or (optionally) Optflow
         for (uint8_t i=0; i<AP_NAKEKF_SOURCE_SET_MAX; i++) {
             if (_source_set[i].posxy == SourceXY::EXTNAV) {
@@ -274,10 +274,10 @@ void AP_NavEKF_Source::align_inactive_sources()
 
     // consider aligning Z position:
     bool align_posz = false;
-    if ((getPosZSource() == SourceZ::BARO) ||
-        (getPosZSource() == SourceZ::RANGEFINDER) ||
-        (getPosZSource() == SourceZ::GPS) ||
-        (getPosZSource() == SourceZ::BEACON)) {
+    if ((getPosZSource(core_number) == SourceZ::BARO) ||
+        (getPosZSource(core_number) == SourceZ::RANGEFINDER) ||
+        (getPosZSource(core_number) == SourceZ::GPS) ||
+        (getPosZSource(core_number) == SourceZ::BEACON)) {
         // ExtNav is not the active source; we do not want to align active source!
         for (uint8_t i=0; i<AP_NAKEKF_SOURCE_SET_MAX; i++) {
             if (_source_set[i].posz == SourceZ::EXTNAV) {
@@ -292,13 +292,13 @@ void AP_NavEKF_Source::align_inactive_sources()
 }
 
 // sensor specific helper functions
-bool AP_NavEKF_Source::usingGPS() const
+bool AP_NavEKF_Source::usingGPS(uint8_t core_index) const
 {
-    return getPosXYSource() == SourceXY::GPS ||
-           getPosZSource() == SourceZ::GPS ||
-           getVelXYSource() == SourceXY::GPS ||
-           getVelZSource() == SourceZ::GPS ||
-           getYawSource() == SourceYaw::GSF;
+    return getPosXYSource(core_index) == SourceXY::GPS ||
+           getPosZSource(core_index) == SourceZ::GPS ||
+           getVelXYSource(core_index) == SourceXY::GPS ||
+           getVelZSource(core_index) == SourceZ::GPS ||
+           getYawSource(core_index) == SourceYaw::GSF;
 }
 
 // true if some parameters have been configured (used during parameter conversion)
diff --git a/libraries/AP_NavEKF/AP_NavEKF_Source.h b/libraries/AP_NavEKF/AP_NavEKF_Source.h
index 8ceb8d1ab4f..8b1ecd7170d 100644
--- a/libraries/AP_NavEKF/AP_NavEKF_Source.h
+++ b/libraries/AP_NavEKF/AP_NavEKF_Source.h
@@ -48,7 +48,9 @@ public:
     // enum for OPTIONS parameter
     enum class SourceOptions {
         FUSE_ALL_VELOCITIES = (1 << 0),                 // fuse all velocities configured in source sets
-        ALIGN_EXTNAV_POS_WHEN_USING_OPTFLOW = (1 << 1)  // align position of inactive sources to ahrs when using optical flow
+        ALIGN_EXTNAV_POS_WHEN_USING_OPTFLOW = (1 << 1),  // align position of inactive sources to ahrs when using optical flow
+        // reserved = (1 << 2),                          // reserved for future use
+        SRC_PER_CORE = (1 << 3)                         // use a separate source set for each core
     };
 
     enum class SourceSetSelection : uint8_t {
@@ -60,27 +62,41 @@ public:
     // initialisation
     void init();
 
+    // This function will get the active source set or get the source set for the core index if SRC_PER_CORE SourceOption is set
+    uint8_t getActiveSourceSet(uint8_t core_index) const {
+        // check if we are using a separate source set for each core
+        if (option_is_set(SourceOptions::SRC_PER_CORE)) {
+            if (core_index >= AP_NAKEKF_SOURCE_SET_MAX) {
+                // we need to return a valid source set
+                return active_source_set;
+            }
+            return core_index;
+        }
+        return active_source_set;
+     }
+
     // get current position source
-    SourceXY getPosXYSource() const { return _source_set[active_source_set].posxy; }
-    SourceZ getPosZSource() const;
+    SourceXY getPosXYSource(uint8_t core_index) const { return _source_set[getActiveSourceSet(core_index)].posxy; }
+
+    SourceZ getPosZSource(uint8_t core_index) const;
 
     // set position, velocity and yaw sources to either 0=primary, 1=secondary, 2=tertiary
     void setPosVelYawSourceSet(SourceSetSelection source_set_idx);
     uint8_t getPosVelYawSourceSet() const { return active_source_set; }
 
     // get/set velocity source
-    SourceXY getVelXYSource() const { return _source_set[active_source_set].velxy; }
-    SourceZ getVelZSource() const { return _source_set[active_source_set].velz; }
+    SourceXY getVelXYSource(uint8_t core_index) const { return _source_set[getActiveSourceSet(core_index)].velxy; }
+    SourceZ getVelZSource(uint8_t core_index) const { return _source_set[getActiveSourceSet(core_index)].velz; }
 
     // true/false of whether velocity source should be used
-    bool useVelXYSource(SourceXY velxy_source) const;
-    bool useVelZSource(SourceZ velz_source) const;
+    bool useVelXYSource(SourceXY velxy_source, uint8_t core_index) const;
+    bool useVelZSource(SourceZ velz_source, uint8_t core_index) const;
 
     // true if a velocity source is configured
-    bool haveVelZSource() const;
+    bool haveVelZSource(uint8_t core_index) const;
 
     // get yaw source
-    SourceYaw getYawSource() const;
+    SourceYaw getYawSource(uint8_t core_index) const;
 
     // align position of inactive sources to ahrs
     void align_inactive_sources();
@@ -88,7 +104,7 @@ public:
     // sensor-specific helper functions
 
     // true if any source is GPS
-    bool usingGPS() const;
+    bool usingGPS(uint8_t core_index) const;
 
     // true if source parameters have been configured (used for parameter conversion)
     bool configured();