From d47f96f1a5923efc7bd63824b155c1bc48a24121 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 14 Mar 2023 14:57:53 -0400 Subject: [PATCH] ekf2: add kconfig option to enable/disable AUX velocity fusion --- boards/px4/fmu-v2/default.px4board | 1 + src/modules/ekf2/CMakeLists.txt | 5 ++++- src/modules/ekf2/EKF/CMakeLists.txt | 5 ++++- src/modules/ekf2/EKF/common.h | 6 +++++- src/modules/ekf2/EKF/control.cpp | 2 ++ src/modules/ekf2/EKF/ekf.cpp | 2 ++ src/modules/ekf2/EKF/ekf.h | 11 ++++++++-- src/modules/ekf2/EKF/ekf_helper.cpp | 2 ++ src/modules/ekf2/EKF/estimator_interface.cpp | 11 +++++++++- src/modules/ekf2/EKF/estimator_interface.h | 4 ++++ src/modules/ekf2/EKF2.cpp | 18 +++++++++++++++++ src/modules/ekf2/EKF2.hpp | 21 +++++++++++++++----- src/modules/ekf2/Kconfig | 7 +++++++ 13 files changed, 84 insertions(+), 11 deletions(-) diff --git a/boards/px4/fmu-v2/default.px4board b/boards/px4/fmu-v2/default.px4board index 22f72e357d..fefb2c73bb 100644 --- a/boards/px4/fmu-v2/default.px4board +++ b/boards/px4/fmu-v2/default.px4board @@ -23,6 +23,7 @@ CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y CONFIG_MODULES_EKF2=y +# CONFIG_EKF2_AUXVEL is not set # CONFIG_EKF2_BARO_COMPENSATION is not set # CONFIG_EKF2_DRAG_FUSION is not set # CONFIG_EKF2_GNSS_YAW is not set diff --git a/src/modules/ekf2/CMakeLists.txt b/src/modules/ekf2/CMakeLists.txt index 8e9a394aab..9fca44475b 100644 --- a/src/modules/ekf2/CMakeLists.txt +++ b/src/modules/ekf2/CMakeLists.txt @@ -68,7 +68,6 @@ add_subdirectory(Utility) set(EKF_SRCS) list(APPEND EKF_SRCS - EKF/auxvel_fusion.cpp EKF/baro_height_control.cpp EKF/bias_estimator.cpp EKF/control.cpp @@ -108,6 +107,10 @@ if(CONFIG_EKF2_AIRSPEED) list(APPEND EKF_SRCS EKF/airspeed_fusion.cpp) endif() +if(CONFIG_EKF2_AUXVEL) + list(APPEND EKF_SRCS EKF/auxvel_fusion.cpp) +endif() + if(CONFIG_EKF2_DRAG_FUSION) list(APPEND EKF_SRCS EKF/drag_fusion.cpp) endif() diff --git a/src/modules/ekf2/EKF/CMakeLists.txt b/src/modules/ekf2/EKF/CMakeLists.txt index d143baf090..2ba91fdb39 100644 --- a/src/modules/ekf2/EKF/CMakeLists.txt +++ b/src/modules/ekf2/EKF/CMakeLists.txt @@ -33,7 +33,6 @@ set(EKF_SRCS) list(APPEND EKF_SRCS - auxvel_fusion.cpp baro_height_control.cpp bias_estimator.cpp control.cpp @@ -73,6 +72,10 @@ if(CONFIG_EKF2_AIRSPEED) list(APPEND EKF_SRCS airspeed_fusion.cpp) endif() +if(CONFIG_EKF2_AUXVEL) + list(APPEND EKF_SRCS auxvel_fusion.cpp) +endif() + if(CONFIG_EKF2_DRAG_FUSION) list(APPEND EKF_SRCS drag_fusion.cpp) endif() diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index 3b3baed475..db11054ba7 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -259,11 +259,13 @@ struct dragSample { }; #endif // CONFIG_EKF2_DRAG_FUSION +#if defined(CONFIG_EKF2_AUXVEL) struct auxVelSample { uint64_t time_us{}; ///< timestamp of the measurement (uSec) Vector2f vel{}; ///< measured NE velocity relative to the local origin (m/sec) Vector2f velVar{}; ///< estimated error variance of the NE velocity (m/sec)**2 }; +#endif // CONFIG_EKF2_AUXVEL struct systemFlagUpdate { uint64_t time_us{}; @@ -310,7 +312,6 @@ struct parameters { float flow_delay_ms{5.0f}; ///< optical flow measurement delay relative to the IMU (mSec) - this is to the middle of the optical flow integration interval float range_delay_ms{5.0f}; ///< range finder measurement delay relative to the IMU (mSec) float ev_delay_ms{175.0f}; ///< off-board vision measurement delay relative to the IMU (mSec) - float auxvel_delay_ms{5.0f}; ///< auxiliary velocity measurement delay relative to the IMU (mSec) // input noise float gyro_noise{1.5e-2f}; ///< IMU angular rate noise used for covariance prediction (rad/sec) @@ -469,9 +470,12 @@ struct parameters { const float vert_innov_test_min{1.0f}; ///< Minimum number of standard deviations of vertical vel/pos innovations required to trigger a vertical acceleration failure const int bad_acc_reset_delay_us{500000}; ///< Continuous time that the vertical position and velocity innovation test must fail before the states are reset (uSec) +#if defined(CONFIG_EKF2_AUXVEL) // auxiliary velocity fusion + float auxvel_delay_ms{5.0f}; ///< auxiliary velocity measurement delay relative to the IMU (mSec) const float auxvel_noise{0.5f}; ///< minimum observation noise, uses reported noise if greater (m/s) const float auxvel_gate{5.0f}; ///< velocity fusion innovation consistency gate size (STD) +#endif // CONFIG_EKF2_AUXVEL // compute synthetic magnetomter Z value if possible int32_t synthesize_mag_z{0}; diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index cb48c0ada0..8142ab9f21 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -125,8 +125,10 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed) // Additional data odometry data from an external estimator can be fused. controlExternalVisionFusion(); +#if defined(CONFIG_EKF2_AUXVEL) // Additional horizontal velocity data from an auxiliary sensor can be fused controlAuxVelFusion(); +#endif // CONFIG_EKF2_AUXVEL controlZeroInnovationHeadingUpdate(); diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index d5e16b3dd7..c0d9987a7c 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/src/modules/ekf2/EKF/ekf.cpp @@ -142,7 +142,9 @@ void Ekf::reset() resetEstimatorAidStatus(_aid_src_mag_heading); resetEstimatorAidStatus(_aid_src_mag); +#if defined(CONFIG_EKF2_AUXVEL) resetEstimatorAidStatus(_aid_src_aux_vel); +#endif // CONFIG_EKF2_AUXVEL resetEstimatorAidStatus(_aid_src_optical_flow); resetEstimatorAidStatus(_aid_src_terrain_optical_flow); diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index b715e7189a..127fc1b73a 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -98,9 +98,11 @@ public: void getRngHgtInnovVar(float &rng_hgt_innov_var) const { rng_hgt_innov_var = _aid_src_rng_hgt.innovation_variance; } void getRngHgtInnovRatio(float &rng_hgt_innov_ratio) const { rng_hgt_innov_ratio = _aid_src_rng_hgt.test_ratio; } +#if defined(CONFIG_EKF2_AUXVEL) void getAuxVelInnov(float aux_vel_innov[2]) const; void getAuxVelInnovVar(float aux_vel_innov[2]) const; void getAuxVelInnovRatio(float &aux_vel_innov_ratio) const { aux_vel_innov_ratio = math::max(_aid_src_aux_vel.test_ratio[0], _aid_src_aux_vel.test_ratio[1]); } +#endif // CONFIG_EKF2_AUXVEL void getFlowInnov(float flow_innov[2]) const; void getFlowInnovVar(float flow_innov_var[2]) const; @@ -477,7 +479,9 @@ public: const auto &aid_src_gravity() const { return _aid_src_gravity; } +#if defined(CONFIG_EKF2_AUXVEL) const auto &aid_src_aux_vel() const { return _aid_src_aux_vel; } +#endif // CONFIG_EKF2_AUXVEL const auto &aid_src_optical_flow() const { return _aid_src_optical_flow; } const auto &aid_src_terrain_optical_flow() const { return _aid_src_terrain_optical_flow; } @@ -624,7 +628,9 @@ private: estimator_aid_source3d_s _aid_src_gravity{}; +#if defined(CONFIG_EKF2_AUXVEL) estimator_aid_source2d_s _aid_src_aux_vel{}; +#endif // CONFIG_EKF2_AUXVEL estimator_aid_source2d_s _aid_src_optical_flow{}; estimator_aid_source2d_s _aid_src_terrain_optical_flow{}; @@ -996,8 +1002,11 @@ private: void controlZeroInnovationHeadingUpdate(); +#if defined(CONFIG_EKF2_AUXVEL) // control fusion of auxiliary velocity observations void controlAuxVelFusion(); + void stopAuxVelFusion(); +#endif // CONFIG_EKF2_AUXVEL void checkVerticalAccelerationHealth(const imuSample &imu_delayed); Likelihood estimateInertialNavFallingLikelihood() const; @@ -1089,8 +1098,6 @@ private: void stopEvVelFusion(); void stopEvYawFusion(); - void stopAuxVelFusion(); - void stopFlowFusion(); void resetFakePosFusion(); diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index abf695fd8f..59910b06a7 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -335,6 +335,7 @@ void Ekf::getEvVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vp vpos = _aid_src_ev_hgt.test_ratio; } +#if defined(CONFIG_EKF2_AUXVEL) void Ekf::getAuxVelInnov(float aux_vel_innov[2]) const { aux_vel_innov[0] = _aid_src_aux_vel.innovation[0]; @@ -346,6 +347,7 @@ void Ekf::getAuxVelInnovVar(float aux_vel_innov_var[2]) const aux_vel_innov_var[0] = _aid_src_aux_vel.innovation_variance[0]; aux_vel_innov_var[1] = _aid_src_aux_vel.innovation_variance[1]; } +#endif // CONFIG_EKF2_AUXVEL void Ekf::getFlowInnov(float flow_innov[2]) const { diff --git a/src/modules/ekf2/EKF/estimator_interface.cpp b/src/modules/ekf2/EKF/estimator_interface.cpp index e6b25b7d7b..ea56b2ef13 100644 --- a/src/modules/ekf2/EKF/estimator_interface.cpp +++ b/src/modules/ekf2/EKF/estimator_interface.cpp @@ -58,7 +58,9 @@ EstimatorInterface::~EstimatorInterface() #if defined(CONFIG_EKF2_DRAG_FUSION) delete _drag_buffer; #endif // CONFIG_EKF2_DRAG_FUSION +#if defined(CONFIG_EKF2_AUXVEL) delete _auxvel_buffer; +#endif // CONFIG_EKF2_AUXVEL } // Accumulate imu data and store to buffer at desired rate @@ -391,6 +393,7 @@ void EstimatorInterface::setExtVisionData(const extVisionSample &evdata) } } +#if defined(CONFIG_EKF2_AUXVEL) void EstimatorInterface::setAuxVelData(const auxVelSample &auxvel_sample) { if (!_initialised) { @@ -425,6 +428,7 @@ void EstimatorInterface::setAuxVelData(const auxVelSample &auxvel_sample) ECL_WARN("aux velocity data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _auxvel_buffer->get_newest().time_us, _min_obs_interval_us); } } +#endif // CONFIG_EKF2_AUXVEL void EstimatorInterface::setSystemFlagData(const systemFlagUpdate &system_flags) { @@ -520,7 +524,12 @@ 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 = math::max((float)_params.sensor_interval_max_ms, _params.auxvel_delay_ms); + 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 // using baro if (_params.baro_ctrl > 0) { diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index da58f3a224..1b4f73de27 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -102,7 +102,9 @@ public: // set external vision position and attitude data void setExtVisionData(const extVisionSample &evdata); +#if defined(CONFIG_EKF2_AUXVEL) void setAuxVelData(const auxVelSample &auxvel_sample); +#endif // CONFIG_EKF2_AUXVEL void setSystemFlagData(const systemFlagUpdate &system_flags); @@ -357,7 +359,9 @@ protected: #endif // CONFIG_EKF2_AIRSPEED RingBuffer *_flow_buffer{nullptr}; RingBuffer *_ext_vision_buffer{nullptr}; +#if defined(CONFIG_EKF2_AUXVEL) RingBuffer *_auxvel_buffer{nullptr}; +#endif // CONFIG_EKF2_AUXVEL RingBuffer *_system_flag_buffer{nullptr}; uint64_t _time_last_gps_buffer_push{0}; diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 1df244e3ea..c4751154e5 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -66,7 +66,9 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _param_ekf2_of_delay(_params->flow_delay_ms), _param_ekf2_rng_delay(_params->range_delay_ms), _param_ekf2_ev_delay(_params->ev_delay_ms), +#if defined(CONFIG_EKF2_AUXVEL) _param_ekf2_avel_delay(_params->auxvel_delay_ms), +#endif // CONFIG_EKF2_AUXVEL _param_ekf2_gyr_noise(_params->gyro_noise), _param_ekf2_acc_noise(_params->accel_noise), _param_ekf2_gyr_b_noise(_params->gyro_bias_p_noise), @@ -208,7 +210,9 @@ EKF2::~EKF2() #endif // CONFIG_EKF2_AIRSPEED perf_free(_msg_missed_distance_sensor_perf); perf_free(_msg_missed_gps_perf); +#if defined(CONFIG_EKF2_AUXVEL) perf_free(_msg_missed_landing_target_pose_perf); +#endif // CONFIG_EKF2_AUXVEL perf_free(_msg_missed_magnetometer_perf); perf_free(_msg_missed_odometry_perf); perf_free(_msg_missed_optical_flow_perf); @@ -327,7 +331,9 @@ int EKF2::print_status() #endif // CONFIG_EKF2_AIRSPEED perf_print_counter(_msg_missed_distance_sensor_perf); perf_print_counter(_msg_missed_gps_perf); +#if defined(CONFIG_EKF2_AUXVEL) perf_print_counter(_msg_missed_landing_target_pose_perf); +#endif // CONFIG_EKF2_AUXVEL perf_print_counter(_msg_missed_magnetometer_perf); perf_print_counter(_msg_missed_odometry_perf); perf_print_counter(_msg_missed_optical_flow_perf); @@ -626,7 +632,9 @@ void EKF2::Run() #if defined(CONFIG_EKF2_AIRSPEED) UpdateAirspeedSample(ekf2_timestamps); #endif // CONFIG_EKF2_AIRSPEED +#if defined(CONFIG_EKF2_AUXVEL) UpdateAuxVelSample(ekf2_timestamps); +#endif // CONFIG_EKF2_AUXVEL UpdateBaroSample(ekf2_timestamps); UpdateExtVisionSample(ekf2_timestamps); UpdateFlowSample(ekf2_timestamps); @@ -855,8 +863,10 @@ void EKF2::PublishAidSourceStatus(const hrt_abstime ×tamp) // gravity PublishAidSourceStatus(_ekf.aid_src_gravity(), _status_gravity_pub_last, _estimator_aid_src_gravity_pub); +#if defined(CONFIG_EKF2_AUXVEL) // aux velocity PublishAidSourceStatus(_ekf.aid_src_aux_vel(), _status_aux_vel_pub_last, _estimator_aid_src_aux_vel_pub); +#endif // CONFIG_EKF2_AUXVEL // optical flow PublishAidSourceStatus(_ekf.aid_src_optical_flow(), _status_optical_flow_pub_last, _estimator_aid_src_optical_flow_pub); @@ -1139,7 +1149,9 @@ void EKF2::PublishInnovations(const hrt_abstime ×tamp) _ekf.getEvVelPosInnov(innovations.ev_hvel, innovations.ev_vvel, innovations.ev_hpos, innovations.ev_vpos); _ekf.getBaroHgtInnov(innovations.baro_vpos); _ekf.getRngHgtInnov(innovations.rng_vpos); +#if defined(CONFIG_EKF2_AUXVEL) _ekf.getAuxVelInnov(innovations.aux_hvel); +#endif // CONFIG_EKF2_AUXVEL _ekf.getFlowInnov(innovations.flow); _ekf.getHeadingInnov(innovations.heading); _ekf.getMagInnov(innovations.mag_field); @@ -1196,7 +1208,9 @@ void EKF2::PublishInnovationTestRatios(const hrt_abstime ×tamp) _ekf.getEvVelPosInnovRatio(test_ratios.ev_hvel[0], test_ratios.ev_vvel, test_ratios.ev_hpos[0], test_ratios.ev_vpos); _ekf.getBaroHgtInnovRatio(test_ratios.baro_vpos); _ekf.getRngHgtInnovRatio(test_ratios.rng_vpos); +#if defined(CONFIG_EKF2_AUXVEL) _ekf.getAuxVelInnovRatio(test_ratios.aux_hvel[0]); +#endif // CONFIG_EKF2_AUXVEL _ekf.getFlowInnovRatio(test_ratios.flow[0]); _ekf.getHeadingInnovRatio(test_ratios.heading); _ekf.getMagInnovRatio(test_ratios.mag_field[0]); @@ -1229,7 +1243,9 @@ void EKF2::PublishInnovationVariances(const hrt_abstime ×tamp) _ekf.getEvVelPosInnovVar(variances.ev_hvel, variances.ev_vvel, variances.ev_hpos, variances.ev_vpos); _ekf.getBaroHgtInnovVar(variances.baro_vpos); _ekf.getRngHgtInnovVar(variances.rng_vpos); +#if defined(CONFIG_EKF2_AUXVEL) _ekf.getAuxVelInnovVar(variances.aux_hvel); +#endif // CONFIG_EKF2_AUXVEL _ekf.getFlowInnovVar(variances.flow); _ekf.getHeadingInnovVar(variances.heading); _ekf.getMagInnovVar(variances.mag_field); @@ -1783,6 +1799,7 @@ void EKF2::UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps) } #endif // CONFIG_EKF2_AIRSPEED +#if defined(CONFIG_EKF2_AUXVEL) void EKF2::UpdateAuxVelSample(ekf2_timestamps_s &ekf2_timestamps) { // EKF auxiliary velocity sample @@ -1810,6 +1827,7 @@ void EKF2::UpdateAuxVelSample(ekf2_timestamps_s &ekf2_timestamps) } } } +#endif // CONFIG_EKF2_AUXVEL void EKF2::UpdateBaroSample(ekf2_timestamps_s &ekf2_timestamps) { diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 0750ab08fc..15019bb3ea 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -75,7 +75,6 @@ #include #include #include -#include #include #include #include @@ -100,6 +99,10 @@ # include #endif // CONFIG_EKF2_AIRSPEED +#if defined(CONFIG_EKF2_AUXVEL) +# include +#endif // CONFIG_EKF2_AUXVEL + extern pthread_mutex_t ekf2_module_mutex; class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem @@ -171,7 +174,9 @@ private: #if defined(CONFIG_EKF2_AIRSPEED) void UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps); #endif // CONFIG_EKF2_AIRSPEED +#if defined(CONFIG_EKF2_AUXVEL) void UpdateAuxVelSample(ekf2_timestamps_s &ekf2_timestamps); +#endif // CONFIG_EKF2_AUXVEL void UpdateBaroSample(ekf2_timestamps_s &ekf2_timestamps); bool UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps); bool UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps); @@ -234,7 +239,6 @@ private: perf_counter_t _msg_missed_air_data_perf{nullptr}; perf_counter_t _msg_missed_distance_sensor_perf{nullptr}; perf_counter_t _msg_missed_gps_perf{nullptr}; - perf_counter_t _msg_missed_landing_target_pose_perf{nullptr}; perf_counter_t _msg_missed_magnetometer_perf{nullptr}; perf_counter_t _msg_missed_odometry_perf{nullptr}; perf_counter_t _msg_missed_optical_flow_perf{nullptr}; @@ -291,8 +295,15 @@ private: hrt_abstime _status_gravity_pub_last{0}; +#if defined(CONFIG_EKF2_AUXVEL) + uORB::Subscription _landing_target_pose_sub {ORB_ID(landing_target_pose)}; + + uORB::PublicationMulti _estimator_aid_src_aux_vel_pub{ORB_ID(estimator_aid_src_aux_vel)}; hrt_abstime _status_aux_vel_pub_last{0}; + perf_counter_t _msg_missed_landing_target_pose_perf{nullptr}; +#endif // CONFIG_EKF2_AUXVEL + hrt_abstime _status_optical_flow_pub_last{0}; hrt_abstime _status_terrain_optical_flow_pub_last{0}; @@ -326,7 +337,6 @@ private: uORB::Subscription _airdata_sub{ORB_ID(vehicle_air_data)}; uORB::Subscription _ev_odom_sub{ORB_ID(vehicle_visual_odometry)}; - uORB::Subscription _landing_target_pose_sub{ORB_ID(landing_target_pose)}; uORB::Subscription _magnetometer_sub{ORB_ID(vehicle_magnetometer)}; uORB::Subscription _sensor_selection_sub{ORB_ID(sensor_selection)}; uORB::Subscription _status_sub{ORB_ID(vehicle_status)}; @@ -398,8 +408,6 @@ private: uORB::PublicationMulti _estimator_aid_src_gravity_pub{ORB_ID(estimator_aid_src_gravity)}; - uORB::PublicationMulti _estimator_aid_src_aux_vel_pub{ORB_ID(estimator_aid_src_aux_vel)}; - uORB::PublicationMulti _estimator_aid_src_optical_flow_pub{ORB_ID(estimator_aid_src_optical_flow)}; uORB::PublicationMulti _estimator_aid_src_terrain_optical_flow_pub{ORB_ID(estimator_aid_src_terrain_optical_flow)}; @@ -433,8 +441,11 @@ private: _param_ekf2_rng_delay, ///< range finder measurement delay relative to the IMU (mSec) (ParamExtFloat) _param_ekf2_ev_delay, ///< off-board vision measurement delay relative to the IMU (mSec) + +#if defined(CONFIG_EKF2_AUXVEL) (ParamExtFloat) _param_ekf2_avel_delay, ///< auxiliary velocity measurement delay relative to the IMU (mSec) +#endif // CONFIG_EKF2_AUXVEL (ParamExtFloat) _param_ekf2_gyr_noise, ///< IMU angular rate noise used for covariance prediction (rad/sec) diff --git a/src/modules/ekf2/Kconfig b/src/modules/ekf2/Kconfig index c764d2b83d..3f3a94c3cb 100644 --- a/src/modules/ekf2/Kconfig +++ b/src/modules/ekf2/Kconfig @@ -21,6 +21,13 @@ depends on MODULES_EKF2 ---help--- EKF2 airspeed fusion support. +menuconfig EKF2_AUXVEL +depends on MODULES_EKF2 + bool "aux velocity fusion support" + default y + ---help--- + EKF2 auxiliary velocity fusion support. + menuconfig EKF2_BARO_COMPENSATION depends on MODULES_EKF2 bool "barometer compensation support"