Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-25 00:31:36 +08:00
Code Issues Actions 7 Packages Projects Releases Wiki Activity

ekf2: refactor airspeed fusion control logic

Browse Source
This commit is contained in:
bresch
2021-10-22 16:35:13 +02:00
committed by Mathieu Bresciani
parent 8873e92c7c
commit 6e8f0e92ff
6 changed files with 66 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/airspeed_fusion.cpp
+1 -2
View File
@@ -57,7 +57,7 @@ void Ekf::fuseAirspeed()
const float R_TAS = sq(math::constrain(_params.eas_noise, 0.5f, 5.0f) *
math::constrain(_airspeed_sample_delayed.eas2tas, 0.9f, 10.0f));
// determine if we need the sideslip fusion to correct states other than wind
// determine if we need the airspeed fusion to correct states other than wind
const bool update_wind_only = !_is_wind_dead_reckoning;
// Intermediate variables
@@ -165,7 +165,6 @@ void Ekf::fuseAirspeed()
if (is_fused) {
_time_last_arsp_fuse = _time_last_imu;
_control_status.flags.fuse_aspd = true;
}
}
src/modules/ekf2/EKF/control.cpp
+39 -15
View File
@@ -1302,29 +1302,53 @@ void Ekf::controlAirDataFusion()
const bool airspeed_timed_out = isTimedOut(_time_last_arsp_fuse, (uint64_t)10e6);
const bool sideslip_timed_out = isTimedOut(_time_last_beta_fuse, (uint64_t)10e6);
if (_control_status.flags.wind &&
(_using_synthetic_position || (airspeed_timed_out && sideslip_timed_out && !(_params.fusion_mode & MASK_USE_DRAG)))) {
if (_using_synthetic_position || (airspeed_timed_out && sideslip_timed_out && !(_params.fusion_mode & MASK_USE_DRAG))) {
_control_status.flags.wind = false;
}
if (_control_status.flags.fuse_aspd && airspeed_timed_out) {
_control_status.flags.fuse_aspd = false;
if (_params.arsp_thr <= 0.f) {
stopAirspeedFusion();
return;
}
// Always try to fuse airspeed data if available and we are in flight
if (!_using_synthetic_position && _tas_data_ready && _control_status.flags.in_air) {
// If starting wind state estimation, reset the wind states and covariances before fusing any data
if (!_control_status.flags.wind) {
// activate the wind states
_control_status.flags.wind = true;
// reset the timout timer to prevent repeated resets
if (_tas_data_ready) {
const bool continuing_conditions_passing = _control_status.flags.in_air && _control_status.flags.fixed_wing && !_using_synthetic_position;
const bool is_airspeed_significant = _airspeed_sample_delayed.true_airspeed > _params.arsp_thr;
const bool starting_conditions_passing = continuing_conditions_passing && is_airspeed_significant;
if (_control_status.flags.fuse_aspd) {
if (continuing_conditions_passing) {
if (is_airspeed_significant) {
fuseAirspeed();
}
const bool is_fusion_failing = isTimedOut(_time_last_arsp_fuse, (uint64_t)10e6);
if (is_fusion_failing) {
stopAirspeedFusion();
}
} else {
stopAirspeedFusion();
}
} else if (starting_conditions_passing) {
// If starting wind state estimation, reset the wind states and covariances before fusing any data
if (!_control_status.flags.wind) {
// activate the wind states
_control_status.flags.wind = true;
// reset the wind speed states and corresponding covariances
resetWindStates();
resetWindCovariance();
}
startAirspeedFusion();
_time_last_arsp_fuse = _time_last_imu;
// reset the wind speed states and corresponding covariances
resetWindStates();
resetWindCovariance();
}
fuseAirspeed();
} else if (_control_status.flags.fuse_aspd && (_imu_sample_delayed.time_us - _airspeed_sample_delayed.time_us > (uint64_t) 1e6)) {
ECL_WARN("Airspeed data stopped");
stopAirspeedFusion();
}
}
src/modules/ekf2/EKF/ekf.h
+3
View File
@@ -954,6 +954,9 @@ private:
return sensor_timestamp + acceptance_interval > _time_last_imu;
}
void startAirspeedFusion();
void stopAirspeedFusion();
void startGpsFusion();
void stopGpsFusion();
void stopGpsPosFusion();
src/modules/ekf2/EKF/ekf_helper.cpp
+15 -5
View File
@@ -1480,6 +1480,14 @@ void Ekf::loadMagCovData()
P.slice<2, 2>(16, 16) = _saved_mag_ef_covmat;
}
void Ekf::startAirspeedFusion() {
_control_status.flags.fuse_aspd = true;
}
void Ekf::stopAirspeedFusion() {
_control_status.flags.fuse_aspd = false;
}
void Ekf::startGpsFusion()
{
resetHorizontalPositionToGps();
@@ -1723,13 +1731,15 @@ bool Ekf::getDataEKFGSF(float *yaw_composite, float *yaw_variance, float yaw[N_M
void Ekf::runYawEKFGSF()
{
float TAS;
float TAS = 0.f;
if (isTimedOut(_airspeed_sample_delayed.time_us, 1000000) && _control_status.flags.fixed_wing) {
TAS = _params.EKFGSF_tas_default;
if (_control_status.flags.fixed_wing) {
if (isTimedOut(_airspeed_sample_delayed.time_us, 1000000)) {
TAS = _params.EKFGSF_tas_default;
} else {
TAS = _airspeed_sample_delayed.true_airspeed;
} else if (_airspeed_sample_delayed.true_airspeed >= _params.arsp_thr) {
TAS = _airspeed_sample_delayed.true_airspeed;
}
}
const Vector3f imu_gyro_bias = getGyroBias();
src/modules/ekf2/EKF2.cpp
+7 -10
View File
@@ -140,6 +140,7 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
_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_arsp_thr(_params->arsp_thr),
_param_ekf2_tau_vel(_params->vel_Tau),
_param_ekf2_tau_pos(_params->pos_Tau),
_param_ekf2_gbias_init(_params->switch_on_gyro_bias),
@@ -1360,16 +1361,12 @@ void EKF2::UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps)
// via the wind estimator that uses EKF velocity estimates.
const float true_airspeed_m_s = airspeed.true_airspeed_m_s * _airspeed_scale_factor;
// only set airspeed data if condition for airspeed fusion are met
if ((_param_ekf2_arsp_thr.get() > FLT_EPSILON) && (true_airspeed_m_s > _param_ekf2_arsp_thr.get())) {
airspeedSample airspeed_sample {
.time_us = airspeed.timestamp,
.true_airspeed = true_airspeed_m_s,
.eas2tas = airspeed.true_airspeed_m_s / airspeed.indicated_airspeed_m_s,
};
_ekf.setAirspeedData(airspeed_sample);
}
airspeedSample airspeed_sample {
.time_us = airspeed.timestamp,
.true_airspeed = true_airspeed_m_s,
.eas2tas = airspeed.true_airspeed_m_s / airspeed.indicated_airspeed_m_s,
};
_ekf.setAirspeedData(airspeed_sample);
ekf2_timestamps.airspeed_timestamp_rel = (int16_t)((int64_t)airspeed.timestamp / 100 -
(int64_t)ekf2_timestamps.timestamp / 100);
src/modules/ekf2/EKF2.hpp
+1 -1
View File
@@ -471,7 +471,7 @@ private:
_param_ekf2_ev_pos_z, ///< Z position of VI sensor focal point in body frame (m)
// control of airspeed and sideslip fusion
(ParamFloat<px4::params::EKF2_ARSP_THR>)
(ParamExtFloat<px4::params::EKF2_ARSP_THR>)
_param_ekf2_arsp_thr, ///< A value of zero will disabled airspeed fusion. Any positive value sets the minimum airspeed which will be used (m/sec)
(ParamInt<px4::params::EKF2_FUSE_BETA>)
_param_ekf2_fuse_beta, ///< Controls synthetic sideslip fusion, 0 disables, 1 enables