From c607941e2f94267894dafedef5ee48c4d4ec5163 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Wed, 12 Apr 2017 09:42:25 +1000 Subject: [PATCH] EKF: Allow external setting of air density for multi-rotor drag fusion --- EKF/drag_fusion.cpp | 3 +-- EKF/estimator_interface.cpp | 1 + EKF/estimator_interface.h | 6 ++++++ 3 files changed, 8 insertions(+), 2 deletions(-) diff --git a/EKF/drag_fusion.cpp b/EKF/drag_fusion.cpp index e3ce47cbd1..f4abf826b1 100644 --- a/EKF/drag_fusion.cpp +++ b/EKF/drag_fusion.cpp @@ -50,8 +50,7 @@ void Ekf::fuseDrag() float Kfusion[24] = {}; // Kalman gain vector float R_ACC = _params.drag_noise; // observation noise in specific force drag (m/sec**2) - float eas2tas = 1.0f; // conversion factor from EAS to TAS - TODO calculate from barometric pressure - float rho = 1.225f / sqrtf(eas2tas); // air density (kg/m**3) + float rho = fmaxf(_air_density, 0.1f); // air density (kg/m**3) // calculate inverse of ballistic coefficient if (_params.bcoef_x < 1.0f || _params.bcoef_y < 1.0f) { diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index 3729b1ecab..2f99e0e9f9 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -63,6 +63,7 @@ EstimatorInterface::EstimatorInterface(): _drag_down_sampled{}, _drag_sample_count(0), _drag_sample_time_dt(0.0f), + _air_density(1.225f), _imu_ticks(0), _imu_updated(false), _initialised(false), diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index b9d0af4640..b5e974315a 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -184,6 +184,9 @@ public: // set flag if synthetic sideslip measurement should be fused void set_fuse_beta_flag(bool fuse_beta) {_control_status.flags.fuse_beta = fuse_beta;} + // set air density used by the multi-rotor specific drag force fusion + void set_air_density(float air_density) {_air_density = air_density;} + // return true if the global position estimate is valid virtual bool global_position_is_valid() = 0; @@ -321,8 +324,11 @@ protected: extVisionSample _ev_sample_delayed; dragSample _drag_sample_delayed; dragSample _drag_down_sampled; // down sampled drag specific force data (filter prediction rate -> observation rate) + + // Used by the multi-rotor specific drag force fusion uint8_t _drag_sample_count; // number of drag specific force samples assumulated at the filter prediction rate float _drag_sample_time_dt; // time integral across all samples used to form _drag_down_sampled (sec) + float _air_density; // air density (kg/m**3) outputSample _output_sample_delayed; // filter output on the delayed time horizon outputSample _output_new; // filter output on the non-delayed time horizon