diff --git a/EKF/common.h b/EKF/common.h index f2b4676667..643ccecdb5 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -266,6 +266,7 @@ struct parameters { float max_vel_for_range_aid{1.0f}; ///< maximum ground velocity for which we allow to use the range finder as height source (if range_aid == 1) int32_t range_aid{0}; ///< allow switching primary height source to range finder if certian 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 data // vision position fusion float ev_innov_gate{5.0f}; ///< vision estimator fusion innovation consistency gate size (STD) diff --git a/EKF/control.cpp b/EKF/control.cpp index 02a28884eb..ac5ca71020 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -103,7 +103,7 @@ void Ekf::controlFusionModes() // calculate 2,2 element of rotation matrix from sensor frame to earth frame _R_rng_to_earth_2_2 = _R_to_earth(2, 0) * _sin_tilt_rng + _R_to_earth(2, 2) * _cos_tilt_rng; _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); + && (_R_rng_to_earth_2_2 > _params.range_cos_max_tilt); checkForStuckRange(); diff --git a/EKF/terrain_estimator.cpp b/EKF/terrain_estimator.cpp index 5d4b5c1fd7..d6dfff8814 100644 --- a/EKF/terrain_estimator.cpp +++ b/EKF/terrain_estimator.cpp @@ -47,7 +47,7 @@ bool Ekf::initHagl() // get most recent range measurement from buffer rangeSample latest_measurement = _range_buffer.get_newest(); - if ((_time_last_imu - latest_measurement.time_us) < 2e5 && _R_rng_to_earth_2_2 > 0.7071f) { + if ((_time_last_imu - latest_measurement.time_us) < 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 _terrain_vpos = _state.pos(2) + latest_measurement.rng * _R_rng_to_earth_2_2; // initialise state variance to variance of measurement @@ -113,7 +113,7 @@ 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 > 0.7071f) { + 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 float meas_hagl = _range_sample_delayed.rng * _R_rng_to_earth_2_2; diff --git a/EKF/vel_pos_fusion.cpp b/EKF/vel_pos_fusion.cpp index b4ee09e446..01284defa5 100644 --- a/EKF/vel_pos_fusion.cpp +++ b/EKF/vel_pos_fusion.cpp @@ -179,7 +179,7 @@ void Ekf::fuseVelPosHeight() // innovation gate size gate_size[5] = fmaxf(_params.baro_innov_gate, 1.0f); - } else if (_control_status.flags.rng_hgt && (_R_rng_to_earth_2_2 > 0.7071f)) { + } else if (_control_status.flags.rng_hgt && (_R_rng_to_earth_2_2 > _params.range_cos_max_tilt)) { fuse_map[5] = true; // use range finder with tilt correction _vel_pos_innov[5] = _state.pos(2) - (-math::max(_range_sample_delayed.rng * _R_rng_to_earth_2_2,