diff --git a/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp b/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp index 9dd464ad8c..4eb4ed8934 100644 --- a/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp @@ -88,7 +88,9 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed) bool is_tilt_good = true; #if defined(CONFIG_EKF2_RANGE_FINDER) - is_tilt_good = (_R_to_earth(2, 2) > _params.range_cos_max_tilt); + // TODO: magic number -- decouple max tilt for flow fusion from max tilt for range fusion + float cosine_max_tilt = 0.7071f; + is_tilt_good = (_R_to_earth(2, 2) > cosine_max_tilt); #endif // CONFIG_EKF2_RANGE_FINDER calcOptFlowBodyRateComp(flow_sample); diff --git a/src/modules/ekf2/EKF/aid_sources/range_finder/jake_range_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/range_finder/jake_range_height_control.cpp index 607a589880..941e60aad8 100644 --- a/src/modules/ekf2/EKF/aid_sources/range_finder/jake_range_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/range_finder/jake_range_height_control.cpp @@ -68,7 +68,8 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) // TODO: move setting params to init function // Set all of the parameters _range_sensor.setPitchOffset(_params.ekf2_rng_pitch); - _range_sensor.setCosMaxTilt(_params.range_cos_max_tilt); + float cosine_max_tilt = 0.866f; // 30 degrees + _range_sensor.setCosMaxTilt(cosine_max_tilt); _rng_consistency_check.setGate(_params.ekf2_rng_k_gate); // Update sensor to earth rotation diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index 63655750f5..998495981a 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -424,7 +424,6 @@ struct parameters { float ekf2_rng_a_hmax{5.0f}; ///< maximum height above ground for which we allow to use the range finder as height source (if rng_control == 1) float ekf2_rng_a_vmax{1.0f}; ///< maximum ground velocity for which we allow to use the range finder as height source (if rng_control == 1) float ekf2_rng_qlty_t{1.0f}; ///< minimum duration during which the reported range finder signal quality needs to be non-zero in order to be declared valid (s) - 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 ekf2_rng_k_gate{1.0f}; ///< gate size used by the range finder kinematic consistency check float ekf2_rng_fog{0.f}; ///< max distance which a blocked range sensor measures (fog, dirt) [m] diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 980777804a..53e818ac2c 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -353,7 +353,7 @@ private: uint32_t _device_id_baro{0}; hrt_abstime _status_baro_hgt_pub_last{0}; - float _last_baro_bias_published{}; + float _last_baro_bias_published{NAN}; uORB::Subscription _airdata_sub{ORB_ID(vehicle_air_data)};