diff --git a/EKF/CMakeLists.txt b/EKF/CMakeLists.txt index 7a736db45b..ea458d67cb 100644 --- a/EKF/CMakeLists.txt +++ b/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) diff --git a/EKF/control.cpp b/EKF/control.cpp index 1090fb606a..9203bd832f 100644 --- a/EKF/control.cpp +++ b/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 diff --git a/EKF/ekf.h b/EKF/ekf.h index dd29e370bd..0fe2ed7342 100644 --- a/EKF/ekf.h +++ b/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 diff --git a/EKF/range_finder_checks.cpp b/EKF/range_finder_checks.cpp new file mode 100644 index 0000000000..90304e6df5 --- /dev/null +++ b/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; + } +} \ No newline at end of file diff --git a/EKF/terrain_estimator.cpp b/EKF/terrain_estimator.cpp index fdd9cbb207..3a2abf1fa7 100644 --- a/EKF/terrain_estimator.cpp +++ b/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); -}