cleanup checks of range finder data

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

EKF/CMakeLists.txt

@@ -46,6 +46,7 @@ add_library(ecl_EKF
 	terrain_estimator.cpp
 	vel_pos_fusion.cpp
 	gps_yaw_fusion.cpp
+	range_finder_checks.cpp
 )
 
 add_dependencies(ecl_EKF prebuild_targets)

EKF/control.cpp

@@ -118,9 +118,9 @@ void Ekf::controlFusionModes()
 	// Get range data from buffer and check validity
 	_range_data_ready = _range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_range_sample_delayed);
 
-	checkRangeDataValidity();
+	updateRangeDataValidity();
 
-	if (_range_data_ready && !_rng_hgt_faulty) {
+	if (_range_data_ready && _rng_hgt_valid) {
 		// correct the range data for position offset relative to the IMU
 		Vector3f pos_offset_body = _params.rng_pos_body - _params.imu_pos_body;
 		Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
@@ -910,42 +910,33 @@ void Ekf::controlHeightSensorTimeouts()
 
 		// handle the case we are using range finder for height
 		if (_control_status.flags.rng_hgt) {
-			// check if range finder data is available
-			const rangeSample &rng_init = _range_buffer.get_newest();
-			bool rng_data_available = ((_time_last_imu - rng_init.time_us) < 2 * RNG_MAX_INTERVAL);
 
 			// check if baro data is available
 			const baroSample &baro_init = _baro_buffer.get_newest();
 			bool baro_data_available = ((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL);
 
 			// reset to baro if we have no range data and baro data is available
-			bool reset_to_baro = !rng_data_available && baro_data_available;
+			bool reset_to_baro = !_rng_hgt_valid && baro_data_available;
 
-			// reset to range data if it is available
-			bool reset_to_rng = rng_data_available;
-
-			if (reset_to_baro) {
-				// set height sensor health
-				_rng_hgt_faulty = true;
-				_baro_hgt_faulty = false;
+			if (_rng_hgt_valid) {
 
 				// reset the height mode
-				setControlBaroHeight();
+				setControlRangeHeight();
 
 				// request a reset
 				reset_height = true;
-				ECL_WARN_TIMESTAMPED("EKF rng hgt timeout - reset to baro");
+				ECL_WARN_TIMESTAMPED("EKF rng hgt timeout - reset to rng hgt");
 
-			} else if (reset_to_rng) {
+			} else if (reset_to_baro) {
 				// set height sensor health
-				_rng_hgt_faulty = false;
+				_baro_hgt_faulty = false;
 
 				// reset the height mode
-				setControlRangeHeight();
+				setControlBaroHeight();
 
 				// request a reset
 				reset_height = true;
-				ECL_WARN_TIMESTAMPED("EKF rng hgt timeout - reset to rng hgt");
+				ECL_WARN_TIMESTAMPED("EKF rng hgt timeout - reset to baro");
 
 			} else {
 				// we have nothing to reset to
@@ -1016,7 +1007,7 @@ void Ekf::controlHeightFusion()
 
 	if (_params.vdist_sensor_type == VDIST_SENSOR_BARO) {
 
-		if (_range_aid_mode_selected && _range_data_ready && !_rng_hgt_faulty) {
+		if (_range_aid_mode_selected && _range_data_ready && _rng_hgt_valid) {
 			setControlRangeHeight();
 			_fuse_height = true;
 
@@ -1062,7 +1053,7 @@ void Ekf::controlHeightFusion()
 	}
 
 	// set the height data source to range if requested
-	if ((_params.vdist_sensor_type == VDIST_SENSOR_RANGE) && !_rng_hgt_faulty) {
+	if ((_params.vdist_sensor_type == VDIST_SENSOR_RANGE) && _rng_hgt_valid) {
 		setControlRangeHeight();
 		_fuse_height = _range_data_ready;
 
@@ -1098,7 +1089,7 @@ void Ekf::controlHeightFusion()
 	// Determine if GPS should be used as the height source
 	if (_params.vdist_sensor_type == VDIST_SENSOR_GPS) {
 
-		if (_range_aid_mode_selected && _range_data_ready && !_rng_hgt_faulty) {
+		if (_range_aid_mode_selected && _range_data_ready && _rng_hgt_valid) {
 			setControlRangeHeight();
 			_fuse_height = true;
 
@@ -1161,7 +1152,7 @@ void Ekf::controlHeightFusion()
 	}
 
 	if ((_time_last_imu - _time_last_hgt_fuse) > 2 * RNG_MAX_INTERVAL && _control_status.flags.rng_hgt
-	    && (!_range_data_ready || _rng_hgt_faulty)) {
+	    && (!_range_data_ready || !_rng_hgt_valid)) {
 
 		// If we are supposed to be using range finder data as the primary height sensor, have missed or rejected measurements
 		// and are on the ground, then synthesise a measurement at the expected on ground value
@@ -1210,78 +1201,6 @@ void Ekf::checkRangeAidSuitability()
 	}
 }
 
-void Ekf::checkRangeDataValidity()
-{
-	// check if out of date
-	if ((_imu_sample_delayed.time_us - _range_sample_delayed .time_us) > 2 * RNG_MAX_INTERVAL) {
-		_rng_hgt_faulty = true;
-		return;
-	}
-
-	// Don't allow faulty flag to clear unless range data is continuous
-	if (_rng_hgt_faulty && !_range_data_continuous) {
-		return;
-	}
-
-	// Don't run the checks after this unless we have retrieved new data from the buffer
-	if (!_range_data_ready) {
-		return;
-	} else {
-		// reset fault status when we get new data
-		_rng_hgt_faulty = (_range_sample_delayed.quality == 0);
-	}
-
-	// Check if excessively tilted
-	if (_R_rng_to_earth_2_2 < _params.range_cos_max_tilt) {
-		_rng_hgt_faulty = true;
-		return;
-	}
-
-	// Check if out of range
-	if ((_range_sample_delayed.rng > _rng_valid_max_val)
-	|| (_range_sample_delayed.rng < _rng_valid_min_val)) {
-		if (_control_status.flags.in_air) {
-			_rng_hgt_faulty = true;
-			return;
-		} else {
-			// Range finders can fail to provide valid readings when resting on the ground
-			// or being handled by the user, which prevents use of as a primary height sensor.
-			// To work around this issue, we replace out of range data with the expected on ground value.
-			_range_sample_delayed.rng = _params.rng_gnd_clearance;
-			return;
-		}
-	}
-
-	// 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) &&
-	    _control_status.flags.in_air) {
-
-		// require a variance of rangefinder values to check for "stuck" measurements
-		if (_rng_stuck_max_val - _rng_stuck_min_val > _params.range_stuck_threshold) {
-			_time_last_rng_ready = _range_sample_delayed.time_us;
-			_rng_stuck_min_val = 0.0f;
-			_rng_stuck_max_val = 0.0f;
-			_control_status.flags.rng_stuck = false;
-
-		} else {
-			if (_range_sample_delayed.rng > _rng_stuck_max_val) {
-				_rng_stuck_max_val = _range_sample_delayed.rng;
-			}
-
-			if (_rng_stuck_min_val < 0.1f || _range_sample_delayed.rng < _rng_stuck_min_val) {
-				_rng_stuck_min_val = _range_sample_delayed.rng;
-			}
-
-			_control_status.flags.rng_stuck = true;
-			_rng_hgt_faulty = true;
-		}
-
-	} else {
-		_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

@@ -464,8 +464,9 @@ private:
 	// height sensor status
 	bool _baro_hgt_faulty{false};		///< true if valid baro data is unavailable for use
 	bool _gps_hgt_intermittent{false};	///< true if gps height into the buffer is intermittent
-	bool _rng_hgt_faulty{false};		///< true if valid range finder height data is unavailable for use
+	bool _rng_hgt_valid{false};		///< true if range finder sample retrieved from buffer is valid
 	int _primary_hgt_source{VDIST_SENSOR_BARO};	///< specifies primary source of height data
+	uint64_t _time_bad_rng_signal_quality{0};	///< timestamp at which range finder signal quality was 0 (used for hysteresis)
 
 	// imu fault status
 	uint64_t _time_bad_vert_accel{0};	///< last time a bad vertical accel was detected (uSec)
@@ -648,7 +649,7 @@ private:
 	bool isRangeAidSuitable() { return _is_range_aid_suitable; }
 
 	// check for "stuck" range finder measurements when rng was not valid for certain period
-	void checkRangeDataValidity();
+	void updateRangeDataValidity();
 
 	// return the square of two floating point numbers - used in auto coded sections
 	static constexpr float sq(float var) { return var * var; }
@@ -697,7 +698,7 @@ private:
 	void resetWindStates();
 
 	// check that the range finder data is continuous
-	void checkRangeDataContinuity();
+	void updateRangeDataContinuity();
 
 	// Increase the yaw error variance of the quaternions
 	// Argument is additional yaw variance in rad**2

EKF/range_finder_checks.cpp

@@ -0,0 +1,144 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019 Estimation and Control Library (ECL). All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name ECL nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file range_finder_checks.cpp
+ * Perform checks on range finder data in order to evaluate validity.
+ *
+ *
+ */
+
+#include "ekf.h"
+
+
+#define RNG_BAD_SIG_HYST (uint64_t)3e6	// range finder data needs to have 3 seconds of non-zero signal quality to be valid
+
+// check that the range finder data is continuous
+void Ekf::updateRangeDataContinuity()
+{
+	// update range data continuous flag (1Hz ie 2000 ms)
+	/* Timing in micro seconds */
+
+	/* Apply a 2.0 sec low pass filter to the time delta from the last range finder updates */
+	float alpha = 0.5f * _dt_update;
+	_dt_last_range_update_filt_us = _dt_last_range_update_filt_us * (1.0f - alpha) + alpha *
+					(_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()
+{
+	updateRangeDataContinuity();
+
+	// check if out of date
+	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) {
+		return;
+	}
+
+	// Don't run the checks after this unless we have retrieved new data from the buffer
+	if (!_range_data_ready) {
+		return;
+	}
+
+	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;
+	}
+
+	// Check if excessively tilted
+	if (_R_rng_to_earth_2_2 < _params.range_cos_max_tilt) {
+		_rng_hgt_valid = false;
+		return;
+	}
+
+	// Check if out of range
+	if ((_range_sample_delayed.rng > _rng_valid_max_val)
+	|| (_range_sample_delayed.rng < _rng_valid_min_val)) {
+		if (_control_status.flags.in_air) {
+			_rng_hgt_valid = false;
+			return;
+		} else {
+			// Range finders can fail to provide valid readings when resting on the ground
+			// or being handled by the user, which prevents use of as a primary height sensor.
+			// To work around this issue, we replace out of range data with the expected on ground value.
+			_range_sample_delayed.rng = _params.rng_gnd_clearance;
+			return;
+		}
+	}
+
+	// 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) &&
+	    _control_status.flags.in_air) {
+
+		// require a variance of rangefinder values to check for "stuck" measurements
+		if (_rng_stuck_max_val - _rng_stuck_min_val > _params.range_stuck_threshold) {
+			_time_last_rng_ready = _range_sample_delayed.time_us;
+			_rng_stuck_min_val = 0.0f;
+			_rng_stuck_max_val = 0.0f;
+			_control_status.flags.rng_stuck = false;
+
+		} else {
+			if (_range_sample_delayed.rng > _rng_stuck_max_val) {
+				_rng_stuck_max_val = _range_sample_delayed.rng;
+			}
+
+			if (_rng_stuck_min_val < 0.1f || _range_sample_delayed.rng < _rng_stuck_min_val) {
+				_rng_stuck_min_val = _range_sample_delayed.rng;
+			}
+
+			_control_status.flags.rng_stuck = true;
+			_rng_hgt_valid = false;
+		}
+
+	} else {
+		_time_last_rng_ready = _range_sample_delayed.time_us;
+	}
+}

EKF/terrain_estimator.cpp

@@ -56,7 +56,7 @@ bool Ekf::initHagl()
 		_terrain_var = sq(_params.rng_gnd_clearance);
 		initialized = true;
 
-	} else if (!_rng_hgt_faulty
+	} else if (_rng_hgt_valid
 		   && (_time_last_imu - latest_measurement.time_us) < (uint64_t)2e5
 		   && _R_rng_to_earth_2_2 > _params.range_cos_max_tilt) {
 		// if we have a fresh measurement, use it to initialise the terrain estimator
@@ -87,9 +87,6 @@ bool Ekf::initHagl()
 
 void Ekf::runTerrainEstimator()
 {
-	// Perform a continuity check on range finder data
-	checkRangeDataContinuity();
-
 	// Perform initialisation check and
 	// on ground, continuously reset the terrain estimator
 	if (!_terrain_initialised || !_control_status.flags.in_air) {
@@ -110,7 +107,7 @@ void Ekf::runTerrainEstimator()
 		_terrain_var = math::constrain(_terrain_var, 0.0f, 1e4f);
 
 		// Fuse range finder data if available
-		if (_range_data_ready && !_rng_hgt_faulty) {
+		if (_range_data_ready && _rng_hgt_valid) {
 			fuseHagl();
 
 			// update range sensor angle parameters in case they have changed
@@ -321,19 +318,3 @@ void Ekf::getHaglInnovVar(float *hagl_innov_var)
 {
 	memcpy(hagl_innov_var, &_hagl_innov_var, sizeof(_hagl_innov_var));
 }
-
-// check that the range finder data is continuous
-void Ekf::checkRangeDataContinuity()
-{
-	// update range data continuous flag (1Hz ie 2000 ms)
-	/* Timing in micro seconds */
-
-	/* Apply a 2.0 sec low pass filter to the time delta from the last range finder updates */
-	float alpha = 0.5f * _dt_update;
-	_dt_last_range_update_filt_us = _dt_last_range_update_filt_us * (1.0f - alpha) + alpha *
-					(_imu_sample_delayed.time_us - _range_sample_delayed.time_us);
-
-	_dt_last_range_update_filt_us = fminf(_dt_last_range_update_filt_us, 4e6f);
-
-	_range_data_continuous = (_dt_last_range_update_filt_us < 2e6f);
-}