fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-16 07:27:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

moved wind related functions into separate file

Browse Source 
Signed-off-by: RomanBapst <bapstroman@gmail.com>
This commit is contained in:
RomanBapst
2024-06-18 08:11:12 +02:00
parent b6c86d841a
commit 9877aed1ef
6 changed files with 109 additions and 65 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/CMakeLists.txt
+4
View File
@@ -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(
src/modules/ekf2/EKF/CMakeLists.txt
+4
View File
@@ -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}
src/modules/ekf2/EKF/covariance.cpp
-8
View File
@@ -334,11 +334,3 @@ void Ekf::resetMagCov()
P.uncorrelateCovarianceSetVariance<State::mag_B.dof>(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.dof>(State::wind_vel.idx, sq(_params.initial_wind_uncertainty));
}
#endif // CONFIG_EKF2_WIND
src/modules/ekf2/EKF/ekf.cpp
-33
View File
@@ -40,7 +40,6 @@
*/
#include "ekf.h"
#include <ekf_derivation/generated/compute_wind_init_and_cov_from_wind_speed_and_direction.h>
#include <mathlib/mathlib.h>
@@ -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<float, 2> 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()
{
src/modules/ekf2/EKF/ekf_helper.cpp
-24
View File
@@ -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)
{
src/modules/ekf2/EKF/wind.cpp
+101
View File
@@ -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 <ekf_derivation/generated/compute_wind_init_and_cov_from_wind_speed_and_direction.h>
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<float, 2> 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.dof>(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();
}