Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-23 22:58:10 +08:00
Code Issues Actions 15 Packages Projects Releases Wiki Activity

EKF: Clean up wind state activation logic

Browse Source 
Ensure wind states are deactivated in one place and always when in a non-position mode.
Activate wind states separately for each observation method.
This commit is contained in:
Paul Riseborough
2020-12-03 20:38:11 +11:00
committed by Daniel Agar
parent 44ebfb8c43
commit ee94980a8f
2 changed files with 14 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/airspeed_fusion.cpp
+1
View File
@@ -158,6 +158,7 @@ void Ekf::fuseAirspeed()
if (is_fused) { if (is_fused) {
_time_last_arsp_fuse = _time_last_imu; _time_last_arsp_fuse = _time_last_imu;
_control_status.flags.fuse_aspd = true;
} }
} }
EKF/control.cpp
+13 -29
View File
@@ -1161,8 +1161,8 @@ void Ekf::controlAirDataFusion()
// If both airspeed and sideslip fusion have timed out and we are not using a drag observation model then we no longer have valid wind estimates // If both airspeed and sideslip fusion have timed out and we are not using a drag observation model then we no longer have valid wind estimates
const bool airspeed_timed_out = isTimedOut(_time_last_arsp_fuse, (uint64_t)10e6); 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); const bool sideslip_timed_out = isTimedOut(_time_last_beta_fuse, (uint64_t)10e6);
if (_control_status.flags.wind &&
if (_control_status.flags.wind && airspeed_timed_out && sideslip_timed_out && !(_params.fusion_mode & MASK_USE_DRAG)) { (_using_synthetic_position || (airspeed_timed_out && sideslip_timed_out && !(_params.fusion_mode & MASK_USE_DRAG)))) {
_control_status.flags.wind = false; _control_status.flags.wind = false;
} }
@@ -1171,19 +1171,13 @@ void Ekf::controlAirDataFusion()
} }
// Always try to fuse airspeed data if available and we are in flight // Always try to fuse airspeed data if available and we are in flight
if (_tas_data_ready && _control_status.flags.in_air) { if (!_using_synthetic_position && _tas_data_ready && _control_status.flags.in_air) {
// always fuse airsped data if we are flying and data is present
if (!_control_status.flags.fuse_aspd) {
_control_status.flags.fuse_aspd = true;
}
// If starting wind state estimation, reset the wind states and covariances before fusing any data // If starting wind state estimation, reset the wind states and covariances before fusing any data
if (!_control_status.flags.wind) { if (!_control_status.flags.wind) {
// activate the wind states // activate the wind states
_control_status.flags.wind = true; _control_status.flags.wind = true;
// reset the timout timer to prevent repeated resets // reset the timout timer to prevent repeated resets
_time_last_arsp_fuse = _time_last_imu; _time_last_arsp_fuse = _time_last_imu;
_time_last_beta_fuse = _time_last_imu;
// reset the wind speed states and corresponding covariances // reset the wind speed states and corresponding covariances
resetWindStates(); resetWindStates();
resetWindCovariance(); resetWindCovariance();
@@ -1195,29 +1189,21 @@ void Ekf::controlAirDataFusion()
void Ekf::controlBetaFusion() void Ekf::controlBetaFusion()
{ {
// control activation and initialisation/reset of wind states required for synthetic sideslip fusion fusion if (_using_synthetic_position) {
return;
// If both airspeed and sideslip fusion have timed out and we are not using a drag observation model then we no longer have valid wind estimates
const bool sideslip_timed_out = isTimedOut(_time_last_beta_fuse, (uint64_t)10e6);
const bool airspeed_timed_out = isTimedOut(_time_last_arsp_fuse, (uint64_t)10e6);
if (_control_status.flags.wind && airspeed_timed_out && sideslip_timed_out && !(_params.fusion_mode & MASK_USE_DRAG)) {
_control_status.flags.wind = false;
} }
// Perform synthetic sideslip fusion when in-air and sideslip fuson had been enabled externally in addition to the following criteria: // Perform synthetic sideslip fusion at regular intervals when in-air and sideslip fuson had been enabled externally:
// Sufficient time has lapsed sice the last fusion
const bool beta_fusion_time_triggered = isTimedOut(_time_last_beta_fuse, _params.beta_avg_ft_us); const bool beta_fusion_time_triggered = isTimedOut(_time_last_beta_fuse, _params.beta_avg_ft_us);
if (beta_fusion_time_triggered &&
if (beta_fusion_time_triggered && _control_status.flags.fuse_beta && _control_status.flags.in_air) { _control_status.flags.fuse_beta &&
_control_status.flags.in_air) {
// If starting wind state estimation, reset the wind states and covariances before fusing any data // If starting wind state estimation, reset the wind states and covariances before fusing any data
if (!_control_status.flags.wind) { if (!_control_status.flags.wind) {
// activate the wind states // activate the wind states
_control_status.flags.wind = true; _control_status.flags.wind = true;
// reset the timeout timers to prevent repeated resets // reset the timeout timers to prevent repeated resets
_time_last_beta_fuse = _time_last_imu; _time_last_beta_fuse = _time_last_imu;
_time_last_arsp_fuse = _time_last_imu;
// reset the wind speed states and corresponding covariances // reset the wind speed states and corresponding covariances
resetWindStates(); resetWindStates();
resetWindCovariance(); resetWindCovariance();
@@ -1229,9 +1215,10 @@ void Ekf::controlBetaFusion()
void Ekf::controlDragFusion() void Ekf::controlDragFusion()
{ {
if (_params.fusion_mode & MASK_USE_DRAG) { if (_params.fusion_mode & MASK_USE_DRAG &&
if (_control_status.flags.in_air !_using_synthetic_position &&
&& !_mag_inhibit_yaw_reset_req) { _control_status.flags.in_air &&
!_mag_inhibit_yaw_reset_req) {
if (!_control_status.flags.wind) { if (!_control_status.flags.wind) {
// reset the wind states and covariances when starting drag accel fusion // reset the wind states and covariances when starting drag accel fusion
_control_status.flags.wind = true; _control_status.flags.wind = true;
@@ -1242,9 +1229,6 @@ void Ekf::controlDragFusion()
fuseDrag(); fuseDrag();
} }
} else {
_control_status.flags.wind = false;
}
} }
} }