Merge remote-tracking branch 'px4/main' into pr-ekf2_gnss_yaw_estimat…

…or_disable

src/modules/ekf2/CMakeLists.txt

@@ -124,7 +124,6 @@ list(APPEND EKF_SRCS
 	EKF/estimator_interface.cpp
 	EKF/fake_height_control.cpp
 	EKF/fake_pos_control.cpp
-	EKF/gravity_fusion.cpp
 	EKF/height_control.cpp
 	EKF/imu_down_sampler.cpp
 	EKF/output_predictor.cpp
@@ -175,6 +174,10 @@ if(CONFIG_EKF2_GNSS_YAW)
 	list(APPEND EKF_SRCS EKF/gps_yaw_fusion.cpp)
 endif()
 
+if(CONFIG_EKF2_GRAVITY_FUSION)
+	list(APPEND EKF_SRCS EKF/gravity_fusion.cpp)
+endif()
+
 if(CONFIG_EKF2_MAGNETOMETER)
 	list(APPEND EKF_SRCS
 		EKF/mag_3d_control.cpp

src/modules/ekf2/EKF/CMakeLists.txt

@@ -41,7 +41,6 @@ list(APPEND EKF_SRCS
 	estimator_interface.cpp
 	fake_height_control.cpp
 	fake_pos_control.cpp
-	gravity_fusion.cpp
 	height_control.cpp
 	imu_down_sampler.cpp
 	output_predictor.cpp
@@ -93,6 +92,10 @@ if(CONFIG_EKF2_GNSS_YAW)
 	list(APPEND EKF_SRCS gps_yaw_fusion.cpp)
 endif()
 
+if(CONFIG_EKF2_GRAVITY_FUSION)
+	list(APPEND EKF_SRCS gravity_fusion.cpp)
+endif()
+
 if(CONFIG_EKF2_MAGNETOMETER)
 	list(APPEND EKF_SRCS
 		mag_3d_control.cpp

src/modules/ekf2/EKF/common.h

@@ -454,8 +454,10 @@ struct parameters {
 	Vector3f ev_pos_body{};                 ///< xyz position of VI-sensor focal point in body frame (m)
 #endif // CONFIG_EKF2_EXTERNAL_VISION
 
+#if defined(CONFIG_EKF2_GRAVITY_FUSION)
 	// gravity fusion
 	float gravity_noise{1.0f};              ///< accelerometer measurement gaussian noise (m/s**2)
+#endif // CONFIG_EKF2_GRAVITY_FUSION
 
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
 	int32_t flow_ctrl{0};

src/modules/ekf2/EKF/control.cpp

@@ -128,7 +128,10 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed)
 #endif // CONFIG_EKF2_DRAG_FUSION
 
 	controlHeightFusion(imu_delayed);
+
+#if defined(CONFIG_EKF2_GRAVITY_FUSION)
 	controlGravityFusion(imu_delayed);
+#endif // CONFIG_EKF2_GRAVITY_FUSION
 
 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
 	// Additional data odometry data from an external estimator can be fused.

src/modules/ekf2/EKF/covariance.cpp

@@ -65,7 +65,12 @@ void Ekf::initialiseCovariance()
 	P.uncorrelateCovarianceSetVariance<State::vel.dof>(State::vel.idx, Vector3f(vel_var, vel_var, sq(1.5f) * vel_var));
 
 	// position
+#if defined(CONFIG_EKF2_BAROMETER)
 	float z_pos_var = sq(fmaxf(_params.baro_noise, 0.01f));
+#else
+	float z_pos_var = sq(1.f);
+#endif // CONFIG_EKF2_BAROMETER
+
 #if defined(CONFIG_EKF2_GNSS)
 	const float xy_pos_var = sq(fmaxf(_params.gps_pos_noise, 0.01f));
 

src/modules/ekf2/EKF/ekf.h

@@ -313,9 +313,13 @@ class Ekf final : public EstimatorInterface
 	void getBetaInnovRatio(float &beta_innov_ratio) const { beta_innov_ratio = _aid_src_sideslip.test_ratio; }
 #endif // CONFIG_EKF2_SIDESLIP
 
+#if defined(CONFIG_EKF2_GRAVITY_FUSION)
+	const auto &aid_src_gravity() const { return _aid_src_gravity; }
+
 	void getGravityInnov(float grav_innov[3]) const { memcpy(grav_innov, _aid_src_gravity.innovation, sizeof(_aid_src_gravity.innovation)); }
 	void getGravityInnovVar(float grav_innov_var[3]) const { memcpy(grav_innov_var, _aid_src_gravity.innovation_variance, sizeof(_aid_src_gravity.innovation_variance)); }
 	void getGravityInnovRatio(float &grav_innov_ratio) const { grav_innov_ratio = Vector3f(_aid_src_gravity.test_ratio).max(); }
+#endif // CONFIG_EKF2_GRAVITY_FUSION
 
 	// get the state vector at the delayed time horizon
 	const matrix::Vector<float, State::size> &getStateAtFusionHorizonAsVector() const { return _state.vector(); }
@@ -558,8 +562,6 @@ class Ekf final : public EstimatorInterface
 	const auto &aid_src_mag() const { return _aid_src_mag; }
 #endif // CONFIG_EKF2_MAGNETOMETER
 
-	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
@@ -735,7 +737,9 @@ class Ekf final : public EstimatorInterface
 # endif // CONFIG_EKF2_GNSS_YAW
 #endif // CONFIG_EKF2_GNSS
 
+#if defined(CONFIG_EKF2_GRAVITY_FUSION)
 	estimator_aid_source3d_s _aid_src_gravity{};
+#endif // CONFIG_EKF2_GRAVITY_FUSION
 
 #if defined(CONFIG_EKF2_AUXVEL)
 	estimator_aid_source2d_s _aid_src_aux_vel{};
@@ -1193,8 +1197,10 @@ class Ekf final : public EstimatorInterface
 	void updateGroundEffect();
 #endif // CONFIG_EKF2_BAROMETER
 
+#if defined(CONFIG_EKF2_GRAVITY_FUSION)
 	// gravity fusion: heuristically enable / disable gravity fusion
 	void controlGravityFusion(const imuSample &imu_delayed);
+#endif // CONFIG_EKF2_GRAVITY_FUSION
 
 	void resetQuatCov(const float yaw_noise = NAN);
 	void resetQuatCov(const Vector3f &euler_noise_ned);

src/modules/ekf2/EKF/estimator_interface.cpp

@@ -563,10 +563,12 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
 	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

src/modules/ekf2/EKF2.cpp

@@ -189,6 +189,16 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
 	_param_ekf2_of_pos_y(_params->flow_pos_body(1)),
 	_param_ekf2_of_pos_z(_params->flow_pos_body(2)),
 #endif // CONFIG_EKF2_OPTICAL_FLOW
+#if defined(CONFIG_EKF2_DRAG_FUSION)
+	_param_ekf2_drag_ctrl(_params->drag_ctrl),
+	_param_ekf2_drag_noise(_params->drag_noise),
+	_param_ekf2_bcoef_x(_params->bcoef_x),
+	_param_ekf2_bcoef_y(_params->bcoef_y),
+	_param_ekf2_mcoef(_params->mcoef),
+#endif // CONFIG_EKF2_DRAG_FUSION
+#if defined(CONFIG_EKF2_GRAVITY_FUSION)
+	_param_ekf2_grav_noise(_params->gravity_noise),
+#endif // CONFIG_EKF2_GRAVITY_FUSION
 	_param_ekf2_imu_pos_x(_params->imu_pos_body(0)),
 	_param_ekf2_imu_pos_y(_params->imu_pos_body(1)),
 	_param_ekf2_imu_pos_z(_params->imu_pos_body(2)),
@@ -199,16 +209,7 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
 	_param_ekf2_abl_acclim(_params->acc_bias_learn_acc_lim),
 	_param_ekf2_abl_gyrlim(_params->acc_bias_learn_gyr_lim),
 	_param_ekf2_abl_tau(_params->acc_bias_learn_tc),
-	_param_ekf2_gyr_b_lim(_params->gyro_bias_lim),
-#if defined(CONFIG_EKF2_DRAG_FUSION)
-	_param_ekf2_drag_ctrl(_params->drag_ctrl),
-	_param_ekf2_drag_noise(_params->drag_noise),
-	_param_ekf2_bcoef_x(_params->bcoef_x),
-	_param_ekf2_bcoef_y(_params->bcoef_y),
-	_param_ekf2_mcoef(_params->mcoef),
-#endif // CONFIG_EKF2_DRAG_FUSION
-	_param_ekf2_grav_noise(_params->gravity_noise)
-
+	_param_ekf2_gyr_b_lim(_params->gyro_bias_lim)
 {
 	// advertise expected minimal topic set immediately to ensure logging
 	_attitude_pub.advertise();
@@ -229,7 +230,9 @@ EKF2::~EKF2()
 	perf_free(_ecl_ekf_update_perf);
 	perf_free(_ecl_ekf_update_full_perf);
 	perf_free(_msg_missed_imu_perf);
+#if defined(CONFIG_EKF2_BAROMETER)
 	perf_free(_msg_missed_air_data_perf);
+#endif // CONFIG_EKF2_BAROMETER
 #if defined(CONFIG_EKF2_AIRSPEED)
 	perf_free(_msg_missed_airspeed_perf);
 #endif // CONFIG_EKF2_AIRSPEED
@@ -398,7 +401,9 @@ int EKF2::print_status()
 	perf_print_counter(_ecl_ekf_update_perf);
 	perf_print_counter(_ecl_ekf_update_full_perf);
 	perf_print_counter(_msg_missed_imu_perf);
+#if defined(CONFIG_EKF2_BAROMETER)
 	perf_print_counter(_msg_missed_air_data_perf);
+#endif // CONFIG_EKF2_BAROMETER
 #if defined(CONFIG_EKF2_AIRSPEED)
 	perf_print_counter(_msg_missed_airspeed_perf);
 #endif // CONFIG_EKF2_AIRSPEED
@@ -1082,8 +1087,10 @@ void EKF2::PublishAidSourceStatus(const hrt_abstime &timestamp)
 	PublishAidSourceStatus(_ekf.aid_src_mag(), _status_mag_pub_last, _estimator_aid_src_mag_pub);
 #endif // CONFIG_EKF2_MAGNETOMETER
 
+#if defined(CONFIG_EKF2_GRAVITY_FUSION)
 	// gravity
 	PublishAidSourceStatus(_ekf.aid_src_gravity(), _status_gravity_pub_last, _estimator_aid_src_gravity_pub);
+#endif // CONFIG_EKF2_GRAVITY_FUSION
 
 #if defined(CONFIG_EKF2_AUXVEL)
 	// aux velocity
@@ -1431,7 +1438,9 @@ void EKF2::PublishInnovations(const hrt_abstime &timestamp)
 	_ekf.getBetaInnov(innovations.beta);
 #endif // CONFIG_EKF2_SIDESLIP
 
+#if defined(CONFIG_EKF2_GRAVITY_FUSION)
 	_ekf.getGravityInnov(innovations.gravity);
+#endif // CONFIG_EKF2_GRAVITY_FUSION
 
 #if defined(CONFIG_EKF2_TERRAIN)
 # if defined(CONFIG_EKF2_RANGE_FINDER)
@@ -1525,7 +1534,9 @@ void EKF2::PublishInnovationTestRatios(const hrt_abstime &timestamp)
 	_ekf.getBetaInnovRatio(test_ratios.beta);
 #endif // CONFIG_EKF2_SIDESLIP
 
+#if defined(CONFIG_EKF2_GRAVITY_FUSION)
 	_ekf.getGravityInnovRatio(test_ratios.gravity[0]);
+#endif // CONFIG_EKF2_GRAVITY_FUSION
 
 #if defined(CONFIG_EKF2_TERRAIN)
 # if defined(CONFIG_EKF2_RANGE_FINDER)
@@ -1590,7 +1601,9 @@ void EKF2::PublishInnovationVariances(const hrt_abstime &timestamp)
 # endif // CONFIG_EKF2_OPTICAL_FLOW
 #endif // CONFIG_EKF2_TERRAIN && CONFIG_EKF2_RANGE_FINDER
 
+#if defined(CONFIG_EKF2_GRAVITY_FUSION)
 	_ekf.getGravityInnovVar(variances.gravity);
+#endif // CONFIG_EKF2_GRAVITY_FUSION
 
 	// Not yet supported
 	variances.aux_vvel = NAN;

src/modules/ekf2/EKF2.hpp

@@ -349,8 +349,6 @@ class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem
 	uORB::PublicationMulti<estimator_bias3d_s> _estimator_ev_pos_bias_pub{ORB_ID(estimator_ev_pos_bias)};
 #endif // CONFIG_EKF2_EXTERNAL_VISION
 
-	hrt_abstime _status_gravity_pub_last{0};
-
 #if defined(CONFIG_EKF2_AUXVEL)
 	uORB::Subscription _landing_target_pose_sub {ORB_ID(landing_target_pose)};
 
@@ -476,8 +474,6 @@ class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem
 	uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_fake_hgt_pub{ORB_ID(estimator_aid_src_fake_hgt)};
 	uORB::PublicationMulti<estimator_aid_source2d_s> _estimator_aid_src_fake_pos_pub{ORB_ID(estimator_aid_src_fake_pos)};
 
-	uORB::PublicationMulti<estimator_aid_source3d_s> _estimator_aid_src_gravity_pub{ORB_ID(estimator_aid_src_gravity)};
-
 	// publications with topic dependent on multi-mode
 	uORB::PublicationMulti<vehicle_attitude_s>           _attitude_pub;
 	uORB::PublicationMulti<vehicle_local_position_s>     _local_position_pub;
@@ -520,6 +516,11 @@ class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem
 # endif // CONFIG_EKF2_GNSS_YAW
 #endif // CONFIG_EKF2_GNSS
 
+#if defined(CONFIG_EKF2_GRAVITY_FUSION)
+	hrt_abstime _status_gravity_pub_last {0};
+	uORB::PublicationMulti<estimator_aid_source3d_s> _estimator_aid_src_gravity_pub{ORB_ID(estimator_aid_src_gravity)};
+#endif // CONFIG_EKF2_GRAVITY_FUSION
+
 	PreFlightChecker _preflt_checker;
 
 	Ekf _ekf;
@@ -724,6 +725,20 @@ class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem
 		_param_ekf2_of_pos_z, ///< Z position of optical flow sensor focal point in body frame (m)
 #endif // CONFIG_EKF2_OPTICAL_FLOW
 
+#if defined(CONFIG_EKF2_DRAG_FUSION)
+		(ParamExtInt<px4::params::EKF2_DRAG_CTRL>) _param_ekf2_drag_ctrl,		///< drag fusion selection
+		// Multi-rotor drag specific force fusion
+		(ParamExtFloat<px4::params::EKF2_DRAG_NOISE>)
+		_param_ekf2_drag_noise,	///< observation noise variance for drag specific force measurements (m/sec**2)**2
+		(ParamExtFloat<px4::params::EKF2_BCOEF_X>) _param_ekf2_bcoef_x,		///< ballistic coefficient along the X-axis (kg/m**2)
+		(ParamExtFloat<px4::params::EKF2_BCOEF_Y>) _param_ekf2_bcoef_y,		///< ballistic coefficient along the Y-axis (kg/m**2)
+		(ParamExtFloat<px4::params::EKF2_MCOEF>) _param_ekf2_mcoef,		///< propeller momentum drag coefficient (1/s)
+#endif // CONFIG_EKF2_DRAG_FUSION
+
+#if defined(CONFIG_EKF2_GRAVITY_FUSION)
+		(ParamExtFloat<px4::params::EKF2_GRAV_NOISE>) _param_ekf2_grav_noise,
+#endif // CONFIG_EKF2_GRAVITY_FUSION
+
 		// sensor positions in body frame
 		(ParamExtFloat<px4::params::EKF2_IMU_POS_X>) _param_ekf2_imu_pos_x,		///< X position of IMU in body frame (m)
 		(ParamExtFloat<px4::params::EKF2_IMU_POS_Y>) _param_ekf2_imu_pos_y,		///< Y position of IMU in body frame (m)
@@ -748,21 +763,9 @@ class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem
 
 		(ParamExtFloat<px4::params::EKF2_GYR_B_LIM>) _param_ekf2_gyr_b_lim,	///< Gyro bias learning limit (rad/s)
 
-#if defined(CONFIG_EKF2_DRAG_FUSION)
-		(ParamExtInt<px4::params::EKF2_DRAG_CTRL>) _param_ekf2_drag_ctrl,		///< drag fusion selection
-		// Multi-rotor drag specific force fusion
-		(ParamExtFloat<px4::params::EKF2_DRAG_NOISE>)
-		_param_ekf2_drag_noise,	///< observation noise variance for drag specific force measurements (m/sec**2)**2
-		(ParamExtFloat<px4::params::EKF2_BCOEF_X>) _param_ekf2_bcoef_x,		///< ballistic coefficient along the X-axis (kg/m**2)
-		(ParamExtFloat<px4::params::EKF2_BCOEF_Y>) _param_ekf2_bcoef_y,		///< ballistic coefficient along the Y-axis (kg/m**2)
-		(ParamExtFloat<px4::params::EKF2_MCOEF>) _param_ekf2_mcoef,		///< propeller momentum drag coefficient (1/s)
-#endif // CONFIG_EKF2_DRAG_FUSION
-
 		// output predictor filter time constants
 		(ParamFloat<px4::params::EKF2_TAU_VEL>) _param_ekf2_tau_vel,
-		(ParamFloat<px4::params::EKF2_TAU_POS>) _param_ekf2_tau_pos,
-
-		(ParamExtFloat<px4::params::EKF2_GRAV_NOISE>) _param_ekf2_grav_noise
+		(ParamFloat<px4::params::EKF2_TAU_POS>) _param_ekf2_tau_pos
 	)
 };
 #endif // !EKF2_HPP

src/modules/ekf2/Kconfig

@@ -75,6 +75,13 @@ depends on MODULES_EKF2
 	---help---
 		EKF2 GNSS yaw fusion support.
 
+menuconfig EKF2_GRAVITY_FUSION
+depends on MODULES_EKF2
+	bool "gravity fusion support"
+	default y
+	---help---
+		EKF2 gravity fusion support.
+
 menuconfig EKF2_MAGNETOMETER
 depends on MODULES_EKF2
 	bool "magnetometer support"

src/modules/navigator/mission.cpp

@@ -141,6 +141,9 @@ Mission::set_current_mission_index(uint16_t index)
 			set_mission_items();
 		}
 
+		// User has actively set new index, reset.
+		_inactivation_index = -1;
+
 		return true;
 	}
 

src/modules/navigator/mission_base.cpp

@@ -243,7 +243,7 @@ MissionBase::on_activation()
 		}
 	}
 
