forked from Archive/PX4-Autopilot
ekf2: move drag fusion activation from AID_MASK to DRAG_CTRL
This commit is contained in:
parent
8b4566b6bb
commit
39c2f95669
|
@ -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 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);
|
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;
|
_control_status.flags.wind = false;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -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_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_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
|
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_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_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
|
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)
|
#if defined(CONFIG_EKF2_DRAG_FUSION)
|
||||||
// multi-rotor drag specific force 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 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_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)
|
float bcoef_y{100.0f}; ///< bluff body drag ballistic coefficient for the Y-axis (kg/m**2)
|
||||||
|
|
|
@ -44,7 +44,7 @@
|
||||||
|
|
||||||
void Ekf::controlDragFusion()
|
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) {
|
!_control_status.flags.fake_pos && _control_status.flags.in_air) {
|
||||||
|
|
||||||
if (!_control_status.flags.wind) {
|
if (!_control_status.flags.wind) {
|
||||||
|
|
|
@ -480,7 +480,7 @@ void EstimatorInterface::setDragData(const imuSample &imu)
|
||||||
{
|
{
|
||||||
// down-sample the drag specific force data by accumulating and calculating the mean when
|
// down-sample the drag specific force data by accumulating and calculating the mean when
|
||||||
// sufficient samples have been collected
|
// 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
|
// Allocate the required buffer size if not previously done
|
||||||
if (_drag_buffer == nullptr) {
|
if (_drag_buffer == nullptr) {
|
||||||
|
|
|
@ -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_abl_tau(_params->acc_bias_learn_tc),
|
||||||
_param_ekf2_gyr_b_lim(_params->gyro_bias_lim),
|
_param_ekf2_gyr_b_lim(_params->gyro_bias_lim),
|
||||||
#if defined(CONFIG_EKF2_DRAG_FUSION)
|
#if defined(CONFIG_EKF2_DRAG_FUSION)
|
||||||
|
_param_ekf2_drag_ctrl(_params->drag_ctrl),
|
||||||
_param_ekf2_drag_noise(_params->drag_noise),
|
_param_ekf2_drag_noise(_params->drag_noise),
|
||||||
_param_ekf2_bcoef_x(_params->bcoef_x),
|
_param_ekf2_bcoef_x(_params->bcoef_x),
|
||||||
_param_ekf2_bcoef_y(_params->bcoef_y),
|
_param_ekf2_bcoef_y(_params->bcoef_y),
|
||||||
|
@ -861,6 +862,28 @@ void EKF2::VerifyParams()
|
||||||
events::send<float>(events::ID("ekf2_aid_mask_imu"), events::Log::Warning,
|
events::send<float>(events::ID("ekf2_aid_mask_imu"), events::Log::Warning,
|
||||||
"Use EKF2_IMU_CTRL instead", _param_ekf2_aid_mask.get());
|
"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 <param>EKF2_AID_MASK</param> is set to {1:.0}.
|
||||||
|
*/
|
||||||
|
events::send<float>(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)
|
void EKF2::PublishAidSourceStatus(const hrt_abstime ×tamp)
|
||||||
|
|
|
@ -692,6 +692,7 @@ private:
|
||||||
(ParamExtFloat<px4::params::EKF2_GYR_B_LIM>) _param_ekf2_gyr_b_lim, ///< Gyro bias learning limit (rad/s)
|
(ParamExtFloat<px4::params::EKF2_GYR_B_LIM>) _param_ekf2_gyr_b_lim, ///< Gyro bias learning limit (rad/s)
|
||||||
|
|
||||||
#if defined(CONFIG_EKF2_DRAG_FUSION)
|
#if defined(CONFIG_EKF2_DRAG_FUSION)
|
||||||
|
(ParamExtInt<px4::params::EKF2_DRAG_CTRL>) _param_ekf2_drag_ctrl, ///< drag fusion selection
|
||||||
// Multi-rotor drag specific force fusion
|
// Multi-rotor drag specific force fusion
|
||||||
(ParamExtFloat<px4::params::EKF2_DRAG_NOISE>)
|
(ParamExtFloat<px4::params::EKF2_DRAG_NOISE>)
|
||||||
_param_ekf2_drag_noise, ///< observation noise variance for drag specific force measurements (m/sec**2)**2
|
_param_ekf2_drag_noise, ///< observation noise variance for drag specific force measurements (m/sec**2)**2
|
||||||
|
|
|
@ -618,7 +618,7 @@ PARAM_DEFINE_FLOAT(EKF2_TAS_GATE, 3.0f);
|
||||||
* 2 : Deprecated, use EKF2_IMU_CTRL instead
|
* 2 : Deprecated, use EKF2_IMU_CTRL instead
|
||||||
* 3 : Deprecated, use EKF2_EV_CTRL instead
|
* 3 : Deprecated, use EKF2_EV_CTRL instead
|
||||||
* 4 : 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
|
* 6 : Deprecated, use EKF2_EV_CTRL instead
|
||||||
* 7 : Deprecated, use EKF2_GPS_CTRL instead
|
* 7 : Deprecated, use EKF2_GPS_CTRL instead
|
||||||
* 8 : Deprecated, use EKF2_EV_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 2 unused
|
||||||
* @bit 3 unused
|
* @bit 3 unused
|
||||||
* @bit 4 unused
|
* @bit 4 unused
|
||||||
* @bit 5 multi-rotor drag fusion
|
* @bit 5 unused
|
||||||
* @bit 6 unused
|
* @bit 6 unused
|
||||||
* @bit 7 unused
|
* @bit 7 unused
|
||||||
* @bit 8 unused
|
* @bit 8 unused
|
||||||
|
@ -1253,6 +1253,20 @@ PARAM_DEFINE_FLOAT(EKF2_EVV_GATE, 3.0f);
|
||||||
*/
|
*/
|
||||||
PARAM_DEFINE_FLOAT(EKF2_EVP_GATE, 5.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.
|
* 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.
|
* 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.
|
* Set this parameter to zero to turn off the bluff body drag model for this axis.
|
||||||
*
|
*
|
||||||
* @group EKF2
|
* @group EKF2
|
||||||
|
@ -1283,7 +1297,7 @@ PARAM_DEFINE_FLOAT(EKF2_BCOEF_X, 100.0f);
|
||||||
/**
|
/**
|
||||||
* Y-axis ballistic coefficient used for multi-rotor wind estimation.
|
* 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.
|
* Set this parameter to zero to turn off the bluff body drag model for this axis.
|
||||||
*
|
*
|
||||||
* @group EKF2
|
* @group EKF2
|
||||||
|
@ -1297,7 +1311,7 @@ PARAM_DEFINE_FLOAT(EKF2_BCOEF_Y, 100.0f);
|
||||||
/**
|
/**
|
||||||
* Propeller momentum drag coefficient used for multi-rotor wind estimation.
|
* 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.
|
* Set this parameter to zero to turn off the momentum drag model for both axis.
|
||||||
*
|
*
|
||||||
* @group EKF2
|
* @group EKF2
|
||||||
|
|
|
@ -285,12 +285,12 @@ matrix::Vector3f EkfWrapper::getDeltaVelBiasVariance() const
|
||||||
|
|
||||||
void EkfWrapper::enableDragFusion()
|
void EkfWrapper::enableDragFusion()
|
||||||
{
|
{
|
||||||
_ekf_params->fusion_mode |= SensorFusionMask::USE_DRAG;
|
_ekf_params->drag_ctrl = 1;
|
||||||
}
|
}
|
||||||
|
|
||||||
void EkfWrapper::disableDragFusion()
|
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)
|
void EkfWrapper::setDragFusionParameters(const float &bcoef_x, const float &bcoef_y, const float &mcoef)
|
||||||
|
|
Loading…
Reference in New Issue