From 0623b2b02d2be1df00209aedf71b7f4f0afb70d2 Mon Sep 17 00:00:00 2001 From: Mathieu Bresciani Date: Fri, 3 Apr 2020 08:28:07 +0200 Subject: [PATCH] Range check cleanup (#782) * EKF: centralize range finder tilt check * Ekf-control: do not double check for terrain estimate validity isRangeAidSuitable can only return true if the terrain estimate is valid so there is no need for an additional check * range_finder_checks: restructure the checks to avoid early returns There is now only one clear path that can lead to the validity being true. Furthermore, if the _rng_hgt_valid is true, we can trust it and we don't need for additional checks such as tilt. The case where we need to provide fake measurements because the drone is on the ground and the range finder data is bad is already handled in "controlHeightFusion" so there is no need to hack the range finder checks with that. * Add Sensor and SensorRangeFinder classes The purpose is to encapsulate the checks for each sensor in a dedicated class with the same interface * SensorRangeFinder: encapsulate in estimator::sensor namespace * EKF: rename _sensor_rng to _range_sensor * Range checks: include limits in valid range * RangeChecks: update comment in the continuity checks * RangeChecks: move more low-level checks in functions Also move setTilt out of the terrain estimator, this is anyway protected internally to not compute cos/sin if the parameter did not change. * Sensor: remove unused virtual functions Those are not required yet but can still be added later * SensorRangeFinder: re-organise member variables Also rename getRangeToEarth to getCosTilt * SensorRangeFinder: split setSensorTilt and setCosMaxTilt functions * SensorRangeFinder: Add a few unit tests - good data - tilt exceeded - max range exceeded * SensorRangeFinder: set hysteresis in us instead of ms * SensorRangeFinder: Add more tests * SensorRangeFinder: update continuity, hysteresis and stuck tests * SensorRangeFinder: rename variables * SensorRangeFinder: get rid of "delayed" specification From the SensorRangeFinder class point of view, it's not relevant to know if the data is delayed or not * SensorRangeFinder: move time_last_valid out of stuck check * SensorRangeFinder: rename file names to sensor_range_finder * SensorRangeFinder: address Kamil's comments * SensorRangeFinder: Add more tilt tests * SensorRangeFinder: store current tilt offset This is to avoid recomputing cos/sin functions at each loop --- EKF/CMakeLists.txt | 2 +- EKF/Sensor.hpp | 76 +++++++ EKF/common.h | 2 - EKF/control.cpp | 67 +++--- EKF/ekf.cpp | 4 +- EKF/ekf.h | 22 -- EKF/ekf_helper.cpp | 12 +- EKF/estimator_interface.h | 8 +- EKF/range_finder_checks.cpp | 139 ------------ EKF/sensor_range_finder.cpp | 139 ++++++++++++ EKF/sensor_range_finder.hpp | 151 +++++++++++++ EKF/terrain_estimator.cpp | 87 ++++--- test/CMakeLists.txt | 1 + test/sensor_simulator/range_finder.cpp | 1 + test/test_SensorRangeFinder.cpp | 299 +++++++++++++++++++++++++ 15 files changed, 747 insertions(+), 263 deletions(-) create mode 100644 EKF/Sensor.hpp delete mode 100644 EKF/range_finder_checks.cpp create mode 100644 EKF/sensor_range_finder.cpp create mode 100644 EKF/sensor_range_finder.hpp create mode 100644 test/test_SensorRangeFinder.cpp diff --git a/EKF/CMakeLists.txt b/EKF/CMakeLists.txt index 09c2a8b48b..b5548bc8ec 100644 --- a/EKF/CMakeLists.txt +++ b/EKF/CMakeLists.txt @@ -47,9 +47,9 @@ add_library(ecl_EKF terrain_estimator.cpp vel_pos_fusion.cpp gps_yaw_fusion.cpp - range_finder_checks.cpp imu_down_sampler.cpp EKFGSF_yaw.cpp + sensor_range_finder.cpp ) add_dependencies(ecl_EKF prebuild_targets) diff --git a/EKF/Sensor.hpp b/EKF/Sensor.hpp new file mode 100644 index 0000000000..0ff2e5cfad --- /dev/null +++ b/EKF/Sensor.hpp @@ -0,0 +1,76 @@ +/**************************************************************************** + * + * Copyright (c) 2020 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 Sensor.hpp + * Abstract class for sensors + * + * @author Mathieu Bresciani + * + */ + +#pragma once + +#include "common.h" + +namespace estimator +{ +namespace sensor +{ + +class Sensor +{ +public: + virtual ~Sensor() {}; + + /* + * run sanity checks on the current data + * this has to be called immediately after + * setting new data + */ + virtual void runChecks(){}; + + /* + * return true if the sensor is healthy + */ + virtual bool isHealthy() const = 0; + + /* + * return true if the delayed sample is healthy + * and can be fused in the estimator + */ + virtual bool isDataHealthy() const = 0; +}; + +} // namespace sensor +} // namespace estimator diff --git a/EKF/common.h b/EKF/common.h index ceb538cdff..ce1743d38f 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -288,8 +288,6 @@ struct parameters { int32_t range_aid{0}; ///< allow switching primary height source to range finder if certain conditions are met float range_aid_innov_gate{1.0f}; ///< gate size used for innovation consistency checks for range aid fusion float range_cos_max_tilt{0.7071f}; ///< cosine of the maximum tilt angle from the vertical that permits use of range finder and flow data - float range_stuck_threshold{0.1f}; ///< minimum variation in range finder reading required to declare a range finder 'unstuck' when readings recommence after being out of range (m) - int32_t range_signal_hysteresis_ms{1000}; ///< minimum duration during which the reported range finder signal quality needs to be non-zero in order to be declared valid (ms) // vision position fusion float ev_vel_innov_gate{3.0f}; ///< vision velocity fusion innovation consistency gate size (STD) diff --git a/EKF/control.cpp b/EKF/control.cpp index a93f6af46d..a581028879 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -116,20 +116,24 @@ void Ekf::controlFusionModes() _delta_time_baro_us = _baro_sample_delayed.time_us - _delta_time_baro_us; } - // calculate 2,2 element of rotation matrix from sensor frame to earth frame - // this is required for use of range finder and flow data - _R_rng_to_earth_2_2 = _R_to_earth(2, 0) * _sin_tilt_rng + _R_to_earth(2, 2) * _cos_tilt_rng; + + { // 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); + const bool is_rng_data_ready = _range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, _range_sensor.getSampleAddress()); + _range_sensor.setDataReadiness(is_rng_data_ready); + _range_sensor.runChecks(_imu_sample_delayed.time_us, _R_to_earth); - updateRangeDataValidity(); + // update range sensor angle parameters in case they have changed + _range_sensor.setTiltOffset(_params.rng_sens_pitch); + _range_sensor.setCosMaxTilt(_params.range_cos_max_tilt); + } - if (_range_data_ready && _rng_hgt_valid) { + if (_range_sensor.isDataHealthy()) { // 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; - _range_sample_delayed.rng += pos_offset_earth(2) / _R_rng_to_earth_2_2; + _range_sensor.setRange(_range_sensor.getRange() + pos_offset_earth(2) / _range_sensor.getCosTilt()); } // We don't fuse flow data immediately because we have to wait for the mid integration point to fall behind the fusion time horizon. @@ -853,7 +857,7 @@ void Ekf::controlHeightSensorTimeouts() } else if (_control_status.flags.rng_hgt) { - if (_rng_hgt_valid) { + if (_range_sensor.isHealthy()) { request_height_reset = true; ECL_WARN_TIMESTAMPED("rng hgt timeout - reset to rng hgt"); @@ -933,7 +937,7 @@ void Ekf::checkVerticalAccelerationHealth() void Ekf::controlHeightFusion() { checkRangeAidSuitability(); - _range_aid_mode_selected = (_params.range_aid == 1) && isRangeAidSuitable(); + const bool do_range_aid = (_params.range_aid == 1) && isRangeAidSuitable(); bool fuse_height = false; @@ -943,22 +947,17 @@ void Ekf::controlHeightFusion() // FALLTHROUGH case VDIST_SENSOR_BARO: - if (_range_aid_mode_selected && _range_data_ready && _rng_hgt_valid) { + if (do_range_aid && _range_sensor.isDataHealthy()) { setControlRangeHeight(); fuse_height = true; // we have just switched to using range finder, calculate height sensor offset such that current // measurement matches our current height estimate if (_control_status_prev.flags.rng_hgt != _control_status.flags.rng_hgt) { - if (isTerrainEstimateValid()) { - _hgt_sensor_offset = _terrain_vpos; - - } else { - _hgt_sensor_offset = _R_rng_to_earth_2_2 * _range_sample_delayed.rng + _state.pos(2); - } + _hgt_sensor_offset = _terrain_vpos; } - } else if (!_range_aid_mode_selected && _baro_data_ready && !_baro_hgt_faulty) { + } else if (!do_range_aid && _baro_data_ready && !_baro_hgt_faulty) { setControlBaroHeight(); fuse_height = true; @@ -990,9 +989,9 @@ void Ekf::controlHeightFusion() break; case VDIST_SENSOR_RANGE: - if (_range_data_ready && _rng_hgt_valid) { + if (_range_sensor.isDataHealthy()) { setControlRangeHeight(); - fuse_height = _range_data_ready; + fuse_height = true; if (_control_status_prev.flags.rng_hgt != _control_status.flags.rng_hgt) { // we have just switched to using range finder, calculate height sensor offset such that current @@ -1002,7 +1001,7 @@ void Ekf::controlHeightFusion() _hgt_sensor_offset = _terrain_vpos; } else if (_control_status.flags.in_air) { - _hgt_sensor_offset = _R_rng_to_earth_2_2 * _range_sample_delayed.rng + _state.pos(2); + _hgt_sensor_offset = _range_sensor.getCosTilt() * _range_sensor.getRange() + _state.pos(2); } else { _hgt_sensor_offset = _params.rng_gnd_clearance; @@ -1025,22 +1024,17 @@ void Ekf::controlHeightFusion() case VDIST_SENSOR_GPS: // Determine if GPS should be used as the height source - if (_range_aid_mode_selected && _range_data_ready && _rng_hgt_valid) { + if (do_range_aid && _range_sensor.isDataHealthy()) { setControlRangeHeight(); fuse_height = true; // we have just switched to using range finder, calculate height sensor offset such that current // measurement matches our current height estimate if (_control_status_prev.flags.rng_hgt != _control_status.flags.rng_hgt) { - if (isTerrainEstimateValid()) { - _hgt_sensor_offset = _terrain_vpos; - - } else { - _hgt_sensor_offset = _R_rng_to_earth_2_2 * _range_sample_delayed.rng + _state.pos(2); - } + _hgt_sensor_offset = _terrain_vpos; } - } else if (!_range_aid_mode_selected && _gps_data_ready && !_gps_hgt_intermittent && _gps_checks_passed) { + } else if (!do_range_aid && _gps_data_ready && !_gps_hgt_intermittent && _gps_checks_passed) { setControlGPSHeight(); fuse_height = true; @@ -1103,14 +1097,14 @@ void Ekf::controlHeightFusion() } if (isTimedOut(_time_last_hgt_fuse, 2 * RNG_MAX_INTERVAL) && _control_status.flags.rng_hgt - && (!_range_data_ready || !_rng_hgt_valid)) { + && (!_range_sensor.isDataHealthy())) { // 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 if (!_control_status.flags.in_air) { - _range_sample_delayed.rng = _params.rng_gnd_clearance; - _range_sample_delayed.time_us = _imu_sample_delayed.time_us; - + _range_sensor.setRange(_params.rng_gnd_clearance); + _range_sensor.setDataReadiness(true); + _range_sensor.setValidity(true); // bypass the checks } fuse_height = true; @@ -1163,15 +1157,14 @@ void Ekf::controlHeightFusion() fuseVerticalPosition(_gps_pos_innov,gps_hgt_innov_gate, gps_hgt_obs_var, _gps_pos_innov_var,_gps_pos_test_ratio); - } else if (_control_status.flags.rng_hgt && (_R_rng_to_earth_2_2 > _params.range_cos_max_tilt)) { - // TODO: Tilt check does not belong here, should not set fuse height to true if tilted + } else if (_control_status.flags.rng_hgt) { Vector2f rng_hgt_innov_gate; Vector3f rng_hgt_obs_var; // use range finder with tilt correction - _rng_hgt_innov(2) = _state.pos(2) - (-math::max(_range_sample_delayed.rng * _R_rng_to_earth_2_2, + _rng_hgt_innov(2) = _state.pos(2) - (-math::max(_range_sensor.getRange() * _range_sensor.getCosTilt(), _params.rng_gnd_clearance)) - _hgt_sensor_offset; // observation variance - user parameter defined - rng_hgt_obs_var(2) = fmaxf((sq(_params.range_noise) + sq(_params.range_noise_scaler * _range_sample_delayed.rng)) * sq(_R_rng_to_earth_2_2), 0.01f); + rng_hgt_obs_var(2) = fmaxf((sq(_params.range_noise) + sq(_params.range_noise_scaler * _range_sensor.getRange())) * sq(_range_sensor.getCosTilt()), 0.01f); // innovation gate size rng_hgt_innov_gate(1) = fmaxf(_params.range_innov_gate, 1.0f); // fuse height information @@ -1197,7 +1190,7 @@ void Ekf::controlHeightFusion() void Ekf::checkRangeAidSuitability() { if (_control_status.flags.in_air - && _rng_hgt_valid + && _range_sensor.isHealthy() && isTerrainEstimateValid() && isHorizontalAidingActive()) { // check if we can use range finder measurements to estimate height, use hysteresis to avoid rapid switching diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 8443d242c3..b70dbda89d 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -75,8 +75,8 @@ void Ekf::reset() _filter_initialised = false; _terrain_initialised = false; - _sin_tilt_rng = sinf(_params.rng_sens_pitch); - _cos_tilt_rng = cosf(_params.rng_sens_pitch); + _range_sensor.setTiltOffset(_params.rng_sens_pitch); + _range_sensor.setCosMaxTilt(_params.range_cos_max_tilt); _control_status.value = 0; _control_status_prev.value = 0; diff --git a/EKF/ekf.h b/EKF/ekf.h index e45bb8a846..e070204fcc 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -312,7 +312,6 @@ private: } _state_reset_status{}; ///< reset event monitoring structure containing velocity, position, height and yaw reset information float _dt_ekf_avg{FILTER_UPDATE_PERIOD_S}; ///< average update rate of the ekf - float _dt_update{0.01f}; ///< delta time since last ekf update. This time can be used for filters which run at the same rate as the Ekf::update() function. (sec) Vector3f _ang_rate_delayed_raw; ///< uncorrected angular rate vector at fusion time horizon (rad/sec) @@ -331,7 +330,6 @@ private: bool _gps_data_ready{false}; ///< true when new GPS data has fallen behind the fusion time horizon and is available to be fused bool _mag_data_ready{false}; ///< true when new magnetometer data has fallen behind the fusion time horizon and is available to be fused bool _baro_data_ready{false}; ///< true when new baro height data has fallen behind the fusion time horizon and is available to be fused - bool _range_data_ready{false}; ///< true when new range finder data has fallen behind the fusion time horizon and is available to be fused bool _flow_data_ready{false}; ///< true when the leading edge of the optical flow integration period has fallen behind the fusion time horizon bool _ev_data_ready{false}; ///< true when new external vision system data has fallen behind the fusion time horizon and is available to be fused bool _tas_data_ready{false}; ///< true when new true airspeed data has fallen behind the fusion time horizon and is available to be fused @@ -348,7 +346,6 @@ 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) uint64_t _time_last_mag{0}; ///< measurement time of last magnetomter sample (uSec) uint64_t _time_last_fake_pos{0}; ///< last time we faked position measurements to constrain tilt errors during operation without external aiding (uSec) @@ -491,17 +488,11 @@ private: float _terrain_var{1e4f}; ///< variance of terrain position estimate (m**2) uint64_t _time_last_hagl_fuse{0}; ///< last system time that the hagl measurement failed it's checks (uSec) bool _terrain_initialised{false}; ///< true when the terrain estimator has been initialized - 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 - 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 // 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_valid{false}; ///< true if range finder sample retrieved from buffer is valid - 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) @@ -510,11 +501,6 @@ private: // variables used to control range aid functionality bool _is_range_aid_suitable{false}; ///< true when range finder can be used in flight as the height reference instead of the primary height sensor - bool _range_aid_mode_selected{false}; ///< true when range finder is being used as the height reference instead of the primary height sensor - - // variables used to check range finder validity data - float _rng_stuck_min_val{0.0f}; ///< minimum value for new rng measurement when being stuck - float _rng_stuck_max_val{0.0f}; ///< maximum value for new rng measurement when being stuck float _height_rate_lpf{0.0f}; @@ -745,12 +731,6 @@ private: void checkRangeAidSuitability(); bool isRangeAidSuitable() { return _is_range_aid_suitable; } - // 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; } @@ -798,8 +778,6 @@ 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); diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index b8d367735d..69ac50769f 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -50,7 +50,7 @@ bool Ekf::resetVelocity() { // used to calculate the velocity change due to the reset - Vector3f vel_before_reset = _state.vel; + const Vector3f vel_before_reset = _state.vel; // reset EKF states if (_control_status.flags.gps && isTimedOut(_last_gps_fail_us, (uint64_t)_min_gps_health_time_us)) { @@ -64,10 +64,10 @@ bool Ekf::resetVelocity() } else if (_control_status.flags.opt_flow) { ECL_INFO_TIMESTAMPED("reset velocity to flow"); // constrain height above ground to be above minimum possible - float heightAboveGndEst = fmaxf((_terrain_vpos - _state.pos(2)), _params.rng_gnd_clearance); + const float heightAboveGndEst = fmaxf((_terrain_vpos - _state.pos(2)), _params.rng_gnd_clearance); // calculate absolute distance from focal point to centre of frame assuming a flat earth - float range = heightAboveGndEst / _R_rng_to_earth_2_2; + const float range = heightAboveGndEst / _range_sensor.getCosTilt(); if ((range - _params.rng_gnd_clearance) > 0.3f && _flow_sample_delayed.dt > 0.05f) { // we should have reliable OF measurements so @@ -227,7 +227,7 @@ void Ekf::resetHeight() // reset the vertical position if (_control_status.flags.rng_hgt) { - float new_pos_down = _hgt_sensor_offset - _range_sample_delayed.rng * _R_rng_to_earth_2_2; + const float new_pos_down = _hgt_sensor_offset - _range_sensor.getRange() * _range_sensor.getCosTilt(); // update the state and associated variance _state.pos(2) = new_pos_down; @@ -1052,9 +1052,9 @@ hagl_max : Maximum height above ground (meters). NaN when limiting is not needed void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, float *hagl_max) { // Calculate range finder limits - const float rangefinder_hagl_min = _rng_valid_min_val; + const float rangefinder_hagl_min = _range_sensor.getValidMinVal(); // Allow use of 75% of rangefinder maximum range to allow for angular motion - const float rangefinder_hagl_max = 0.75f * _rng_valid_max_val; + const float rangefinder_hagl_max = 0.75f * _range_sensor.getValidMaxVal(); // Calculate optical flow limits // Allow ground relative velocity to use 50% of available flow sensor range to allow for angular motion diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index c9f524fcf6..09c666e1ff 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -47,6 +47,7 @@ #include "AlphaFilter.hpp" #include "imu_down_sampler.hpp" #include "EKFGSF_yaw.h" +#include "sensor_range_finder.hpp" #include #include @@ -230,8 +231,7 @@ public: // set sensor limitations reported by the rangefinder void set_rangefinder_limits(float min_distance, float max_distance) { - _rng_valid_min_val = min_distance; - _rng_valid_max_val = max_distance; + _range_sensor.setLimits(min_distance, max_distance); } // set sensor limitations reported by the optical flow sensor @@ -436,7 +436,7 @@ protected: magSample _mag_sample_delayed{}; baroSample _baro_sample_delayed{}; gpsSample _gps_sample_delayed{}; - rangeSample _range_sample_delayed{}; + sensor::SensorRangeFinder _range_sensor{}; airspeedSample _airspeed_sample_delayed{}; flowSample _flow_sample_delayed{}; extVisionSample _ev_sample_delayed{}; @@ -450,8 +450,6 @@ protected: float _air_density{CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C}; // air density (kg/m**3) // Sensor limitations - float _rng_valid_min_val{0.0f}; ///< minimum distance that the rangefinder can measure (m) - float _rng_valid_max_val{0.0f}; ///< maximum distance that the rangefinder can measure (m) float _flow_max_rate{0.0f}; ///< maximum angular flow rate that the optical flow sensor can measure (rad/s) float _flow_min_distance{0.0f}; ///< minimum distance that the optical flow sensor can operate at (m) float _flow_max_distance{0.0f}; ///< maximum distance that the optical flow sensor can operate at (m) diff --git a/EKF/range_finder_checks.cpp b/EKF/range_finder_checks.cpp deleted file mode 100644 index bf54e50a7a..0000000000 --- a/EKF/range_finder_checks.cpp +++ /dev/null @@ -1,139 +0,0 @@ -/**************************************************************************** - * - * 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" - -// 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); -} - -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 && !isRangeDataContinuous()) { - 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 (_imu_sample_delayed.time_us - _time_bad_rng_signal_quality > (unsigned)_params.range_signal_hysteresis_ms) { - _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; - } - } - - updateRangeDataStuck(); - - _rng_hgt_valid = _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) && - _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; - } - - } else { - _time_last_rng_ready = _range_sample_delayed.time_us; - } -} diff --git a/EKF/sensor_range_finder.cpp b/EKF/sensor_range_finder.cpp new file mode 100644 index 0000000000..afa8a2b263 --- /dev/null +++ b/EKF/sensor_range_finder.cpp @@ -0,0 +1,139 @@ +/**************************************************************************** + * + * Copyright (c) 2020 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 sensor_range_finder.cpp + * + * @author Mathieu Bresciani + * + */ + +#include "sensor_range_finder.hpp" + +namespace estimator +{ +namespace sensor +{ + +void SensorRangeFinder::runChecks(const uint64_t current_time_us, const Dcmf &R_to_earth) +{ + updateSensorToEarthRotation(R_to_earth); + updateValidity(current_time_us); +} + +void SensorRangeFinder::updateSensorToEarthRotation(const Dcmf &R_to_earth) +{ + // calculate 2,2 element of rotation matrix from sensor frame to earth frame + // this is required for use of range finder and flow data + _cos_tilt_rng_to_earth = R_to_earth(2, 0) * _sin_tilt_rng + R_to_earth(2, 2) * _cos_tilt_rng; +} + +void SensorRangeFinder::updateValidity(uint64_t current_time_us) +{ + updateDtDataLpf(current_time_us); + + if (isSampleOutOfDate(current_time_us) || !isDataContinuous()) { + _is_sample_valid = false; + return; + } + + // Don't run the checks unless we have retrieved new data from the buffer + if (_is_sample_ready) { + _is_sample_valid = false; + + if (_sample.quality == 0) { + _time_bad_quality_us = current_time_us; + + } else if (current_time_us - _time_bad_quality_us > _quality_hyst_us) { + // We did not receive bad quality data for some time + + if (isTiltOk() && isDataInRange()) { + updateStuckCheck(); + + if (!_is_stuck) { + _is_sample_valid = true; + _time_last_valid_us = _sample.time_us; + } + } + } + } +} + +void SensorRangeFinder::updateDtDataLpf(uint64_t current_time_us) +{ + // Calculate a first order IIR low-pass filtered time of arrival between samples using a 2 second time constant. + float alpha = 0.5f * _dt_update; + _dt_data_lpf = _dt_data_lpf * (1.0f - alpha) + alpha * (current_time_us - _sample.time_us); + + // Apply spike protection to the filter state. + _dt_data_lpf = fminf(_dt_data_lpf, 4e6f); +} + +inline bool SensorRangeFinder::isSampleOutOfDate(uint64_t current_time_us) const +{ + return (current_time_us - _sample.time_us) > 2 * RNG_MAX_INTERVAL; +} + +inline bool SensorRangeFinder::isDataInRange() const +{ + return (_sample.rng >= _rng_valid_min_val) && (_sample.rng <= _rng_valid_max_val); +} + +void SensorRangeFinder::updateStuckCheck() +{ + // 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 (((_sample.time_us - _time_last_valid_us) > (uint64_t)10e6)) { + + // require a variance of rangefinder values to check for "stuck" measurements + if (_stuck_max_val - _stuck_min_val > _stuck_threshold) { + _stuck_min_val = 0.0f; + _stuck_max_val = 0.0f; + _is_stuck = false; + + } else { + if (_sample.rng > _stuck_max_val) { + _stuck_max_val = _sample.rng; + } + + if (_stuck_min_val < 0.1f || _sample.rng < _stuck_min_val) { + _stuck_min_val = _sample.rng; + } + + _is_stuck = true; + } + } +} + +} // namespace sensor +} // namespace estimator diff --git a/EKF/sensor_range_finder.hpp b/EKF/sensor_range_finder.hpp new file mode 100644 index 0000000000..7eb7592648 --- /dev/null +++ b/EKF/sensor_range_finder.hpp @@ -0,0 +1,151 @@ +/**************************************************************************** + * + * Copyright (c) 2020 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 sensor_range_finder.hpp + * Range finder class containing all the required checks + * + * @author Mathieu Bresciani + * + */ +#pragma once + +#include "Sensor.hpp" +#include + +namespace estimator +{ +namespace sensor +{ + +class SensorRangeFinder : public Sensor +{ +public: + SensorRangeFinder() = default; + ~SensorRangeFinder() override = default; + + void runChecks(uint64_t current_time_us, const matrix::Dcmf &R_to_earth); + bool isHealthy() const override { return _is_sample_valid; } + bool isDataHealthy() const override { return _is_sample_ready && _is_sample_valid; } + + void setSample(const rangeSample &sample) { + _sample = sample; + _is_sample_ready = true; + } + + // This is required because of the ring buffer + // TODO: move the ring buffer here + rangeSample* getSampleAddress() { return &_sample; } + + void setTiltOffset(float new_tilt_offset) + { + if (fabsf(_tilt_offset_rad - new_tilt_offset) > FLT_EPSILON) { + _sin_tilt_rng = sinf(new_tilt_offset); + _cos_tilt_rng = cosf(new_tilt_offset); + _tilt_offset_rad = new_tilt_offset; + } + } + + void setCosMaxTilt(float cos_max_tilt) { _range_cos_max_tilt = cos_max_tilt; } + + void setLimits(float min_distance, float max_distance) { + _rng_valid_min_val = min_distance; + _rng_valid_max_val = max_distance; + } + + float getCosTilt() const { return _cos_tilt_rng_to_earth; } + + void setRange(float rng) { _sample.rng = rng; } + float getRange() const { return _sample.rng; } + + void setDataReadiness(bool is_ready) { _is_sample_ready = is_ready; } + void setValidity(bool is_valid) { _is_sample_valid = is_valid; } + + float getValidMinVal() const { return _rng_valid_min_val; } + float getValidMaxVal() const { return _rng_valid_max_val; } + +private: + void updateSensorToEarthRotation(const matrix::Dcmf &R_to_earth); + + void updateValidity(uint64_t current_time_us); + void updateDtDataLpf(uint64_t current_time_us); + bool isSampleOutOfDate(uint64_t current_time_us) const; + bool isDataContinuous() const { return _dt_data_lpf < 2e6f; } + bool isTiltOk() const { return _cos_tilt_rng_to_earth > _range_cos_max_tilt; } + bool isDataInRange() const; + void updateStuckCheck(); + + rangeSample _sample{}; + + bool _is_sample_ready{}; ///< true when new range finder data has fallen behind the fusion time horizon and is available to be fused + bool _is_sample_valid{}; ///< true if range finder sample retrieved from buffer is valid + uint64_t _time_last_valid_us{}; ///< time the last range finder measurement was ready (uSec) + + /* + * Stuck check + */ + bool _is_stuck{}; + float _stuck_threshold{0.1f}; ///< minimum variation in range finder reading required to declare a range finder 'unstuck' when readings recommence after being out of range (m) + float _stuck_min_val{}; ///< minimum value for new rng measurement when being stuck + float _stuck_max_val{}; ///< maximum value for new rng measurement when being stuck + + /* + * Data regularity check + */ + static constexpr float _dt_update{0.01f}; ///< delta time since last ekf update TODO: this should be a parameter + float _dt_data_lpf{}; ///< filtered value of the delta time elapsed since the last range measurement came into the filter (uSec) + + /* + * Tilt check + */ + float _cos_tilt_rng_to_earth{}; ///< 2,2 element of the rotation matrix from sensor frame to earth frame + float _range_cos_max_tilt{0.7071f}; ///< cosine of the maximum tilt angle from the vertical that permits use of range finder and flow data + float _tilt_offset_rad{3.14f}; ///< range finder tilt rotation about the Y body axis + float _sin_tilt_rng{0.0f}; ///< sine of the range finder tilt rotation about the Y body axis + float _cos_tilt_rng{-1.0f}; ///< cosine of the range finder tilt rotation about the Y body axis + + /* + * Range check + */ + float _rng_valid_min_val{}; ///< minimum distance that the rangefinder can measure (m) + float _rng_valid_max_val{}; ///< maximum distance that the rangefinder can measure (m) + + /* + * Quality check + */ + uint64_t _time_bad_quality_us{}; ///< timestamp at which range finder signal quality was 0 (used for hysteresis) + uint64_t _quality_hyst_us{1000000}; ///< minimum duration during which the reported range finder signal quality needs to be non-zero in order to be declared valid (us) +}; + +} // namespace sensor +} // namespace estimator diff --git a/EKF/terrain_estimator.cpp b/EKF/terrain_estimator.cpp index 9465c86489..74986f4d51 100644 --- a/EKF/terrain_estimator.cpp +++ b/EKF/terrain_estimator.cpp @@ -57,11 +57,11 @@ bool Ekf::initHagl() initialized = true; } else if ((_params.terrain_fusion_mode & TerrainFusionMask::TerrainFuseRangeFinder) - && _rng_hgt_valid - && isRecent(latest_measurement.time_us, (uint64_t)2e5) - && _R_rng_to_earth_2_2 > _params.range_cos_max_tilt) { + && _range_sensor.isHealthy() + && isRecent(latest_measurement.time_us, (uint64_t)2e5)) { // if we have a fresh measurement, use it to initialise the terrain estimator - _terrain_vpos = _state.pos(2) + latest_measurement.rng * _R_rng_to_earth_2_2; + // TODO: the latest measurment did not go through the checks and could be invalid! + _terrain_vpos = _state.pos(2) + latest_measurement.rng * _range_sensor.getCosTilt(); // initialise state variance to variance of measurement _terrain_var = sq(_params.range_noise); // success @@ -113,13 +113,8 @@ void Ekf::runTerrainEstimator() _terrain_var = math::constrain(_terrain_var, 0.0f, 1e4f); // Fuse range finder data if available - if (_range_data_ready && _rng_hgt_valid) { + if (_range_sensor.isDataHealthy()) { fuseHagl(); - - // update range sensor angle parameters in case they have changed - // we do this here to avoid doing those calculations at a high rate - _sin_tilt_rng = sinf(_params.rng_sens_pitch); - _cos_tilt_rng = cosf(_params.rng_sens_pitch); } if (_flow_for_terrain_data_ready) { @@ -138,53 +133,47 @@ void Ekf::runTerrainEstimator() void Ekf::fuseHagl() { - // If the vehicle is excessively tilted, do not try to fuse range finder observations - if (_R_rng_to_earth_2_2 > _params.range_cos_max_tilt) { - // get a height above ground measurement from the range finder assuming a flat earth - const float meas_hagl = _range_sample_delayed.rng * _R_rng_to_earth_2_2; + // get a height above ground measurement from the range finder assuming a flat earth + const float meas_hagl = _range_sensor.getRange() * _range_sensor.getCosTilt(); - // predict the hagl from the vehicle position and terrain height - const float pred_hagl = _terrain_vpos - _state.pos(2); + // predict the hagl from the vehicle position and terrain height + const float pred_hagl = _terrain_vpos - _state.pos(2); - // calculate the innovation - _hagl_innov = pred_hagl - meas_hagl; + // calculate the innovation + _hagl_innov = pred_hagl - meas_hagl; - // calculate the observation variance adding the variance of the vehicles own height uncertainty - const float obs_variance = fmaxf(P(9,9) * _params.vehicle_variance_scaler, 0.0f) - + sq(_params.range_noise) - + sq(_params.range_noise_scaler * _range_sample_delayed.rng); + // calculate the observation variance adding the variance of the vehicles own height uncertainty + const float obs_variance = fmaxf(P(9,9) * _params.vehicle_variance_scaler, 0.0f) + + sq(_params.range_noise) + + sq(_params.range_noise_scaler * _range_sensor.getRange()); - // calculate the innovation variance - limiting it to prevent a badly conditioned fusion - _hagl_innov_var = fmaxf(_terrain_var + obs_variance, obs_variance); + // calculate the innovation variance - limiting it to prevent a badly conditioned fusion + _hagl_innov_var = fmaxf(_terrain_var + obs_variance, obs_variance); - // perform an innovation consistency check and only fuse data if it passes - const float gate_size = fmaxf(_params.range_innov_gate, 1.0f); - _hagl_test_ratio = sq(_hagl_innov) / (sq(gate_size) * _hagl_innov_var); + // perform an innovation consistency check and only fuse data if it passes + const float gate_size = fmaxf(_params.range_innov_gate, 1.0f); + _hagl_test_ratio = sq(_hagl_innov) / (sq(gate_size) * _hagl_innov_var); - if (_hagl_test_ratio <= 1.0f) { - // calculate the Kalman gain - float gain = _terrain_var / _hagl_innov_var; - // correct the state - _terrain_vpos -= gain * _hagl_innov; - // correct the variance - _terrain_var = fmaxf(_terrain_var * (1.0f - gain), 0.0f); - // record last successful fusion event - _time_last_hagl_fuse = _time_last_imu; - _innov_check_fail_status.flags.reject_hagl = false; - - } else { - // If we have been rejecting range data for too long, reset to measurement - if (isTimedOut(_time_last_hagl_fuse, (uint64_t)10E6)) { - _terrain_vpos = _state.pos(2) + meas_hagl; - _terrain_var = obs_variance; - - } else { - _innov_check_fail_status.flags.reject_hagl = true; - } - } + if (_hagl_test_ratio <= 1.0f) { + // calculate the Kalman gain + float gain = _terrain_var / _hagl_innov_var; + // correct the state + _terrain_vpos -= gain * _hagl_innov; + // correct the variance + _terrain_var = fmaxf(_terrain_var * (1.0f - gain), 0.0f); + // record last successful fusion event + _time_last_hagl_fuse = _time_last_imu; + _innov_check_fail_status.flags.reject_hagl = false; } else { - _innov_check_fail_status.flags.reject_hagl = true; + // If we have been rejecting range data for too long, reset to measurement + if (isTimedOut(_time_last_hagl_fuse, (uint64_t)10E6)) { + _terrain_vpos = _state.pos(2) + meas_hagl; + _terrain_var = obs_variance; + + } else { + _innov_check_fail_status.flags.reject_hagl = true; + } } } diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 048b68a811..9d5b4bc2ca 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -48,6 +48,7 @@ set(SRCS test_EKF_externalVision.cpp test_EKF_airspeed.cpp test_EKF_withReplayData.cpp + test_SensorRangeFinder.cpp ) add_executable(ECL_GTESTS ${SRCS}) diff --git a/test/sensor_simulator/range_finder.cpp b/test/sensor_simulator/range_finder.cpp index 07eb8ca128..79f596546b 100644 --- a/test/sensor_simulator/range_finder.cpp +++ b/test/sensor_simulator/range_finder.cpp @@ -17,6 +17,7 @@ void RangeFinder::send(uint64_t time) { _range_sample.time_us = time; _ekf->setRangeData(_range_sample); + _ekf->set_rangefinder_limits(0.2f, 20.f); // This usually comes from the driver } void RangeFinder::setData(float range_data_meters, int8_t range_quality) diff --git a/test/test_SensorRangeFinder.cpp b/test/test_SensorRangeFinder.cpp new file mode 100644 index 0000000000..662cd28e10 --- /dev/null +++ b/test/test_SensorRangeFinder.cpp @@ -0,0 +1,299 @@ +/**************************************************************************** + * + * Copyright (c) 2020 ECL Development Team. 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 PX4 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. + * + ****************************************************************************/ + +#include +#include +#include "EKF/common.h" +#include "EKF/sensor_range_finder.hpp" +#include + +using estimator::rangeSample; +using matrix::Dcmf; +using matrix::Eulerf; +using namespace estimator::sensor; + +class SensorRangeFinderTest : public ::testing::Test { +public: + // Setup the Ekf with synthetic measurements + void SetUp() override + { + _range_finder.setTiltOffset(0.f); + _range_finder.setCosMaxTilt(0.707f); + _range_finder.setLimits(_min_range, _max_range); + } + + // Use this method to clean up any memory, network etc. after each test + void TearDown() override + { + } + +protected: + SensorRangeFinder _range_finder{}; + const rangeSample _good_sample{1.f, (uint64_t)2e6, 100}; // {range, time_us, quality} + const float _min_range{0.5f}; + const float _max_range{10.f}; + + void updateSensorAtRate(uint64_t duration_us, uint64_t dt_update_us, uint64_t dt_sensor_us); + void testTilt(const Eulerf &euler, bool should_pass); +}; + +void SensorRangeFinderTest::updateSensorAtRate(uint64_t duration_us, uint64_t dt_update_us, uint64_t dt_sensor_us) +{ + const Dcmf attitude{Eulerf(0.f, 0.f, 0.f)}; + + rangeSample new_sample = _good_sample; + uint64_t t_now_us = _good_sample.time_us; + for (int i = 0; i < int(duration_us / dt_update_us); i++) { + t_now_us += dt_update_us; + if ((i % int(dt_sensor_us / dt_update_us)) == 0) { + new_sample.rng += 0.2f; // update the range to not trigger the stuck detection + if (new_sample.rng > _max_range) { + new_sample.rng = _min_range; + } + new_sample.time_us = t_now_us; + _range_finder.setSample(new_sample); + } + _range_finder.runChecks(t_now_us, attitude); + } +} + +void SensorRangeFinderTest::testTilt(const Eulerf &euler, bool should_pass) +{ + const Dcmf attitude{euler}; + _range_finder.setSample(_good_sample); + _range_finder.runChecks(_good_sample.time_us, attitude); + + if (should_pass) { + EXPECT_TRUE(_range_finder.isDataHealthy()); + EXPECT_TRUE(_range_finder.isHealthy()); + + } else { + EXPECT_FALSE(_range_finder.isDataHealthy()); + EXPECT_FALSE(_range_finder.isHealthy()); + } +} + +TEST_F(SensorRangeFinderTest, setRange) +{ + rangeSample sample{}; + sample.rng = 1.f; + sample.time_us = 1e6; + sample.quality = 9; + + _range_finder.setRange(sample.rng); + _range_finder.setDataReadiness(true); + _range_finder.setValidity(true); + EXPECT_TRUE(_range_finder.isDataHealthy()); + EXPECT_TRUE(_range_finder.isHealthy()); +} + +TEST_F(SensorRangeFinderTest, goodData) +{ + // WHEN: the drone is leveled and the data is good + const Dcmf attitude{Eulerf(0.f, 0.f, 0.f)}; + _range_finder.setSample(_good_sample); + _range_finder.runChecks(_good_sample.time_us, attitude); + + // THEN: the data can be used for aiding + EXPECT_TRUE(_range_finder.isDataHealthy()); + EXPECT_TRUE(_range_finder.isHealthy()); +} + +TEST_F(SensorRangeFinderTest, tiltExceeded) +{ + const Eulerf zero(0.f, 0.f, 0.f); + const Eulerf pitch_46(0.f, 0.8f, 0.f); + const Eulerf pitch_minus46(0.f, -0.8f, 0.f); + const Eulerf pitch_40(0.f, 0.7f, 1.f); + const Eulerf pitch_minus40(0.f, -0.7f, 0.f); + const Eulerf roll_46(0.8f, 0.f, 0.f); + const Eulerf roll_minus46(-0.8f, 0.f, 0.f); + const Eulerf roll_40(0.7f, 0.f, 2.f); + const Eulerf roll_minus40(-0.7f, 0.f, 3.f); + const Eulerf roll_28_pitch_minus28(0.5f, -0.5f, 4.f); + const Eulerf roll_46_pitch_minus46(0.8f, -0.8f, 4.f); + + testTilt(zero, true); + testTilt(pitch_46, false); + testTilt(pitch_minus46, false); + testTilt(pitch_40, true); + testTilt(pitch_minus40, true); + testTilt(roll_46, false); + testTilt(roll_minus46, false); + testTilt(roll_40, true); + testTilt(roll_minus40, true); + testTilt(roll_28_pitch_minus28, true); + testTilt(roll_46_pitch_minus46, false); + +} + +TEST_F(SensorRangeFinderTest, rangeMaxExceeded) +{ + const Dcmf attitude{Eulerf(0.f, 0.f, 0.f)}; + + // WHEN: the measured range is larger than the maximum + rangeSample bad_sample = _good_sample; + bad_sample.rng = _max_range + 0.01f; + _range_finder.setSample(bad_sample); + _range_finder.runChecks(bad_sample.time_us, attitude); + + // THEN: the data should be marked as unhealthy + EXPECT_FALSE(_range_finder.isDataHealthy()); + EXPECT_FALSE(_range_finder.isHealthy()); +} + +TEST_F(SensorRangeFinderTest, rangeMinExceeded) +{ + const Dcmf attitude{Eulerf(0.f, 0.f, 0.f)}; + + // WHEN: the measured range is shorter than the minimum + rangeSample bad_sample = _good_sample; + bad_sample.rng = _min_range - 0.01f; + _range_finder.setSample(bad_sample); + _range_finder.runChecks(bad_sample.time_us, attitude); + + // THEN: the data should be marked as unhealthy + EXPECT_FALSE(_range_finder.isDataHealthy()); + EXPECT_FALSE(_range_finder.isHealthy()); +} + +TEST_F(SensorRangeFinderTest, outOfDate) +{ + const Dcmf attitude{Eulerf(0.f, 0.f, 0.f)}; + + // WHEN: the data is outdated + rangeSample outdated_sample = _good_sample; + outdated_sample.time_us = 0; + uint64_t t_now = _good_sample.time_us; + _range_finder.setSample(outdated_sample); + _range_finder.runChecks(t_now, attitude); + + // THEN: the data should be marked as unhealthy + EXPECT_FALSE(_range_finder.isDataHealthy()); + EXPECT_FALSE(_range_finder.isHealthy()); +} + +TEST_F(SensorRangeFinderTest, rangeStuck) +{ + const Dcmf attitude{Eulerf(0.f, 0.f, 0.f)}; + + // WHEN: the data is first not valid and then + // constantly the same + rangeSample new_sample = _good_sample; + const uint64_t dt = 3e5; + const uint64_t stuck_timeout = 11e6; + new_sample.quality = 0; + for (int i = 0; i < int(stuck_timeout / dt); i++) { + _range_finder.setSample(new_sample); + _range_finder.runChecks(new_sample.time_us, attitude); + new_sample.time_us += dt; + } + EXPECT_FALSE(_range_finder.isDataHealthy()); + EXPECT_FALSE(_range_finder.isHealthy()); + + new_sample.quality = 100; + // we need a few sample to pass the hysteresis check + for (int i = 0; i < int(2e6 / dt); i++) { + _range_finder.setSample(new_sample); + _range_finder.runChecks(new_sample.time_us, attitude); + new_sample.time_us += dt; + } + + // THEN: the data should be marked as unhealthy + // because the sensor is "stuck" + EXPECT_FALSE(_range_finder.isDataHealthy()); + EXPECT_FALSE(_range_finder.isHealthy()); +} + +TEST_F(SensorRangeFinderTest, qualityHysteresis) +{ + const Dcmf attitude{Eulerf(0.f, 0.f, 0.f)}; + + // WHEN: the data is first bad and then good + rangeSample new_sample = _good_sample; + + new_sample.quality = 0; + _range_finder.setSample(new_sample); + _range_finder.runChecks(new_sample.time_us, attitude); + EXPECT_FALSE(_range_finder.isDataHealthy()); + EXPECT_FALSE(_range_finder.isHealthy()); + + new_sample.quality = _good_sample.quality; + _range_finder.setSample(new_sample); + _range_finder.runChecks(new_sample.time_us, attitude); + EXPECT_FALSE(_range_finder.isDataHealthy()); + EXPECT_FALSE(_range_finder.isHealthy()); + + // AND: we need to put enough good data to pass the hysteresis + const uint64_t dt = 3e5; + const uint64_t hyst_time = 1e6; + for (int i = 0; i < int(hyst_time / dt) + 2; i++) { + _range_finder.setSample(new_sample); + _range_finder.runChecks(new_sample.time_us, attitude); + new_sample.time_us += dt; + } + + // THEN: the data is again declared healthy + EXPECT_TRUE(_range_finder.isDataHealthy()); + EXPECT_TRUE(_range_finder.isHealthy()); +} + +TEST_F(SensorRangeFinderTest, continuity) +{ + const Dcmf attitude{Eulerf(0.f, 0.f, 0.f)}; + + // WHEN: the data rate is too slow + const uint64_t dt_update_us = 10e3; + uint64_t dt_sensor_us = 4e6; + uint64_t duration_us = 8e6; + updateSensorAtRate(duration_us, dt_update_us, dt_sensor_us); + + // THEN: the data should be marked as unhealthy + // Note that it also fails the out-of-date test here + EXPECT_FALSE(_range_finder.isDataHealthy()); + EXPECT_FALSE(_range_finder.isHealthy()); + + // AND WHEN: the data rate is acceptable + dt_sensor_us = 3e5; + duration_us = 5e5; + updateSensorAtRate(duration_us, dt_update_us, dt_sensor_us); + + // THEN: it should still fail until the filter converge + // to the new datarate + EXPECT_FALSE(_range_finder.isDataHealthy()); + EXPECT_FALSE(_range_finder.isHealthy()); + + updateSensorAtRate(duration_us, dt_update_us, dt_sensor_us); + EXPECT_TRUE(_range_finder.isDataHealthy()); + EXPECT_TRUE(_range_finder.isHealthy()); +}