dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

ekf2: don't store _drag_sample_delayed

This commit is contained in:
Daniel Agar 2022-02-01 09:40:03 -05:00
parent 138ff7a316
commit 452b5e94b4
4 changed files with 12 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -1047,18 +1047,21 @@ void Ekf::controlBetaFusion()
void Ekf::controlDragFusion()
{
if ((_params.fusion_mode & MASK_USE_DRAG) &&
!_using_synthetic_position &&
_control_status.flags.in_air &&
!_mag_inhibit_yaw_reset_req) {
if ((_params.fusion_mode & MASK_USE_DRAG) && _drag_buffer &&
!_using_synthetic_position && _control_status.flags.in_air && !_mag_inhibit_yaw_reset_req) {
if (!_control_status.flags.wind) {
// reset the wind states and covariances when starting drag accel fusion
_control_status.flags.wind = true;
resetWind();
} else if (_drag_buffer && _drag_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &_drag_sample_delayed)) {
fuseDrag();
}
dragSample drag_sample;
if (_drag_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &drag_sample)) {
fuseDrag(drag_sample);
}
}
}

4
src/modules/ekf2/EKF/drag_fusion.cpp
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;

2
src/modules/ekf2/EKF/ekf.h
View File

@ -622,7 +622,7 @@ private:
void fuseSideslip();
// fuse body frame drag specific forces for multi-rotor wind estimation
void fuseDrag();
void fuseDrag(const dragSample &drag_sample);
void fuseBaroHgt();
void fuseGpsHgt();

1
src/modules/ekf2/EKF/estimator_interface.h
View File

@ -295,7 +295,6 @@ protected:
flowSample _flow_sample_delayed{};
extVisionSample _ev_sample_delayed{};
extVisionSample _ev_sample_delayed_prev{};
dragSample _drag_sample_delayed{};
dragSample _drag_down_sampled{}; // down sampled drag specific force data (filter prediction rate -> observation rate)
auxVelSample _auxvel_sample_delayed{};