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

AP_NavEKF2: do not check the second range finder if RANGEFINDER_MAX_INSTANCES is 1

Browse Source
This commit is contained in:
Tatsuya Yamaguchi 2022-06-06 11:25:45 +09:00 committed by Andrew Tridgell
parent 0f04611970
commit a51c3206b2
2 changed files with 12 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
View File

@ -39,7 +39,7 @@ void NavEKF2_core::readRangeFinder(void)
// store samples and sample time into a ring buffer if valid
// use data from two range finders if available
for (uint8_t sensorIndex = 0; sensorIndex <= 1; sensorIndex++) {
for (uint8_t sensorIndex = 0; sensorIndex < ARRAY_SIZE(rngMeasIndex); sensorIndex++) {
auto *sensor = _rng->get_backend(sensorIndex);
if (sensor == nullptr) {
continue;
@ -56,7 +56,7 @@ void NavEKF2_core::readRangeFinder(void)
}
// check for three fresh samples
bool sampleFresh[2][3] = {};
bool sampleFresh[DOWNWARD_RANGEFINDER_MAX_INSTANCES][3] = {};
for (uint8_t index = 0; index <= 2; index++) {
sampleFresh[sensorIndex][index] = (imuSampleTime_ms - storedRngMeasTime_ms[sensorIndex][index]) < 500;
}

13
libraries/AP_NavEKF2/AP_NavEKF2_core.h
View File

@ -69,6 +69,13 @@
#define EK2_POSXY_STATE_LIMIT 1.0e6
#endif
// maximum number of downward facing rangefinder instances available
#if RANGEFINDER_MAX_INSTANCES > 1
#define DOWNWARD_RANGEFINDER_MAX_INSTANCES 2
#else
#define DOWNWARD_RANGEFINDER_MAX_INSTANCES 1
#endif
class AP_AHRS;
class NavEKF2_core : public NavEKF_core_common
@ -1004,11 +1011,11 @@ private:
// Range finder
ftype baroHgtOffset; // offset applied when when switching to use of Baro height
ftype rngOnGnd; // Expected range finder reading in metres when vehicle is on ground
ftype storedRngMeas[2][3]; // Ringbuffer of stored range measurements for dual range sensors
uint32_t storedRngMeasTime_ms[2][3]; // Ringbuffers of stored range measurement times for dual range sensors
uint32_t lastRngMeasTime_ms; // Timestamp of last range measurement
uint8_t rngMeasIndex[2]; // Current range measurement ringbuffer index for dual range sensors
bool terrainHgtStable; // true when the terrain height is stable enough to be used as a height reference
ftype storedRngMeas[DOWNWARD_RANGEFINDER_MAX_INSTANCES][3]; // Ringbuffer of stored range measurements for dual range sensors
uint32_t storedRngMeasTime_ms[DOWNWARD_RANGEFINDER_MAX_INSTANCES][3]; // Ringbuffers of stored range measurement times for dual range sensors
uint8_t rngMeasIndex[DOWNWARD_RANGEFINDER_MAX_INSTANCES]; // Current range measurement ringbuffer index for dual range sensors
// Range Beacon Sensor Fusion
EKF_obs_buffer_t<rng_bcn_elements> storedRangeBeacon; // Beacon range buffer