diff --git a/boards/px4/fmu-v2/default.px4board b/boards/px4/fmu-v2/default.px4board index fefb2c73bb..1e55f5c5a9 100644 --- a/boards/px4/fmu-v2/default.px4board +++ b/boards/px4/fmu-v2/default.px4board @@ -26,6 +26,7 @@ CONFIG_MODULES_EKF2=y # CONFIG_EKF2_AUXVEL is not set # CONFIG_EKF2_BARO_COMPENSATION is not set # CONFIG_EKF2_DRAG_FUSION is not set +# CONFIG_EKF2_EXTERNAL_VISION is not set # CONFIG_EKF2_GNSS_YAW is not set # CONFIG_EKF2_SIDESLIP is not set CONFIG_MODULES_FLIGHT_MODE_MANAGER=y diff --git a/src/modules/ekf2/CMakeLists.txt b/src/modules/ekf2/CMakeLists.txt index 9fca44475b..7c8c37414c 100644 --- a/src/modules/ekf2/CMakeLists.txt +++ b/src/modules/ekf2/CMakeLists.txt @@ -76,11 +76,6 @@ list(APPEND EKF_SRCS EKF/ekf_helper.cpp EKF/EKFGSF_yaw.cpp EKF/estimator_interface.cpp - EKF/ev_control.cpp - EKF/ev_height_control.cpp - EKF/ev_pos_control.cpp - EKF/ev_vel_control.cpp - EKF/ev_yaw_control.cpp EKF/fake_height_control.cpp EKF/fake_pos_control.cpp EKF/gnss_height_control.cpp @@ -115,6 +110,16 @@ if(CONFIG_EKF2_DRAG_FUSION) list(APPEND EKF_SRCS EKF/drag_fusion.cpp) endif() +if(CONFIG_EKF2_EXTERNAL_VISION) + list(APPEND EKF_SRCS + EKF/ev_control.cpp + EKF/ev_height_control.cpp + EKF/ev_pos_control.cpp + EKF/ev_vel_control.cpp + EKF/ev_yaw_control.cpp + ) +endif() + if(CONFIG_EKF2_GNSS_YAW) list(APPEND EKF_SRCS EKF/gps_yaw_fusion.cpp) endif() diff --git a/src/modules/ekf2/EKF/CMakeLists.txt b/src/modules/ekf2/EKF/CMakeLists.txt index 2ba91fdb39..7cae087bca 100644 --- a/src/modules/ekf2/EKF/CMakeLists.txt +++ b/src/modules/ekf2/EKF/CMakeLists.txt @@ -41,11 +41,6 @@ list(APPEND EKF_SRCS ekf_helper.cpp EKFGSF_yaw.cpp estimator_interface.cpp - ev_control.cpp - ev_height_control.cpp - ev_pos_control.cpp - ev_vel_control.cpp - ev_yaw_control.cpp fake_height_control.cpp fake_pos_control.cpp gnss_height_control.cpp @@ -80,6 +75,17 @@ if(CONFIG_EKF2_DRAG_FUSION) list(APPEND EKF_SRCS drag_fusion.cpp) endif() +if(CONFIG_EKF2_EXTERNAL_VISION) + list(APPEND EKF_SRCS + ev_control.cpp + ev_height_control.cpp + ev_pos_control.cpp + ev_vel_control.cpp + ev_yaw_control.cpp + ) +endif() + + if(CONFIG_EKF2_GNSS_YAW) list(APPEND EKF_SRCS gps_yaw_fusion.cpp) endif() diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index db11054ba7..ffecad5e2d 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -238,6 +238,7 @@ struct flowSample { uint8_t quality{}; ///< quality indicator between 0 and 255 }; +#if defined(CONFIG_EKF2_EXTERNAL_VISION) struct extVisionSample { uint64_t time_us{}; ///< timestamp of the measurement (uSec) Vector3f pos{}; ///< XYZ position in external vision's local reference frame (m) - Z must be aligned with down axis @@ -251,6 +252,7 @@ struct extVisionSample { uint8_t reset_counter{}; int8_t quality{}; ///< quality indicator between 0 and 100 }; +#endif // CONFIG_EKF2_EXTERNAL_VISION #if defined(CONFIG_EKF2_DRAG_FUSION) struct dragSample { @@ -299,7 +301,6 @@ struct parameters { int32_t baro_ctrl{1}; int32_t gnss_ctrl{GnssCtrl::HPOS | GnssCtrl::VEL}; int32_t rng_ctrl{RngCtrl::CONDITIONAL}; - int32_t ev_ctrl{0}; int32_t terrain_fusion_mode{TerrainFusionMask::TerrainFuseRangeFinder | TerrainFusionMask::TerrainFuseOpticalFlow}; ///< aiding source(s) selection bitmask for the terrain estimator @@ -311,7 +312,6 @@ struct parameters { float gps_delay_ms{110.0f}; ///< GPS measurement delay relative to the IMU (mSec) float flow_delay_ms{5.0f}; ///< optical flow measurement delay relative to the IMU (mSec) - this is to the middle of the optical flow integration interval float range_delay_ms{5.0f}; ///< range finder measurement delay relative to the IMU (mSec) - float ev_delay_ms{175.0f}; ///< off-board vision 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) @@ -395,7 +395,11 @@ 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 +#if defined(CONFIG_EKF2_EXTERNAL_VISION) // vision position fusion + int32_t ev_ctrl{0}; + float ev_delay_ms{175.0f}; ///< off-board vision measurement delay relative to the IMU (mSec) + float ev_vel_noise{0.1f}; ///< minimum allowed observation noise for EV velocity fusion (m/sec) float ev_pos_noise{0.1f}; ///< minimum allowed observation noise for EV position fusion (m) float ev_att_noise{0.1f}; ///< minimum allowed observation noise for EV attitude fusion (rad/sec) @@ -403,6 +407,7 @@ 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)) +#endif // CONFIG_EKF2_EXTERNAL_VISION // gravity fusion float gravity_noise{1.0f}; ///< accelerometer measurement gaussian noise (m/s**2) diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index 8142ab9f21..04790bae63 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -122,8 +122,10 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed) controlHeightFusion(imu_delayed); controlGravityFusion(imu_delayed); +#if defined(CONFIG_EKF2_EXTERNAL_VISION) // Additional data odometry data from an external estimator can be fused. controlExternalVisionFusion(); +#endif // CONFIG_EKF2_EXTERNAL_VISION #if defined(CONFIG_EKF2_AUXVEL) // Additional horizontal velocity data from an auxiliary sensor can be fused diff --git a/src/modules/ekf2/EKF/covariance.cpp b/src/modules/ekf2/EKF/covariance.cpp index 863dbfc9ab..3072f94c60 100644 --- a/src/modules/ekf2/EKF/covariance.cpp +++ b/src/modules/ekf2/EKF/covariance.cpp @@ -444,14 +444,22 @@ void Ekf::fixCovarianceErrors(bool force_symmetry) if (fabsf(down_dvel_bias) > dVel_bias_lim) { bool bad_vz_gps = _control_status.flags.gps && (down_dvel_bias * _aid_src_gnss_vel.innovation[2] < 0.0f); +#if defined(CONFIG_EKF2_EXTERNAL_VISION) bool bad_vz_ev = _control_status.flags.ev_vel && (down_dvel_bias * _aid_src_ev_vel.innovation[2] < 0.0f); +#else + bool bad_vz_ev = false; +#endif // CONFIG_EKF2_EXTERNAL_VISION 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); bool bad_z_rng = _control_status.flags.rng_hgt && (down_dvel_bias * _aid_src_rng_hgt.innovation < 0.0f); - bool bad_z_ev = _control_status.flags.ev_hgt && (down_dvel_bias * _aid_src_ev_hgt.innovation < 0.0f); +#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); +#else + bool bad_z_ev = false; +#endif // CONFIG_EKF2_EXTERNAL_VISION if (bad_z_baro || bad_z_gps || bad_z_rng || bad_z_ev) { bad_acc_bias = true; diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index c0d9987a7c..530df6dddf 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/src/modules/ekf2/EKF/ekf.cpp @@ -126,10 +126,12 @@ void Ekf::reset() resetEstimatorAidStatus(_aid_src_fake_pos); resetEstimatorAidStatus(_aid_src_fake_hgt); +#if defined(CONFIG_EKF2_EXTERNAL_VISION) resetEstimatorAidStatus(_aid_src_ev_hgt); resetEstimatorAidStatus(_aid_src_ev_pos); resetEstimatorAidStatus(_aid_src_ev_vel); resetEstimatorAidStatus(_aid_src_ev_yaw); +#endif // CONFIG_EKF2_EXTERNAL_VISION resetEstimatorAidStatus(_aid_src_gnss_hgt); resetEstimatorAidStatus(_aid_src_gnss_pos); diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 127fc1b73a..1c35db73c9 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -86,9 +86,11 @@ public: void getGpsVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos) const; void getGpsVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) const; +#if defined(CONFIG_EKF2_EXTERNAL_VISION) void getEvVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) const; void getEvVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos) const; void getEvVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) const; +#endif // CONFIG_EKF2_EXTERNAL_VISION void getBaroHgtInnov(float &baro_hgt_innov) const { baro_hgt_innov = _aid_src_baro_hgt.innovation; } void getBaroHgtInnovVar(float &baro_hgt_innov_var) const { baro_hgt_innov_var = _aid_src_baro_hgt.innovation_variance; } @@ -140,10 +142,12 @@ public: } #endif // CONFIG_EKF2_GNSS_YAW +#if defined(CONFIG_EKF2_EXTERNAL_VISION) if (_control_status.flags.ev_yaw) { heading_innov = _aid_src_ev_yaw.innovation; return; } +#endif // CONFIG_EKF2_EXTERNAL_VISION } void getHeadingInnovVar(float &heading_innov_var) const @@ -165,10 +169,12 @@ public: } #endif // CONFIG_EKF2_GNSS_YAW +#if defined(CONFIG_EKF2_EXTERNAL_VISION) if (_control_status.flags.ev_yaw) { heading_innov_var = _aid_src_ev_yaw.innovation_variance; return; } +#endif // CONFIG_EKF2_EXTERNAL_VISION } void getHeadingInnovRatio(float &heading_innov_ratio) const @@ -190,10 +196,12 @@ public: } #endif // CONFIG_EKF2_GNSS_YAW +#if defined(CONFIG_EKF2_EXTERNAL_VISION) if (_control_status.flags.ev_yaw) { heading_innov_ratio = _aid_src_ev_yaw.test_ratio; return; } +#endif // CONFIG_EKF2_EXTERNAL_VISION } void getMagInnov(float mag_innov[3]) const { memcpy(mag_innov, _aid_src_mag.innovation, sizeof(_aid_src_mag.innovation)); } @@ -443,9 +451,11 @@ public: 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(); } const BiasEstimator::status &getEvPosBiasEstimatorStatus(int i) const { return _ev_pos_b_est.getStatus(i); } +#endif // CONFIG_EKF2_EXTERNAL_VISION #if defined(CONFIG_EKF2_AIRSPEED) const auto &aid_src_airspeed() const { return _aid_src_airspeed; } @@ -461,10 +471,12 @@ public: const auto &aid_src_fake_hgt() const { return _aid_src_fake_hgt; } const auto &aid_src_fake_pos() const { return _aid_src_fake_pos; } +#if defined(CONFIG_EKF2_EXTERNAL_VISION) const auto &aid_src_ev_hgt() const { return _aid_src_ev_hgt; } const auto &aid_src_ev_pos() const { return _aid_src_ev_pos; } const auto &aid_src_ev_vel() const { return _aid_src_ev_vel; } const auto &aid_src_ev_yaw() const { return _aid_src_ev_yaw; } +#endif // CONFIG_EKF2_EXTERNAL_VISION const auto &aid_src_gnss_hgt() const { return _aid_src_gnss_hgt; } const auto &aid_src_gnss_pos() const { return _aid_src_gnss_pos; } @@ -525,9 +537,6 @@ private: bool _filter_initialised{false}; ///< true when the EKF sttes and covariances been initialised - float _ev_yaw_pred_prev{}; ///< previous value of yaw state used by odometry fusion (m) - bool _inhibit_ev_yaw_use{false}; ///< true when the vision yaw data should not be used (e.g.: NE fusion requires true North) - // 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 @@ -547,10 +556,6 @@ private: uint64_t _time_last_zero_velocity_fuse{0}; ///< last time of zero velocity update (uSec) uint64_t _time_last_healthy_rng_data{0}; - uint8_t _nb_ev_pos_reset_available{0}; - uint8_t _nb_ev_vel_reset_available{0}; - uint8_t _nb_ev_yaw_reset_available{0}; - Vector3f _last_known_pos{}; ///< last known local position vector (m) uint64_t _time_acc_bias_check{0}; ///< last time the accel bias check passed (uSec) @@ -609,11 +614,20 @@ private: estimator_aid_source2d_s _aid_src_fake_pos{}; estimator_aid_source1d_s _aid_src_fake_hgt{}; +#if defined(CONFIG_EKF2_EXTERNAL_VISION) estimator_aid_source1d_s _aid_src_ev_hgt{}; estimator_aid_source2d_s _aid_src_ev_pos{}; estimator_aid_source3d_s _aid_src_ev_vel{}; estimator_aid_source1d_s _aid_src_ev_yaw{}; + float _ev_yaw_pred_prev{}; ///< previous value of yaw state used by odometry fusion (m) + + uint8_t _nb_ev_pos_reset_available{0}; + uint8_t _nb_ev_vel_reset_available{0}; + uint8_t _nb_ev_yaw_reset_available{0}; +#endif // CONFIG_EKF2_EXTERNAL_VISION + bool _inhibit_ev_yaw_use{false}; ///< true when the vision yaw data should not be used (e.g.: NE fusion requires true North) + estimator_aid_source1d_s _aid_src_gnss_hgt{}; estimator_aid_source2d_s _aid_src_gnss_pos{}; estimator_aid_source3d_s _aid_src_gnss_vel{}; @@ -946,18 +960,21 @@ private: // Control the filter fusion modes void controlFusionModes(const imuSample &imu_delayed); +#if defined(CONFIG_EKF2_EXTERNAL_VISION) // control fusion of external vision observations void controlExternalVisionFusion(); - void controlEvHeightFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source1d_s &aid_src); - void controlEvPosFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source2d_s &aid_src); + void controlEvVelFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source3d_s &aid_src); + void controlEvYawFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source1d_s &aid_src); + void startEvPosFusion(const Vector2f &measurement, const Vector2f &measurement_var, estimator_aid_source2d_s &aid_src); void updateEvPosFusion(const Vector2f &measurement, const Vector2f &measurement_var, bool quality_sufficient, bool reset, estimator_aid_source2d_s &aid_src); void stopEvPosFusion(); - - void controlEvVelFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source3d_s &aid_src); - void controlEvYawFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source1d_s &aid_src); + void stopEvHgtFusion(); + void stopEvVelFusion(); + void stopEvYawFusion(); +#endif // CONFIG_EKF2_EXTERNAL_VISION // control fusion of optical flow observations void controlOpticalFlowFusion(const imuSample &imu_delayed); @@ -1029,7 +1046,6 @@ private: void stopBaroHgtFusion(); void stopGpsHgtFusion(); void stopRngHgtFusion(); - void stopEvHgtFusion(); void updateGroundEffect(); @@ -1095,9 +1111,6 @@ private: void stopGpsPosFusion(); void stopGpsVelFusion(); - void stopEvVelFusion(); - void stopEvYawFusion(); - void stopFlowFusion(); void resetFakePosFusion(); @@ -1121,8 +1134,11 @@ 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}; PositionBiasEstimator _ev_pos_b_est{static_cast(PositionSensor::EV), _position_sensor_ref}; +#endif // CONFIG_EKF2_EXTERNAL_VISION // Resets the main Nav EKf yaw to the estimator from the EKF-GSF yaw estimator // Resets the horizontal velocity and position to the default navigation sensor diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 59910b06a7..2790c614a3 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -150,7 +150,9 @@ void Ekf::resetHorizontalPositionTo(const Vector2f &new_horz_pos, const Vector2f _state_reset_status.reset_count.posNE++; +#if defined(CONFIG_EKF2_EXTERNAL_VISION) _ev_pos_b_est.setBias(_ev_pos_b_est.getBias() - _state_reset_status.posNE_change); +#endif // CONFIG_EKF2_EXTERNAL_VISION //_gps_pos_b_est.setBias(_gps_pos_b_est.getBias() + _state_reset_status.posNE_change); // Reset the timout timer @@ -196,7 +198,9 @@ void Ekf::resetVerticalPositionTo(const float new_vert_pos, float new_vert_pos_v _state_reset_status.reset_count.posD++; _baro_b_est.setBias(_baro_b_est.getBias() + delta_z); +#if defined(CONFIG_EKF2_EXTERNAL_VISION) _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); _rng_hgt_b_est.setBias(_rng_hgt_b_est.getBias() + delta_z); @@ -304,6 +308,7 @@ void Ekf::getGpsVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &v vpos = _aid_src_gnss_hgt.test_ratio; } +#if defined(CONFIG_EKF2_EXTERNAL_VISION) void Ekf::getEvVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) const { hvel[0] = _aid_src_ev_vel.innovation[0]; @@ -334,6 +339,7 @@ void Ekf::getEvVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vp hpos = fmaxf(_aid_src_ev_pos.test_ratio[0], _aid_src_ev_pos.test_ratio[1]); vpos = _aid_src_ev_hgt.test_ratio; } +#endif // CONFIG_EKF2_EXTERNAL_VISION #if defined(CONFIG_EKF2_AUXVEL) void Ekf::getAuxVelInnov(float aux_vel_innov[2]) const @@ -466,9 +472,11 @@ void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) const hpos_err = math::max(hpos_err, Vector2f(_aid_src_gnss_pos.innovation).norm()); } +#if defined(CONFIG_EKF2_EXTERNAL_VISION) if (_control_status.flags.ev_pos) { hpos_err = math::max(hpos_err, Vector2f(_aid_src_ev_pos.innovation).norm()); } +#endif // CONFIG_EKF2_EXTERNAL_VISION } *ekf_eph = hpos_err; @@ -489,9 +497,11 @@ void Ekf::get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv) const hpos_err = math::max(hpos_err, Vector2f(_aid_src_gnss_pos.innovation).norm()); } +#if defined(CONFIG_EKF2_EXTERNAL_VISION) if (_control_status.flags.ev_pos) { hpos_err = math::max(hpos_err, Vector2f(_aid_src_ev_pos.innovation).norm()); } +#endif // CONFIG_EKF2_EXTERNAL_VISION } *ekf_eph = hpos_err; @@ -516,14 +526,17 @@ void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) const if (_control_status.flags.gps) { vel_err_conservative = math::max(vel_err_conservative, Vector2f(_aid_src_gnss_pos.innovation).norm()); + } - } else if (_control_status.flags.ev_pos) { +#if defined(CONFIG_EKF2_EXTERNAL_VISION) + if (_control_status.flags.ev_pos) { vel_err_conservative = math::max(vel_err_conservative, Vector2f(_aid_src_ev_pos.innovation).norm()); } if (_control_status.flags.ev_vel) { vel_err_conservative = math::max(vel_err_conservative, Vector2f(_aid_src_ev_vel.innovation).norm()); } +#endif // CONFIG_EKF2_EXTERNAL_VISION hvel_err = math::max(hvel_err, vel_err_conservative); } @@ -637,6 +650,12 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f } #endif // CONFIG_EKF2_GNSS_YAW +#if defined(CONFIG_EKF2_EXTERNAL_VISION) + if (_control_status.flags.ev_yaw) { + mag = math::max(mag, sqrtf(_aid_src_ev_yaw.test_ratio)); + } +#endif // CONFIG_EKF2_EXTERNAL_VISION + // return the largest velocity and position innovation test ratio vel = NAN; pos = NAN; @@ -649,6 +668,7 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f pos = math::max(gps_pos, FLT_MIN); } +#if defined(CONFIG_EKF2_EXTERNAL_VISION) if (_control_status.flags.ev_vel) { float ev_vel = sqrtf(Vector3f(_aid_src_ev_vel.test_ratio).max()); vel = math::max(vel, ev_vel, FLT_MIN); @@ -658,6 +678,7 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f float ev_pos = sqrtf(Vector2f(_aid_src_ev_pos.test_ratio).max()); pos = math::max(pos, ev_pos, FLT_MIN); } +#endif // CONFIG_EKF2_EXTERNAL_VISION if (isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow)) { float of_vel = sqrtf(Vector2f(_aid_src_optical_flow.test_ratio).max()); @@ -683,10 +704,12 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f n_hgt_sources++; } +#if defined(CONFIG_EKF2_EXTERNAL_VISION) if (_control_status.flags.ev_hgt) { hgt_sum += sqrtf(_aid_src_ev_hgt.test_ratio); n_hgt_sources++; } +#endif // CONFIG_EKF2_EXTERNAL_VISION if (n_hgt_sources > 0) { hgt = math::max(hgt_sum / static_cast(n_hgt_sources), FLT_MIN); @@ -1136,9 +1159,12 @@ bool Ekf::resetYawToEKFGSF() _control_status.flags.gps_yaw_fault = true; _warning_events.flags.emergency_yaw_reset_gps_yaw_stopped = true; - } else if (_control_status.flags.ev_yaw) { + } +#if defined(CONFIG_EKF2_EXTERNAL_VISION) + if (_control_status.flags.ev_yaw) { _inhibit_ev_yaw_use = true; } +#endif // CONFIG_EKF2_EXTERNAL_VISION return true; } diff --git a/src/modules/ekf2/EKF/estimator_interface.cpp b/src/modules/ekf2/EKF/estimator_interface.cpp index ea56b2ef13..47fa234c09 100644 --- a/src/modules/ekf2/EKF/estimator_interface.cpp +++ b/src/modules/ekf2/EKF/estimator_interface.cpp @@ -54,7 +54,9 @@ EstimatorInterface::~EstimatorInterface() delete _airspeed_buffer; #endif // CONFIG_EKF2_AIRSPEED delete _flow_buffer; +#if defined(CONFIG_EKF2_EXTERNAL_VISION) delete _ext_vision_buffer; +#endif // CONFIG_EKF2_EXTERNAL_VISION #if defined(CONFIG_EKF2_DRAG_FUSION) delete _drag_buffer; #endif // CONFIG_EKF2_DRAG_FUSION @@ -355,7 +357,7 @@ void EstimatorInterface::setOpticalFlowData(const flowSample &flow) } } -// set attitude and position data derived from an external vision system +#if defined(CONFIG_EKF2_EXTERNAL_VISION) void EstimatorInterface::setExtVisionData(const extVisionSample &evdata) { if (!_initialised) { @@ -392,6 +394,7 @@ void EstimatorInterface::setExtVisionData(const extVisionSample &evdata) ECL_WARN("EV data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _ext_vision_buffer->get_newest().time_us, _min_obs_interval_us); } } +#endif // CONFIG_EKF2_EXTERNAL_VISION #if defined(CONFIG_EKF2_AUXVEL) void EstimatorInterface::setAuxVelData(const auxVelSample &auxvel_sample) @@ -561,9 +564,11 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp) max_time_delay_ms = math::max(_params.flow_delay_ms, max_time_delay_ms); } +#if defined(CONFIG_EKF2_EXTERNAL_VISION) if (_params.ev_ctrl > 0) { max_time_delay_ms = math::max(_params.ev_delay_ms, max_time_delay_ms); } +#endif // CONFIG_EKF2_EXTERNAL_VISION const float filter_update_period_ms = _params.filter_update_interval_us / 1000.f; @@ -703,9 +708,11 @@ void EstimatorInterface::print_status() printf("flow buffer: %d/%d (%d Bytes)\n", _flow_buffer->entries(), _flow_buffer->get_length(), _flow_buffer->get_total_size()); } +#if defined(CONFIG_EKF2_EXTERNAL_VISION) if (_ext_vision_buffer) { printf("vision buffer: %d/%d (%d Bytes)\n", _ext_vision_buffer->entries(), _ext_vision_buffer->get_length(), _ext_vision_buffer->get_total_size()); } +#endif // CONFIG_EKF2_EXTERNAL_VISION #if defined(CONFIG_EKF2_DRAG_FUSION) if (_drag_buffer) { diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index 1b4f73de27..1a1606d668 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -99,8 +99,10 @@ public: // 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); +#if defined(CONFIG_EKF2_EXTERNAL_VISION) // set external vision position and attitude data void setExtVisionData(const extVisionSample &evdata); +#endif // CONFIG_EKF2_EXTERNAL_VISION #if defined(CONFIG_EKF2_AUXVEL) void setAuxVelData(const auxVelSample &auxvel_sample); @@ -295,11 +297,17 @@ 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{}; #endif // CONFIG_EKF2_AIRSPEED + flowSample _flow_sample_delayed{}; + +#if defined(CONFIG_EKF2_EXTERNAL_VISION) extVisionSample _ev_sample_prev{}; +#endif // CONFIG_EKF2_EXTERNAL_VISION + RangeFinderConsistencyCheck _rng_consistency_check; float _air_density{CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C}; // air density (kg/m**3) @@ -358,7 +366,11 @@ protected: RingBuffer *_airspeed_buffer{nullptr}; #endif // CONFIG_EKF2_AIRSPEED RingBuffer *_flow_buffer{nullptr}; + +#if defined(CONFIG_EKF2_EXTERNAL_VISION) RingBuffer *_ext_vision_buffer{nullptr}; + uint64_t _time_last_ext_vision_buffer_push{0}; +#endif // CONFIG_EKF2_EXTERNAL_VISION #if defined(CONFIG_EKF2_AUXVEL) RingBuffer *_auxvel_buffer{nullptr}; #endif // CONFIG_EKF2_AUXVEL @@ -368,7 +380,6 @@ protected: 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_ext_vision_buffer_push{0}; uint64_t _time_last_gnd_effect_on{0}; diff --git a/src/modules/ekf2/EKF/ev_yaw_control.cpp b/src/modules/ekf2/EKF/ev_yaw_control.cpp index 90864e0ffb..e7f1d3c643 100644 --- a/src/modules/ekf2/EKF/ev_yaw_control.cpp +++ b/src/modules/ekf2/EKF/ev_yaw_control.cpp @@ -178,9 +178,11 @@ void Ekf::controlEvYawFusion(const extVisionSample &ev_sample, const bool common void Ekf::stopEvYawFusion() { +#if defined(CONFIG_EKF2_EXTERNAL_VISION) if (_control_status.flags.ev_yaw) { resetEstimatorAidStatus(_aid_src_ev_yaw); _control_status.flags.ev_yaw = false; } +#endif // CONFIG_EKF2_EXTERNAL_VISION } diff --git a/src/modules/ekf2/EKF/gps_control.cpp b/src/modules/ekf2/EKF/gps_control.cpp index 6f166680c6..b33627eb25 100644 --- a/src/modules/ekf2/EKF/gps_control.cpp +++ b/src/modules/ekf2/EKF/gps_control.cpp @@ -119,6 +119,7 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed) _aid_src_gnss_pos); _aid_src_gnss_pos.fusion_enabled = (_params.gnss_ctrl & GnssCtrl::HPOS); +#if defined(CONFIG_EKF2_EXTERNAL_VISION) // if GPS is otherwise ready to go, but yaw_align is blocked by EV give mag a chance to start if (_control_status.flags.tilt_align && _NED_origin_initialised && gps_checks_passing && !gps_checks_failing) { @@ -139,7 +140,7 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed) } } } - +#endif // CONFIG_EKF2_EXTERNAL_VISION // Determine if we should use GPS aiding for velocity and horizontal position // To start using GPS we need angular alignment completed, the local NED origin set and GPS data that has not failed checks recently @@ -202,6 +203,7 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed) } else { if (starting_conditions_passing) { +#if defined(CONFIG_EKF2_EXTERNAL_VISION) // Do not use external vision for yaw if using GPS because yaw needs to be // defined relative to an NED reference frame if (_control_status.flags.ev_yaw) { @@ -209,6 +211,7 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed) stopEvYawFusion(); _inhibit_ev_yaw_use = true; } +#endif // CONFIG_EKF2_EXTERNAL_VISION ECL_INFO("starting GPS fusion"); _information_events.flags.starting_gps_fusion = true; @@ -396,7 +399,7 @@ void Ekf::startGpsYawFusion(const gpsSample &gps_sample) ECL_INFO("starting GPS yaw fusion"); _control_status.flags.yaw_align = true; _control_status.flags.mag_dec = false; - stopEvYawFusion(); + stopMagHdgFusion(); stopMag3DFusion(); _control_status.flags.gps_yaw = true; diff --git a/src/modules/ekf2/EKF/height_control.cpp b/src/modules/ekf2/EKF/height_control.cpp index d8eaf90484..90dbeb8c83 100644 --- a/src/modules/ekf2/EKF/height_control.cpp +++ b/src/modules/ekf2/EKF/height_control.cpp @@ -166,11 +166,11 @@ Likelihood Ekf::estimateInertialNavFallingLikelihood() const enum class ReferenceType { PRESSURE, GNSS, GROUND }; struct { - ReferenceType ref_type; - float innov; - float innov_var; - bool failed_min; - bool failed_lim; + ReferenceType ref_type{}; + float innov{0.f}; + float innov_var{0.f}; + bool failed_min{false}; + bool failed_lim{false}; } checks[6] {}; if (_control_status.flags.baro_hgt) { @@ -189,6 +189,7 @@ Likelihood Ekf::estimateInertialNavFallingLikelihood() const checks[3] = {ReferenceType::GROUND, _aid_src_rng_hgt.innovation, _aid_src_rng_hgt.innovation_variance}; } +#if defined(CONFIG_EKF2_EXTERNAL_VISION) if (_control_status.flags.ev_hgt) { checks[4] = {ReferenceType::GROUND, _aid_src_ev_hgt.innovation, _aid_src_ev_hgt.innovation_variance}; } @@ -196,6 +197,7 @@ Likelihood Ekf::estimateInertialNavFallingLikelihood() const if (_control_status.flags.ev_vel) { checks[5] = {ReferenceType::GROUND, _aid_src_ev_vel.innovation[2], _aid_src_ev_vel.innovation_variance[2]}; } +#endif // CONFIG_EKF2_EXTERNAL_VISION // Compute the check based on innovation ratio for all the sources for (unsigned i = 0; i < 6; i++) { diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index c4751154e5..bdb22797e6 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -65,7 +65,6 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _param_ekf2_gps_delay(_params->gps_delay_ms), _param_ekf2_of_delay(_params->flow_delay_ms), _param_ekf2_rng_delay(_params->range_delay_ms), - _param_ekf2_ev_delay(_params->ev_delay_ms), #if defined(CONFIG_EKF2_AUXVEL) _param_ekf2_avel_delay(_params->auxvel_delay_ms), #endif // CONFIG_EKF2_AUXVEL @@ -132,6 +131,8 @@ 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), +#if defined(CONFIG_EKF2_EXTERNAL_VISION) + _param_ekf2_ev_delay(_params->ev_delay_ms), _param_ekf2_ev_ctrl(_params->ev_ctrl), _param_ekf2_ev_qmin(_params->ev_quality_minimum), _param_ekf2_evp_noise(_params->ev_pos_noise), @@ -139,6 +140,10 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _param_ekf2_eva_noise(_params->ev_att_noise), _param_ekf2_evv_gate(_params->ev_vel_innov_gate), _param_ekf2_evp_gate(_params->ev_pos_innov_gate), + _param_ekf2_ev_pos_x(_params->ev_pos_body(0)), + _param_ekf2_ev_pos_y(_params->ev_pos_body(1)), + _param_ekf2_ev_pos_z(_params->ev_pos_body(2)), +#endif // CONFIG_EKF2_EXTERNAL_VISION _param_ekf2_grav_noise(_params->gravity_noise), _param_ekf2_of_n_min(_params->flow_noise), _param_ekf2_of_n_max(_params->flow_noise_qual_min), @@ -156,9 +161,6 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _param_ekf2_of_pos_x(_params->flow_pos_body(0)), _param_ekf2_of_pos_y(_params->flow_pos_body(1)), _param_ekf2_of_pos_z(_params->flow_pos_body(2)), - _param_ekf2_ev_pos_x(_params->ev_pos_body(0)), - _param_ekf2_ev_pos_y(_params->ev_pos_body(1)), - _param_ekf2_ev_pos_z(_params->ev_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), @@ -240,6 +242,8 @@ bool EKF2::multi_init(int imu, int mag) _estimator_baro_bias_pub.advertise(); } +#if defined(CONFIG_EKF2_EXTERNAL_VISION) + // EV advertise if (_param_ekf2_ev_ctrl.get() & static_cast(EvCtrl::VPOS)) { _estimator_aid_src_ev_hgt_pub.advertise(); @@ -259,6 +263,8 @@ bool EKF2::multi_init(int imu, int mag) _estimator_aid_src_ev_yaw_pub.advertise(); } +#endif // CONFIG_EKF2_EXTERNAL_VISION + // GNSS advertise if (_param_ekf2_gps_ctrl.get() & static_cast(GnssCtrl::VPOS)) { _estimator_aid_src_gnss_hgt_pub.advertise(); @@ -636,7 +642,9 @@ void EKF2::Run() UpdateAuxVelSample(ekf2_timestamps); #endif // CONFIG_EKF2_AUXVEL UpdateBaroSample(ekf2_timestamps); +#if defined(CONFIG_EKF2_EXTERNAL_VISION) UpdateExtVisionSample(ekf2_timestamps); +#endif // CONFIG_EKF2_EXTERNAL_VISION UpdateFlowSample(ekf2_timestamps); UpdateGpsSample(ekf2_timestamps); UpdateMagSample(ekf2_timestamps); @@ -658,7 +666,9 @@ void EKF2::Run() PublishBaroBias(now); PublishGnssHgtBias(now); PublishRngHgtBias(now); +#if defined(CONFIG_EKF2_EXTERNAL_VISION) PublishEvPosBias(now); +#endif // CONFIG_EKF2_EXTERNAL_VISION PublishEventFlags(now); PublishGpsStatus(now); PublishInnovations(now); @@ -749,6 +759,8 @@ void EKF2::VerifyParams() "GPS enabled by EKF2_HGT_REF", _param_ekf2_gps_ctrl.get()); } +#if defined(CONFIG_EKF2_EXTERNAL_VISION) + if ((_param_ekf2_hgt_ref.get() == HeightSensor::EV) && !(_param_ekf2_ev_ctrl.get() & static_cast(EvCtrl::VPOS))) { _param_ekf2_ev_ctrl.set(_param_ekf2_ev_ctrl.get() | static_cast(EvCtrl::VPOS)); @@ -798,6 +810,7 @@ void EKF2::VerifyParams() "Use EKF2_EV_CTRL instead", _param_ekf2_aid_mask.get()); } +#endif // CONFIG_EKF2_EXTERNAL_VISION // IMU EKF2_AID_MASK -> EKF2_IMU_CTRL (2023-01-31) if (_param_ekf2_aid_mask.get() & SensorFusionMask::DEPRECATED_INHIBIT_ACC_BIAS) { @@ -840,11 +853,13 @@ void EKF2::PublishAidSourceStatus(const hrt_abstime ×tamp) PublishAidSourceStatus(_ekf.aid_src_fake_pos(), _status_fake_pos_pub_last, _estimator_aid_src_fake_pos_pub); PublishAidSourceStatus(_ekf.aid_src_fake_hgt(), _status_fake_hgt_pub_last, _estimator_aid_src_fake_hgt_pub); +#if defined(CONFIG_EKF2_EXTERNAL_VISION) // external vision (EV) hgt/pos/vel/yaw PublishAidSourceStatus(_ekf.aid_src_ev_hgt(), _status_ev_hgt_pub_last, _estimator_aid_src_ev_hgt_pub); PublishAidSourceStatus(_ekf.aid_src_ev_pos(), _status_ev_pos_pub_last, _estimator_aid_src_ev_pos_pub); PublishAidSourceStatus(_ekf.aid_src_ev_vel(), _status_ev_vel_pub_last, _estimator_aid_src_ev_vel_pub); PublishAidSourceStatus(_ekf.aid_src_ev_yaw(), _status_ev_yaw_pub_last, _estimator_aid_src_ev_yaw_pub); +#endif // CONFIG_EKF2_EXTERNAL_VISION // GNSS hgt/pos/vel/yaw PublishAidSourceStatus(_ekf.aid_src_gnss_hgt(), _status_gnss_hgt_pub_last, _estimator_aid_src_gnss_hgt_pub); @@ -934,6 +949,7 @@ void EKF2::PublishRngHgtBias(const hrt_abstime ×tamp) } } +#if defined(CONFIG_EKF2_EXTERNAL_VISION) void EKF2::PublishEvPosBias(const hrt_abstime ×tamp) { if (_ekf.aid_src_ev_hgt().timestamp_sample) { @@ -966,6 +982,7 @@ void EKF2::PublishEvPosBias(const hrt_abstime ×tamp) } } } +#endif // CONFIG_EKF2_EXTERNAL_VISION estimator_bias_s EKF2::fillEstimatorBiasMsg(const BiasEstimator::status &status, uint64_t timestamp_sample_us, uint64_t timestamp, uint32_t device_id) @@ -1146,7 +1163,9 @@ void EKF2::PublishInnovations(const hrt_abstime ×tamp) estimator_innovations_s innovations{}; innovations.timestamp_sample = _ekf.time_delayed_us(); _ekf.getGpsVelPosInnov(innovations.gps_hvel, innovations.gps_vvel, innovations.gps_hpos, innovations.gps_vpos); +#if defined(CONFIG_EKF2_EXTERNAL_VISION) _ekf.getEvVelPosInnov(innovations.ev_hvel, innovations.ev_vvel, innovations.ev_hpos, innovations.ev_vpos); +#endif // CONFIG_EKF2_EXTERNAL_VISION _ekf.getBaroHgtInnov(innovations.baro_vpos); _ekf.getRngHgtInnov(innovations.rng_vpos); #if defined(CONFIG_EKF2_AUXVEL) @@ -1184,13 +1203,16 @@ void EKF2::PublishInnovations(const hrt_abstime ×tamp) // TODO: move to run before publications _preflt_checker.setUsingGpsAiding(_ekf.control_status_flags().gps); _preflt_checker.setUsingFlowAiding(_ekf.control_status_flags().opt_flow); + +#if defined(CONFIG_EKF2_EXTERNAL_VISION) _preflt_checker.setUsingEvPosAiding(_ekf.control_status_flags().ev_pos); _preflt_checker.setUsingEvVelAiding(_ekf.control_status_flags().ev_vel); + _preflt_checker.setUsingEvHgtAiding(_ekf.control_status_flags().ev_hgt); +#endif // CONFIG_EKF2_EXTERNAL_VISION _preflt_checker.setUsingBaroHgtAiding(_ekf.control_status_flags().baro_hgt); _preflt_checker.setUsingGpsHgtAiding(_ekf.control_status_flags().gps_hgt); _preflt_checker.setUsingRngHgtAiding(_ekf.control_status_flags().rng_hgt); - _preflt_checker.setUsingEvHgtAiding(_ekf.control_status_flags().ev_hgt); _preflt_checker.setVehicleCanObserveHeadingInFlight(_ekf.control_status_flags().fixed_wing); @@ -1205,7 +1227,9 @@ void EKF2::PublishInnovationTestRatios(const hrt_abstime ×tamp) test_ratios.timestamp_sample = _ekf.time_delayed_us(); _ekf.getGpsVelPosInnovRatio(test_ratios.gps_hvel[0], test_ratios.gps_vvel, test_ratios.gps_hpos[0], test_ratios.gps_vpos); +#if defined(CONFIG_EKF2_EXTERNAL_VISION) _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); _ekf.getRngHgtInnovRatio(test_ratios.rng_vpos); #if defined(CONFIG_EKF2_AUXVEL) @@ -1240,7 +1264,9 @@ void EKF2::PublishInnovationVariances(const hrt_abstime ×tamp) estimator_innovations_s variances{}; variances.timestamp_sample = _ekf.time_delayed_us(); _ekf.getGpsVelPosInnovVar(variances.gps_hvel, variances.gps_vvel, variances.gps_hpos, variances.gps_vpos); +#if defined(CONFIG_EKF2_EXTERNAL_VISION) _ekf.getEvVelPosInnovVar(variances.ev_hvel, variances.ev_vvel, variances.ev_hpos, variances.ev_vpos); +#endif // CONFIG_EKF2_EXTERNAL_VISION _ekf.getBaroHgtInnovVar(variances.baro_vpos); _ekf.getRngHgtInnovVar(variances.rng_vpos); #if defined(CONFIG_EKF2_AUXVEL) @@ -1873,6 +1899,7 @@ void EKF2::UpdateBaroSample(ekf2_timestamps_s &ekf2_timestamps) } } +#if defined(CONFIG_EKF2_EXTERNAL_VISION) bool EKF2::UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps) { // EKF external vision sample @@ -2026,6 +2053,7 @@ bool EKF2::UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps) return new_ev_odom; } +#endif // CONFIG_EKF2_EXTERNAL_VISION bool EKF2::UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps) { diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 15019bb3ea..f0611de962 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -151,7 +151,10 @@ private: void PublishBaroBias(const hrt_abstime ×tamp); void PublishGnssHgtBias(const hrt_abstime ×tamp); void PublishRngHgtBias(const hrt_abstime ×tamp); + +#if defined(CONFIG_EKF2_EXTERNAL_VISION) void PublishEvPosBias(const hrt_abstime ×tamp); +#endif // CONFIG_EKF2_EXTERNAL_VISION estimator_bias_s fillEstimatorBiasMsg(const BiasEstimator::status &status, uint64_t timestamp_sample_us, uint64_t timestamp, uint32_t device_id = 0); void PublishEventFlags(const hrt_abstime ×tamp); @@ -278,11 +281,23 @@ private: hrt_abstime _status_fake_hgt_pub_last{0}; hrt_abstime _status_fake_pos_pub_last{0}; +#if defined(CONFIG_EKF2_EXTERNAL_VISION) + uORB::PublicationMulti _estimator_aid_src_ev_hgt_pub {ORB_ID(estimator_aid_src_ev_hgt)}; + uORB::PublicationMulti _estimator_aid_src_ev_pos_pub{ORB_ID(estimator_aid_src_ev_pos)}; + uORB::PublicationMulti _estimator_aid_src_ev_vel_pub{ORB_ID(estimator_aid_src_ev_vel)}; + uORB::PublicationMulti _estimator_aid_src_ev_yaw_pub{ORB_ID(estimator_aid_src_ev_yaw)}; hrt_abstime _status_ev_hgt_pub_last{0}; hrt_abstime _status_ev_pos_pub_last{0}; hrt_abstime _status_ev_vel_pub_last{0}; hrt_abstime _status_ev_yaw_pub_last{0}; + matrix::Vector3f _last_ev_bias_published{}; + + uORB::Subscription _ev_odom_sub{ORB_ID(vehicle_visual_odometry)}; + + uORB::PublicationMulti _estimator_ev_pos_bias_pub{ORB_ID(estimator_ev_pos_bias)}; +#endif // CONFIG_EKF2_EXTERNAL_VISION + hrt_abstime _status_gnss_hgt_pub_last{0}; hrt_abstime _status_gnss_pos_pub_last{0}; hrt_abstime _status_gnss_vel_pub_last{0}; @@ -310,7 +325,6 @@ private: float _last_baro_bias_published{}; float _last_gnss_hgt_bias_published{}; float _last_rng_hgt_bias_published{}; - matrix::Vector3f _last_ev_bias_published{}; #if defined(CONFIG_EKF2_AIRSPEED) uORB::Subscription _airspeed_sub {ORB_ID(airspeed)}; @@ -336,7 +350,6 @@ private: uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; uORB::Subscription _airdata_sub{ORB_ID(vehicle_air_data)}; - uORB::Subscription _ev_odom_sub{ORB_ID(vehicle_visual_odometry)}; uORB::Subscription _magnetometer_sub{ORB_ID(vehicle_magnetometer)}; uORB::Subscription _sensor_selection_sub{ORB_ID(sensor_selection)}; uORB::Subscription _status_sub{ORB_ID(vehicle_status)}; @@ -372,7 +385,6 @@ private: 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::PublicationMulti _estimator_ev_pos_bias_pub{ORB_ID(estimator_ev_pos_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)}; @@ -391,11 +403,6 @@ private: 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)}; - uORB::PublicationMulti _estimator_aid_src_ev_hgt_pub{ORB_ID(estimator_aid_src_ev_hgt)}; - uORB::PublicationMulti _estimator_aid_src_ev_pos_pub{ORB_ID(estimator_aid_src_ev_pos)}; - uORB::PublicationMulti _estimator_aid_src_ev_vel_pub{ORB_ID(estimator_aid_src_ev_vel)}; - uORB::PublicationMulti _estimator_aid_src_ev_yaw_pub{ORB_ID(estimator_aid_src_ev_yaw)}; - uORB::PublicationMulti _estimator_aid_src_gnss_hgt_pub{ORB_ID(estimator_aid_src_gnss_hgt)}; uORB::PublicationMulti _estimator_aid_src_gnss_pos_pub{ORB_ID(estimator_aid_src_gnss_pos)}; uORB::PublicationMulti _estimator_aid_src_gnss_vel_pub{ORB_ID(estimator_aid_src_gnss_vel)}; @@ -439,8 +446,6 @@ private: _param_ekf2_of_delay, ///< optical flow measurement delay relative to the IMU (mSec) - this is to the middle of the optical flow integration interval (ParamExtFloat) _param_ekf2_rng_delay, ///< range finder measurement delay relative to the IMU (mSec) - (ParamExtFloat) - _param_ekf2_ev_delay, ///< off-board vision measurement delay relative to the IMU (mSec) #if defined(CONFIG_EKF2_AUXVEL) (ParamExtFloat) @@ -571,21 +576,32 @@ private: (ParamExtFloat) _param_ekf2_rng_k_gate, ///< range finder kinematic consistency gate size (STD) +#if defined(CONFIG_EKF2_EXTERNAL_VISION) // vision estimate fusion + (ParamExtFloat) + _param_ekf2_ev_delay, ///< off-board vision measurement delay relative to the IMU (mSec) + (ParamExtInt) _param_ekf2_ev_ctrl, ///< external vision (EV) control selection - (ParamInt) - _param_ekf2_ev_noise_md, ///< determine source of vision observation noise + (ParamInt) _param_ekf2_ev_noise_md, ///< determine source of vision observation noise (ParamExtInt) _param_ekf2_ev_qmin, (ParamExtFloat) - _param_ekf2_evp_noise, ///< default position observation noise for exernal vision measurements (m) + _param_ekf2_evp_noise, ///< default position observation noise for exernal vision measurements (m) (ParamExtFloat) - _param_ekf2_evv_noise, ///< default velocity observation noise for exernal vision measurements (m/s) + _param_ekf2_evv_noise, ///< default velocity observation noise for exernal vision measurements (m/s) (ParamExtFloat) - _param_ekf2_eva_noise, ///< default angular observation noise for exernal vision measurements (rad) + _param_ekf2_eva_noise, ///< default angular observation noise for exernal vision measurements (rad) (ParamExtFloat) - _param_ekf2_evv_gate, ///< external vision velocity innovation consistency gate size (STD) + _param_ekf2_evv_gate, ///< external vision velocity innovation consistency gate size (STD) (ParamExtFloat) - _param_ekf2_evp_gate, ///< external vision position innovation consistency gate size (STD) + _param_ekf2_evp_gate, ///< external vision position innovation consistency gate size (STD) + + (ParamExtFloat) + _param_ekf2_ev_pos_x, ///< X position of VI sensor focal point in body frame (m) + (ParamExtFloat) + _param_ekf2_ev_pos_y, ///< Y position of VI sensor focal point in body frame (m) + (ParamExtFloat) + _param_ekf2_ev_pos_z, ///< Z position of VI sensor focal point in body frame (m) +#endif // CONFIG_EKF2_EXTERNAL_VISION (ParamExtFloat) _param_ekf2_grav_noise, ///< default accelerometer noise for gravity fusion measurements (m/s**2) @@ -616,12 +632,6 @@ private: _param_ekf2_of_pos_y, ///< Y position of optical flow sensor focal point in body frame (m) (ParamExtFloat) _param_ekf2_of_pos_z, ///< Z position of optical flow sensor focal point in body frame (m) - (ParamExtFloat) - _param_ekf2_ev_pos_x, ///< X position of VI sensor focal point in body frame (m) - (ParamExtFloat) - _param_ekf2_ev_pos_y, ///< Y position of VI sensor focal point in body frame (m) - (ParamExtFloat) - _param_ekf2_ev_pos_z, ///< Z position of VI sensor focal point in body frame (m) // output predictor filter time constants (ParamFloat) diff --git a/src/modules/ekf2/Kconfig b/src/modules/ekf2/Kconfig index 3f3a94c3cb..e26ce74307 100644 --- a/src/modules/ekf2/Kconfig +++ b/src/modules/ekf2/Kconfig @@ -42,6 +42,13 @@ depends on MODULES_EKF2 ---help--- EKF2 drag fusion support. +menuconfig EKF2_EXTERNAL_VISION +depends on MODULES_EKF2 + bool "external vision (EV) fusion support" + default y + ---help--- + EKF2 external vision (EV) fusion support. + menuconfig EKF2_GNSS_YAW depends on MODULES_EKF2 bool "GNSS yaw fusion support"