From 32a5bd32ad85349d2e79a11c4cd3936a7304864b Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 20 Mar 2023 12:41:42 -0400 Subject: [PATCH] ekf2: add kconfig option to enable/disable range finder fusion (and terrain estimator) --- src/modules/ekf2/CMakeLists.txt | 13 +- src/modules/ekf2/EKF/CMakeLists.txt | 13 +- src/modules/ekf2/EKF/common.h | 34 ++-- src/modules/ekf2/EKF/covariance.cpp | 18 +- src/modules/ekf2/EKF/ekf.cpp | 15 +- src/modules/ekf2/EKF/ekf.h | 174 ++++++++++-------- src/modules/ekf2/EKF/ekf_helper.cpp | 17 +- src/modules/ekf2/EKF/estimator_interface.cpp | 10 +- src/modules/ekf2/EKF/estimator_interface.h | 41 +++-- src/modules/ekf2/EKF/height_control.cpp | 4 + src/modules/ekf2/EKF/mag_control.cpp | 4 + src/modules/ekf2/EKF/terrain_estimator.cpp | 5 + src/modules/ekf2/EKF2.cpp | 71 +++++-- src/modules/ekf2/EKF2.hpp | 70 ++++--- src/modules/ekf2/Kconfig | 8 + src/modules/ekf2/Utility/PreFlightChecker.cpp | 10 +- src/modules/ekf2/Utility/PreFlightChecker.hpp | 9 +- 17 files changed, 343 insertions(+), 173 deletions(-) diff --git a/src/modules/ekf2/CMakeLists.txt b/src/modules/ekf2/CMakeLists.txt index a81f15efb3..9b7a6e493f 100644 --- a/src/modules/ekf2/CMakeLists.txt +++ b/src/modules/ekf2/CMakeLists.txt @@ -87,10 +87,6 @@ list(APPEND EKF_SRCS EKF/mag_control.cpp EKF/mag_fusion.cpp EKF/output_predictor.cpp - EKF/range_finder_consistency_check.cpp - EKF/range_height_control.cpp - EKF/sensor_range_finder.cpp - EKF/terrain_estimator.cpp EKF/vel_pos_fusion.cpp EKF/zero_innovation_heading_update.cpp EKF/zero_velocity_update.cpp @@ -129,6 +125,15 @@ if(CONFIG_EKF2_OPTICAL_FLOW) ) endif() +if(CONFIG_EKF2_RANGE_FINDER) + list(APPEND EKF_SRCS + EKF/range_finder_consistency_check.cpp + EKF/range_height_control.cpp + EKF/sensor_range_finder.cpp + EKF/terrain_estimator.cpp + ) +endif() + if(CONFIG_EKF2_SIDESLIP) list(APPEND EKF_SRCS EKF/sideslip_fusion.cpp) endif() diff --git a/src/modules/ekf2/EKF/CMakeLists.txt b/src/modules/ekf2/EKF/CMakeLists.txt index e804d5b866..50cfd236fd 100644 --- a/src/modules/ekf2/EKF/CMakeLists.txt +++ b/src/modules/ekf2/EKF/CMakeLists.txt @@ -52,10 +52,6 @@ list(APPEND EKF_SRCS mag_control.cpp mag_fusion.cpp output_predictor.cpp - range_finder_consistency_check.cpp - range_height_control.cpp - sensor_range_finder.cpp - terrain_estimator.cpp vel_pos_fusion.cpp zero_innovation_heading_update.cpp zero_velocity_update.cpp @@ -94,6 +90,15 @@ if(CONFIG_EKF2_OPTICAL_FLOW) ) endif() +if(CONFIG_EKF2_RANGE_FINDER) + list(APPEND EKF_SRCS + range_finder_consistency_check.cpp + range_height_control.cpp + sensor_range_finder.cpp + terrain_estimator.cpp + ) +endif() + if(CONFIG_EKF2_SIDESLIP) list(APPEND EKF_SRCS sideslip_fusion.cpp) endif() diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index d6113bb4bf..fb5b042800 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -108,10 +108,12 @@ enum MagFuseType : uint8_t { NONE = 5 ///< Do not use magnetometer under any circumstance. Other sources of yaw may be used if selected via the EKF2_AID_MASK parameter. }; +#if defined(CONFIG_EKF2_RANGE_FINDER) enum TerrainFusionMask : uint8_t { TerrainFuseRangeFinder = (1 << 0), TerrainFuseOpticalFlow = (1 << 1) }; +#endif // CONFIG_EKF2_RANGE_FINDER enum HeightSensor : uint8_t { BARO = 0, @@ -300,9 +302,6 @@ struct parameters { int32_t position_sensor_ref{static_cast(PositionSensor::GNSS)}; int32_t baro_ctrl{1}; int32_t gnss_ctrl{GnssCtrl::HPOS | GnssCtrl::VEL}; - int32_t rng_ctrl{RngCtrl::CONDITIONAL}; - int32_t terrain_fusion_mode{TerrainFusionMask::TerrainFuseRangeFinder | - TerrainFusionMask::TerrainFuseOpticalFlow}; ///< aiding source(s) selection bitmask for the terrain estimator int32_t sensor_interval_max_ms{10}; ///< maximum time of arrival difference between non IMU sensor updates. Sets the size of the observation buffers. (mSec) @@ -310,7 +309,6 @@ struct parameters { float mag_delay_ms{0.0f}; ///< magnetometer measurement delay relative to the IMU (mSec) float baro_delay_ms{0.0f}; ///< barometer height measurement delay relative to the IMU (mSec) float gps_delay_ms{110.0f}; ///< GPS measurement delay relative to the IMU (mSec) - float range_delay_ms{5.0f}; ///< range finder measurement delay relative to the IMU (mSec) // input noise float gyro_noise{1.5e-2f}; ///< IMU angular rate noise used for covariance prediction (rad/sec) @@ -324,10 +322,6 @@ struct parameters { float wind_vel_nsd{1.0e-2f}; ///< process noise spectral density for wind velocity prediction (m/sec**2/sqrt(Hz)) const float wind_vel_nsd_scaler{0.5f}; ///< scaling of wind process noise with vertical velocity - float terrain_p_noise{5.0f}; ///< process noise for terrain offset (m/sec) - float terrain_gradient{0.5f}; ///< gradient of terrain used to estimate process noise due to changing position (m/m) - const float terrain_timeout{10.f}; ///< maximum time for invalid bottom distance measurements before resetting terrain estimate (s) - // initialization errors float switch_on_gyro_bias{0.1f}; ///< 1-sigma gyro bias uncertainty at switch on (rad/sec) float switch_on_accel_bias{0.2f}; ///< 1-sigma accelerometer bias uncertainty at switch on (m/sec**2) @@ -379,7 +373,14 @@ struct parameters { const float beta_avg_ft_us{150000.0f}; ///< The average time between synthetic sideslip measurements (uSec) #endif // CONFIG_EKF2_SIDESLIP +#if defined(CONFIG_EKF2_RANGE_FINDER) // range finder fusion + int32_t rng_ctrl{RngCtrl::CONDITIONAL}; + + int32_t terrain_fusion_mode{TerrainFusionMask::TerrainFuseRangeFinder | + TerrainFusionMask::TerrainFuseOpticalFlow}; ///< aiding source(s) selection bitmask for the terrain estimator + + float range_delay_ms{5.0f}; ///< range finder measurement delay relative to the IMU (mSec) float range_noise{0.1f}; ///< observation noise for range finder measurements (m) float range_innov_gate{5.0f}; ///< range finder fusion innovation consistency gate size (STD) float rng_hgt_bias_nsd{0.13f}; ///< process noise for range height bias estimation (m/s/sqrt(Hz)) @@ -394,6 +395,14 @@ struct parameters { 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_kin_consistency_gate{1.0f}; ///< gate size used by the range finder kinematic consistency check + Vector3f rng_pos_body{}; ///< xyz position of range sensor in body frame (m) + + float terrain_p_noise{5.0f}; ///< process noise for terrain offset (m/sec) + float terrain_gradient{0.5f}; ///< gradient of terrain used to estimate process noise due to changing position (m/m) + const float terrain_timeout{10.f}; ///< maximum time for invalid bottom distance measurements before resetting terrain estimate (s) + +#endif // CONFIG_EKF2_RANGE_FINDER + #if defined(CONFIG_EKF2_EXTERNAL_VISION) // vision position fusion int32_t ev_ctrl{0}; @@ -406,6 +415,8 @@ struct parameters { float ev_vel_innov_gate{3.0f}; ///< vision velocity fusion innovation consistency gate size (STD) float ev_pos_innov_gate{5.0f}; ///< vision position fusion innovation consistency gate size (STD) float ev_hgt_bias_nsd{0.13f}; ///< process noise for vision height bias estimation (m/s/sqrt(Hz)) + + Vector3f ev_pos_body{}; ///< xyz position of VI-sensor focal point in body frame (m) #endif // CONFIG_EKF2_EXTERNAL_VISION // gravity fusion @@ -419,6 +430,8 @@ struct parameters { float flow_noise_qual_min{0.5f}; ///< observation noise for optical flow LOS rate measurements when flow sensor quality is at the minimum useable (rad/sec) int32_t flow_qual_min{1}; ///< minimum acceptable quality integer from the flow sensor float flow_innov_gate{3.0f}; ///< optical flow fusion innovation consistency gate size (STD) + + Vector3f flow_pos_body{}; ///< xyz position of range sensor focal point in body frame (m) #endif // CONFIG_EKF2_OPTICAL_FLOW // these parameters control the strictness of GPS quality checks used to determine if the GPS is @@ -435,9 +448,6 @@ struct parameters { // XYZ offset of sensors in body axes (m) Vector3f imu_pos_body{}; ///< xyz position of IMU in body frame (m) Vector3f gps_pos_body{}; ///< xyz position of the GPS antenna in body frame (m) - Vector3f rng_pos_body{}; ///< xyz position of range sensor in body frame (m) - Vector3f flow_pos_body{}; ///< xyz position of range sensor focal point in body frame (m) - Vector3f ev_pos_body{}; ///< xyz position of VI-sensor focal point in body frame (m) // accel bias learning control float acc_bias_lim{0.4f}; ///< maximum accel bias magnitude (m/sec**2) @@ -617,6 +627,7 @@ union ekf_solution_status_u { uint16_t value; }; +#if defined(CONFIG_EKF2_RANGE_FINDER) union terrain_fusion_status_u { struct { bool range_finder : 1; ///< 0 - true if we are fusing range finder data @@ -624,6 +635,7 @@ union terrain_fusion_status_u { } flags; uint8_t value; }; +#endif // CONFIG_EKF2_RANGE_FINDER // define structure used to communicate information events union information_event_status_u { diff --git a/src/modules/ekf2/EKF/covariance.cpp b/src/modules/ekf2/EKF/covariance.cpp index 3072f94c60..b6726fdf6b 100644 --- a/src/modules/ekf2/EKF/covariance.cpp +++ b/src/modules/ekf2/EKF/covariance.cpp @@ -69,16 +69,17 @@ void Ekf::initialiseCovariance() // position P(7,7) = sq(fmaxf(_params.gps_pos_noise, 0.01f)); P(8,8) = P(7,7); + P(9,9) = sq(fmaxf(_params.baro_noise, 0.01f)); + if (_control_status.flags.gps_hgt) { + P(9,9) = sq(fmaxf(1.5f * _params.gps_pos_noise, 0.01f)); + } + +#if defined(CONFIG_EKF2_RANGE_FINDER) if (_control_status.flags.rng_hgt) { P(9,9) = sq(fmaxf(_params.range_noise, 0.01f)); - - } else if (_control_status.flags.gps_hgt) { - P(9,9) = sq(fmaxf(1.5f * _params.gps_pos_noise, 0.01f)); - - } else { - P(9,9) = sq(fmaxf(_params.baro_noise, 0.01f)); } +#endif // CONFIG_EKF2_RANGE_FINDER // gyro bias _prev_delta_ang_bias_var(0) = P(10,10) = sq(_params.switch_on_gyro_bias * dt); @@ -453,7 +454,12 @@ void Ekf::fixCovarianceErrors(bool force_symmetry) if (bad_vz_gps || bad_vz_ev) { bool bad_z_baro = _control_status.flags.baro_hgt && (down_dvel_bias * _aid_src_baro_hgt.innovation < 0.0f); bool bad_z_gps = _control_status.flags.gps_hgt && (down_dvel_bias * _aid_src_gnss_hgt.innovation < 0.0f); + +#if defined(CONFIG_EKF2_RANGE_FINDER) bool bad_z_rng = _control_status.flags.rng_hgt && (down_dvel_bias * _aid_src_rng_hgt.innovation < 0.0f); +#else + bool bad_z_rng = false; +#endif // CONFIG_EKF2_RANGE_FINDER #if defined(CONFIG_EKF2_EXTERNAL_VISION) bool bad_z_ev = _control_status.flags.ev_hgt && (down_dvel_bias * _aid_src_ev_hgt.innovation < 0.0f); diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index a929408274..dec868e988 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/src/modules/ekf2/EKF/ekf.cpp @@ -63,9 +63,11 @@ void Ekf::reset() _state.wind_vel.setZero(); _state.quat_nominal.setIdentity(); +#if defined(CONFIG_EKF2_RANGE_FINDER) _range_sensor.setPitchOffset(_params.rng_sens_pitch); _range_sensor.setCosMaxTilt(_params.range_cos_max_tilt); _range_sensor.setQualityHysteresis(_params.range_valid_quality_s); +#endif // CONFIG_EKF2_RANGE_FINDER _control_status.value = 0; _control_status_prev.value = 0; @@ -94,17 +96,13 @@ void Ekf::reset() _time_last_hor_vel_fuse = 0; _time_last_ver_vel_fuse = 0; _time_last_heading_fuse = 0; - - _time_last_flow_terrain_fuse = 0; _time_last_zero_velocity_fuse = 0; - _time_last_healthy_rng_data = 0; _last_known_pos.setZero(); _time_acc_bias_check = 0; _gps_checks_passed = false; - _gps_alt_ref = NAN; _baro_counter = 0; @@ -115,7 +113,6 @@ void Ekf::reset() _clip_counter = 0; resetEstimatorAidStatus(_aid_src_baro_hgt); - resetEstimatorAidStatus(_aid_src_rng_hgt); #if defined(CONFIG_EKF2_AIRSPEED) resetEstimatorAidStatus(_aid_src_airspeed); #endif // CONFIG_EKF2_AIRSPEED @@ -152,6 +149,10 @@ void Ekf::reset() resetEstimatorAidStatus(_aid_src_optical_flow); resetEstimatorAidStatus(_aid_src_terrain_optical_flow); #endif // CONFIG_EKF2_OPTICAL_FLOW + +#if defined(CONFIG_EKF2_RANGE_FINDER) + resetEstimatorAidStatus(_aid_src_rng_hgt); +#endif // CONFIG_EKF2_RANGE_FINDER } bool Ekf::update() @@ -179,8 +180,10 @@ bool Ekf::update() // control fusion of observation data controlFusionModes(imu_sample_delayed); +#if defined(CONFIG_EKF2_RANGE_FINDER) // run a separate filter for terrain estimation runTerrainEstimator(imu_sample_delayed); +#endif // CONFIG_EKF2_RANGE_FINDER _output_predictor.correctOutputStates(imu_sample_delayed.time_us, getGyroBias(), getAccelBias(), _state.quat_nominal, _state.vel, _state.pos); @@ -218,8 +221,10 @@ bool Ekf::initialiseFilter() // initialise the state covariance matrix now we have starting values for all the states initialiseCovariance(); +#if defined(CONFIG_EKF2_RANGE_FINDER) // Initialise the terrain estimator initHagl(); +#endif // CONFIG_EKF2_RANGE_FINDER // reset the output predictor state history to match the EKF initial values _output_predictor.alignOutputFilter(_state.quat_nominal, _state.vel, _state.pos); diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 1b6f2e688b..db21a6b3f1 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -95,17 +95,41 @@ public: void getBaroHgtInnovVar(float &baro_hgt_innov_var) const { baro_hgt_innov_var = _aid_src_baro_hgt.innovation_variance; } void getBaroHgtInnovRatio(float &baro_hgt_innov_ratio) const { baro_hgt_innov_ratio = _aid_src_baro_hgt.test_ratio; } +#if defined(CONFIG_EKF2_RANGE_FINDER) + // range height + const BiasEstimator::status &getRngHgtBiasEstimatorStatus() const { return _rng_hgt_b_est.getStatus(); } + const auto &aid_src_rng_hgt() const { return _aid_src_rng_hgt; } + void getRngHgtInnov(float &rng_hgt_innov) const { rng_hgt_innov = _aid_src_rng_hgt.innovation; } void getRngHgtInnovVar(float &rng_hgt_innov_var) const { rng_hgt_innov_var = _aid_src_rng_hgt.innovation_variance; } void getRngHgtInnovRatio(float &rng_hgt_innov_ratio) const { rng_hgt_innov_ratio = _aid_src_rng_hgt.test_ratio; } -#if defined(CONFIG_EKF2_AUXVEL) - void getAuxVelInnov(float aux_vel_innov[2]) const; - void getAuxVelInnovVar(float aux_vel_innov[2]) const; - void getAuxVelInnovRatio(float &aux_vel_innov_ratio) const { aux_vel_innov_ratio = math::max(_aid_src_aux_vel.test_ratio[0], _aid_src_aux_vel.test_ratio[1]); } -#endif // CONFIG_EKF2_AUXVEL + void getHaglInnov(float &hagl_innov) const { hagl_innov = _hagl_innov; } + void getHaglInnovVar(float &hagl_innov_var) const { hagl_innov_var = _hagl_innov_var; } + void getHaglInnovRatio(float &hagl_innov_ratio) const { hagl_innov_ratio = _hagl_test_ratio; } + + void getHaglRateInnov(float &hagl_rate_innov) const { hagl_rate_innov = _rng_consistency_check.getInnov(); } + void getHaglRateInnovVar(float &hagl_rate_innov_var) const { hagl_rate_innov_var = _rng_consistency_check.getInnovVar(); } + void getHaglRateInnovRatio(float &hagl_rate_innov_ratio) const { hagl_rate_innov_ratio = _rng_consistency_check.getSignedTestRatioLpf(); } + + // terrain estimate + bool isTerrainEstimateValid() const; + + uint8_t getTerrainEstimateSensorBitfield() const { return _hagl_sensor_status.value; } + + // get the estimated terrain vertical position relative to the NED origin + float getTerrainVertPos() const { return _terrain_vpos; }; + + // get the number of times the vertical terrain position has been reset + uint8_t getTerrainVertPosResetCounter() const { return _terrain_vpos_reset_counter; }; + + // get the terrain variance + float get_terrain_var() const { return _terrain_var; } +#endif // CONFIG_EKF2_RANGE_FINDER #if defined(CONFIG_EKF2_OPTICAL_FLOW) + const auto &aid_src_optical_flow() const { return _aid_src_optical_flow; } + void getFlowInnov(float flow_innov[2]) const; void getFlowInnovVar(float flow_innov_var[2]) const; void getFlowInnovRatio(float &flow_innov_ratio) const { flow_innov_ratio = math::max(_aid_src_optical_flow.test_ratio[0], _aid_src_optical_flow.test_ratio[1]); } @@ -123,10 +147,15 @@ public: void getTerrainFlowInnovVar(float flow_innov_var[2]) const; void getTerrainFlowInnovRatio(float &flow_innov_ratio) const { flow_innov_ratio = math::max(_aid_src_terrain_optical_flow.test_ratio[0], _aid_src_terrain_optical_flow.test_ratio[1]); } - const auto &aid_src_optical_flow() const { return _aid_src_optical_flow; } const auto &aid_src_terrain_optical_flow() const { return _aid_src_terrain_optical_flow; } #endif // CONFIG_EKF2_OPTICAL_FLOW +#if defined(CONFIG_EKF2_AUXVEL) + void getAuxVelInnov(float aux_vel_innov[2]) const; + void getAuxVelInnovVar(float aux_vel_innov[2]) const; + void getAuxVelInnovRatio(float &aux_vel_innov_ratio) const { aux_vel_innov_ratio = math::max(_aid_src_aux_vel.test_ratio[0], _aid_src_aux_vel.test_ratio[1]); } +#endif // CONFIG_EKF2_AUXVEL + void getHeadingInnov(float &heading_innov) const { if (_control_status.flags.mag_hdg) { @@ -230,14 +259,6 @@ public: void getBetaInnovRatio(float &beta_innov_ratio) const { beta_innov_ratio = _aid_src_sideslip.test_ratio; } #endif // CONFIG_EKF2_SIDESLIP - void getHaglInnov(float &hagl_innov) const { hagl_innov = _hagl_innov; } - void getHaglInnovVar(float &hagl_innov_var) const { hagl_innov_var = _hagl_innov_var; } - void getHaglInnovRatio(float &hagl_innov_ratio) const { hagl_innov_ratio = _hagl_test_ratio; } - - void getHaglRateInnov(float &hagl_rate_innov) const { hagl_rate_innov = _rng_consistency_check.getInnov(); } - void getHaglRateInnovVar(float &hagl_rate_innov_var) const { hagl_rate_innov_var = _rng_consistency_check.getInnovVar(); } - void getHaglRateInnovRatio(float &hagl_rate_innov_ratio) const { hagl_rate_innov_ratio = _rng_consistency_check.getSignedTestRatioLpf(); } - void getGravityInnov(float grav_innov[3]) const { memcpy(grav_innov, _aid_src_gravity.innovation, sizeof(_aid_src_gravity.innovation)); } void getGravityInnovVar(float grav_innov_var[3]) const { memcpy(grav_innov_var, _aid_src_gravity.innovation_variance, sizeof(_aid_src_gravity.innovation_variance)); } void getGravityInnovRatio(float &grav_innov_ratio) const { grav_innov_ratio = Vector3f(_aid_src_gravity.test_ratio).max(); } @@ -331,8 +352,6 @@ public: return !_vertical_velocity_deadreckon_time_exceeded && !_control_status.flags.fake_hgt; } - bool isTerrainEstimateValid() const; - bool isYawFinalAlignComplete() const { const bool is_using_mag = (_control_status.flags.mag_3D || _control_status.flags.mag_hdg); @@ -343,17 +362,6 @@ public: && (is_mag_alignment_in_flight_complete || !is_using_mag); } - uint8_t getTerrainEstimateSensorBitfield() const { return _hagl_sensor_status.value; } - - // get the estimated terrain vertical position relative to the NED origin - float getTerrainVertPos() const { return _terrain_vpos; }; - - // get the number of times the vertical terrain position has been reset - uint8_t getTerrainVertPosResetCounter() const { return _terrain_vpos_reset_counter; }; - - // get the terrain variance - float get_terrain_var() const { return _terrain_var; } - // gyro bias (states 10, 11, 12) Vector3f getGyroBias() const { return _state.delta_ang_bias / _dt_ekf_avg; } // get the gyroscope bias in rad/s Vector3f getGyroBiasVariance() const { return Vector3f{P(10, 10), P(11, 11), P(12, 12)} / sq(_dt_ekf_avg); } // get the gyroscope bias variance in rad/s @@ -454,7 +462,7 @@ public: uint8_t getHeightSensorRef() const { return _height_sensor_ref; } const BiasEstimator::status &getBaroBiasEstimatorStatus() const { return _baro_b_est.getStatus(); } const BiasEstimator::status &getGpsHgtBiasEstimatorStatus() const { return _gps_hgt_b_est.getStatus(); } - const BiasEstimator::status &getRngHgtBiasEstimatorStatus() const { return _rng_hgt_b_est.getStatus(); } + #if defined(CONFIG_EKF2_EXTERNAL_VISION) const BiasEstimator::status &getEvHgtBiasEstimatorStatus() const { return _ev_hgt_b_est.getStatus(); } @@ -470,7 +478,6 @@ public: #endif // CONFIG_EKF2_SIDESLIP const auto &aid_src_baro_hgt() const { return _aid_src_baro_hgt; } - const auto &aid_src_rng_hgt() const { return _aid_src_rng_hgt; } const auto &aid_src_fake_hgt() const { return _aid_src_fake_hgt; } const auto &aid_src_fake_pos() const { return _aid_src_fake_pos; } @@ -540,8 +547,6 @@ private: // booleans true when fresh sensor data is available at the fusion time horizon bool _gps_data_ready{false}; ///< true when new GPS 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 _flow_for_terrain_data_ready{false}; /// same flag as "_flow_data_ready" but used for separate terrain estimator uint64_t _time_last_horizontal_aiding{0}; ///< amount of time we have been doing inertial only deadreckoning (uSec) uint64_t _time_last_v_pos_aiding{0}; @@ -552,10 +557,7 @@ private: uint64_t _time_last_hor_vel_fuse{0}; ///< time the last fusion of horizontal velocity measurements was performed (uSec) uint64_t _time_last_ver_vel_fuse{0}; ///< time the last fusion of verticalvelocity measurements was performed (uSec) uint64_t _time_last_heading_fuse{0}; - - uint64_t _time_last_flow_terrain_fuse{0}; ///< time the last fusion of optical flow measurements for terrain estimation were performed (uSec) uint64_t _time_last_zero_velocity_fuse{0}; ///< last time of zero velocity update (uSec) - uint64_t _time_last_healthy_rng_data{0}; Vector3f _last_known_pos{}; ///< last known local position vector (m) @@ -589,8 +591,30 @@ private: Vector2f _drag_innov_var{}; ///< multirotor drag measurement innovation variance ((m/sec**2)**2) #endif // CONFIG_EKF2_DRAG_FUSION +#if defined(CONFIG_EKF2_RANGE_FINDER) + estimator_aid_source1d_s _aid_src_rng_hgt{}; + + HeightBiasEstimator _rng_hgt_b_est{HeightSensor::RANGE, _height_sensor_ref}; + float _hagl_innov{0.0f}; ///< innovation of the last height above terrain measurement (m) float _hagl_innov_var{0.0f}; ///< innovation variance for the last height above terrain measurement (m**2) + float _hagl_test_ratio{}; // height above terrain measurement innovation consistency check ratio + + uint64_t _time_last_healthy_rng_data{0}; + + // Terrain height state estimation + float _terrain_vpos{0.0f}; ///< estimated vertical position of the terrain underneath the vehicle in local NED frame (m) + float _terrain_var{1e4f}; ///< variance of terrain position estimate (m**2) + uint8_t _terrain_vpos_reset_counter{0}; ///< number of times _terrain_vpos has been reset + uint64_t _time_last_hagl_fuse{0}; ///< last system time that a range sample was fused by the terrain estimator + terrain_fusion_status_u _hagl_sensor_status{}; ///< Struct indicating type of sensor used to estimate height above ground + + float _last_on_ground_posD{0.0f}; ///< last vertical position when the in_air status was false (m) +#endif // CONFIG_EKF2_RANGE_FINDER + +#if defined(CONFIG_EKF2_OPTICAL_FLOW) + estimator_aid_source2d_s _aid_src_optical_flow{}; + estimator_aid_source2d_s _aid_src_terrain_optical_flow{}; // optical flow processing Vector3f _flow_gyro_bias{}; ///< bias errors in optical flow sensor rate gyro outputs (rad/sec) @@ -603,8 +627,11 @@ private: uint64_t _time_good_motion_us{0}; ///< last system time that on-ground motion was within limits (uSec) Vector2f _flow_compensated_XY_rad{}; ///< measured delta angle of the image about the X and Y body axes after removal of body rotation (rad), RH rotation is positive + bool _flow_data_ready{false}; ///< true when the leading edge of the optical flow integration period has fallen behind the fusion time horizon + uint64_t _time_last_flow_terrain_fuse{0}; ///< time the last fusion of optical flow measurements for terrain estimation were performed (uSec) +#endif // CONFIG_EKF2_OPTICAL_FLOW + estimator_aid_source1d_s _aid_src_baro_hgt{}; - estimator_aid_source1d_s _aid_src_rng_hgt{}; #if defined(CONFIG_EKF2_AIRSPEED) estimator_aid_source1d_s _aid_src_airspeed{}; #endif // CONFIG_EKF2_AIRSPEED @@ -647,11 +674,6 @@ private: estimator_aid_source2d_s _aid_src_aux_vel{}; #endif // CONFIG_EKF2_AUXVEL -#if defined(CONFIG_EKF2_OPTICAL_FLOW) - estimator_aid_source2d_s _aid_src_optical_flow{}; - estimator_aid_source2d_s _aid_src_terrain_optical_flow{}; -#endif // CONFIG_EKF2_OPTICAL_FLOW - // variables used for the GPS quality checks Vector3f _gps_pos_deriv_filt{}; ///< GPS NED position derivative (m/sec) Vector2f _gps_velNE_filt{}; ///< filtered GPS North and East velocity (m/sec) @@ -678,7 +700,6 @@ private: AlphaFilter _baro_lpf{0.1f}; ///< filtered barometric height measurement (m) // Variables used to control activation of post takeoff functionality - float _last_on_ground_posD{0.0f}; ///< last vertical position when the in_air status was false (m) uint64_t _flt_mag_align_start_time{0}; ///< time that inflight magnetic field alignment started (uSec) uint64_t _time_last_mov_3d_mag_suitable{0}; ///< last system time that sufficient movement to use 3-axis magnetometer fusion was detected (uSec) Vector3f _saved_mag_bf_variance {}; ///< magnetic field state variances that have been saved for use at the next initialisation (Gauss**2) @@ -696,13 +717,6 @@ private: Vector3f _prev_delta_ang_bias_var{}; ///< saved delta angle XYZ bias variances (rad/sec) Vector3f _prev_dvel_bias_var{}; ///< saved delta velocity XYZ bias variances (m/sec)**2 - // Terrain height state estimation - float _terrain_vpos{0.0f}; ///< estimated vertical position of the terrain underneath the vehicle in local NED frame (m) - float _terrain_var{1e4f}; ///< variance of terrain position estimate (m**2) - uint8_t _terrain_vpos_reset_counter{0}; ///< number of times _terrain_vpos has been reset - uint64_t _time_last_hagl_fuse{0}; ///< last system time that a range sample was fused by the terrain estimator - terrain_fusion_status_u _hagl_sensor_status{}; ///< Struct indicating type of sensor used to estimate height above ground - // height sensor status bool _baro_hgt_faulty{false}; ///< true if baro data have been declared faulty TODO: move to fault flags bool _gps_intermittent{true}; ///< true if data into the buffer is intermittent @@ -829,16 +843,19 @@ private: void fuseVelocity(estimator_aid_source2d_s &vel_aid_src); void fuseVelocity(estimator_aid_source3d_s &vel_aid_src); - // calculate optical flow body angular rate compensation - // returns false if bias corrected body rate data is unavailable - bool calcOptFlowBodyRateComp(); +#if defined(CONFIG_EKF2_RANGE_FINDER) + // range height + void controlRangeHeightFusion(); + bool isConditionalRangeAidSuitable(); + void stopRngHgtFusion(); - // initialise the terrain vertical position estimator + // terrain vertical position estimator void initHagl(); - void runTerrainEstimator(const imuSample &imu_delayed); void predictHagl(const imuSample &imu_delayed); + float getTerrainVPos() const { return isTerrainEstimateValid() ? _terrain_vpos : _last_on_ground_posD; } + // update the terrain vertical position estimate using a height above ground measurement from the range finder void controlHaglRngFusion(); void fuseHaglRng(); @@ -848,22 +865,37 @@ private: void stopHaglRngFusion(); float getRngVar(); + void controlHaglFakeFusion(); +#endif // CONFIG_EKF2_RANGE_FINDER + #if defined(CONFIG_EKF2_OPTICAL_FLOW) - // update the terrain vertical position estimate using an optical flow measurement - void controlHaglFlowFusion(); - void startHaglFlowFusion(); - void resetHaglFlow(); - void stopHaglFlowFusion(); - void fuseFlowForTerrain(estimator_aid_source2d_s &flow); + // control fusion of optical flow observations + void controlOpticalFlowFusion(const imuSample &imu_delayed); + void stopFlowFusion(); + + void updateOnGroundMotionForOpticalFlowChecks(); + void resetOnGroundMotionForOpticalFlowChecks(); + + // calculate the measurement variance for the optical flow sensor + float calcOptFlowMeasVar(const flowSample &flow_sample); + + // calculate optical flow body angular rate compensation + // returns false if bias corrected body rate data is unavailable + bool calcOptFlowBodyRateComp(); // fuse optical flow line of sight rate measurements void updateOptFlow(estimator_aid_source2d_s &aid_src); void fuseOptFlow(); float predictFlowRange(); Vector2f predictFlowVelBody(); -#endif // CONFIG_EKF2_OPTICAL_FLOW - void controlHaglFakeFusion(); + // update the terrain vertical position estimate using an optical flow measurement + void controlHaglFlowFusion(); + void startHaglFlowFusion(); + void resetHaglFlow(); + void stopHaglFlowFusion(); + void fuseFlowForTerrain(estimator_aid_source2d_s &flow); +#endif // CONFIG_EKF2_OPTICAL_FLOW // reset the heading and magnetic field states using the declination and magnetometer measurements // return true if successful @@ -981,11 +1013,6 @@ private: void stopEvYawFusion(); #endif // CONFIG_EKF2_EXTERNAL_VISION - // control fusion of optical flow observations - void controlOpticalFlowFusion(const imuSample &imu_delayed); - void updateOnGroundMotionForOpticalFlowChecks(); - void resetOnGroundMotionForOpticalFlowChecks(); - // control fusion of GPS observations void controlGpsFusion(const imuSample &imu_delayed); bool shouldResetGpsFusion() const; @@ -994,8 +1021,6 @@ private: // control fusion of magnetometer observations void controlMagFusion(); - float getTerrainVPos() const { return isTerrainEstimateValid() ? _terrain_vpos : _last_on_ground_posD; } - bool magReset(); bool haglYawResetReq(); @@ -1038,9 +1063,6 @@ private: void checkHeightSensorRefFallback(); void controlBaroHeightFusion(); void controlGnssHeightFusion(const gpsSample &gps_sample); - void controlRangeHeightFusion(); - - bool isConditionalRangeAidSuitable(); void stopMagFusion(); void stopMag3DFusion(); @@ -1050,16 +1072,12 @@ private: void stopBaroHgtFusion(); void stopGpsHgtFusion(); - void stopRngHgtFusion(); void updateGroundEffect(); // gravity fusion: heuristically enable / disable gravity fusion void controlGravityFusion(const imuSample &imu_delayed); - // calculate the measurement variance for the optical flow sensor - float calcOptFlowMeasVar(const flowSample &flow_sample); - // initialise the quaternion covariances using rotation vector variances // do not call before quaternion states are initialised void initialiseQuatCovariances(Vector3f &rot_vec_var); @@ -1075,9 +1093,6 @@ private: void resetWind(); void resetWindToZero(); - // check that the range finder data is continuous - void updateRangeDataContinuity(); - // Increase the yaw error variance of the quaternions // Argument is additional yaw variance in rad**2 void increaseQuatYawErrVariance(float yaw_variance); @@ -1116,8 +1131,6 @@ private: void stopGpsPosFusion(); void stopGpsVelFusion(); - void stopFlowFusion(); - void resetFakePosFusion(); void stopFakePosFusion(); @@ -1138,7 +1151,6 @@ private: HeightBiasEstimator _baro_b_est{HeightSensor::BARO, _height_sensor_ref}; HeightBiasEstimator _gps_hgt_b_est{HeightSensor::GNSS, _height_sensor_ref}; - HeightBiasEstimator _rng_hgt_b_est{HeightSensor::RANGE, _height_sensor_ref}; #if defined(CONFIG_EKF2_EXTERNAL_VISION) HeightBiasEstimator _ev_hgt_b_est{HeightSensor::EV, _height_sensor_ref}; diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index a36918d325..9e89016abb 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -202,7 +202,9 @@ void Ekf::resetVerticalPositionTo(const float new_vert_pos, float new_vert_pos_v _ev_hgt_b_est.setBias(_ev_hgt_b_est.getBias() - delta_z); #endif // CONFIG_EKF2_EXTERNAL_VISION _gps_hgt_b_est.setBias(_gps_hgt_b_est.getBias() + delta_z); +#if defined(CONFIG_EKF2_RANGE_FINDER) _rng_hgt_b_est.setBias(_rng_hgt_b_est.getBias() + delta_z); +#endif // CONFIG_EKF2_RANGE_FINDER // Reset the timout timer _time_last_hgt_fuse = _time_delayed_us; @@ -564,6 +566,7 @@ void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, fl *hagl_min = NAN; *hagl_max = NAN; +#if defined(CONFIG_EKF2_RANGE_FINDER) // Calculate range finder limits const float rangefinder_hagl_min = _range_sensor.getValidMinVal(); @@ -578,6 +581,7 @@ void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, fl *hagl_min = rangefinder_hagl_min; *hagl_max = rangefinder_hagl_max; } +#endif // CONFIG_EKF2_RANGE_FINDER #if defined(CONFIG_EKF2_OPTICAL_FLOW) // Keep within flow AND range sensor limits when exclusively using optical flow @@ -708,10 +712,12 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f n_hgt_sources++; } +#if defined(CONFIG_EKF2_RANGE_FINDER) if (_control_status.flags.rng_hgt) { hgt_sum += sqrtf(_aid_src_rng_hgt.test_ratio); n_hgt_sources++; } +#endif // CONFIG_EKF2_RANGE_FINDER #if defined(CONFIG_EKF2_EXTERNAL_VISION) if (_control_status.flags.ev_hgt) { @@ -732,8 +738,10 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f tas = sqrtf(_aid_src_airspeed.test_ratio); #endif // CONFIG_EKF2_AIRSPEED +#if defined(CONFIG_EKF2_RANGE_FINDER) // return the terrain height innovation test ratio hagl = sqrtf(_hagl_test_ratio); +#endif // CONFIG_EKF2_RANGE_FINDER #if defined(CONFIG_EKF2_SIDESLIP) // return the synthetic sideslip innovation test ratio @@ -744,7 +752,7 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f // return a bitmask integer that describes which state estimates are valid void Ekf::get_ekf_soln_status(uint16_t *status) const { - ekf_solution_status_u soln_status; + ekf_solution_status_u soln_status{}; // TODO: Is this accurate enough? soln_status.flags.attitude = _control_status.flags.tilt_align && _control_status.flags.yaw_align && (_fault_status.value == 0); soln_status.flags.velocity_horiz = (isHorizontalAidingActive() || (_control_status.flags.fuse_beta && _control_status.flags.fuse_aspd)) && (_fault_status.value == 0); @@ -752,7 +760,9 @@ void Ekf::get_ekf_soln_status(uint16_t *status) const soln_status.flags.pos_horiz_rel = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.opt_flow) && (_fault_status.value == 0); soln_status.flags.pos_horiz_abs = (_control_status.flags.gps || _control_status.flags.ev_pos) && (_fault_status.value == 0); soln_status.flags.pos_vert_abs = soln_status.flags.velocity_vert; +#if defined(CONFIG_EKF2_RANGE_FINDER) soln_status.flags.pos_vert_agl = isTerrainEstimateValid(); +#endif // CONFIG_EKF2_RANGE_FINDER soln_status.flags.const_pos_mode = !soln_status.flags.velocity_horiz; soln_status.flags.pred_pos_horiz_rel = soln_status.flags.pos_horiz_rel; soln_status.flags.pred_pos_horiz_abs = soln_status.flags.pos_horiz_abs; @@ -1016,12 +1026,15 @@ void Ekf::initialiseQuatCovariances(Vector3f &rot_vec_var) void Ekf::updateGroundEffect() { if (_control_status.flags.in_air && !_control_status.flags.fixed_wing) { +#if defined(CONFIG_EKF2_RANGE_FINDER) if (isTerrainEstimateValid()) { // automatically set ground effect if terrain is valid float height = _terrain_vpos - _state.pos(2); _control_status.flags.gnd_effect = (height < _params.gnd_effect_max_hgt); - } else if (_control_status.flags.gnd_effect) { + } else +#endif // CONFIG_EKF2_RANGE_FINDER + if (_control_status.flags.gnd_effect) { // Turn off ground effect compensation if it times out if (isTimedOut(_time_last_gnd_effect_on, GNDEFFECT_TIMEOUT)) { _control_status.flags.gnd_effect = false; diff --git a/src/modules/ekf2/EKF/estimator_interface.cpp b/src/modules/ekf2/EKF/estimator_interface.cpp index 7d2cf3a4ee..5cd78ebf35 100644 --- a/src/modules/ekf2/EKF/estimator_interface.cpp +++ b/src/modules/ekf2/EKF/estimator_interface.cpp @@ -49,13 +49,15 @@ EstimatorInterface::~EstimatorInterface() delete _gps_buffer; delete _mag_buffer; delete _baro_buffer; +#if defined(CONFIG_EKF2_RANGE_FINDER) delete _range_buffer; +#endif // CONFIG_EKF2_RANGE_FINDER #if defined(CONFIG_EKF2_AIRSPEED) delete _airspeed_buffer; #endif // CONFIG_EKF2_AIRSPEED #if defined(CONFIG_EKF2_OPTICAL_FLOW) delete _flow_buffer; -#endif // _flow_buffer +#endif // CONFIG_EKF2_OPTICAL_FLOW #if defined(CONFIG_EKF2_EXTERNAL_VISION) delete _ext_vision_buffer; #endif // CONFIG_EKF2_EXTERNAL_VISION @@ -288,6 +290,7 @@ void EstimatorInterface::setAirspeedData(const airspeedSample &airspeed_sample) } #endif // CONFIG_EKF2_AIRSPEED +#if defined(CONFIG_EKF2_RANGE_FINDER) void EstimatorInterface::setRangeData(const rangeSample &range_sample) { if (!_initialised) { @@ -323,6 +326,7 @@ void EstimatorInterface::setRangeData(const rangeSample &range_sample) ECL_WARN("range data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _range_buffer->get_newest().time_us, _min_obs_interval_us); } } +#endif // CONFIG_EKF2_RANGE_FINDER #if defined(CONFIG_EKF2_OPTICAL_FLOW) void EstimatorInterface::setOpticalFlowData(const flowSample &flow) @@ -555,10 +559,12 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp) max_time_delay_ms = math::max(_params.mag_delay_ms, max_time_delay_ms); } +#if defined(CONFIG_EKF2_RANGE_FINDER) // using range finder if ((_params.rng_ctrl != RngCtrl::DISABLED)) { max_time_delay_ms = math::max(_params.range_delay_ms, max_time_delay_ms); } +#endif // CONFIG_EKF2_RANGE_FINDER if (_params.gnss_ctrl > 0) { max_time_delay_ms = math::max(_params.gps_delay_ms, max_time_delay_ms); @@ -700,9 +706,11 @@ void EstimatorInterface::print_status() printf("baro buffer: %d/%d (%d Bytes)\n", _baro_buffer->entries(), _baro_buffer->get_length(), _baro_buffer->get_total_size()); } +#if defined(CONFIG_EKF2_RANGE_FINDER) if (_range_buffer) { printf("range buffer: %d/%d (%d Bytes)\n", _range_buffer->entries(), _range_buffer->get_length(), _range_buffer->get_total_size()); } +#endif // CONFIG_EKF2_RANGE_FINDER #if defined(CONFIG_EKF2_AIRSPEED) if (_airspeed_buffer) { diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index 5513b3c695..7126f57c8b 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -64,11 +64,14 @@ #include "common.h" #include "RingBuffer.h" #include "imu_down_sampler.hpp" -#include "range_finder_consistency_check.hpp" -#include "sensor_range_finder.hpp" #include "utils.hpp" #include "output_predictor.h" +#if defined(CONFIG_EKF2_RANGE_FINDER) +# include "range_finder_consistency_check.hpp" +# include "sensor_range_finder.hpp" +#endif // CONFIG_EKF2_RANGE_FINDER + #include #include #include @@ -94,8 +97,18 @@ public: void setAirspeedData(const airspeedSample &airspeed_sample); #endif // CONFIG_EKF2_AIRSPEED +#if defined(CONFIG_EKF2_RANGE_FINDER) void setRangeData(const rangeSample &range_sample); + // set sensor limitations reported by the rangefinder + void set_rangefinder_limits(float min_distance, float max_distance) + { + _range_sensor.setLimits(min_distance, max_distance); + } + + const rangeSample &get_rng_sample_delayed() { return *(_range_sensor.getSampleAddress()); } +#endif // CONFIG_EKF2_RANGE_FINDER + #if defined(CONFIG_EKF2_OPTICAL_FLOW) // if optical flow sensor gyro delta angles are not available, set gyro_xyz vector fields to NaN and the EKF will use its internal delta angle data instead void setOpticalFlowData(const flowSample &flow); @@ -163,12 +176,6 @@ public: // set air density used by the multi-rotor specific drag force fusion void set_air_density(float air_density) { _air_density = air_density; } - // set sensor limitations reported by the rangefinder - void set_rangefinder_limits(float min_distance, float max_distance) - { - _range_sensor.setLimits(min_distance, max_distance); - } - // the flags considered are opt_flow, gps, ev_vel and ev_pos bool isOnlyActiveSourceOfHorizontalAiding(bool aiding_flag) const; @@ -250,7 +257,6 @@ public: const uint64_t &time_delayed_us() const { return _time_delayed_us; } const gpsSample &get_gps_sample_delayed() const { return _gps_sample_delayed; } - const rangeSample &get_rng_sample_delayed() { return *(_range_sensor.getSampleAddress()); } const bool &global_origin_valid() const { return _NED_origin_initialised; } const MapProjection &global_origin() const { return _pos_ref; } @@ -298,7 +304,7 @@ protected: // measurement samples capturing measurements on the delayed time horizon gpsSample _gps_sample_delayed{}; - sensor::SensorRangeFinder _range_sensor{}; + #if defined(CONFIG_EKF2_AIRSPEED) airspeedSample _airspeed_sample_delayed{}; @@ -308,9 +314,13 @@ protected: extVisionSample _ev_sample_prev{}; #endif // CONFIG_EKF2_EXTERNAL_VISION - RangeFinderConsistencyCheck _rng_consistency_check; +#if defined(CONFIG_EKF2_RANGE_FINDER) + RingBuffer *_range_buffer{nullptr}; + uint64_t _time_last_range_buffer_push{0}; - float _air_density{CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C}; // air density (kg/m**3) + sensor::SensorRangeFinder _range_sensor{}; + RangeFinderConsistencyCheck _rng_consistency_check; +#endif // CONFIG_EKF2_RANGE_FINDER #if defined(CONFIG_EKF2_OPTICAL_FLOW) RingBuffer *_flow_buffer{nullptr}; @@ -323,6 +333,8 @@ protected: float _flow_max_distance{10.f}; ///< maximum distance that the optical flow sensor can operate at (m) #endif // CONFIG_EKF2_OPTICAL_FLOW + float _air_density{CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C}; // air density (kg/m**3) + bool _imu_updated{false}; // true if the ekf should update (completed downsampling process) bool _initialised{false}; // true if the ekf interface instance (data buffering) is initialized @@ -339,8 +351,6 @@ protected: uint64_t _time_last_gps_yaw_buffer_push{0}; #endif // CONFIG_EKF2_GNSS_YAW - float _hagl_test_ratio{}; // height above terrain measurement innovation consistency check ratio - #if defined(CONFIG_EKF2_DRAG_FUSION) RingBuffer *_drag_buffer{nullptr}; dragSample _drag_down_sampled{}; // down sampled drag specific force data (filter prediction rate -> observation rate) @@ -367,7 +377,7 @@ protected: RingBuffer *_gps_buffer{nullptr}; RingBuffer *_mag_buffer{nullptr}; RingBuffer *_baro_buffer{nullptr}; - RingBuffer *_range_buffer{nullptr}; + #if defined(CONFIG_EKF2_AIRSPEED) RingBuffer *_airspeed_buffer{nullptr}; #endif // CONFIG_EKF2_AIRSPEED @@ -384,7 +394,6 @@ protected: uint64_t _time_last_gps_buffer_push{0}; uint64_t _time_last_mag_buffer_push{0}; uint64_t _time_last_baro_buffer_push{0}; - uint64_t _time_last_range_buffer_push{0}; uint64_t _time_last_gnd_effect_on{0}; diff --git a/src/modules/ekf2/EKF/height_control.cpp b/src/modules/ekf2/EKF/height_control.cpp index 90dbeb8c83..c9cfff5a3b 100644 --- a/src/modules/ekf2/EKF/height_control.cpp +++ b/src/modules/ekf2/EKF/height_control.cpp @@ -45,7 +45,9 @@ void Ekf::controlHeightFusion(const imuSample &imu_delayed) controlBaroHeightFusion(); controlGnssHeightFusion(_gps_sample_delayed); +#if defined(CONFIG_EKF2_RANGE_FINDER) controlRangeHeightFusion(); +#endif // CONFIG_EKF2_RANGE_FINDER checkHeightSensorRefFallback(); } @@ -185,9 +187,11 @@ Likelihood Ekf::estimateInertialNavFallingLikelihood() const checks[2] = {ReferenceType::GNSS, _aid_src_gnss_vel.innovation[2], _aid_src_gnss_vel.innovation_variance[2]}; } +#if defined(CONFIG_EKF2_RANGE_FINDER) if (_control_status.flags.rng_hgt) { checks[3] = {ReferenceType::GROUND, _aid_src_rng_hgt.innovation, _aid_src_rng_hgt.innovation_variance}; } +#endif // CONFIG_EKF2_RANGE_FINDER #if defined(CONFIG_EKF2_EXTERNAL_VISION) if (_control_status.flags.ev_hgt) { diff --git a/src/modules/ekf2/EKF/mag_control.cpp b/src/modules/ekf2/EKF/mag_control.cpp index f2f48381ad..47d892737f 100644 --- a/src/modules/ekf2/EKF/mag_control.cpp +++ b/src/modules/ekf2/EKF/mag_control.cpp @@ -238,9 +238,13 @@ bool Ekf::haglYawResetReq() if (_control_status.flags.in_air && _control_status.flags.yaw_align && !_control_status.flags.mag_aligned_in_flight) { // Check if height has increased sufficiently to be away from ground magnetic anomalies // and request a yaw reset if not already requested. +#if defined(CONFIG_EKF2_RANGE_FINDER) static constexpr float mag_anomalies_max_hagl = 1.5f; const bool above_mag_anomalies = (getTerrainVPos() - _state.pos(2)) > mag_anomalies_max_hagl; return above_mag_anomalies; +#else + return true; +#endif // CONFIG_EKF2_RANGE_FINDER } return false; diff --git a/src/modules/ekf2/EKF/terrain_estimator.cpp b/src/modules/ekf2/EKF/terrain_estimator.cpp index bd07cce0ef..a44e6cf3a3 100644 --- a/src/modules/ekf2/EKF/terrain_estimator.cpp +++ b/src/modules/ekf2/EKF/terrain_estimator.cpp @@ -199,6 +199,7 @@ void Ekf::resetHaglRng() _terrain_var = getRngVar(); _terrain_vpos_reset_counter++; _time_last_hagl_fuse = _time_delayed_us; + _time_last_healthy_rng_data = 0; } void Ekf::stopHaglRngFusion() @@ -212,6 +213,8 @@ void Ekf::stopHaglRngFusion() _hagl_sensor_status.flags.range_finder = false; } + + _time_last_healthy_rng_data = 0; } void Ekf::fuseHaglRng() @@ -417,11 +420,13 @@ bool Ekf::isTerrainEstimateValid() const return true; } +#if defined(CONFIG_EKF2_OPTICAL_FLOW) // we have been fusing optical flow measurements for terrain estimation within the last 5 seconds // this can only be the case if the main filter does not fuse optical flow if (_hagl_sensor_status.flags.flow && isRecent(_time_last_flow_terrain_fuse, (uint64_t)5e6)) { return true; } +#endif // CONFIG_EKF2_OPTICAL_FLOW return false; } diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 3f06df9ff5..f7fefd4412 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -63,7 +63,6 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _param_ekf2_mag_delay(_params->mag_delay_ms), _param_ekf2_baro_delay(_params->baro_delay_ms), _param_ekf2_gps_delay(_params->gps_delay_ms), - _param_ekf2_rng_delay(_params->range_delay_ms), #if defined(CONFIG_EKF2_AUXVEL) _param_ekf2_avel_delay(_params->auxvel_delay_ms), #endif // CONFIG_EKF2_AUXVEL @@ -74,8 +73,6 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _param_ekf2_mag_e_noise(_params->mage_p_noise), _param_ekf2_mag_b_noise(_params->magb_p_noise), _param_ekf2_wind_nsd(_params->wind_vel_nsd), - _param_ekf2_terr_noise(_params->terrain_p_noise), - _param_ekf2_terr_grad(_params->terrain_gradient), _param_ekf2_gps_v_noise(_params->gps_vel_noise), _param_ekf2_gps_p_noise(_params->gps_pos_noise), _param_ekf2_noaid_noise(_params->pos_noaid_noise), @@ -117,9 +114,10 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _param_ekf2_hgt_ref(_params->height_sensor_ref), _param_ekf2_baro_ctrl(_params->baro_ctrl), _param_ekf2_gps_ctrl(_params->gnss_ctrl), - _param_ekf2_rng_ctrl(_params->rng_ctrl), - _param_ekf2_terr_mask(_params->terrain_fusion_mode), _param_ekf2_noaid_tout(_params->valid_timeout_max), +#if defined(CONFIG_EKF2_RANGE_FINDER) + _param_ekf2_rng_ctrl(_params->rng_ctrl), + _param_ekf2_rng_delay(_params->range_delay_ms), _param_ekf2_rng_noise(_params->range_noise), _param_ekf2_rng_sfe(_params->range_noise_scaler), _param_ekf2_rng_gate(_params->range_innov_gate), @@ -130,6 +128,13 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _param_ekf2_rng_a_igate(_params->range_aid_innov_gate), _param_ekf2_rng_qlty_t(_params->range_valid_quality_s), _param_ekf2_rng_k_gate(_params->range_kin_consistency_gate), + _param_ekf2_rng_pos_x(_params->rng_pos_body(0)), + _param_ekf2_rng_pos_y(_params->rng_pos_body(1)), + _param_ekf2_rng_pos_z(_params->rng_pos_body(2)), + _param_ekf2_terr_mask(_params->terrain_fusion_mode), + _param_ekf2_terr_noise(_params->terrain_p_noise), + _param_ekf2_terr_grad(_params->terrain_gradient), +#endif // CONFIG_EKF2_RANGE_FINDER #if defined(CONFIG_EKF2_EXTERNAL_VISION) _param_ekf2_ev_delay(_params->ev_delay_ms), _param_ekf2_ev_ctrl(_params->ev_ctrl), @@ -160,9 +165,6 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _param_ekf2_gps_pos_x(_params->gps_pos_body(0)), _param_ekf2_gps_pos_y(_params->gps_pos_body(1)), _param_ekf2_gps_pos_z(_params->gps_pos_body(2)), - _param_ekf2_rng_pos_x(_params->rng_pos_body(0)), - _param_ekf2_rng_pos_y(_params->rng_pos_body(1)), - _param_ekf2_rng_pos_z(_params->rng_pos_body(2)), _param_ekf2_gbias_init(_params->switch_on_gyro_bias), _param_ekf2_abias_init(_params->switch_on_accel_bias), _param_ekf2_angerr_init(_params->initial_tilt_err), @@ -212,7 +214,9 @@ EKF2::~EKF2() #if defined(CONFIG_EKF2_AIRSPEED) perf_free(_msg_missed_airspeed_perf); #endif // CONFIG_EKF2_AIRSPEED +#if defined(CONFIG_EKF2_RANGE_FINDER) perf_free(_msg_missed_distance_sensor_perf); +#endif // CONFIG_EKF2_RANGE_FINDER perf_free(_msg_missed_gps_perf); #if defined(CONFIG_EKF2_AUXVEL) perf_free(_msg_missed_landing_target_pose_perf); @@ -294,12 +298,16 @@ bool EKF2::multi_init(int imu, int mag) #endif // CONFIG_EKF2_GNSS_YAW +#if defined(CONFIG_EKF2_RANGE_FINDER) + // RNG advertise if (_param_ekf2_rng_ctrl.get()) { _estimator_aid_src_rng_hgt_pub.advertise(); _estimator_rng_hgt_bias_pub.advertise(); } +#endif // CONFIG_EKF2_RANGE_FINDER + _attitude_pub.advertise(); _local_position_pub.advertise(); _global_position_pub.advertise(); @@ -341,7 +349,9 @@ int EKF2::print_status() #if defined(CONFIG_EKF2_AIRSPEED) perf_print_counter(_msg_missed_airspeed_perf); #endif // CONFIG_EKF2_AIRSPEED +#if defined(CONFIG_EKF2_RANGE_FINDER) perf_print_counter(_msg_missed_distance_sensor_perf); +#endif // CONFIG_EKF2_RANGE_FINDER perf_print_counter(_msg_missed_gps_perf); #if defined(CONFIG_EKF2_AUXVEL) perf_print_counter(_msg_missed_landing_target_pose_perf); @@ -658,7 +668,9 @@ void EKF2::Run() #endif // CONFIG_EKF2_OPTICAL_FLOW UpdateGpsSample(ekf2_timestamps); UpdateMagSample(ekf2_timestamps); +#if defined(CONFIG_EKF2_RANGE_FINDER) UpdateRangeSample(ekf2_timestamps); +#endif // CONFIG_EKF2_RANGE_FINDER UpdateSystemFlagsSample(ekf2_timestamps); // run the EKF update and output @@ -675,7 +687,9 @@ void EKF2::Run() // publish status/logging messages PublishBaroBias(now); PublishGnssHgtBias(now); +#if defined(CONFIG_EKF2_RANGE_FINDER) PublishRngHgtBias(now); +#endif // CONFIG_EKF2_RANGE_FINDER #if defined(CONFIG_EKF2_EXTERNAL_VISION) PublishEvPosBias(now); #endif // CONFIG_EKF2_EXTERNAL_VISION @@ -749,6 +763,8 @@ void EKF2::VerifyParams() "Baro enabled by EKF2_HGT_REF", _param_ekf2_baro_ctrl.get()); } +#if defined(CONFIG_EKF2_RANGE_FINDER) + if ((_param_ekf2_hgt_ref.get() == HeightSensor::RANGE) && (_param_ekf2_rng_ctrl.get() == RngCtrl::DISABLED)) { _param_ekf2_rng_ctrl.set(1); _param_ekf2_rng_ctrl.commit(); @@ -760,6 +776,8 @@ void EKF2::VerifyParams() "Range enabled by EKF2_HGT_REF", _param_ekf2_rng_ctrl.get()); } +#endif // CONFIG_EKF2_RANGE_FINDER + if ((_param_ekf2_hgt_ref.get() == HeightSensor::GNSS) && !(_param_ekf2_gps_ctrl.get() & GnssCtrl::VPOS)) { _param_ekf2_gps_ctrl.set(_param_ekf2_gps_ctrl.get() | (GnssCtrl::VPOS | GnssCtrl::HPOS | GnssCtrl::VEL)); _param_ekf2_gps_ctrl.commit(); @@ -858,8 +876,10 @@ void EKF2::PublishAidSourceStatus(const hrt_abstime ×tamp) // baro height PublishAidSourceStatus(_ekf.aid_src_baro_hgt(), _status_baro_hgt_pub_last, _estimator_aid_src_baro_hgt_pub); +#if defined(CONFIG_EKF2_RANGE_FINDER) // RNG height PublishAidSourceStatus(_ekf.aid_src_rng_hgt(), _status_rng_hgt_pub_last, _estimator_aid_src_rng_hgt_pub); +#endif // CONFIG_EKF2_RANGE_FINDER // fake position PublishAidSourceStatus(_ekf.aid_src_fake_pos(), _status_fake_pos_pub_last, _estimator_aid_src_fake_pos_pub); @@ -950,6 +970,7 @@ void EKF2::PublishGnssHgtBias(const hrt_abstime ×tamp) } } +#if defined(CONFIG_EKF2_RANGE_FINDER) void EKF2::PublishRngHgtBias(const hrt_abstime ×tamp) { if (_ekf.get_rng_sample_delayed().time_us != 0) { @@ -962,6 +983,7 @@ void EKF2::PublishRngHgtBias(const hrt_abstime ×tamp) } } } +#endif // CONFIG_EKF2_RANGE_FINDER #if defined(CONFIG_EKF2_EXTERNAL_VISION) void EKF2::PublishEvPosBias(const hrt_abstime ×tamp) @@ -1093,7 +1115,7 @@ void EKF2::PublishGlobalPosition(const hrt_abstime ×tamp) const Vector3f position{_ekf.getPosition()}; // generate and publish global position data - vehicle_global_position_s global_pos; + vehicle_global_position_s global_pos{}; global_pos.timestamp_sample = timestamp; // Position of local NED origin in GPS / WGS84 frame @@ -1118,16 +1140,19 @@ void EKF2::PublishGlobalPosition(const hrt_abstime ×tamp) _ekf.get_ekf_gpos_accuracy(&global_pos.eph, &global_pos.epv); + global_pos.terrain_alt = NAN; + global_pos.terrain_alt_valid = false; + +#if defined(CONFIG_EKF2_RANGE_FINDER) + if (_ekf.isTerrainEstimateValid()) { // Terrain altitude in m, WGS84 global_pos.terrain_alt = _ekf.getEkfGlobalOriginAltitude() - _ekf.getTerrainVertPos(); global_pos.terrain_alt_valid = true; - - } else { - global_pos.terrain_alt = NAN; - global_pos.terrain_alt_valid = false; } +#endif // CONFIG_EKF2_RANGE_FINDER + global_pos.dead_reckoning = _ekf.control_status_flags().inertial_dead_reckoning || _ekf.control_status_flags().wind_dead_reckoning; @@ -1181,7 +1206,9 @@ void EKF2::PublishInnovations(const hrt_abstime ×tamp) _ekf.getEvVelPosInnov(innovations.ev_hvel, innovations.ev_vvel, innovations.ev_hpos, innovations.ev_vpos); #endif // CONFIG_EKF2_EXTERNAL_VISION _ekf.getBaroHgtInnov(innovations.baro_vpos); +#if defined(CONFIG_EKF2_RANGE_FINDER) _ekf.getRngHgtInnov(innovations.rng_vpos); +#endif // CONFIG_EKF2_RANGE_FINDER #if defined(CONFIG_EKF2_AUXVEL) _ekf.getAuxVelInnov(innovations.aux_hvel); #endif // CONFIG_EKF2_AUXVEL @@ -1200,8 +1227,10 @@ void EKF2::PublishInnovations(const hrt_abstime ×tamp) #if defined(CONFIG_EKF2_SIDESLIP) _ekf.getBetaInnov(innovations.beta); #endif // CONFIG_EKF2_SIDESLIP +#if defined(CONFIG_EKF2_RANGE_FINDER) _ekf.getHaglInnov(innovations.hagl); _ekf.getHaglRateInnov(innovations.hagl_rate); +#endif // CONFIG_EKF2_RANGE_FINDER _ekf.getGravityInnov(innovations.gravity); // Not yet supported innovations.aux_vvel = NAN; @@ -1230,7 +1259,9 @@ void EKF2::PublishInnovations(const hrt_abstime ×tamp) _preflt_checker.setUsingBaroHgtAiding(_ekf.control_status_flags().baro_hgt); _preflt_checker.setUsingGpsHgtAiding(_ekf.control_status_flags().gps_hgt); +#if defined(CONFIG_EKF2_RANGE_FINDER) _preflt_checker.setUsingRngHgtAiding(_ekf.control_status_flags().rng_hgt); +#endif // CONFIG_EKF2_RANGE_FINDER _preflt_checker.setVehicleCanObserveHeadingInFlight(_ekf.control_status_flags().fixed_wing); @@ -1249,7 +1280,9 @@ void EKF2::PublishInnovationTestRatios(const hrt_abstime ×tamp) _ekf.getEvVelPosInnovRatio(test_ratios.ev_hvel[0], test_ratios.ev_vvel, test_ratios.ev_hpos[0], test_ratios.ev_vpos); #endif // CONFIG_EKF2_EXTERNAL_VISION _ekf.getBaroHgtInnovRatio(test_ratios.baro_vpos); +#if defined(CONFIG_EKF2_RANGE_FINDER) _ekf.getRngHgtInnovRatio(test_ratios.rng_vpos); +#endif // CONFIG_EKF2_RANGE_FINDER #if defined(CONFIG_EKF2_AUXVEL) _ekf.getAuxVelInnovRatio(test_ratios.aux_hvel[0]); #endif // CONFIG_EKF2_AUXVEL @@ -1268,8 +1301,10 @@ void EKF2::PublishInnovationTestRatios(const hrt_abstime ×tamp) #if defined(CONFIG_EKF2_SIDESLIP) _ekf.getBetaInnovRatio(test_ratios.beta); #endif // CONFIG_EKF2_SIDESLIP +#if defined(CONFIG_EKF2_RANGE_FINDER) _ekf.getHaglInnovRatio(test_ratios.hagl); _ekf.getHaglRateInnovRatio(test_ratios.hagl_rate); +#endif // CONFIG_EKF2_RANGE_FINDER _ekf.getGravityInnovRatio(test_ratios.gravity[0]); // Not yet supported test_ratios.aux_vvel = NAN; @@ -1288,7 +1323,9 @@ void EKF2::PublishInnovationVariances(const hrt_abstime ×tamp) _ekf.getEvVelPosInnovVar(variances.ev_hvel, variances.ev_vvel, variances.ev_hpos, variances.ev_vpos); #endif // CONFIG_EKF2_EXTERNAL_VISION _ekf.getBaroHgtInnovVar(variances.baro_vpos); +#if defined(CONFIG_EKF2_RANGE_FINDER) _ekf.getRngHgtInnovVar(variances.rng_vpos); +#endif // CONFIG_EKF2_RANGE_FINDER #if defined(CONFIG_EKF2_AUXVEL) _ekf.getAuxVelInnovVar(variances.aux_hvel); #endif // CONFIG_EKF2_AUXVEL @@ -1307,8 +1344,10 @@ void EKF2::PublishInnovationVariances(const hrt_abstime ×tamp) #if defined(CONFIG_EKF2_SIDESLIP) _ekf.getBetaInnovVar(variances.beta); #endif // CONFIG_EKF2_SIDESLIP +#if defined(CONFIG_EKF2_RANGE_FINDER) _ekf.getHaglInnovVar(variances.hagl); _ekf.getHaglRateInnovVar(variances.hagl_rate); +#endif // CONFIG_EKF2_RANGE_FINDER _ekf.getGravityInnovVar(variances.gravity); // Not yet supported variances.aux_vvel = NAN; @@ -1319,7 +1358,7 @@ void EKF2::PublishInnovationVariances(const hrt_abstime ×tamp) void EKF2::PublishLocalPosition(const hrt_abstime ×tamp) { - vehicle_local_position_s lpos; + vehicle_local_position_s lpos{}; // generate vehicle local position data lpos.timestamp_sample = timestamp; @@ -1376,11 +1415,13 @@ void EKF2::PublishLocalPosition(const hrt_abstime ×tamp) lpos.delta_heading = Eulerf(delta_q_reset).psi(); lpos.heading_good_for_control = _ekf.isYawFinalAlignComplete(); +#if defined(CONFIG_EKF2_RANGE_FINDER) // Distance to bottom surface (ground) in meters // constrain the distance to ground to _rng_gnd_clearance lpos.dist_bottom = math::max(_ekf.getTerrainVertPos() - lpos.z, _param_ekf2_min_rng.get()); lpos.dist_bottom_valid = _ekf.isTerrainEstimateValid(); lpos.dist_bottom_sensor_bitfield = _ekf.getTerrainEstimateSensorBitfield(); +#endif // CONFIG_EKF2_RANGE_FINDER _ekf.get_ekf_lpos_accuracy(&lpos.eph, &lpos.epv); _ekf.get_ekf_vel_accuracy(&lpos.evh, &lpos.evv); @@ -2228,6 +2269,7 @@ void EKF2::UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps) } } +#if defined(CONFIG_EKF2_RANGE_FINDER) void EKF2::UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps) { distance_sensor_s distance_sensor; @@ -2297,6 +2339,7 @@ void EKF2::UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps) _distance_sensor_selected = -1; } } +#endif // CONFIG_EKF2_RANGE_FINDER void EKF2::UpdateSystemFlagsSample(ekf2_timestamps_s &ekf2_timestamps) { diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index af7a5b632e..95b7ece543 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -64,7 +64,6 @@ #include #include #include -#include #include #include #include @@ -106,6 +105,10 @@ # include #endif // CONFIG_EKF2_OPTICAL_FLOW +#if defined(CONFIG_EKF2_RANGE_FINDER) +# include +#endif // CONFIG_EKF2_RANGE_FINDER + extern pthread_mutex_t ekf2_module_mutex; class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem @@ -153,7 +156,10 @@ private: void PublishAttitude(const hrt_abstime ×tamp); void PublishBaroBias(const hrt_abstime ×tamp); void PublishGnssHgtBias(const hrt_abstime ×tamp); + +#if defined(CONFIG_EKF2_RANGE_FINDER) void PublishRngHgtBias(const hrt_abstime ×tamp); +#endif // CONFIG_EKF2_RANGE_FINDER #if defined(CONFIG_EKF2_EXTERNAL_VISION) void PublishEvPosBias(const hrt_abstime ×tamp); @@ -194,7 +200,9 @@ private: #endif // CONFIG_EKF2_OPTICAL_FLOW void UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps); void UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps); +#if defined(CONFIG_EKF2_RANGE_FINDER) void UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps); +#endif // CONFIG_EKF2_RANGE_FINDER void UpdateSystemFlagsSample(ekf2_timestamps_s &ekf2_timestamps); // Used to check, save and use learned accel/gyro/mag biases @@ -249,7 +257,6 @@ private: perf_counter_t _ecl_ekf_update_full_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": ECL full update")}; perf_counter_t _msg_missed_imu_perf{perf_alloc(PC_COUNT, MODULE_NAME": IMU message missed")}; perf_counter_t _msg_missed_air_data_perf{nullptr}; - perf_counter_t _msg_missed_distance_sensor_perf{nullptr}; perf_counter_t _msg_missed_gps_perf{nullptr}; perf_counter_t _msg_missed_magnetometer_perf{nullptr}; perf_counter_t _msg_missed_odometry_perf{nullptr}; @@ -376,10 +383,16 @@ private: uORB::SubscriptionCallbackWorkItem _sensor_combined_sub{this, ORB_ID(sensor_combined)}; uORB::SubscriptionCallbackWorkItem _vehicle_imu_sub{this, ORB_ID(vehicle_imu)}; +#if defined(CONFIG_EKF2_RANGE_FINDER) + uORB::PublicationMulti _estimator_rng_hgt_bias_pub {ORB_ID(estimator_rng_hgt_bias)}; + uORB::PublicationMulti _estimator_aid_src_rng_hgt_pub{ORB_ID(estimator_aid_src_rng_hgt)}; + uORB::SubscriptionMultiArray _distance_sensor_subs{ORB_ID::distance_sensor}; hrt_abstime _last_range_sensor_update{0}; int _distance_sensor_selected{-1}; // because we can have several distance sensor instances with different orientations unsigned _distance_sensor_last_generation{0}; + perf_counter_t _msg_missed_distance_sensor_perf{nullptr}; +#endif // CONFIG_EKF2_RANGE_FINDER bool _callback_registered{false}; @@ -399,7 +412,6 @@ private: uORB::PublicationMulti _ekf2_timestamps_pub{ORB_ID(ekf2_timestamps)}; uORB::PublicationMulti _estimator_baro_bias_pub{ORB_ID(estimator_baro_bias)}; uORB::PublicationMulti _estimator_gnss_hgt_bias_pub{ORB_ID(estimator_gnss_hgt_bias)}; - uORB::PublicationMulti _estimator_rng_hgt_bias_pub{ORB_ID(estimator_rng_hgt_bias)}; uORB::PublicationMultiData _estimator_event_flags_pub{ORB_ID(estimator_event_flags)}; uORB::PublicationMulti _estimator_gps_status_pub{ORB_ID(estimator_gps_status)}; uORB::PublicationMulti _estimator_innovation_test_ratios_pub{ORB_ID(estimator_innovation_test_ratios)}; @@ -412,7 +424,6 @@ private: uORB::PublicationMulti _yaw_est_pub{ORB_ID(yaw_estimator_status)}; uORB::PublicationMulti _estimator_aid_src_baro_hgt_pub {ORB_ID(estimator_aid_src_baro_hgt)}; - uORB::PublicationMulti _estimator_aid_src_rng_hgt_pub{ORB_ID(estimator_aid_src_rng_hgt)}; uORB::PublicationMulti _estimator_aid_src_fake_hgt_pub{ORB_ID(estimator_aid_src_fake_hgt)}; uORB::PublicationMulti _estimator_aid_src_fake_pos_pub{ORB_ID(estimator_aid_src_fake_pos)}; @@ -453,8 +464,6 @@ private: _param_ekf2_baro_delay, ///< barometer height measurement delay relative to the IMU (mSec) (ParamExtFloat) _param_ekf2_gps_delay, ///< GPS measurement delay relative to the IMU (mSec) - (ParamExtFloat) - _param_ekf2_rng_delay, ///< range finder measurement delay relative to the IMU (mSec) #if defined(CONFIG_EKF2_AUXVEL) (ParamExtFloat) @@ -477,9 +486,6 @@ private: _param_ekf2_mag_b_noise, ///< process noise for body magnetic field prediction (Gauss/sec) (ParamExtFloat) _param_ekf2_wind_nsd, ///< process noise spectral density for wind velocity prediction (m/sec**2/sqrt(Hz)) - (ParamExtFloat) _param_ekf2_terr_noise, ///< process noise for terrain offset (m/sec) - (ParamExtFloat) - _param_ekf2_terr_grad, ///< gradient of terrain used to estimate process noise due to changing position (m/m) (ParamExtFloat) _param_ekf2_gps_v_noise, ///< minimum allowed observation noise for gps velocity fusion (m/sec) @@ -560,30 +566,47 @@ private: (ParamExtInt) _param_ekf2_hgt_ref, ///< selects the primary source for height data (ParamExtInt) _param_ekf2_baro_ctrl,///< barometer control selection (ParamExtInt) _param_ekf2_gps_ctrl, ///< GPS control selection - (ParamExtInt) _param_ekf2_rng_ctrl, ///< range finder control selection - (ParamExtInt) - _param_ekf2_terr_mask, ///< bitmasked integer that selects which of range finder and optical flow aiding sources will be used for terrain estimation + (ParamExtInt) _param_ekf2_noaid_tout, ///< maximum lapsed time from last fusion of measurements that constrain drift before the EKF will report the horizontal nav solution invalid (uSec) +#if defined(CONFIG_EKF2_RANGE_FINDER) // range finder fusion + (ParamExtInt) _param_ekf2_rng_ctrl, ///< range finder control selection + (ParamExtFloat) + _param_ekf2_rng_delay, ///< range finder measurement delay relative to the IMU (mSec) (ParamExtFloat) - _param_ekf2_rng_noise, ///< observation noise for range finder measurements (m) - (ParamExtFloat) _param_ekf2_rng_sfe, ///< scale factor from range to range noise (m/m) + _param_ekf2_rng_noise, ///< observation noise for range finder measurements (m) + (ParamExtFloat) + _param_ekf2_rng_sfe, ///< scale factor from range to range noise (m/m) (ParamExtFloat) - _param_ekf2_rng_gate, ///< range finder fusion innovation consistency gate size (STD) - (ParamExtFloat) _param_ekf2_min_rng, ///< minimum valid value for range when on ground (m) - (ParamExtFloat) _param_ekf2_rng_pitch, ///< range sensor pitch offset (rad) + _param_ekf2_rng_gate, ///< range finder fusion innovation consistency gate size (STD) + (ParamExtFloat) + _param_ekf2_min_rng, ///< minimum valid value for range when on ground (m) + (ParamExtFloat) _param_ekf2_rng_pitch, ///< range sensor pitch offset (rad) (ParamExtFloat) - _param_ekf2_rng_a_vmax, ///< maximum allowed horizontal velocity for range aid (m/s) + _param_ekf2_rng_a_vmax, ///< maximum allowed horizontal velocity for range aid (m/s) (ParamExtFloat) - _param_ekf2_rng_a_hmax, ///< maximum allowed absolute altitude (AGL) for range aid (m) + _param_ekf2_rng_a_hmax, ///< maximum allowed absolute altitude (AGL) for range aid (m) (ParamExtFloat) - _param_ekf2_rng_a_igate, ///< gate size used for innovation consistency checks for range aid fusion (STD) + _param_ekf2_rng_a_igate, ///< gate size used for innovation consistency checks for range aid fusion (STD) (ParamExtFloat) - _param_ekf2_rng_qlty_t, ///< Minimum duration during which the reported range finder signal quality needs to be non-zero in order to be declared valid (s) + _param_ekf2_rng_qlty_t, ///< Minimum duration during which the reported range finder signal quality needs to be non-zero in order to be declared valid (s) (ParamExtFloat) - _param_ekf2_rng_k_gate, ///< range finder kinematic consistency gate size (STD) + _param_ekf2_rng_k_gate, ///< range finder kinematic consistency gate size (STD) + (ParamExtFloat) + _param_ekf2_rng_pos_x, ///< X position of range finder in body frame (m) + (ParamExtFloat) + _param_ekf2_rng_pos_y, ///< Y position of range finder in body frame (m) + (ParamExtFloat) + _param_ekf2_rng_pos_z, ///< Z position of range finder in body frame (m) + + (ParamExtInt) + _param_ekf2_terr_mask, ///< bitmasked integer that selects which of range finder and optical flow aiding sources will be used for terrain estimation + (ParamExtFloat) _param_ekf2_terr_noise, ///< process noise for terrain offset (m/sec) + (ParamExtFloat) + _param_ekf2_terr_grad, ///< gradient of terrain used to estimate process noise due to changing position (m/m) +#endif // CONFIG_EKF2_RANGE_FINDER #if defined(CONFIG_EKF2_EXTERNAL_VISION) // vision estimate fusion @@ -642,9 +665,6 @@ private: (ParamExtFloat) _param_ekf2_gps_pos_x, ///< X position of GPS antenna in body frame (m) (ParamExtFloat) _param_ekf2_gps_pos_y, ///< Y position of GPS antenna in body frame (m) (ParamExtFloat) _param_ekf2_gps_pos_z, ///< Z position of GPS antenna in body frame (m) - (ParamExtFloat) _param_ekf2_rng_pos_x, ///< X position of range finder in body frame (m) - (ParamExtFloat) _param_ekf2_rng_pos_y, ///< Y position of range finder in body frame (m) - (ParamExtFloat) _param_ekf2_rng_pos_z, ///< Z position of range finder in body frame (m) // output predictor filter time constants (ParamFloat) diff --git a/src/modules/ekf2/Kconfig b/src/modules/ekf2/Kconfig index 543fa7f7f2..6d27130363 100644 --- a/src/modules/ekf2/Kconfig +++ b/src/modules/ekf2/Kconfig @@ -60,9 +60,17 @@ menuconfig EKF2_OPTICAL_FLOW depends on MODULES_EKF2 bool "optical flow fusion support" default y + depends on EKF2_RANGE_FINDER ---help--- EKF2 optical flow fusion support. +menuconfig EKF2_RANGE_FINDER +depends on MODULES_EKF2 + bool "range finder fusion support" + default y + ---help--- + EKF2 range finder fusion support. + menuconfig EKF2_SIDESLIP depends on MODULES_EKF2 bool "sideslip fusion support" diff --git a/src/modules/ekf2/Utility/PreFlightChecker.cpp b/src/modules/ekf2/Utility/PreFlightChecker.cpp index fa80e5336b..bf7b62f6ac 100644 --- a/src/modules/ekf2/Utility/PreFlightChecker.cpp +++ b/src/modules/ekf2/Utility/PreFlightChecker.cpp @@ -117,11 +117,15 @@ bool PreFlightChecker::preFlightCheckHeightFailed(const estimator_innovations_s has_failed |= checkInnovFailed(gps_hgt_innov_lpf, innov.gps_vpos, _hgt_innov_test_lim, _hgt_innov_spike_lim); } +#if defined(CONFIG_EKF2_RANGE_FINDER) + if (_is_using_rng_hgt_aiding) { const float rng_hgt_innov_lpf = _filter_rng_hgt_innov.update(innov.rng_vpos, alpha, _hgt_innov_spike_lim); has_failed |= checkInnovFailed(rng_hgt_innov_lpf, innov.rng_vpos, _hgt_innov_test_lim, _hgt_innov_spike_lim); } +#endif // CONFIG_EKF2_RANGE_FINDER + if (_is_using_ev_hgt_aiding) { const float ev_hgt_innov_lpf = _filter_ev_hgt_innov.update(innov.ev_vpos, alpha, _hgt_innov_spike_lim); has_failed |= checkInnovFailed(ev_hgt_innov_lpf, innov.ev_vpos, _hgt_innov_test_lim, _hgt_innov_spike_lim); @@ -150,7 +154,6 @@ void PreFlightChecker::reset() _is_using_ev_vel_aiding = false; _is_using_baro_hgt_aiding = false; _is_using_gps_hgt_aiding = false; - _is_using_rng_hgt_aiding = false; _is_using_ev_hgt_aiding = false; _has_heading_failed = false; _has_horiz_vel_failed = false; @@ -161,10 +164,13 @@ void PreFlightChecker::reset() _filter_vel_d_innov.reset(); _filter_baro_hgt_innov.reset(); _filter_gps_hgt_innov.reset(); - _filter_rng_hgt_innov.reset(); _filter_ev_hgt_innov.reset(); _filter_heading_innov.reset(); +#if defined(CONFIG_EKF2_RANGE_FINDER) + _is_using_rng_hgt_aiding = false; + _filter_rng_hgt_innov.reset(); +#endif // CONFIG_EKF2_RANGE_FINDER #if defined(CONFIG_EKF2_OPTICAL_FLOW) _is_using_flow_aiding = false; diff --git a/src/modules/ekf2/Utility/PreFlightChecker.hpp b/src/modules/ekf2/Utility/PreFlightChecker.hpp index 6dfd8b55c0..5e9af2c4a9 100644 --- a/src/modules/ekf2/Utility/PreFlightChecker.hpp +++ b/src/modules/ekf2/Utility/PreFlightChecker.hpp @@ -83,7 +83,9 @@ public: void setUsingBaroHgtAiding(bool val) { _is_using_baro_hgt_aiding = val; } void setUsingGpsHgtAiding(bool val) { _is_using_gps_hgt_aiding = val; } +#if defined(CONFIG_EKF2_RANGE_FINDER) void setUsingRngHgtAiding(bool val) { _is_using_rng_hgt_aiding = val; } +#endif // CONFIG_EKF2_RANGE_FINDER void setUsingEvHgtAiding(bool val) { _is_using_ev_hgt_aiding = val; } bool hasHeadingFailed() const { return _has_heading_failed; } @@ -157,7 +159,6 @@ private: bool _is_using_baro_hgt_aiding{}; bool _is_using_gps_hgt_aiding{}; - bool _is_using_rng_hgt_aiding{}; bool _is_using_ev_hgt_aiding{}; // Low-pass filters for innovation pre-flight checks @@ -169,9 +170,13 @@ private: // Preflight low pass filter height innovation (m) InnovationLpf _filter_baro_hgt_innov; InnovationLpf _filter_gps_hgt_innov; - InnovationLpf _filter_rng_hgt_innov; InnovationLpf _filter_ev_hgt_innov; +#if defined(CONFIG_EKF2_RANGE_FINDER) + bool _is_using_rng_hgt_aiding {}; + InnovationLpf _filter_rng_hgt_innov; +#endif // CONFIG_EKF2_RANGE_FINDER + #if defined(CONFIG_EKF2_OPTICAL_FLOW) bool _is_using_flow_aiding {}; InnovationLpf _filter_flow_x_innov; ///< Preflight low pass filter optical flow innovation (rad)