EKF2: publish wind after external wind init (#23747)

* publish wind after external wind init * move ext-wind-init to estimator-interface * Update src/modules/ekf2/EKF/estimator_interface.h Co-authored-by: Mathieu Bresciani <[email protected]> * Update src/modules/ekf2/EKF/estimator_interface.h Co-authored-by: Mathieu Bresciani <[email protected]> --------- Co-authored-by: Mathieu Bresciani <[email protected]>
PX4 · Oct 1, 2024 · fe40ccd · fe40ccd
1 parent 8aec2d7
commit fe40ccd
Show file tree

Hide file tree

Showing 2 changed files with 16 additions and 14 deletions.
diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h
@@ -152,6 +152,17 @@ class Ekf final : public EstimatorInterface
 	// get the wind velocity in m/s
 	const Vector2f &getWindVelocity() const { return _state.wind_vel; };
 	Vector2f getWindVelocityVariance() const { return getStateVariance<State::wind_vel>(); }
+
+	/**
+	* @brief Resets the wind states to an external observation
+	*
+	* @param wind_speed The wind speed in m/s
+	* @param wind_direction The azimuth (from true north) to where the wind is heading in radians
+	* @param wind_speed_accuracy The 1 sigma accuracy of the wind speed estimate in m/s
+	* @param wind_direction_accuracy The 1 sigma accuracy of the wind direction estimate in radians
+	*/
+	void resetWindToExternalObservation(float wind_speed, float wind_direction, float wind_speed_accuracy,
+					    float wind_direction_accuracy);
 #endif // CONFIG_EKF2_WIND
 
 	template <const IdxDof &S>
@@ -405,18 +416,6 @@ class Ekf final : public EstimatorInterface
 	bool resetGlobalPosToExternalObservation(double latitude, double longitude, float altitude, float eph, float epv,
 			uint64_t timestamp_observation);
 
-	/**
-	* @brief Resets the wind states to an external observation
-	*
-	* @param wind_speed The wind speed in m/s
-	* @param wind_direction The azimuth (from true north) to where the wind is heading in radians
-	* @param wind_speed_accuracy The 1 sigma accuracy of the wind speed estimate in m/s
-	* @param wind_direction_accuracy The 1 sigma accuracy of the wind direction estimate in radians
-	*/
-	void resetWindToExternalObservation(float wind_speed, float wind_direction, float wind_speed_accuracy,
-					    float wind_direction_accuracy);
-	bool _external_wind_init{false};
-
 	void updateParameters();
 
 	friend class AuxGlobalPosition;

diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h
@@ -187,8 +187,9 @@ class EstimatorInterface
 	// get vehicle landed status data
 	bool get_in_air_status() const { return _control_status.flags.in_air; }
 
-	// get wind estimation status
-	bool get_wind_status() const { return _control_status.flags.wind; }
+#if defined(CONFIG_EKF2_WIND)
+	bool get_wind_status() const { return _control_status.flags.wind || _external_wind_init; }
+#endif // CONFIG_EKF2_WIND
 
 	// set vehicle is fixed wing status
 	void set_is_fixed_wing(bool is_fixed_wing) { _control_status.flags.fixed_wing = is_fixed_wing; }
@@ -464,6 +465,8 @@ class EstimatorInterface
 	float _mag_strength{NAN};
 #endif // CONFIG_EKF2_MAGNETOMETER
 
+	bool _external_wind_init{false};
+
 	// this is the current status of the filter control modes
 	filter_control_status_u _control_status{};