From 21edc6aee158454651fdad15d47d467f2e35faae Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Mon, 26 Feb 2024 15:09:46 +1100
Subject: [PATCH] AP_NavEKF3: do not trust number of beacons to not change

if the count from the beacon library changes we may end up looking at memory we shouldn't
---
 libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp      |  3 ++-
 libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp | 11 ++++++++---
 libraries/AP_NavEKF3/AP_NavEKF3_core.h           |  1 +
 3 files changed, 11 insertions(+), 4 deletions(-)

diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp
index 1f60ae65f932c..93e3db98d5b90 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp
@@ -238,7 +238,8 @@ void NavEKF3_core::Log_Write_Beacon(uint64_t time_us)
     }
 
     // Ensure that beacons are not skipped due to calling this function at a rate lower than the updates
-    if (rngBcn.fuseDataReportIndex >= rngBcn.N) {
+    if (rngBcn.fuseDataReportIndex >= rngBcn.N ||
+        rngBcn.fuseDataReportIndex > rngBcn.numFusionReports) {
         rngBcn.fuseDataReportIndex = 0;
     }
 
diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp
index bc495582acf3e..6fc98f0814829 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp
@@ -39,9 +39,14 @@ void NavEKF3_core::BeaconFusion::InitialiseVariables()
     fuseDataReportIndex = 0;
     delete[] fusionReport;
     fusionReport = nullptr;
+    numFusionReports = 0;
     auto *beacon = AP::dal().beacon();
     if (beacon != nullptr) {
-        fusionReport = new BeaconFusion::FusionReport[beacon->count()];
+        const uint8_t count = beacon->count();
+        fusionReport = new BeaconFusion::FusionReport[count];
+        if (fusionReport != nullptr) {
+            numFusionReports = count;
+        }
     }
     posOffsetNED.zero();
     originEstInit = false;
@@ -311,7 +316,7 @@ void NavEKF3_core::FuseRngBcn()
         }
 
         // Update the fusion report
-        if (rngBcn.fusionReport && rngBcn.dataDelayed.beacon_ID < dal.beacon()->count()) {
+        if (rngBcn.dataDelayed.beacon_ID < rngBcn.numFusionReports) {
             auto &report = rngBcn.fusionReport[rngBcn.dataDelayed.beacon_ID];
             report.beaconPosNED = rngBcn.dataDelayed.beacon_posNED;
             report.innov = rngBcn.innov;
@@ -550,7 +555,7 @@ void NavEKF3_core::FuseRngBcnStatic()
             rngBcn.alignmentCompleted = true;
         }
         // Update the fusion report
-        if (rngBcn.fusionReport && rngBcn.dataDelayed.beacon_ID < dal.beacon()->count()) {
+        if (rngBcn.dataDelayed.beacon_ID < rngBcn.numFusionReports) {
             auto &report = rngBcn.fusionReport[rngBcn.dataDelayed.beacon_ID];
             report.beaconPosNED = rngBcn.dataDelayed.beacon_posNED;
             report.innov = rngBcn.innov;
diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h
index d9304f12383cf..a59de0a82dcc0 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h
@@ -1372,6 +1372,7 @@ class NavEKF3_core : public NavEKF_core_common
             ftype testRatio;    // innovation consistency test ratio
             Vector3F beaconPosNED; // beacon NED position
         } *fusionReport;
+        uint8_t numFusionReports;
     } rngBcn;
 #endif  // if EK3_FEATURE_BEACON_FUSION