Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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
This commit is contained in:
Peter Barker 2024-02-26 15:09:46 +11:00 committed by Andrew Tridgell
parent a517d5fed1
commit 21edc6aee1
3 changed files with 11 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp
View File

@ -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;
}

11
libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp
View File

@ -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;

1
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -1372,6 +1372,7 @@ private:
ftype testRatio; // innovation consistency test ratio
Vector3F beaconPosNED; // beacon NED position
} *fusionReport;
uint8_t numFusionReports;
} rngBcn;
#endif // if EK3_FEATURE_BEACON_FUSION