From 39c2f95669ff57ba98771bec6bd0994428975afc Mon Sep 17 00:00:00 2001 From: bresch Date: Wed, 26 Apr 2023 15:42:24 +0200 Subject: [PATCH] ekf2: move drag fusion activation from AID_MASK to DRAG_CTRL --- src/modules/ekf2/EKF/airspeed_fusion.cpp | 2 +- src/modules/ekf2/EKF/common.h | 3 ++- src/modules/ekf2/EKF/drag_fusion.cpp | 2 +- src/modules/ekf2/EKF/estimator_interface.cpp | 2 +- src/modules/ekf2/EKF2.cpp | 23 ++++++++++++++++++ src/modules/ekf2/EKF2.hpp | 1 + src/modules/ekf2/ekf2_params.c | 24 +++++++++++++++---- .../test/sensor_simulator/ekf_wrapper.cpp | 4 ++-- 8 files changed, 50 insertions(+), 11 deletions(-) diff --git a/src/modules/ekf2/EKF/airspeed_fusion.cpp b/src/modules/ekf2/EKF/airspeed_fusion.cpp index 1f39dd0600..62afda7447 100644 --- a/src/modules/ekf2/EKF/airspeed_fusion.cpp +++ b/src/modules/ekf2/EKF/airspeed_fusion.cpp @@ -57,7 +57,7 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed) const bool airspeed_timed_out = isTimedOut(_aid_src_airspeed.time_last_fuse, (uint64_t)10e6); const bool sideslip_timed_out = isTimedOut(_aid_src_sideslip.time_last_fuse, (uint64_t)10e6); - if (_control_status.flags.fake_pos || (airspeed_timed_out && sideslip_timed_out && !(_params.fusion_mode & SensorFusionMask::USE_DRAG))) { + if (_control_status.flags.fake_pos || (airspeed_timed_out && sideslip_timed_out && (_params.drag_ctrl == 0))) { _control_status.flags.wind = false; } diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index fb5b042800..72e04f0819 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -162,7 +162,7 @@ enum SensorFusionMask : uint16_t { DEPRECATED_INHIBIT_ACC_BIAS = (1<<2), ///< set to true to inhibit estimation of accelerometer delta velocity bias DEPRECATED_USE_EXT_VIS_POS = (1<<3), ///< set to true to use external vision position data DEPRECATED_USE_EXT_VIS_YAW = (1<<4), ///< set to true to use external vision quaternion data for yaw - USE_DRAG = (1<<5), ///< set to true to use the multi-rotor drag model to estimate wind + DEPRECATED_USE_DRAG = (1<<5), ///< set to true to use the multi-rotor drag model to estimate wind DEPRECATED_ROTATE_EXT_VIS = (1<<6), ///< set to true to if the EV observations are in a non NED reference frame and need to be rotated before being used DEPRECATED_USE_GPS_YAW = (1<<7), ///< set to true to use GPS yaw data if available (DEPRECATED, use gnss_ctrl) DEPRECATED_USE_EXT_VIS_VEL = (1<<8), ///< set to true to use external vision velocity data @@ -477,6 +477,7 @@ struct parameters { #if defined(CONFIG_EKF2_DRAG_FUSION) // multi-rotor drag specific force fusion + int32_t drag_ctrl{0}; float drag_noise{2.5f}; ///< observation noise variance for drag specific force measurements (m/sec**2)**2 float bcoef_x{100.0f}; ///< bluff body drag ballistic coefficient for the X-axis (kg/m**2) float bcoef_y{100.0f}; ///< bluff body drag ballistic coefficient for the Y-axis (kg/m**2) diff --git a/src/modules/ekf2/EKF/drag_fusion.cpp b/src/modules/ekf2/EKF/drag_fusion.cpp index 62ecc00c6e..ca15715b7c 100644 --- a/src/modules/ekf2/EKF/drag_fusion.cpp +++ b/src/modules/ekf2/EKF/drag_fusion.cpp @@ -44,7 +44,7 @@ void Ekf::controlDragFusion() { - if ((_params.fusion_mode & SensorFusionMask::USE_DRAG) && _drag_buffer && + if ((_params.drag_ctrl > 0) && _drag_buffer && !_control_status.flags.fake_pos && _control_status.flags.in_air) { if (!_control_status.flags.wind) { diff --git a/src/modules/ekf2/EKF/estimator_interface.cpp b/src/modules/ekf2/EKF/estimator_interface.cpp index a5a0f0f6ff..10aefc3532 100644 --- a/src/modules/ekf2/EKF/estimator_interface.cpp +++ b/src/modules/ekf2/EKF/estimator_interface.cpp @@ -480,7 +480,7 @@ void EstimatorInterface::setDragData(const imuSample &imu) { // down-sample the drag specific force data by accumulating and calculating the mean when // sufficient samples have been collected - if ((_params.fusion_mode & SensorFusionMask::USE_DRAG)) { + if (_params.drag_ctrl > 0) { // Allocate the required buffer size if not previously done if (_drag_buffer == nullptr) { diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 72e206a59a..b590d92b83 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -174,6 +174,7 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _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), @@ -861,6 +862,28 @@ void EKF2::VerifyParams() events::send(events::ID("ekf2_aid_mask_imu"), events::Log::Warning, "Use EKF2_IMU_CTRL instead", _param_ekf2_aid_mask.get()); } + +#if defined(CONFIG_EKF2_DRAG_FUSION) + + if (_param_ekf2_aid_mask.get() & SensorFusionMask::DEPRECATED_USE_DRAG) { + // EKF2_DRAG_CTRL enable drag fusion + _param_ekf2_drag_ctrl.set(1); + + // EKF2_AID_MASK clear deprecated bits + _param_ekf2_aid_mask.set(_param_ekf2_aid_mask.get() & ~(SensorFusionMask::DEPRECATED_USE_DRAG)); + + _param_ekf2_drag_ctrl.commit(); + _param_ekf2_aid_mask.commit(); + + mavlink_log_critical(&_mavlink_log_pub, "EKF2 drag fusion use EKF2_DRAG_CTRL instead of EKF2_AID_MASK\n"); + /* EVENT + * @description EKF2_AID_MASK is set to {1:.0}. + */ + events::send(events::ID("ekf2_aid_mask_drag"), events::Log::Warning, + "Use EKF2_DRAG_CTRL instead", _param_ekf2_aid_mask.get()); + } + +#endif // CONFIG_EKF2_DRAG_FUSION } void EKF2::PublishAidSourceStatus(const hrt_abstime ×tamp) diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 95b7ece543..ae1f0d0f06 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -692,6 +692,7 @@ private: (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 diff --git a/src/modules/ekf2/ekf2_params.c b/src/modules/ekf2/ekf2_params.c index d9c5a73716..4526ac0de1 100644 --- a/src/modules/ekf2/ekf2_params.c +++ b/src/modules/ekf2/ekf2_params.c @@ -618,7 +618,7 @@ PARAM_DEFINE_FLOAT(EKF2_TAS_GATE, 3.0f); * 2 : Deprecated, use EKF2_IMU_CTRL instead * 3 : Deprecated, use EKF2_EV_CTRL instead * 4 : Deprecated, use EKF2_EV_CTRL instead - * 5 : Set to true to enable multi-rotor drag specific force fusion + * 5 : Deprecated. use EKF2_DRAG_CTRL instead * 6 : Deprecated, use EKF2_EV_CTRL instead * 7 : Deprecated, use EKF2_GPS_CTRL instead * 8 : Deprecated, use EKF2_EV_CTRL instead @@ -631,7 +631,7 @@ PARAM_DEFINE_FLOAT(EKF2_TAS_GATE, 3.0f); * @bit 2 unused * @bit 3 unused * @bit 4 unused - * @bit 5 multi-rotor drag fusion + * @bit 5 unused * @bit 6 unused * @bit 7 unused * @bit 8 unused @@ -1253,6 +1253,20 @@ PARAM_DEFINE_FLOAT(EKF2_EVV_GATE, 3.0f); */ PARAM_DEFINE_FLOAT(EKF2_EVP_GATE, 5.0f); +/** + * Multirotor wind estimation selection + * + * Activate wind speed estimation using specific-force measurements and + * a drag model defined by EKF2_BCOEF_[XY] and EKF2_MCOEF. + * + * Only use on vehicles that have their thrust aligned with the Z axis and + * no thrust in the XY plane. + * + * @group EKF2 + * @boolean + */ +PARAM_DEFINE_INT32(EKF2_DRAG_CTRL, 0); + /** * Specific drag force observation noise variance used by the multi-rotor specific drag force model. * @@ -1269,7 +1283,7 @@ PARAM_DEFINE_FLOAT(EKF2_DRAG_NOISE, 2.5f); /** * X-axis ballistic coefficient used for multi-rotor wind estimation. * - * This parameter controls the prediction of drag produced by bluff body drag along the forward/reverse axis when flying a multi-copter which enables estimation of wind drift when enabled by the EKF2_AID_MASK parameter. The drag produced by this effect scales with speed squared. The predicted drag from the rotors is specified separately by the EKF2_MCOEF parameter. + * This parameter controls the prediction of drag produced by bluff body drag along the forward/reverse axis when flying a multi-copter which enables estimation of wind drift when enabled by the EKF2_DRAG_CTRL parameter. The drag produced by this effect scales with speed squared. The predicted drag from the rotors is specified separately by the EKF2_MCOEF parameter. * Set this parameter to zero to turn off the bluff body drag model for this axis. * * @group EKF2 @@ -1283,7 +1297,7 @@ PARAM_DEFINE_FLOAT(EKF2_BCOEF_X, 100.0f); /** * Y-axis ballistic coefficient used for multi-rotor wind estimation. * - * This parameter controls the prediction of drag produced by bluff body drag along the right/left axis when flying a multi-copter, which enables estimation of wind drift when enabled by the EKF2_AID_MASK parameter. The drag produced by this effect scales with speed squared. The predicted drag from the rotors is specified separately by the EKF2_MCOEF parameter. + * This parameter controls the prediction of drag produced by bluff body drag along the right/left axis when flying a multi-copter, which enables estimation of wind drift when enabled by the EKF2_DRAG_CTRL parameter. The drag produced by this effect scales with speed squared. The predicted drag from the rotors is specified separately by the EKF2_MCOEF parameter. * Set this parameter to zero to turn off the bluff body drag model for this axis. * * @group EKF2 @@ -1297,7 +1311,7 @@ PARAM_DEFINE_FLOAT(EKF2_BCOEF_Y, 100.0f); /** * Propeller momentum drag coefficient used for multi-rotor wind estimation. * - * This parameter controls the prediction of drag produced by the propellers when flying a multi-copter, which enables estimation of wind drift when enabled by the EKF2_AID_MASK parameter. The drag produced by this effect scales with speed not speed squared and is produced because some of the air velocity normal to the propeller axis of rotation is lost when passing through the rotor disc. This changes the momentum of the flow which creates a drag reaction force. When comparing un-ducted propellers of the same diameter, the effect is roughly proportional to the area of the propeller blades when viewed side on and changes with propeller selection. Momentum drag is significantly higher for ducted rotors. To account for the drag produced by the body which scales with speed squared, see documentation for the EKF2_BCOEF_X and EKF2_BCOEF_Y parameters. + * This parameter controls the prediction of drag produced by the propellers when flying a multi-copter, which enables estimation of wind drift when enabled by the EKF2_DRAG_CTRL parameter. The drag produced by this effect scales with speed not speed squared and is produced because some of the air velocity normal to the propeller axis of rotation is lost when passing through the rotor disc. This changes the momentum of the flow which creates a drag reaction force. When comparing un-ducted propellers of the same diameter, the effect is roughly proportional to the area of the propeller blades when viewed side on and changes with propeller selection. Momentum drag is significantly higher for ducted rotors. To account for the drag produced by the body which scales with speed squared, see documentation for the EKF2_BCOEF_X and EKF2_BCOEF_Y parameters. * Set this parameter to zero to turn off the momentum drag model for both axis. * * @group EKF2 diff --git a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp index 900fb15aa3..1be9c2c43a 100644 --- a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp +++ b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp @@ -285,12 +285,12 @@ matrix::Vector3f EkfWrapper::getDeltaVelBiasVariance() const void EkfWrapper::enableDragFusion() { - _ekf_params->fusion_mode |= SensorFusionMask::USE_DRAG; + _ekf_params->drag_ctrl = 1; } void EkfWrapper::disableDragFusion() { - _ekf_params->fusion_mode &= ~SensorFusionMask::USE_DRAG; + _ekf_params->drag_ctrl = 0; } void EkfWrapper::setDragFusionParameters(const float &bcoef_x, const float &bcoef_y, const float &mcoef)