diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp index c002eb22ee..877e1d654c 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp +++ b/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; } diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp index 36ca9b2570..d9daf0bc03 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp +++ b/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(); diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp index 70d3c81658..4bb037032d 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp +++ b/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; - rngBcnFusionReport[rngBcnDataDelayed.beacon_ID].innov = innovRngBcn; - rngBcnFusionReport[rngBcnDataDelayed.beacon_ID].innovVar = varInnovRngBcn; - rngBcnFusionReport[rngBcnDataDelayed.beacon_ID].rng = rngBcnDataDelayed.rng; - rngBcnFusionReport[rngBcnDataDelayed.beacon_ID].testRatio = rngBcnTestRatio; + if (rngBcnFusionReport && rngBcnDataDelayed.beacon_ID < dal.beacon()->count()) { + rngBcnFusionReport[rngBcnDataDelayed.beacon_ID].beaconPosNED = rngBcnDataDelayed.beacon_posNED; + rngBcnFusionReport[rngBcnDataDelayed.beacon_ID].innov = innovRngBcn; + rngBcnFusionReport[rngBcnDataDelayed.beacon_ID].innovVar = varInnovRngBcn; + 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; - rngBcnFusionReport[rngBcnDataDelayed.beacon_ID].innov = innovRngBcn; - rngBcnFusionReport[rngBcnDataDelayed.beacon_ID].innovVar = varInnovRngBcn; - rngBcnFusionReport[rngBcnDataDelayed.beacon_ID].rng = rngBcnDataDelayed.rng; - rngBcnFusionReport[rngBcnDataDelayed.beacon_ID].testRatio = rngBcnTestRatio; + if (rngBcnFusionReport && rngBcnDataDelayed.beacon_ID < dal.beacon()->count()) { + rngBcnFusionReport[rngBcnDataDelayed.beacon_ID].beaconPosNED = rngBcnDataDelayed.beacon_posNED; + rngBcnFusionReport[rngBcnDataDelayed.beacon_ID].innov = innovRngBcn; + rngBcnFusionReport[rngBcnDataDelayed.beacon_ID].innovVar = varInnovRngBcn; + rngBcnFusionReport[rngBcnDataDelayed.beacon_ID].rng = rngBcnDataDelayed.rng; + rngBcnFusionReport[rngBcnDataDelayed.beacon_ID].testRatio = rngBcnTestRatio; + } } } diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp index 78bb6c2470..5029d924b3 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp +++ b/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; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index 03b6d683a4..4c29df497a 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/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