aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF3: 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:26:06 +09:00 committed by Andrew Tridgell
parent a51c3206b2
commit 366a325fc0
2 changed files with 11 additions and 5 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

@ -34,7 +34,7 @@ void NavEKF3_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++) {
const auto *sensor = _rng->get_backend(sensorIndex);
if (sensor == nullptr) {
continue;
@ -51,7 +51,7 @@ void NavEKF3_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;
}

12
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -108,6 +108,12 @@
#define WIND_VEL_VARIANCE_MAX 400.0f
#define WIND_VEL_VARIANCE_MIN 0.25f
// 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 NavEKF3_core : public NavEKF_core_common
{
@ -1246,11 +1252,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
// body frame odometry fusion
#if EK3_FEATURE_BODY_ODOM