fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: move controlDragFusion control.cpp -> drag_fusion.cpp

Browse Source
This commit is contained in:
Daniel Agar 2023-02-21 10:01:09 -05:00
parent c08a387c5a
commit b4e845d7c0
2 changed files with 21 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
src/modules/ekf2/EKF/control.cpp
View File

@ -218,27 +218,6 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed)
updateDeadReckoningStatus();
}
void Ekf::controlDragFusion()
{
if ((_params.fusion_mode & SensorFusionMask::USE_DRAG) && _drag_buffer &&
!_control_status.flags.fake_pos && _control_status.flags.in_air) {
if (!_control_status.flags.wind) {
// reset the wind states and covariances when starting drag accel fusion
_control_status.flags.wind = true;
resetWind();
}
dragSample drag_sample;
if (_drag_buffer->pop_first_older_than(_time_delayed_us, &drag_sample)) {
fuseDrag(drag_sample);
}
}
}
void Ekf::controlAuxVelFusion()
{
if (_auxvel_buffer) {

21
src/modules/ekf2/EKF/drag_fusion.cpp
View File

@ -42,6 +42,27 @@
#include <mathlib/mathlib.h>
void Ekf::controlDragFusion()
{
if ((_params.fusion_mode & SensorFusionMask::USE_DRAG) && _drag_buffer &&
!_control_status.flags.fake_pos && _control_status.flags.in_air) {
if (!_control_status.flags.wind) {
// reset the wind states and covariances when starting drag accel fusion
_control_status.flags.wind = true;
resetWind();
}
dragSample drag_sample;
if (_drag_buffer->pop_first_older_than(_time_delayed_us, &drag_sample)) {
fuseDrag(drag_sample);
}
}
}
void Ekf::fuseDrag(const dragSample &drag_sample)
{
const float R_ACC = fmaxf(_params.drag_noise, 0.5f); // observation noise variance in specific force drag (m/sec**2)**2