fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-28 08:00:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: set horizon using specific parameter

Browse Source 
Some sensors can have their delay included in the timestamp. In this
case, the buffer cannot be sized using the _DELAY parameter.
This commit is contained in:
bresch
2024-04-05 13:49:03 +02:00
committed by Daniel Agar
parent 6796945d0b
commit a8a67fbf8f
5 changed files with 93 additions and 99 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF2.cpp
+77 -40
View File
@@ -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<float, float>(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 &timestamp)