addressed review comments

Signed-off-by: RomanBapst <bapstroman@gmail.com>

EKF/control.cpp

@@ -1176,7 +1176,7 @@ void Ekf::checkRangeAidSuitability()
 				    || _control_status.flags.opt_flow;
 
 	if (_control_status.flags.in_air
-	    && !_rng_hgt_faulty
+	    && _rng_hgt_valid
 	    && isTerrainEstimateValid()
 	    && horz_vel_valid) {
 		// check if we can use range finder measurements to estimate height, use hysteresis to avoid rapid switching

EKF/ekf.h

@@ -457,7 +457,6 @@ private:
 	float _sin_tilt_rng{0.0f};		///< sine of the range finder tilt rotation about the Y body axis
 	float _cos_tilt_rng{0.0f};		///< cosine of the range finder tilt rotation about the Y body axis
 	float _R_rng_to_earth_2_2{0.0f};	///< 2,2 element of the rotation matrix from sensor frame to earth frame
-	bool _range_data_continuous{false};	///< true when we are receiving range finder data faster than a 2Hz average
 	float _dt_last_range_update_filt_us{0.0f};	///< filtered value of the delta time elapsed since the last range measurement came into the filter (uSec)
 	bool _hagl_valid{false};		///< true when the height above ground estimate is valid
 
@@ -648,9 +647,12 @@ private:
 	void checkRangeAidSuitability();
 	bool isRangeAidSuitable() { return _is_range_aid_suitable; }
 
-	// check for "stuck" range finder measurements when rng was not valid for certain period
+	// update _rng_hgt_valid, which indicates if the current range sample has passed validity checks
 	void updateRangeDataValidity();
 
+	// check for "stuck" range finder measurements when rng was not valid for certain period
+	void updateRangeDataStuck();
+
 	// return the square of two floating point numbers - used in auto coded sections
 	static constexpr float sq(float var) { return var * var; }
 
@@ -700,6 +702,8 @@ private:
 	// check that the range finder data is continuous
 	void updateRangeDataContinuity();
 
+	bool isRangeDataContinuous() { return _dt_last_range_update_filt_us < 2e6f; }
+
 	// Increase the yaw error variance of the quaternions
 	// Argument is additional yaw variance in rad**2
 	void increaseQuatYawErrVariance(float yaw_variance);

EKF/range_finder_checks.cpp

@@ -55,13 +55,6 @@ void Ekf::updateRangeDataContinuity()
 					(_imu_sample_delayed.time_us - _range_sample_delayed.time_us);
 
 	_dt_last_range_update_filt_us = fminf(_dt_last_range_update_filt_us, 4e6f);
-
-	if (_dt_last_range_update_filt_us < 2e6f) {
-		_range_data_continuous = true;
-
-	} else {
-		_range_data_continuous = false;
-	}
 }
 
 void Ekf::updateRangeDataValidity()
@@ -69,13 +62,13 @@ void Ekf::updateRangeDataValidity()
 	updateRangeDataContinuity();
 
 	// check if out of date
-	if ((_imu_sample_delayed.time_us - _range_sample_delayed .time_us) > 2 * RNG_MAX_INTERVAL) {
+	if ((_imu_sample_delayed.time_us - _range_sample_delayed.time_us) > 2 * RNG_MAX_INTERVAL) {
 		_rng_hgt_valid = false;
 		return;
 	}
 
 	// Don't allow faulty flag to clear unless range data is continuous
-	if (!_rng_hgt_valid && !_range_data_continuous) {
+	if (!_rng_hgt_valid && !isRangeDataContinuous()) {
 		return;
 	}
 
@@ -87,10 +80,7 @@ void Ekf::updateRangeDataValidity()
 	if (_range_sample_delayed.quality == 0) {
 		_time_bad_rng_signal_quality = _imu_sample_delayed.time_us;
 		_rng_hgt_valid = false;
-	} else if (_time_bad_rng_signal_quality > 0 && _imu_sample_delayed.time_us - _time_bad_rng_signal_quality > RNG_BAD_SIG_HYST) {
-		_time_bad_rng_signal_quality = 0;
-		_rng_hgt_valid = true;
-	} else if (_time_bad_rng_signal_quality == 0) {
+	} else if (_imu_sample_delayed.time_us - _time_bad_rng_signal_quality > RNG_BAD_SIG_HYST) {
 		_rng_hgt_valid = true;
 	}
 
@@ -115,6 +105,13 @@ void Ekf::updateRangeDataValidity()
 		}
 	}
 
+	updateRangeDataStuck();
+
+	_rng_hgt_valid = !_control_status.flags.rng_stuck;
+}
+
+void Ekf::updateRangeDataStuck()
+{
 	// Check for "stuck" range finder measurements when range was not valid for certain period
 	// This handles a failure mode observed with some lidar sensors
 	if (((_range_sample_delayed.time_us - _time_last_rng_ready) > (uint64_t)10e6) &&
@@ -137,7 +134,6 @@ void Ekf::updateRangeDataValidity()
 			}
 
 			_control_status.flags.rng_stuck = true;
-			_rng_hgt_valid = false;
 		}
 
 	} else {