Merge pull request #314 from PX4/pr-check_stuck_rng

Pr check stuck rng

EKF/control.cpp

@@ -105,6 +105,8 @@ void Ekf::controlFusionModes()
 	_range_data_ready = _range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_range_sample_delayed)
 			&& (_R_rng_to_earth_2_2 > 0.7071f);
 
+	checkForStuckRange();
+
 	_flow_data_ready = _flow_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_flow_sample_delayed)
 			&&  (_R_to_earth(2, 2) > 0.7071f);
 	_ev_data_ready = _ext_vision_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_ev_sample_delayed);
@@ -952,6 +954,34 @@ bool Ekf::rangeAidConditionsMet(bool in_range_aid_mode)
 	}
 }
 
+void Ekf::checkForStuckRange()
+{
+	if (_range_data_ready && _range_sample_delayed.time_us - _time_last_rng_ready > 10e6 &&
+			_control_status.flags.in_air) {
+		_rng_stuck = true;
+
+		//require a variance of rangefinder values to check for "stuck" measurements
+		if (_rng_check_max_val - _rng_check_min_val > 1.0f) {
+			_time_last_rng_ready = _range_sample_delayed.time_us;
+			_rng_check_min_val = 0.0f;
+			_rng_check_max_val = 0.0f;
+			_rng_stuck = false;
+
+		} else {
+			if (_range_sample_delayed.rng > _rng_check_max_val)
+				_rng_check_max_val = _range_sample_delayed.rng;
+
+			if (_rng_check_min_val < 0.1f || _range_sample_delayed.rng < _rng_check_min_val)
+				_rng_check_min_val = _range_sample_delayed.rng;
+
+			_range_data_ready = false;
+		}
+
+	} else if (_range_data_ready) {
+		_time_last_rng_ready = _range_sample_delayed.time_us;
+	}
+}
+
 void Ekf::controlAirDataFusion()
 {
         // control activation and initialisation/reset of wind states required for airspeed fusion

EKF/ekf.h

@@ -264,6 +264,7 @@ private:
 	uint64_t _time_last_of_fuse{0};		///< time the last fusion of optical flow measurements were performed (uSec)
 	uint64_t _time_last_arsp_fuse{0};	///< time the last fusion of airspeed measurements were performed (uSec)
 	uint64_t _time_last_beta_fuse{0};	///< time the last fusion of synthetic sideslip measurements were performed (uSec)
+	uint64_t _time_last_rng_ready{0};	///< time the last range finder measurement was ready (uSec)
 	Vector2f _last_known_posNE;		///< last known local NE position vector (m)
 	float _last_disarmed_posD{0.0f};	///< vertical position recorded at arming (m)
 	float _imu_collection_time_adj{0.0f};	///< the amount of time the IMU collection needs to be advanced to meet the target set by FILTER_UPDATE_PERIOD_MS (sec)
@@ -386,6 +387,11 @@ private:
 	// variables used to control range aid functionality
 	bool _in_range_aid_mode;		///< true when range finder is to be used as the height reference instead of the primary height sensor
 
+	// variables used to check for "stuck" rng data
+	bool _rng_stuck{false};		///< true when rng data wasn't ready for more than 10s and new rng values haven't changed enough
+	float _rng_check_min_val{0.0f};		///< minimum value for new rng measurement when being stuck
+	float _rng_check_max_val{0.0f};		///< maximum value for new rng measurement when being stuck
+
 	// update the real time complementary filter states. This includes the prediction
 	// and the correction step
 	void calculateOutputStates();
@@ -520,7 +526,10 @@ private:
 	// control for combined height fusion mode (implemented for switching between baro and range height)
 	void controlHeightFusion();
 
-    bool rangeAidConditionsMet(bool in_range_aid_mode);
+	bool rangeAidConditionsMet(bool in_range_aid_mode);
+
+	// check for "stuck" range finder measurements when rng was not valid for certain period
+	void checkForStuckRange();
 
 	// return the square of two floating point numbers - used in auto coded sections
 	inline float sq(float var)

EKF/terrain_estimator.cpp

@@ -93,7 +93,7 @@ void Ekf::runTerrainEstimator()
 		_terrain_var = math::constrain(_terrain_var, 0.0f, 1e4f);
 
 		// Fuse range finder data if available
-		if (_range_data_ready) {
+		if (_range_data_ready && !_rng_stuck) {
 			fuseHagl();
 
 			// update range sensor angle parameters in case they have changed
@@ -158,7 +158,8 @@ void Ekf::fuseHagl()
 // return true if the terrain estimate is valid
 bool Ekf::get_terrain_valid()
 {
-	if (_terrain_initialised && _range_data_continuous && !_innov_check_fail_status.flags.reject_hagl) {
+	if (_terrain_initialised && _range_data_continuous && !_rng_stuck &&
+		  !_innov_check_fail_status.flags.reject_hagl) {
 		return true;
 
 	} else {