diff --git a/EKF/common.h b/EKF/common.h index ebb80c911f..c4f75ffaed 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -140,6 +140,11 @@ struct extVisionSample { uint64_t time_us; // timestamp of the measurement in microseconds }; +struct dragSample { + Vector2f accelXY; // measured specific force along the X and Y body axes (m/s**2) + uint64_t time_us; // timestamp in microseconds of the measurement +}; + // Integer definitions for vdist_sensor_type #define VDIST_SENSOR_BARO 0 // Use baro height #define VDIST_SENSOR_GPS 1 // Use GPS height diff --git a/EKF/control.cpp b/EKF/control.cpp index f19fabfe73..b394348a60 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -865,12 +865,14 @@ void Ekf::controlDragFusion() { if (_params.fusion_mode & MASK_USE_DRAG && _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; resetWindStates(); resetWindCovariance(); - } else { + } else if (_drag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_drag_sample_delayed)) { fuseDrag(); + } } else { _control_status.flags.wind = false; diff --git a/EKF/drag_fusion.cpp b/EKF/drag_fusion.cpp index a62b03dba2..e3ce47cbd1 100644 --- a/EKF/drag_fusion.cpp +++ b/EKF/drag_fusion.cpp @@ -75,11 +75,6 @@ void Ekf::fuseDrag() float vwn = _state.wind_vel(0); float vwe = _state.wind_vel(1); - // calculate measured XY specific forces if enough data - if (_imu_sample_delayed.delta_vel_dt < 1e-3f) { - return; - } - // predicted specific forces // calculate relative wind velocity in earth frame and rotte into body frame Vector3f rel_wind; @@ -95,11 +90,11 @@ void Ekf::fuseDrag() // calculate observation jacobiam and Kalman gain vectors if (axis_index == 0) { // Estimate the airspeed from the measured drag force and ballistic coefficient - float mea_acc = (_imu_sample_delayed.delta_vel(axis_index) - _state.accel_bias(axis_index))/_imu_sample_delayed.delta_vel_dt; + float mea_acc = _drag_sample_delayed.accelXY(axis_index) - _state.accel_bias(axis_index) / _dt_ekf_avg; float airSpd = sqrtf((2.0f * fabsf(mea_acc)) / (BC_inv_x * rho)); // Estimate the derivative of specific force wrt airspeed along the X axis - // Limit lower value to prevent artithmetic exceptions + // Limit lower value to prevent arithmetic exceptions float Kacc = fmax(1e-1f,rho * BC_inv_x * airSpd); SH_ACC[0] = sq(q0) + sq(q1) - sq(q2) - sq(q3); @@ -165,11 +160,11 @@ void Ekf::fuseDrag() } else if (axis_index == 1) { // Estimate the airspeed from the measured drag force and ballistic coefficient - float mea_acc = (_imu_sample_delayed.delta_vel(axis_index) - _state.accel_bias(axis_index))/_imu_sample_delayed.delta_vel_dt; + float mea_acc = _drag_sample_delayed.accelXY(axis_index) - _state.accel_bias(axis_index) / _dt_ekf_avg; float airSpd = sqrtf((2.0f * fabsf(mea_acc)) / (BC_inv_y * rho)); // Estimate the derivative of specific force wrt airspeed along the X axis - // Limit lower value to prevent artithmetic exceptions + // Limit lower value to prevent arithmetic exceptions float Kacc = fmax(1e-1f,rho * BC_inv_y * airSpd); SH_ACC[0] = sq(q0) - sq(q1) + sq(q2) - sq(q3); diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index 1eb37e809f..3729b1ecab 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -59,6 +59,10 @@ EstimatorInterface::EstimatorInterface(): _airspeed_sample_delayed{}, _flow_sample_delayed{}, _ev_sample_delayed{}, + _drag_sample_delayed{}, + _drag_down_sampled{}, + _drag_sample_count(0), + _drag_sample_time_dt(0.0f), _imu_ticks(0), _imu_updated(false), _initialised(false), @@ -143,6 +147,42 @@ void EstimatorInterface::setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, u _imu_ticks = 0; _imu_updated = true; + // down-sample the drag specific force data by accumulating and calculating the mean when + // sufficient samples have been collected + if (_params.fusion_mode & MASK_USE_DRAG) { + _drag_sample_count ++; + // note acceleration is accumulated as a delta velocity + _drag_down_sampled.accelXY(0) += imu_sample_new.delta_vel(0); + _drag_down_sampled.accelXY(1) += imu_sample_new.delta_vel(1); + _drag_down_sampled.time_us += imu_sample_new.time_us; + _drag_sample_time_dt += imu_sample_new.delta_vel_dt; + + // calculate the downsample ratio for drag specific force data + uint8_t min_sample_ratio = (uint8_t) ceil((float)_imu_buffer_length / _obs_buffer_length); + if (min_sample_ratio < 5) { + min_sample_ratio = 5; + + } + + // calculate and store means from accumulated values + if (_drag_sample_count >= min_sample_ratio) { + // note conversion from accumulated delta velocity to acceleration + _drag_down_sampled.accelXY(0) /= _drag_sample_time_dt; + _drag_down_sampled.accelXY(1) /= _drag_sample_time_dt; + _drag_down_sampled.time_us /= _drag_sample_count; + + // write to buffer + _drag_buffer.push(_drag_down_sampled); + + // reset accumulators + _drag_sample_count = 0; + _drag_down_sampled.accelXY.zero(); + _drag_down_sampled.time_us = 0; + _drag_sample_time_dt = 0.0f; + + } + } + // get the oldest data from the buffer _imu_sample_delayed = _imu_buffer.get_oldest(); @@ -387,6 +427,7 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp) _airspeed_buffer.allocate(_obs_buffer_length) && _flow_buffer.allocate(_obs_buffer_length) && _ext_vision_buffer.allocate(_obs_buffer_length) && + _drag_buffer.allocate(_obs_buffer_length) && _output_buffer.allocate(_imu_buffer_length))) { ECL_ERR("EKF buffer allocation failed!"); unallocate_buffers(); @@ -409,6 +450,8 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp) _flow_buffer.push(flow_sample_init); extVisionSample ext_vision_sample_init = {}; _ext_vision_buffer.push(ext_vision_sample_init); + dragSample drag_sample_init = {}; + _drag_buffer.push(drag_sample_init); } // zero the data in the imu data and output observer state buffers diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index 9f373a07ab..b9d0af4640 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -319,6 +319,10 @@ protected: airspeedSample _airspeed_sample_delayed; flowSample _flow_sample_delayed; extVisionSample _ev_sample_delayed; + dragSample _drag_sample_delayed; + dragSample _drag_down_sampled; // down sampled drag specific force data (filter prediction rate -> observation rate) + 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) outputSample _output_sample_delayed; // filter output on the delayed time horizon outputSample _output_new; // filter output on the non-delayed time horizon @@ -368,6 +372,7 @@ protected: RingBuffer _flow_buffer; RingBuffer _ext_vision_buffer; RingBuffer _output_buffer; + RingBuffer _drag_buffer; uint64_t _time_last_imu; // timestamp of last imu sample in microseconds uint64_t _time_last_gps; // timestamp of last gps measurement in microseconds