fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 02:03:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: don't store _drag_sample_delayed

Browse Source
This commit is contained in:
Daniel Agar
2022-02-02 17:43:25 -05:00
parent 138ff7a316
commit 452b5e94b4
4 changed files with 12 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/drag_fusion.cpp
+2 -2
View File
@@ -44,7 +44,7 @@
#include <mathlib/mathlib.h>
void Ekf::fuseDrag()
void Ekf::fuseDrag(const dragSample &drag_sample)
{
SparseVector24f<0,1,2,3,4,5,6,22,23> Hfusion; // Observation Jacobians
Vector24f Kfusion; // Kalman gain vector
@@ -89,7 +89,7 @@ void Ekf::fuseDrag()
// perform sequential fusion of XY specific forces
for (uint8_t axis_index = 0; axis_index < 2; axis_index++) {
// measured drag acceleration corrected for sensor bias
const float mea_acc = _drag_sample_delayed.accelXY(axis_index) - _state.delta_vel_bias(axis_index) / _dt_ekf_avg;
const float mea_acc = drag_sample.accelXY(axis_index) - _state.delta_vel_bias(axis_index) / _dt_ekf_avg;
// predicted drag force sign is opposite to predicted wind relative velocity
const float drag_sign = (rel_wind_body(axis_index) >= 0.f) ? -1.f : 1.f;