Mirrors_Framework/ardupilot
1
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot.git synced 2026-10-02 10:23:25 +08:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: save memory on beacon debug structure

Browse Source 
only allocate when beacons are enabled. This relies on a guarantee in
the DAL that the beacon count cannot change
This commit is contained in:
Andrew Tridgell
2020-12-01 10:53:08 +11:00
parent 7b08053ef2
commit bdba430e55
5 changed files with 21 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp
+1 -1
View File
@@ -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
-1
View File
@@ -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
+14 -10
View File
@@ -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;
}
}
}
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
+5 -1
View File
@@ -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
+1 -1
View File
@@ -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