From a8a67fbf8f5f105e08843b5c9b9932fbe006a618 Mon Sep 17 00:00:00 2001 From: bresch Date: Fri, 5 Apr 2024 13:49:03 +0200 Subject: [PATCH] ekf2: set horizon using specific parameter Some sensors can have their delay included in the timestamp. In this case, the buffer cannot be sized using the _DELAY parameter. --- src/modules/ekf2/EKF/common.h | 2 +- src/modules/ekf2/EKF/estimator_interface.cpp | 60 +--------- src/modules/ekf2/EKF2.cpp | 117 ++++++++++++------- src/modules/ekf2/EKF2.hpp | 1 + src/modules/ekf2/module.yaml | 12 ++ 5 files changed, 93 insertions(+), 99 deletions(-) diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index aaabe89eaa..e760bfbd9d 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -265,7 +265,7 @@ struct parameters { int32_t height_sensor_ref{static_cast(HeightSensor::BARO)}; int32_t position_sensor_ref{static_cast(PositionSensor::GNSS)}; - int32_t sensor_interval_max_ms{10}; ///< maximum time of arrival difference between non IMU sensor updates. Sets the size of the observation buffers. (mSec) + float delay_max_ms{110.f}; ///< maximum time delay of all the aiding sensors. Sets the size of the observation buffers. (mSec) // input noise float gyro_noise{1.5e-2f}; ///< IMU angular rate noise used for covariance prediction (rad/sec) diff --git a/src/modules/ekf2/EKF/estimator_interface.cpp b/src/modules/ekf2/EKF/estimator_interface.cpp index 0b384bfb56..980328f820 100644 --- a/src/modules/ekf2/EKF/estimator_interface.cpp +++ b/src/modules/ekf2/EKF/estimator_interface.cpp @@ -516,71 +516,15 @@ void EstimatorInterface::setDragData(const imuSample &imu) bool EstimatorInterface::initialise_interface(uint64_t timestamp) { - // find the maximum time delay the buffers are required to handle - - // it's reasonable to assume that aux velocity device has low delay. TODO: check the delay only if the aux device is used - float max_time_delay_ms = _params.sensor_interval_max_ms; - - // aux vel -#if defined(CONFIG_EKF2_AUXVEL) - max_time_delay_ms = math::max(_params.auxvel_delay_ms, max_time_delay_ms); -#endif // CONFIG_EKF2_AUXVEL - -#if defined(CONFIG_EKF2_BAROMETER) - // using baro - if (_params.baro_ctrl > 0) { - max_time_delay_ms = math::max(_params.baro_delay_ms, max_time_delay_ms); - } -#endif // CONFIG_EKF2_BAROMETER - -#if defined(CONFIG_EKF2_AIRSPEED) - // using airspeed - if (_params.arsp_thr > FLT_EPSILON) { - max_time_delay_ms = math::max(_params.airspeed_delay_ms, max_time_delay_ms); - } -#endif // CONFIG_EKF2_AIRSPEED - -#if defined(CONFIG_EKF2_MAGNETOMETER) - // mag mode - if (_params.mag_fusion_type != MagFuseType::NONE) { - max_time_delay_ms = math::max(_params.mag_delay_ms, max_time_delay_ms); - } -#endif // CONFIG_EKF2_MAGNETOMETER - -#if defined(CONFIG_EKF2_RANGE_FINDER) - // using range finder - if ((_params.rng_ctrl != static_cast(RngCtrl::DISABLED))) { - max_time_delay_ms = math::max(_params.range_delay_ms, max_time_delay_ms); - } -#endif // CONFIG_EKF2_RANGE_FINDER - -#if defined(CONFIG_EKF2_GNSS) - if (_params.gnss_ctrl > 0) { - max_time_delay_ms = math::max(_params.gps_delay_ms, max_time_delay_ms); - } -#endif // CONFIG_EKF2_GNSS - -#if defined(CONFIG_EKF2_OPTICAL_FLOW) - if (_params.flow_ctrl > 0) { - max_time_delay_ms = math::max(_params.flow_delay_ms, max_time_delay_ms); - } -#endif // CONFIG_EKF2_OPTICAL_FLOW - -#if defined(CONFIG_EKF2_EXTERNAL_VISION) - if (_params.ev_ctrl > 0) { - max_time_delay_ms = math::max(_params.ev_delay_ms, max_time_delay_ms); - } -#endif // CONFIG_EKF2_EXTERNAL_VISION - const float filter_update_period_ms = _params.filter_update_interval_us / 1000.f; // calculate the IMU buffer length required to accomodate the maximum delay with some allowance for jitter - _imu_buffer_length = math::max(2, (int)ceilf(max_time_delay_ms / filter_update_period_ms)); + _imu_buffer_length = math::max(2, (int)ceilf(_params.delay_max_ms / filter_update_period_ms)); // set the observation 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 - const float ekf_delay_ms = max_time_delay_ms * 1.5f; + const float ekf_delay_ms = _params.delay_max_ms * 1.5f; _obs_buffer_length = roundf(ekf_delay_ms / filter_update_period_ms); // limit to be no longer than the IMU buffer (we can't process data faster than the EKF prediction rate) diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index e98ddd9a0b..f531d99ce7 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -64,6 +64,7 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): #endif // CONFIG_EKF2_WIND _params(_ekf.getParamHandle()), _param_ekf2_predict_us(_params->filter_update_interval_us), + _param_ekf2_delay_max(_params->delay_max_ms), _param_ekf2_imu_ctrl(_params->imu_ctrl), #if defined(CONFIG_EKF2_AUXVEL) _param_ekf2_avel_delay(_params->auxvel_delay_ms), @@ -437,46 +438,6 @@ void EKF2::Run() #endif // CONFIG_EKF2_AIRSPEED -#if defined(CONFIG_EKF2_BAROMETER) - - // if using baro ensure sensor interval minimum is sufficient to accommodate system averaged baro output - if (_params->baro_ctrl == 1) { - float sens_baro_rate = 0.f; - - if (param_get(param_find("SENS_BARO_RATE"), &sens_baro_rate) == PX4_OK) { - if (sens_baro_rate > 0) { - float interval_ms = roundf(1000.f / sens_baro_rate); - - if (PX4_ISFINITE(interval_ms) && (interval_ms > _params->sensor_interval_max_ms)) { - PX4_DEBUG("updating sensor_interval_max_ms %.3f -> %.3f", (double)_params->sensor_interval_max_ms, (double)interval_ms); - _params->sensor_interval_max_ms = interval_ms; - } - } - } - } - -#endif // CONFIG_EKF2_BAROMETER - -#if defined(CONFIG_EKF2_MAGNETOMETER) - - // if using mag ensure sensor interval minimum is sufficient to accommodate system averaged mag output - if (_params->mag_fusion_type != MagFuseType::NONE) { - float sens_mag_rate = 0.f; - - if (param_get(param_find("SENS_MAG_RATE"), &sens_mag_rate) == PX4_OK) { - if (sens_mag_rate > 0) { - float interval_ms = roundf(1000.f / sens_mag_rate); - - if (PX4_ISFINITE(interval_ms) && (interval_ms > _params->sensor_interval_max_ms)) { - PX4_DEBUG("updating sensor_interval_max_ms %.3f -> %.3f", (double)_params->sensor_interval_max_ms, (double)interval_ms); - _params->sensor_interval_max_ms = interval_ms; - } - } - } - } - -#endif // CONFIG_EKF2_MAGNETOMETER - _ekf.updateParameters(); } @@ -822,6 +783,82 @@ void EKF2::VerifyParams() } #endif // CONFIG_EKF2_MAGNETOMETER + + float delay_max = _param_ekf2_delay_max.get(); + +#if defined(CONFIG_EKF2_AUXVEL) + + if (_param_ekf2_avel_delay.get() > delay_max) { + delay_max = _param_ekf2_avel_delay.get(); + } + +#endif // CONFIG_EKF2_AUXVEL + +#if defined(CONFIG_EKF2_BAROMETER) + + if (_param_ekf2_baro_delay.get() > delay_max) { + delay_max = _param_ekf2_baro_delay.get(); + } + +#endif // CONFIG_EKF2_BAROMETER + +#if defined(CONFIG_EKF2_AIRSPEED) + + if (_param_ekf2_asp_delay.get() > delay_max) { + delay_max = _param_ekf2_asp_delay.get(); + } + +#endif // CONFIG_EKF2_AIRSPEED + +#if defined(CONFIG_EKF2_MAGNETOMETER) + + if (_param_ekf2_mag_delay.get() > delay_max) { + delay_max = _param_ekf2_mag_delay.get(); + } + +#endif // CONFIG_EKF2_MAGNETOMETER + +#if defined(CONFIG_EKF2_RANGE_FINDER) + + if (_param_ekf2_rng_delay.get() > delay_max) { + delay_max = _param_ekf2_rng_delay.get(); + } + +#endif // CONFIG_EKF2_RANGE_FINDER + +#if defined(CONFIG_EKF2_GNSS) + + if (_param_ekf2_gps_delay.get() > delay_max) { + delay_max = _param_ekf2_gps_delay.get(); + } + +#endif // CONFIG_EKF2_GNSS + +#if defined(CONFIG_EKF2_OPTICAL_FLOW) + + if (_param_ekf2_of_delay.get() > delay_max) { + delay_max = _param_ekf2_of_delay.get(); + } + +#endif // CONFIG_EKF2_OPTICAL_FLOW + +#if defined(CONFIG_EKF2_EXTERNAL_VISION) + + if (_param_ekf2_ev_delay.get() > delay_max) { + delay_max = _param_ekf2_ev_delay.get(); + } + +#endif // CONFIG_EKF2_EXTERNAL_VISION + + if (delay_max > _param_ekf2_delay_max.get()) { + /* EVENT + * @description EKF2_DELAY_MAX({1}ms) is too small compared to the maximum sensor delay ({2}) + */ + events::send(events::ID("nf_delay_max_too_small"), events::Log::Warning, + "EKF2_DELAY_MAX increased to {2}ms, please reboot", _param_ekf2_delay_max.get(), + delay_max); + _param_ekf2_delay_max.commit_no_notification(delay_max); + } } void EKF2::PublishAidSourceStatus(const hrt_abstime ×tamp) diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index ff425e891d..28c76dd130 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -498,6 +498,7 @@ private: DEFINE_PARAMETERS( (ParamExtInt) _param_ekf2_predict_us, + (ParamExtFloat) _param_ekf2_delay_max, (ParamExtInt) _param_ekf2_imu_ctrl, #if defined(CONFIG_EKF2_AUXVEL) diff --git a/src/modules/ekf2/module.yaml b/src/modules/ekf2/module.yaml index 030e26779f..81c067a36d 100644 --- a/src/modules/ekf2/module.yaml +++ b/src/modules/ekf2/module.yaml @@ -29,6 +29,18 @@ parameters: default: 7 min: 0 max: 7 + EKF2_DELAY_MAX: + description: + short: Maximum delay of all the aiding sensors + long: Defines the delay between the current time and the delayed-time horizon. + This value should be at least as large as the largest EKF2_XXX_DELAY parameter. + type: float + default: 200 + min: 0 + max: 1000 + unit: ms + reboot_required: true + decimal: 1 EKF2_MAG_DELAY: description: short: Magnetometer measurement delay relative to IMU measurements