diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index a81b729092..9d12acb9e0 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -259,10 +259,10 @@ bool Ekf::initialiseFilter(void) // increment the sample count and apply a LPF to the measurement _mag_counter ++; // don't start using data until we can be certain all bad initial data has been flushed - if (_mag_counter == (uint8_t)(OBS_BUFFER_LENGTH + 1)) { + if (_mag_counter == (uint8_t)(_obs_buffer_length + 1)) { // initialise filter states _mag_filt_state = _mag_sample_delayed.mag; - } else if (_mag_counter > (uint8_t)(OBS_BUFFER_LENGTH + 1)) { + } else if (_mag_counter > (uint8_t)(_obs_buffer_length + 1)) { // noise filter the data _mag_filt_state = _mag_filt_state * 0.9f + _mag_sample_delayed.mag * 0.1f; } @@ -306,10 +306,10 @@ bool Ekf::initialiseFilter(void) // increment the sample count and apply a LPF to the measurement _hgt_counter ++; // don't start using data until we can be certain all bad initial data has been flushed - if (_hgt_counter == (uint8_t)(OBS_BUFFER_LENGTH + 1)) { + if (_hgt_counter == (uint8_t)(_obs_buffer_length + 1)) { // initialise filter states _rng_filt_state = _range_sample_delayed.rng; - } else if (_hgt_counter > (uint8_t)(OBS_BUFFER_LENGTH + 1)) { + } else if (_hgt_counter > (uint8_t)(_obs_buffer_length + 1)) { // noise filter the data _rng_filt_state = 0.9f * _rng_filt_state + 0.1f * _range_sample_delayed.rng; } @@ -330,10 +330,10 @@ bool Ekf::initialiseFilter(void) // increment the sample count and apply a LPF to the measurement _hgt_counter ++; // don't start using data until we can be certain all bad initial data has been flushed - if (_hgt_counter == (uint8_t)(OBS_BUFFER_LENGTH + 1)) { + if (_hgt_counter == (uint8_t)(_obs_buffer_length + 1)) { // initialise filter states _baro_hgt_offset = _baro_sample_delayed.hgt; - } else if (_hgt_counter > (uint8_t)(OBS_BUFFER_LENGTH + 1)) { + } else if (_hgt_counter > (uint8_t)(_obs_buffer_length + 1)) { // noise filter the data _baro_hgt_offset = 0.9f * _baro_hgt_offset + 0.1f * _baro_sample_delayed.hgt; } @@ -347,9 +347,9 @@ bool Ekf::initialiseFilter(void) } // check to see if we have enough measurements and return false if not - bool hgt_count_fail = _hgt_counter <= 2*OBS_BUFFER_LENGTH; - bool mag_count_fail = _mag_counter <= 2*OBS_BUFFER_LENGTH; - bool ev_count_fail = ((_params.fusion_mode & MASK_USE_EVPOS) || (_params.fusion_mode & MASK_USE_EVYAW)) && (_ev_counter <= 2*OBS_BUFFER_LENGTH); + bool hgt_count_fail = _hgt_counter <= 2*_obs_buffer_length; + bool mag_count_fail = _mag_counter <= 2*_obs_buffer_length; + bool ev_count_fail = ((_params.fusion_mode & MASK_USE_EVPOS) || (_params.fusion_mode & MASK_USE_EVYAW)) && (_ev_counter <= 2*_obs_buffer_length); if (hgt_count_fail || mag_count_fail || ev_count_fail) { return false; diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index c2c6c8ac11..d22b33ee76 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -48,8 +48,8 @@ EstimatorInterface::EstimatorInterface(): - OBS_BUFFER_LENGTH(10), - IMU_BUFFER_LENGTH(30), + _obs_buffer_length(10), + _imu_buffer_length(30), _min_obs_interval_us(0), _dt_imu_avg(0.0f), _imu_ticks(0), @@ -140,7 +140,7 @@ void EstimatorInterface::setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, u // calculate the minimum interval between observations required to guarantee no loss of data // this will occur if data is overwritten before its time stamp falls behind the fusion time horizon - _min_obs_interval_us = (_imu_sample_new.time_us - _imu_sample_delayed.time_us)/(OBS_BUFFER_LENGTH - 1); + _min_obs_interval_us = (_imu_sample_new.time_us - _imu_sample_delayed.time_us)/(_obs_buffer_length - 1); } else { _imu_updated = false; @@ -351,36 +351,36 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp) math::max(_params.airspeed_delay_ms, _params.baro_delay_ms)))))); // calculate the IMU buffer length required to accomodate the maximum delay with some allowance for jitter - IMU_BUFFER_LENGTH = (max_time_delay_ms / FILTER_UPDATE_PERIOD_MS) + 1; + _imu_buffer_length = (max_time_delay_ms / FILTER_UPDATE_PERIOD_MS) + 1; // set the observaton buffer length to handle the minimum time of arrival between observations in combination // with the worst case delay from current time to ekf fusion time // allow for worst case 50% extension of the ekf fusion time horizon delay due to timing jitter uint16_t ekf_delay_ms = max_time_delay_ms + (int)(ceil((float)max_time_delay_ms * 0.5f)); - OBS_BUFFER_LENGTH = (ekf_delay_ms / _params.sensor_interval_min_ms) + 1; + _obs_buffer_length = (ekf_delay_ms / _params.sensor_interval_min_ms) + 1; // limit to be no longer than the IMU buffer (we can't process data faster than the EKF prediction rate) - OBS_BUFFER_LENGTH = math::min(OBS_BUFFER_LENGTH,IMU_BUFFER_LENGTH); + _obs_buffer_length = math::min(_obs_buffer_length,_imu_buffer_length); - ECL_INFO("EKF IMU buffer length = %i",(int)IMU_BUFFER_LENGTH); - ECL_INFO("EKF observation buffer length = %i",(int)OBS_BUFFER_LENGTH); + ECL_INFO("EKF IMU buffer length = %i",(int)_imu_buffer_length); + ECL_INFO("EKF observation buffer length = %i",(int)_obs_buffer_length); - if (!(_imu_buffer.allocate(IMU_BUFFER_LENGTH) && - _gps_buffer.allocate(OBS_BUFFER_LENGTH) && - _mag_buffer.allocate(OBS_BUFFER_LENGTH) && - _baro_buffer.allocate(OBS_BUFFER_LENGTH) && - _range_buffer.allocate(OBS_BUFFER_LENGTH) && - _airspeed_buffer.allocate(OBS_BUFFER_LENGTH) && - _flow_buffer.allocate(OBS_BUFFER_LENGTH) && - _ext_vision_buffer.allocate(OBS_BUFFER_LENGTH) && - _output_buffer.allocate(IMU_BUFFER_LENGTH))) { + if (!(_imu_buffer.allocate(_imu_buffer_length) && + _gps_buffer.allocate(_obs_buffer_length) && + _mag_buffer.allocate(_obs_buffer_length) && + _baro_buffer.allocate(_obs_buffer_length) && + _range_buffer.allocate(_obs_buffer_length) && + _airspeed_buffer.allocate(_obs_buffer_length) && + _flow_buffer.allocate(_obs_buffer_length) && + _ext_vision_buffer.allocate(_obs_buffer_length) && + _output_buffer.allocate(_imu_buffer_length))) { ECL_ERR("EKF buffer allocation failed!"); unallocate_buffers(); return false; } // zero the data in the observation buffers - for (int index=0; index < OBS_BUFFER_LENGTH; index++) { + for (int index=0; index < _obs_buffer_length; index++) { gpsSample gps_sample_init = {}; _gps_buffer.push(gps_sample_init); magSample mag_sample_init = {}; @@ -398,7 +398,7 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp) } // zero the data in the imu data and output observer state buffers - for (int index=0; index < IMU_BUFFER_LENGTH; index++) { + for (int index=0; index < _imu_buffer_length; index++) { imuSample imu_sample_init = {}; _imu_buffer.push(imu_sample_init); outputSample output_sample_init = {}; diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index 71764332a4..42e5308de5 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -280,14 +280,14 @@ protected: max freq (Hz) = (OBS_BUFFER_LENGTH - 1) / (IMU_BUFFER_LENGTH * FILTER_UPDATE_PERIOD_MS * 0.001) This can be adjusted to match the max sensor data rate plus some margin for jitter. */ - uint8_t OBS_BUFFER_LENGTH; + uint8_t _obs_buffer_length; /* IMU_BUFFER_LENGTH defines how many IMU samples we buffer which sets the time delay from current time to the EKF fusion time horizon and therefore the maximum sensor time offset relative to the IMU that we can compensate for. max sensor time offet (msec) = IMU_BUFFER_LENGTH * FILTER_UPDATE_PERIOD_MS This can be adjusted to a value that is FILTER_UPDATE_PERIOD_MS longer than the maximum observation time delay. */ - uint8_t IMU_BUFFER_LENGTH; + uint8_t _imu_buffer_length; static const unsigned FILTER_UPDATE_PERIOD_MS = 10; // ekf prediction period in milliseconds unsigned _min_obs_interval_us; // minimum time interval between observations that will guarantee data is not lost (usec)