fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 12:13:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source 
* 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 <brescianimathieu@gmail.com>

* Update src/modules/ekf2/EKF/estimator_interface.h

Co-authored-by: Mathieu Bresciani <brescianimathieu@gmail.com>

---------

Co-authored-by: Mathieu Bresciani <brescianimathieu@gmail.com>
This commit is contained in:
Marco Hauswirth
2024-10-01 14:33:30 +02:00
committed by GitHub
co-authored by Mathieu Bresciani
parent 8aec2d7c9f
commit fe40ccd193
2 changed files with 16 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/ekf.h
+11 -12
View File
@@ -152,6 +152,17 @@ public:
// get the wind velocity in m/s // get the wind velocity in m/s
const Vector2f &getWindVelocity() const { return _state.wind_vel; }; const Vector2f &getWindVelocity() const { return _state.wind_vel; };
Vector2f getWindVelocityVariance() const { return getStateVariance<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 #endif // CONFIG_EKF2_WIND
template <const IdxDof &S> template <const IdxDof &S>
@@ -405,18 +416,6 @@ public:
bool resetGlobalPosToExternalObservation(double latitude, double longitude, float altitude, float eph, float epv, bool resetGlobalPosToExternalObservation(double latitude, double longitude, float altitude, float eph, float epv,
uint64_t timestamp_observation); 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(); void updateParameters();
friend class AuxGlobalPosition; friend class AuxGlobalPosition;
src/modules/ekf2/EKF/estimator_interface.h
+5 -2
View File
@@ -187,8 +187,9 @@ public:
// get vehicle landed status data // get vehicle landed status data
bool get_in_air_status() const { return _control_status.flags.in_air; } bool get_in_air_status() const { return _control_status.flags.in_air; }
// get wind estimation status #if defined(CONFIG_EKF2_WIND)
bool get_wind_status() const { return _control_status.flags.wind; } bool get_wind_status() const { return _control_status.flags.wind || _external_wind_init; }
#endif // CONFIG_EKF2_WIND
// set vehicle is fixed wing status // set vehicle is fixed wing status
void set_is_fixed_wing(bool is_fixed_wing) { _control_status.flags.fixed_wing = is_fixed_wing; } void set_is_fixed_wing(bool is_fixed_wing) { _control_status.flags.fixed_wing = is_fixed_wing; }
@@ -464,6 +465,8 @@ protected:
float _mag_strength{NAN}; float _mag_strength{NAN};
#endif // CONFIG_EKF2_MAGNETOMETER #endif // CONFIG_EKF2_MAGNETOMETER
bool _external_wind_init{false};
// this is the current status of the filter control modes // this is the current status of the filter control modes
filter_control_status_u _control_status{}; filter_control_status_u _control_status{};