Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-01 21:48:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: shrink beacon arrays and add assert

Browse Source
This commit is contained in:
Randy Mackay 2020-08-19 11:23:40 +09:00
parent d292ba028a
commit 8c877f02af
2 changed files with 6 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
View File

@ -814,6 +814,10 @@ void NavEKF3_core::readAirSpdData()
// check for new range beacon data and push to data buffer if available
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_Beacon *beacon = AP::beacon();

4
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -1280,7 +1280,7 @@ private:
bool rngBcnTimeout; // boolean true if range beacon measurements have failed innovation consistency checks for too long
float varInnovRngBcn; // range beacon observation innovation variance (m^2)
float innovRngBcn; // range beacon observation innovation (m)
uint32_t lastTimeRngBcn_ms[10]; // last time we received a range beacon measurement (msec)
uint32_t lastTimeRngBcn_ms[4]; // last time we received a range beacon measurement (msec)
bool rngBcnDataToFuse; // true when there is new range beacon data to fuse
Vector3f beaconVehiclePosNED; // NED position estimate from the beacon system (NED)
float beaconVehiclePosErr; // estimated position error from the beacon system (m)
@ -1319,7 +1319,7 @@ private:
float innovVar; // innovation variance (m^2)
float testRatio; // innovation consistency test ratio
Vector3f beaconPosNED; // beacon NED position
} rngBcnFusionReport[10];
} rngBcnFusionReport[4];
// height source selection logic
uint8_t activeHgtSource; // integer defining active height source