diff --git a/src/modules/ekf2/CMakeLists.txt b/src/modules/ekf2/CMakeLists.txt index 3f5d9d142f..3755faddfc 100644 --- a/src/modules/ekf2/CMakeLists.txt +++ b/src/modules/ekf2/CMakeLists.txt @@ -216,6 +216,10 @@ if(CONFIG_EKF2_TERRAIN) list(APPEND EKF_SRCS EKF/terrain_control.cpp) endif() +if(CONFIG_EKF2_WIND) + list(APPEND EKF_SRCS EKF/wind.cpp) +endif () + add_subdirectory(EKF) px4_add_module( diff --git a/src/modules/ekf2/EKF/CMakeLists.txt b/src/modules/ekf2/EKF/CMakeLists.txt index 1134d1942b..e3462f7c50 100644 --- a/src/modules/ekf2/EKF/CMakeLists.txt +++ b/src/modules/ekf2/EKF/CMakeLists.txt @@ -138,6 +138,10 @@ if(CONFIG_EKF2_TERRAIN) list(APPEND EKF_SRCS terrain_control.cpp) endif() +if(CONFIG_EKF2_WIND) + list(APPEND EKF_SRCS wind.cpp) +endif () + include_directories(${CMAKE_CURRENT_SOURCE_DIR}) add_library(ecl_EKF ${EKF_SRCS} diff --git a/src/modules/ekf2/EKF/covariance.cpp b/src/modules/ekf2/EKF/covariance.cpp index 60a865d2de..e76ca01153 100644 --- a/src/modules/ekf2/EKF/covariance.cpp +++ b/src/modules/ekf2/EKF/covariance.cpp @@ -334,11 +334,3 @@ void Ekf::resetMagCov() P.uncorrelateCovarianceSetVariance(State::mag_B.idx, sq(_params.mag_noise)); } #endif // CONFIG_EKF2_MAGNETOMETER - -#if defined(CONFIG_EKF2_WIND) -void Ekf::resetWindCov() -{ - // start with a small initial uncertainty to improve the initial estimate - P.uncorrelateCovarianceSetVariance(State::wind_vel.idx, sq(_params.initial_wind_uncertainty)); -} -#endif // CONFIG_EKF2_WIND diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index e7d04ca0f2..1571aa06ca 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/src/modules/ekf2/EKF/ekf.cpp @@ -40,7 +40,6 @@ */ #include "ekf.h" -#include #include @@ -300,38 +299,6 @@ void Ekf::resetGlobalPosToExternalObservation(double lat_deg, double lon_deg, fl ECL_INFO("reset position to external observation"); _information_events.flags.reset_pos_to_ext_obs = true; } -void Ekf::resetWindToExternalObservation(float wind_speed, float wind_direction, float wind_speed_accuracy, float wind_direction_accuracy) -{ - const float wind_speed_constrained = math::max(wind_speed, 0.0f); - - // wind direction is given as azimuth where wind blows FROM, we need direction where wind blows TO - const float wind_direction_rad = wrap_pi(math::radians(wind_direction) + M_PI_F); - - matrix::SquareMatrix P_wind; - Vector2f wind; - - sym::ComputeWindInitAndCovFromWindSpeedAndDirection(wind_speed_constrained, wind_direction_rad, wind_speed_accuracy, wind_direction_accuracy, &wind, &P_wind); - - const Vector2f wind_var = P_wind.diag(); - - ECL_INFO("reset wind states to external observation"); - _information_events.flags.reset_wind_to_ext_obs = true; - - resetWindTo(wind, wind_var); -} - -void Ekf::resetWindTo(const Vector2f &wind, const Vector2f &wind_var) -{ - _state.wind_vel = wind; - - if (PX4_ISFINITE(wind_var(0))) { - P.uncorrelateCovarianceSetVariance<1>(State::wind_vel.idx, math::max(sq(_params.initial_wind_uncertainty), wind_var(0))); - } - - if (PX4_ISFINITE(wind_var(1))) { - P.uncorrelateCovarianceSetVariance<1>(State::wind_vel.idx + 1, math::max(sq(_params.initial_wind_uncertainty), wind_var(1))); - } -} void Ekf::updateParameters() { diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 94b1046dba..aee876767c 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -683,30 +683,6 @@ void Ekf::updateGroundEffect() } #endif // CONFIG_EKF2_BAROMETER -#if defined(CONFIG_EKF2_WIND) -void Ekf::resetWind() -{ -#if defined(CONFIG_EKF2_AIRSPEED) - if (_control_status.flags.fuse_aspd && isRecent(_airspeed_sample_delayed.time_us, 1e6)) { - resetWindUsingAirspeed(_airspeed_sample_delayed); - return; - } -#endif // CONFIG_EKF2_AIRSPEED - - resetWindToZero(); -} - -void Ekf::resetWindToZero() -{ - ECL_INFO("reset wind to zero"); - - // If we don't have an airspeed measurement, then assume the wind is zero - _state.wind_vel.setZero(); - - resetWindCov(); -} - -#endif // CONFIG_EKF2_WIND void Ekf::updateIMUBiasInhibit(const imuSample &imu_delayed) { diff --git a/src/modules/ekf2/EKF/wind.cpp b/src/modules/ekf2/EKF/wind.cpp new file mode 100644 index 0000000000..5bdde187cb --- /dev/null +++ b/src/modules/ekf2/EKF/wind.cpp @@ -0,0 +1,101 @@ +/**************************************************************************** + * + * Copyright (c) 2024 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file wind.cpp + * Helper functions for wind states + */ + +#include "ekf.h" +#include + +void Ekf::resetWindToExternalObservation(float wind_speed, float wind_direction, float wind_speed_accuracy, float wind_direction_accuracy) +{ + const float wind_speed_constrained = math::max(wind_speed, 0.0f); + + // wind direction is given as azimuth where wind blows FROM, we need direction where wind blows TO + const float wind_direction_rad = wrap_pi(math::radians(wind_direction) + M_PI_F); + + matrix::SquareMatrix P_wind; + Vector2f wind; + + sym::ComputeWindInitAndCovFromWindSpeedAndDirection(wind_speed_constrained, wind_direction_rad, wind_speed_accuracy, wind_direction_accuracy, &wind, &P_wind); + + const Vector2f wind_var = P_wind.diag(); + + ECL_INFO("reset wind states to external observation"); + _information_events.flags.reset_wind_to_ext_obs = true; + + resetWindTo(wind, wind_var); +} + +void Ekf::resetWindTo(const Vector2f &wind, const Vector2f &wind_var) +{ + _state.wind_vel = wind; + + if (PX4_ISFINITE(wind_var(0))) { + P.uncorrelateCovarianceSetVariance<1>(State::wind_vel.idx, math::max(sq(_params.initial_wind_uncertainty), wind_var(0))); + } + + if (PX4_ISFINITE(wind_var(1))) { + P.uncorrelateCovarianceSetVariance<1>(State::wind_vel.idx + 1, math::max(sq(_params.initial_wind_uncertainty), wind_var(1))); + } +} + +void Ekf::resetWindCov() +{ + // start with a small initial uncertainty to improve the initial estimate + P.uncorrelateCovarianceSetVariance(State::wind_vel.idx, sq(_params.initial_wind_uncertainty)); +} + +void Ekf::resetWind() +{ +#if defined(CONFIG_EKF2_AIRSPEED) + if (_control_status.flags.fuse_aspd && isRecent(_airspeed_sample_delayed.time_us, 1e6)) { + resetWindUsingAirspeed(_airspeed_sample_delayed); + return; + } +#endif // CONFIG_EKF2_AIRSPEED + + resetWindToZero(); +} + +void Ekf::resetWindToZero() +{ + ECL_INFO("reset wind to zero"); + + // If we don't have an airspeed measurement, then assume the wind is zero + _state.wind_vel.setZero(); + + resetWindCov(); +}