From a51c3206b2b923097abf3ede14ad412a8b50e53b Mon Sep 17 00:00:00 2001 From: Tatsuya Yamaguchi Date: Mon, 6 Jun 2022 11:25:45 +0900 Subject: [PATCH] AP_NavEKF2: do not check the second range finder if RANGEFINDER_MAX_INSTANCES is 1 --- libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp | 4 ++-- libraries/AP_NavEKF2/AP_NavEKF2_core.h | 13 ++++++++++--- 2 files changed, 12 insertions(+), 5 deletions(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp index ab2116c92b..61f5ac6c83 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp @@ -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; } diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.h b/libraries/AP_NavEKF2/AP_NavEKF2_core.h index 8c74d18fb3..9179482f4d 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.h @@ -68,6 +68,13 @@ #else #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; @@ -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 storedRangeBeacon; // Beacon range buffer