fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +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
parent 8aec2d7c9f
commit fe40ccd193
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
2 changed files with 16 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
src/modules/ekf2/EKF/ekf.h
View File

@ -152,6 +152,17 @@ public:
// 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 @@ public:
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;

7
src/modules/ekf2/EKF/estimator_interface.h
View File

@ -187,8 +187,9 @@ public:
// 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 @@ protected:
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{};