AP_NavEKF3: save memory on beacon debug structure

only allocate when beacons are enabled. This relies on a guarantee in
the DAL that the beacon count cannot change

libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp

@@ -217,7 +217,7 @@ void NavEKF3_core::Log_Write_Beacon(uint64_t time_us)
         return;
     }
 
-    if (!statesInitialised || N_beacons == 0) {
+    if (!statesInitialised || N_beacons == 0 || rngBcnFusionReport == nullptr) {
         return;
     }
 

libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp

@@ -839,7 +839,6 @@ void NavEKF3_core::readRngBcnData()
 {
     // check that arrays are large enough
     static_assert(ARRAY_SIZE(lastTimeRngBcn_ms) >= AP_BEACON_MAX_BEACONS, "lastTimeRngBcn_ms should have at least AP_BEACON_MAX_BEACONS elements");
-    static_assert(ARRAY_SIZE(rngBcnFusionReport) >= AP_BEACON_MAX_BEACONS, "rngBcnFusionReport should have at least AP_BEACON_MAX_BEACONS elements");
 
     // get the location of the beacon data
     const AP_DAL_Beacon *beacon = dal.beacon();

libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp

@@ -260,11 +260,13 @@ void NavEKF3_core::FuseRngBcn()
         }
 
         // Update the fusion report
-        rngBcnFusionReport[rngBcnDataDelayed.beacon_ID].beaconPosNED = rngBcnDataDelayed.beacon_posNED;
+        if (rngBcnFusionReport && rngBcnDataDelayed.beacon_ID < dal.beacon()->count()) {
-        rngBcnFusionReport[rngBcnDataDelayed.beacon_ID].innov = innovRngBcn;
+            rngBcnFusionReport[rngBcnDataDelayed.beacon_ID].beaconPosNED = rngBcnDataDelayed.beacon_posNED;
-        rngBcnFusionReport[rngBcnDataDelayed.beacon_ID].innovVar = varInnovRngBcn;
+            rngBcnFusionReport[rngBcnDataDelayed.beacon_ID].innov = innovRngBcn;
-        rngBcnFusionReport[rngBcnDataDelayed.beacon_ID].rng = rngBcnDataDelayed.rng;
+            rngBcnFusionReport[rngBcnDataDelayed.beacon_ID].innovVar = varInnovRngBcn;
-        rngBcnFusionReport[rngBcnDataDelayed.beacon_ID].testRatio = rngBcnTestRatio;
+            rngBcnFusionReport[rngBcnDataDelayed.beacon_ID].rng = rngBcnDataDelayed.rng;
+            rngBcnFusionReport[rngBcnDataDelayed.beacon_ID].testRatio = rngBcnTestRatio;
+        }
     }
 }
 
@@ -496,11 +498,13 @@ void NavEKF3_core::FuseRngBcnStatic()
             rngBcnAlignmentCompleted = true;
         }
         // Update the fusion report
-        rngBcnFusionReport[rngBcnDataDelayed.beacon_ID].beaconPosNED = rngBcnDataDelayed.beacon_posNED;
+        if (rngBcnFusionReport && rngBcnDataDelayed.beacon_ID < dal.beacon()->count()) {
-        rngBcnFusionReport[rngBcnDataDelayed.beacon_ID].innov = innovRngBcn;
+            rngBcnFusionReport[rngBcnDataDelayed.beacon_ID].beaconPosNED = rngBcnDataDelayed.beacon_posNED;
-        rngBcnFusionReport[rngBcnDataDelayed.beacon_ID].innovVar = varInnovRngBcn;
+            rngBcnFusionReport[rngBcnDataDelayed.beacon_ID].innov = innovRngBcn;
-        rngBcnFusionReport[rngBcnDataDelayed.beacon_ID].rng = rngBcnDataDelayed.rng;
+            rngBcnFusionReport[rngBcnDataDelayed.beacon_ID].innovVar = varInnovRngBcn;
-        rngBcnFusionReport[rngBcnDataDelayed.beacon_ID].testRatio = rngBcnTestRatio;
+            rngBcnFusionReport[rngBcnDataDelayed.beacon_ID].rng = rngBcnDataDelayed.rng;
+            rngBcnFusionReport[rngBcnDataDelayed.beacon_ID].testRatio = rngBcnTestRatio;
+        }
     }
 }
 

libraries/AP_NavEKF3/AP_NavEKF3_core.cpp

@@ -360,7 +360,11 @@ void NavEKF3_core::InitialiseVariables()
     bcnPosOffsetMinVar = 0.0f;
     minOffsetStateChangeFilt = 0.0f;
     rngBcnFuseDataReportIndex = 0;
-    memset(&rngBcnFusionReport, 0, sizeof(rngBcnFusionReport));
+    if (dal.beacon()) {
+        if (rngBcnFusionReport == nullptr) {
+            rngBcnFusionReport = new rngBcnFusionReport_t[dal.beacon()->count()];
+        }
+    }
     bcnPosOffsetNED.zero();
     bcnOriginEstInit = false;
 

libraries/AP_NavEKF3/AP_NavEKF3_core.h

@@ -1247,7 +1247,7 @@ private:
         float innovVar;     // innovation variance (m^2)
         float testRatio;    // innovation consistency test ratio
         Vector3f beaconPosNED; // beacon NED position
-    } rngBcnFusionReport[4];
+    } *rngBcnFusionReport;
 
     // height source selection logic
     AP_NavEKF_Source::SourceZ activeHgtSource;  // active height source