Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-20 20:03:54 +08:00
Code Issues Actions 8 Packages Projects Releases Wiki Activity

ekf2: move controlBetaFusion control.cpp -> sideslip_fusion.cpp

Browse Source
This commit is contained in:
Daniel Agar
2023-02-21 10:00:37 -05:00
parent d61b8c21b2
commit c08a387c5a
2 changed files with 31 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/control.cpp
-31
View File
@@ -218,37 +218,6 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed)
updateDeadReckoningStatus();
}
void Ekf::controlBetaFusion(const imuSample &imu_delayed)
{
_control_status.flags.fuse_beta = _params.beta_fusion_enabled && _control_status.flags.fixed_wing
&& _control_status.flags.in_air && !_control_status.flags.fake_pos;
if (_control_status.flags.fuse_beta) {
// Perform synthetic sideslip fusion at regular intervals when in-air and sideslip fusion had been enabled externally:
const bool beta_fusion_time_triggered = isTimedOut(_aid_src_sideslip.time_last_fuse, _params.beta_avg_ft_us);
if (beta_fusion_time_triggered) {
updateSideslip(_aid_src_sideslip);
_innov_check_fail_status.flags.reject_sideslip = _aid_src_sideslip.innovation_rejected;
// 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 timeout timers to prevent repeated resets
_aid_src_sideslip.time_last_fuse = imu_delayed.time_us;
resetWind();
}
if (Vector2f(Vector2f(_state.vel) - _state.wind_vel).longerThan(7.f)) {
fuseSideslip(_aid_src_sideslip);
}
}
}
}
void Ekf::controlDragFusion()
{
if ((_params.fusion_mode & SensorFusionMask::USE_DRAG) && _drag_buffer &&
src/modules/ekf2/EKF/sideslip_fusion.cpp
+31
View File
@@ -47,6 +47,37 @@
#include <mathlib/mathlib.h>
void Ekf::controlBetaFusion(const imuSample &imu_delayed)
{
_control_status.flags.fuse_beta = _params.beta_fusion_enabled && _control_status.flags.fixed_wing
&& _control_status.flags.in_air && !_control_status.flags.fake_pos;
if (_control_status.flags.fuse_beta) {
// Perform synthetic sideslip fusion at regular intervals when in-air and sideslip fusion had been enabled externally:
const bool beta_fusion_time_triggered = isTimedOut(_aid_src_sideslip.time_last_fuse, _params.beta_avg_ft_us);
if (beta_fusion_time_triggered) {
updateSideslip(_aid_src_sideslip);
_innov_check_fail_status.flags.reject_sideslip = _aid_src_sideslip.innovation_rejected;
// 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 timeout timers to prevent repeated resets
_aid_src_sideslip.time_last_fuse = imu_delayed.time_us;
resetWind();
}
if (Vector2f(Vector2f(_state.vel) - _state.wind_vel).longerThan(7.f)) {
fuseSideslip(_aid_src_sideslip);
}
}
}
}
void Ekf::updateSideslip(estimator_aid_source1d_s &sideslip) const
{
// reset flags