-	checkClimbRequired(resume_index);
+	checkClimbRequired(_mission.current_seq);
 	set_mission_items();
 
 	_inactivation_index = -1; // reset
@@ -557,6 +557,7 @@ MissionBase::checkMissionRestart()
 	if (_system_disarmed_while_inactive && _mission_has_been_activated && (_mission.count > 0U)
 	    && ((_mission.current_seq + 1) == _mission.count)) {
 		setMissionIndex(0);
+		_inactivation_index = -1; // reset
 		_is_current_planned_mission_item_valid = isMissionValid(_mission);
 		resetMissionJumpCounter();
 		_navigator->reset_cruising_speed();

src/modules/navigator/mission_base.h

@@ -295,6 +295,7 @@ class MissionBase : public MissionBlock, public ModuleParams
 	bool _system_disarmed_while_inactive{false};		/**< Flag indicating if the system has been disarmed while mission is inactive*/
 	mission_s _mission;					/**< Currently active mission*/
 	float _mission_init_climb_altitude_amsl{NAN}; 		/**< altitude AMSL the vehicle will climb to when mission starts */
+	int _inactivation_index{-1}; // index of mission item at which the mission was paused. Used to resume survey missions at previous waypoint to not lose images.
 
 	DatamanCache _dataman_cache{"mission_dm_cache_miss", 10}; /**< Dataman cache of mission items*/
 	DatamanClient	&_dataman_client = _dataman_cache.client(); /**< Dataman client*/
@@ -428,7 +429,6 @@ class MissionBase : public MissionBlock, public ModuleParams
 	int32_t _load_mission_index{-1}; /**< Mission inted of loaded mission items in dataman cache*/
 	int32_t _dataman_cache_size_signed; /**< Size of the dataman cache. A negativ value indicates that previous mission items should be loaded, a positiv value the next mission items*/
 
-	int _inactivation_index{-1}; // index of mission item at which the mission was paused. Used to resume survey missions at previous waypoint to not lose images.
 	bool _align_heading_necessary{false}; // if true, heading of vehicle needs to be aligned with heading of next waypoint. Used to create new mission items for heading alignment.
 
 	mission_item_s _last_gimbal_configure_item {};