diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 67dfd51a5a99..22558074677c 100644 --- 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(); } + + /** + * @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 @@ -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 index 302d113f8d12..3fd302df0062 100644 --- 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{};