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
ArduPilot · Feb 28, 2024 · 21edc6a · 21edc6a
1 parent a517d5f
commit 21edc6a
Show file tree

Hide file tree

Showing 3 changed files with 11 additions and 4 deletions.
diff --git 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
@@ -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
@@ -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