AP_NavEKF2: make AP_RANGEFINDER_ENABLED remove more code

libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp

@@ -5,6 +5,8 @@
 #include <AP_DAL/AP_DAL.h>
 #include <AP_InternalError/AP_InternalError.h>
 
+#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.

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;

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) {

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;

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<rng_bcn_elements> storedRangeBeacon; // Beacon range buffer