diff --git a/src/modules/ekf2/CMakeLists.txt b/src/modules/ekf2/CMakeLists.txt index 8f10bae93c84..37fc6c005ba6 100644 --- a/src/modules/ekf2/CMakeLists.txt +++ b/src/modules/ekf2/CMakeLists.txt @@ -124,7 +124,6 @@ list(APPEND EKF_SRCS EKF/estimator_interface.cpp EKF/fake_height_control.cpp EKF/fake_pos_control.cpp - EKF/gravity_fusion.cpp EKF/height_control.cpp EKF/imu_down_sampler.cpp EKF/output_predictor.cpp @@ -175,6 +174,10 @@ if(CONFIG_EKF2_GNSS_YAW) list(APPEND EKF_SRCS EKF/gps_yaw_fusion.cpp) endif() +if(CONFIG_EKF2_GRAVITY_FUSION) + list(APPEND EKF_SRCS EKF/gravity_fusion.cpp) +endif() + if(CONFIG_EKF2_MAGNETOMETER) list(APPEND EKF_SRCS EKF/mag_3d_control.cpp diff --git a/src/modules/ekf2/EKF/CMakeLists.txt b/src/modules/ekf2/EKF/CMakeLists.txt index 23ebe86cbdf8..5753750a9a07 100644 --- a/src/modules/ekf2/EKF/CMakeLists.txt +++ b/src/modules/ekf2/EKF/CMakeLists.txt @@ -42,7 +42,6 @@ list(APPEND EKF_SRCS estimator_interface.cpp fake_height_control.cpp fake_pos_control.cpp - gravity_fusion.cpp height_control.cpp imu_down_sampler.cpp output_predictor.cpp @@ -93,6 +92,10 @@ if(CONFIG_EKF2_GNSS_YAW) list(APPEND EKF_SRCS gps_yaw_fusion.cpp) endif() +if(CONFIG_EKF2_GRAVITY_FUSION) + list(APPEND EKF_SRCS gravity_fusion.cpp) +endif() + if(CONFIG_EKF2_MAGNETOMETER) list(APPEND EKF_SRCS mag_3d_control.cpp diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index 81137ad2407b..a6b20b39784d 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -454,8 +454,10 @@ struct parameters { Vector3f ev_pos_body{}; ///< xyz position of VI-sensor focal point in body frame (m) #endif // CONFIG_EKF2_EXTERNAL_VISION +#if defined(CONFIG_EKF2_GRAVITY_FUSION) // gravity fusion float gravity_noise{1.0f}; ///< accelerometer measurement gaussian noise (m/s**2) +#endif // CONFIG_EKF2_GRAVITY_FUSION #if defined(CONFIG_EKF2_OPTICAL_FLOW) int32_t flow_ctrl{0}; diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index 704c3e435008..91acf6660b5c 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -128,7 +128,10 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed) #endif // CONFIG_EKF2_DRAG_FUSION controlHeightFusion(imu_delayed); + +#if defined(CONFIG_EKF2_GRAVITY_FUSION) controlGravityFusion(imu_delayed); +#endif // CONFIG_EKF2_GRAVITY_FUSION #if defined(CONFIG_EKF2_EXTERNAL_VISION) // Additional data odometry data from an external estimator can be fused. diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 1691b9a7aa50..0415b6d3d7ee 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -310,9 +310,13 @@ class Ekf final : public EstimatorInterface void getBetaInnovRatio(float &beta_innov_ratio) const { beta_innov_ratio = _aid_src_sideslip.test_ratio; } #endif // CONFIG_EKF2_SIDESLIP +#if defined(CONFIG_EKF2_GRAVITY_FUSION) + const auto &aid_src_gravity() const { return _aid_src_gravity; } + 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(); } +#endif // CONFIG_EKF2_GRAVITY_FUSION // get the state vector at the delayed time horizon const matrix::Vector &getStateAtFusionHorizonAsVector() const { return _state.vector(); } @@ -554,8 +558,6 @@ class Ekf final : public EstimatorInterface const auto &aid_src_mag() const { return _aid_src_mag; } #endif // CONFIG_EKF2_MAGNETOMETER - const auto &aid_src_gravity() const { return _aid_src_gravity; } - #if defined(CONFIG_EKF2_AUXVEL) const auto &aid_src_aux_vel() const { return _aid_src_aux_vel; } #endif // CONFIG_EKF2_AUXVEL @@ -731,7 +733,9 @@ class Ekf final : public EstimatorInterface # endif // CONFIG_EKF2_GNSS_YAW #endif // CONFIG_EKF2_GNSS +#if defined(CONFIG_EKF2_GRAVITY_FUSION) estimator_aid_source3d_s _aid_src_gravity{}; +#endif // CONFIG_EKF2_GRAVITY_FUSION #if defined(CONFIG_EKF2_AUXVEL) estimator_aid_source2d_s _aid_src_aux_vel{}; @@ -1184,8 +1188,10 @@ class Ekf final : public EstimatorInterface void updateGroundEffect(); #endif // CONFIG_EKF2_BAROMETER +#if defined(CONFIG_EKF2_GRAVITY_FUSION) // gravity fusion: heuristically enable / disable gravity fusion void controlGravityFusion(const imuSample &imu_delayed); +#endif // CONFIG_EKF2_GRAVITY_FUSION void resetQuatCov(const float yaw_noise = NAN); void resetQuatCov(const Vector3f &euler_noise_ned); diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 0b85928e0bee..80c15714b35d 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -189,6 +189,16 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _param_ekf2_of_pos_y(_params->flow_pos_body(1)), _param_ekf2_of_pos_z(_params->flow_pos_body(2)), #endif // CONFIG_EKF2_OPTICAL_FLOW +#if defined(CONFIG_EKF2_DRAG_FUSION) + _param_ekf2_drag_ctrl(_params->drag_ctrl), + _param_ekf2_drag_noise(_params->drag_noise), + _param_ekf2_bcoef_x(_params->bcoef_x), + _param_ekf2_bcoef_y(_params->bcoef_y), + _param_ekf2_mcoef(_params->mcoef), +#endif // CONFIG_EKF2_DRAG_FUSION +#if defined(CONFIG_EKF2_GRAVITY_FUSION) + _param_ekf2_grav_noise(_params->gravity_noise), +#endif // CONFIG_EKF2_GRAVITY_FUSION _param_ekf2_imu_pos_x(_params->imu_pos_body(0)), _param_ekf2_imu_pos_y(_params->imu_pos_body(1)), _param_ekf2_imu_pos_z(_params->imu_pos_body(2)), @@ -199,16 +209,7 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _param_ekf2_abl_acclim(_params->acc_bias_learn_acc_lim), _param_ekf2_abl_gyrlim(_params->acc_bias_learn_gyr_lim), _param_ekf2_abl_tau(_params->acc_bias_learn_tc), - _param_ekf2_gyr_b_lim(_params->gyro_bias_lim), -#if defined(CONFIG_EKF2_DRAG_FUSION) - _param_ekf2_drag_ctrl(_params->drag_ctrl), - _param_ekf2_drag_noise(_params->drag_noise), - _param_ekf2_bcoef_x(_params->bcoef_x), - _param_ekf2_bcoef_y(_params->bcoef_y), - _param_ekf2_mcoef(_params->mcoef), -#endif // CONFIG_EKF2_DRAG_FUSION - _param_ekf2_grav_noise(_params->gravity_noise) - + _param_ekf2_gyr_b_lim(_params->gyro_bias_lim) { // advertise expected minimal topic set immediately to ensure logging _attitude_pub.advertise(); @@ -1086,8 +1087,10 @@ void EKF2::PublishAidSourceStatus(const hrt_abstime ×tamp) PublishAidSourceStatus(_ekf.aid_src_mag(), _status_mag_pub_last, _estimator_aid_src_mag_pub); #endif // CONFIG_EKF2_MAGNETOMETER +#if defined(CONFIG_EKF2_GRAVITY_FUSION) // gravity PublishAidSourceStatus(_ekf.aid_src_gravity(), _status_gravity_pub_last, _estimator_aid_src_gravity_pub); +#endif // CONFIG_EKF2_GRAVITY_FUSION #if defined(CONFIG_EKF2_AUXVEL) // aux velocity @@ -1435,7 +1438,9 @@ void EKF2::PublishInnovations(const hrt_abstime ×tamp) _ekf.getBetaInnov(innovations.beta); #endif // CONFIG_EKF2_SIDESLIP +#if defined(CONFIG_EKF2_GRAVITY_FUSION) _ekf.getGravityInnov(innovations.gravity); +#endif // CONFIG_EKF2_GRAVITY_FUSION #if defined(CONFIG_EKF2_TERRAIN) # if defined(CONFIG_EKF2_RANGE_FINDER) @@ -1529,7 +1534,9 @@ void EKF2::PublishInnovationTestRatios(const hrt_abstime ×tamp) _ekf.getBetaInnovRatio(test_ratios.beta); #endif // CONFIG_EKF2_SIDESLIP +#if defined(CONFIG_EKF2_GRAVITY_FUSION) _ekf.getGravityInnovRatio(test_ratios.gravity[0]); +#endif // CONFIG_EKF2_GRAVITY_FUSION #if defined(CONFIG_EKF2_TERRAIN) # if defined(CONFIG_EKF2_RANGE_FINDER) @@ -1594,7 +1601,9 @@ void EKF2::PublishInnovationVariances(const hrt_abstime ×tamp) # endif // CONFIG_EKF2_OPTICAL_FLOW #endif // CONFIG_EKF2_TERRAIN && CONFIG_EKF2_RANGE_FINDER +#if defined(CONFIG_EKF2_GRAVITY_FUSION) _ekf.getGravityInnovVar(variances.gravity); +#endif // CONFIG_EKF2_GRAVITY_FUSION // Not yet supported variances.aux_vvel = NAN; diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 2465b774cc46..93285ef98aed 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -349,8 +349,6 @@ class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem uORB::PublicationMulti _estimator_ev_pos_bias_pub{ORB_ID(estimator_ev_pos_bias)}; #endif // CONFIG_EKF2_EXTERNAL_VISION - hrt_abstime _status_gravity_pub_last{0}; - #if defined(CONFIG_EKF2_AUXVEL) uORB::Subscription _landing_target_pose_sub {ORB_ID(landing_target_pose)}; @@ -476,8 +474,6 @@ class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem 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_gravity_pub{ORB_ID(estimator_aid_src_gravity)}; - // publications with topic dependent on multi-mode uORB::PublicationMulti _attitude_pub; uORB::PublicationMulti _local_position_pub; @@ -518,6 +514,11 @@ class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem # endif // CONFIG_EKF2_GNSS_YAW #endif // CONFIG_EKF2_GNSS +#if defined(CONFIG_EKF2_GRAVITY_FUSION) + hrt_abstime _status_gravity_pub_last {0}; + uORB::PublicationMulti _estimator_aid_src_gravity_pub{ORB_ID(estimator_aid_src_gravity)}; +#endif // CONFIG_EKF2_GRAVITY_FUSION + PreFlightChecker _preflt_checker; Ekf _ekf; @@ -722,6 +723,20 @@ class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem _param_ekf2_of_pos_z, ///< Z position of optical flow sensor focal point in body frame (m) #endif // CONFIG_EKF2_OPTICAL_FLOW +#if defined(CONFIG_EKF2_DRAG_FUSION) + (ParamExtInt) _param_ekf2_drag_ctrl, ///< drag fusion selection + // Multi-rotor drag specific force fusion + (ParamExtFloat) + _param_ekf2_drag_noise, ///< observation noise variance for drag specific force measurements (m/sec**2)**2 + (ParamExtFloat) _param_ekf2_bcoef_x, ///< ballistic coefficient along the X-axis (kg/m**2) + (ParamExtFloat) _param_ekf2_bcoef_y, ///< ballistic coefficient along the Y-axis (kg/m**2) + (ParamExtFloat) _param_ekf2_mcoef, ///< propeller momentum drag coefficient (1/s) +#endif // CONFIG_EKF2_DRAG_FUSION + +#if defined(CONFIG_EKF2_GRAVITY_FUSION) + (ParamExtFloat) _param_ekf2_grav_noise, +#endif // CONFIG_EKF2_GRAVITY_FUSION + // sensor positions in body frame (ParamExtFloat) _param_ekf2_imu_pos_x, ///< X position of IMU in body frame (m) (ParamExtFloat) _param_ekf2_imu_pos_y, ///< Y position of IMU in body frame (m) @@ -746,21 +761,9 @@ class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem (ParamExtFloat) _param_ekf2_gyr_b_lim, ///< Gyro bias learning limit (rad/s) -#if defined(CONFIG_EKF2_DRAG_FUSION) - (ParamExtInt) _param_ekf2_drag_ctrl, ///< drag fusion selection - // Multi-rotor drag specific force fusion - (ParamExtFloat) - _param_ekf2_drag_noise, ///< observation noise variance for drag specific force measurements (m/sec**2)**2 - (ParamExtFloat) _param_ekf2_bcoef_x, ///< ballistic coefficient along the X-axis (kg/m**2) - (ParamExtFloat) _param_ekf2_bcoef_y, ///< ballistic coefficient along the Y-axis (kg/m**2) - (ParamExtFloat) _param_ekf2_mcoef, ///< propeller momentum drag coefficient (1/s) -#endif // CONFIG_EKF2_DRAG_FUSION - // output predictor filter time constants (ParamFloat) _param_ekf2_tau_vel, - (ParamFloat) _param_ekf2_tau_pos, - - (ParamExtFloat) _param_ekf2_grav_noise + (ParamFloat) _param_ekf2_tau_pos ) }; #endif // !EKF2_HPP diff --git a/src/modules/ekf2/Kconfig b/src/modules/ekf2/Kconfig index 4a39f9d4228d..442d30b6ee6f 100644 --- a/src/modules/ekf2/Kconfig +++ b/src/modules/ekf2/Kconfig @@ -75,6 +75,13 @@ depends on MODULES_EKF2 ---help--- EKF2 GNSS yaw fusion support. +menuconfig EKF2_GRAVITY_FUSION +depends on MODULES_EKF2 + bool "gravity fusion support" + default y + ---help--- + EKF2 gravity fusion support. + menuconfig EKF2_MAGNETOMETER depends on MODULES_EKF2 bool "magnetometer support"