diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp index d2608d60c2..009a59f358 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp @@ -5,6 +5,8 @@ #include #include +#if AP_RANGEFINDER_ENABLED + extern const AP_HAL::HAL& hal; @@ -110,6 +112,7 @@ void NavEKF2_core::readRangeFinder(void) } } } +#endif // write the raw optical flow measurements // this needs to be called externally. diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp index 32820bd9dd..6d5937dd22 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp @@ -60,12 +60,16 @@ bool NavEKF2_core::getHeightControlLimit(float &height) const // only ask for limiting if we are doing optical flow only navigation if (frontend->_fusionModeGPS == 3 && (PV_AidingMode == AID_RELATIVE) && flowDataValid) { // If are doing optical flow nav, ensure the height above ground is within range finder limits after accounting for vehicle tilt and control errors +#if AP_RANGEFINDER_ENABLED const auto *_rng = dal.rangefinder(); if (_rng == nullptr) { // we really, really shouldn't be here. return false; } height = MAX(float(_rng->max_distance_cm_orient(ROTATION_PITCH_270)) * 0.007f - 1.0f, 1.0f); +#else + return false; +#endif // If we are are not using the range finder as the height reference, then compensate for the difference between terrain and EKF origin if (frontend->_altSource != 1) { height -= terrainState; diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp index 3e7b99ee34..debcaaf2c2 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp @@ -942,6 +942,7 @@ void NavEKF2_core::FuseVelPosNED() // select the height measurement to be fused from the available baro, range finder and GPS sources void NavEKF2_core::selectHeightForFusion() { +#if AP_RANGEFINDER_ENABLED // Read range finder data and check for new data in the buffer // This data is used by both height and optical flow fusion processing readRangeFinder(); @@ -961,6 +962,7 @@ void NavEKF2_core::selectHeightForFusion() } } } +#endif // read baro height data from the sensor and check for new data in the buffer readBaroData(); @@ -971,6 +973,7 @@ void NavEKF2_core::selectHeightForFusion() if (extNavUsedForPos) { // always use external navigation as the height source if using for position. activeHgtSource = HGT_SOURCE_EXTNAV; +#if AP_RANGEFINDER_ENABLED } else if ((frontend->_altSource == 1) && _rng && rangeFinderDataIsFresh) { // user has specified the range finder as a primary height source activeHgtSource = HGT_SOURCE_RNG; @@ -1004,6 +1007,7 @@ void NavEKF2_core::selectHeightForFusion() // reliable terrain and range finder so start using range finder height activeHgtSource = HGT_SOURCE_RNG; } +#endif // AP_RANGEFINDER_ENABLED } else if (frontend->_altSource == 0) { activeHgtSource = HGT_SOURCE_BARO; } else if ((frontend->_altSource == 2) && ((imuSampleTime_ms - lastTimeGpsReceived_ms) < 500) && validOrigin && gpsAccuracyGood) { diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp index e8c35d768a..caf78f8745 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp @@ -243,9 +243,11 @@ void NavEKF2_core::InitialiseVariables() delAngBiasLearned = false; memset(&filterStatus, 0, sizeof(filterStatus)); activeHgtSource = 0; +#if AP_RANGEFINDER_ENABLED memset(&rngMeasIndex, 0, sizeof(rngMeasIndex)); memset(&storedRngMeasTime_ms, 0, sizeof(storedRngMeasTime_ms)); memset(&storedRngMeas, 0, sizeof(storedRngMeas)); +#endif terrainHgtStable = true; ekfOriginHgtVar = 0.0f; ekfGpsRefHgt = 0.0; diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.h b/libraries/AP_NavEKF2/AP_NavEKF2_core.h index d9f0adb2ce..fcb48be5cc 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.h @@ -70,12 +70,14 @@ #endif // maximum number of downward facing rangefinder instances available +#if AP_RANGEFINDER_ENABLED #if RANGEFINDER_MAX_INSTANCES > 1 #define DOWNWARD_RANGEFINDER_MAX_INSTANCES 2 #else #define DOWNWARD_RANGEFINDER_MAX_INSTANCES 1 #endif - +#endif + class AP_AHRS; class NavEKF2_core : public NavEKF_core_common @@ -700,9 +702,11 @@ private: // update inflight calculaton that determines if GPS data is good enough for reliable navigation void calcGpsGoodForFlight(void); +#if AP_RANGEFINDER_ENABLED // Read the range finder and take new measurements if available // Apply a median filter to range finder data void readRangeFinder(); +#endif // check if the vehicle has taken off during optical flow navigation by looking at inertial and range finder data void detectOptFlowTakeoff(void); @@ -1014,9 +1018,11 @@ private: ftype rngOnGnd; // Expected range finder reading in metres when vehicle is on ground uint32_t lastRngMeasTime_ms; // Timestamp of last range measurement bool terrainHgtStable; // true when the terrain height is stable enough to be used as a height reference +#if AP_RANGEFINDER_ENABLED 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 +#endif // Range Beacon Sensor Fusion EKF_obs_buffer_t storedRangeBeacon; // Beacon range buffer