From 81d39677ab9ccb48a5c3bda9dbbf5c503172cb52 Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Mon, 14 Nov 2022 09:50:01 +1100
Subject: [PATCH] AP_NavEKF2: honour AP_BEACON_ENABLED

if the DAL cuts its APIs out based on AP_BEACON_ENABLED we have to modify EKF2 to not use those APIs
---
 libraries/AP_NavEKF2/AP_NavEKF2_Logging.cpp      | 4 ++++
 libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp | 2 ++
 libraries/AP_NavEKF2/AP_NavEKF2_RngBcnFusion.cpp | 2 ++
 libraries/AP_NavEKF2/AP_NavEKF2_core.cpp         | 2 ++
 4 files changed, 10 insertions(+)

diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Logging.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Logging.cpp
index 9f92a2eb01..5f5d25f048 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2_Logging.cpp
+++ b/libraries/AP_NavEKF2/AP_NavEKF2_Logging.cpp
@@ -199,6 +199,7 @@ void NavEKF2_core::Log_Write_Quaternion(uint64_t time_us) const
     AP::logger().WriteBlock(&pktq1, sizeof(pktq1));
 }
 
+#if AP_BEACON_ENABLED
 void NavEKF2_core::Log_Write_Beacon(uint64_t time_us)
 {
     if (core_index != frontend->primary) {
@@ -248,6 +249,7 @@ void NavEKF2_core::Log_Write_Beacon(uint64_t time_us)
     AP::logger().WriteBlock(&pkt0, sizeof(pkt0));
     rngBcnFuseDataReportIndex++;
 }
+#endif  // AP_BEACON_ENABLED
 
 void NavEKF2_core::Log_Write_Timing(uint64_t time_us)
 {
@@ -314,8 +316,10 @@ void NavEKF2_core::Log_Write(uint64_t time_us)
     Log_Write_Quaternion(time_us);
     Log_Write_GSF(time_us);
 
+#if AP_BEACON_ENABLED
     // write range beacon fusion debug packet if the range value is non-zero
     Log_Write_Beacon(time_us);
+#endif
 
     Log_Write_Timing(time_us);
 }
diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
index e275a1ce75..db29459a7c 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
+++ b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
@@ -795,6 +795,7 @@ void NavEKF2_core::readAirSpdData()
 *              Range Beacon Measurements                *
 ********************************************************/
 
+#if AP_BEACON_ENABLED
 // check for new range beacon data and push to data buffer if available
 void NavEKF2_core::readRngBcnData()
 {
@@ -901,6 +902,7 @@ void NavEKF2_core::readRngBcnData()
     rngBcnDataToFuse = storedRangeBeacon.recall(rngBcnDataDelayed,imuDataDelayed.time_ms);
 
 }
+#endif  // AP_BEACON_ENABLED
 
 /*
   update timing statistics structure
diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_RngBcnFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_RngBcnFusion.cpp
index 57acfeee52..2479410e56 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2_RngBcnFusion.cpp
+++ b/libraries/AP_NavEKF2/AP_NavEKF2_RngBcnFusion.cpp
@@ -5,6 +5,7 @@
 *                   FUSE MEASURED_DATA                  *
 ********************************************************/
 
+#if AP_BEACON_ENABLED
 // select fusion of range beacon measurements
 void NavEKF2_core::SelectRngBcnFusion()
 {
@@ -570,3 +571,4 @@ void NavEKF2_core::CalcRangeBeaconPosDownOffset(ftype obsVar, Vector3F &vehicleP
 
 }
 
+#endif  // AP_BEACON_ENABLED
diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
index 45bcc369a3..a9c8b7d83f 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
+++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
@@ -573,8 +573,10 @@ void NavEKF2_core::UpdateFilter(bool predict)
         // Muat be run after SelectVelPosFusion() so that fresh GPS data is available
         runYawEstimatorCorrection();
 
+#if AP_BEACON_ENABLED
         // Update states using range beacon data
         SelectRngBcnFusion();
+#endif
 
         // Update states using optical flow data
         SelectFlowFusion();