ekf2: add kconfig option to enable/disable AUX velocity fusion

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

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()

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()

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};

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();
 

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);

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();

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
 {

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) {

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<flowSample> 	*_flow_buffer{nullptr};
 	RingBuffer<extVisionSample> *_ext_vision_buffer{nullptr};
+#if defined(CONFIG_EKF2_AUXVEL)
 	RingBuffer<auxVelSample> *_auxvel_buffer{nullptr};
+#endif // CONFIG_EKF2_AUXVEL
 	RingBuffer<systemFlagUpdate> *_system_flag_buffer{nullptr};
 
 	uint64_t _time_last_gps_buffer_push{0};

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 &timestamp)
 	// 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 &timestamp)
 	_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 &timestamp)
 	_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 &timestamp)
 	_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)
 {

src/modules/ekf2/EKF2.hpp

@@ -75,7 +75,6 @@
 #include <uORB/topics/estimator_states.h>
 #include <uORB/topics/estimator_status.h>
 #include <uORB/topics/estimator_status_flags.h>
-#include <uORB/topics/landing_target_pose.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/sensor_selection.h>
@@ -100,6 +99,10 @@
 # include <uORB/topics/airspeed_validated.h>
 #endif // CONFIG_EKF2_AIRSPEED
 
+#if defined(CONFIG_EKF2_AUXVEL)
+# include <uORB/topics/landing_target_pose.h>
+#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_source2d_s> _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_source3d_s> _estimator_aid_src_gravity_pub{ORB_ID(estimator_aid_src_gravity)};
 
-	uORB::PublicationMulti<estimator_aid_source2d_s> _estimator_aid_src_aux_vel_pub{ORB_ID(estimator_aid_src_aux_vel)};
-
 	uORB::PublicationMulti<estimator_aid_source2d_s> _estimator_aid_src_optical_flow_pub{ORB_ID(estimator_aid_src_optical_flow)};
 	uORB::PublicationMulti<estimator_aid_source2d_s> _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<px4::params::EKF2_EV_DELAY>)
 		_param_ekf2_ev_delay,	///< off-board vision measurement delay relative to the IMU (mSec)
+
+#if defined(CONFIG_EKF2_AUXVEL)
 		(ParamExtFloat<px4::params::EKF2_AVEL_DELAY>)
 		_param_ekf2_avel_delay,	///< auxiliary velocity measurement delay relative to the IMU (mSec)
+#endif // CONFIG_EKF2_AUXVEL
 
 		(ParamExtFloat<px4::params::EKF2_GYR_NOISE>)
 		_param_ekf2_gyr_noise,	///< IMU angular rate noise used for covariance prediction (rad/sec)

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"