diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index a06c7551f0..5eb9355291 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -35,7 +35,6 @@ px4_add_git_submodule(TARGET git_matrix PATH "matrix") px4_add_git_submodule(TARGET git_monocypher PATH "crypto/monocypher") add_subdirectory(airspeed) -add_subdirectory(airspeed_validator) add_subdirectory(avoidance) add_subdirectory(battery) add_subdirectory(bezier) @@ -70,4 +69,5 @@ add_subdirectory(terrain_estimation) add_subdirectory(tunes) add_subdirectory(version) add_subdirectory(weather_vane) +add_subdirectory(wind_estimator) add_subdirectory(world_magnetic_model) diff --git a/src/lib/ecl/CMakeLists.txt b/src/lib/ecl/CMakeLists.txt index afcc010c54..f2d8643b0a 100644 --- a/src/lib/ecl/CMakeLists.txt +++ b/src/lib/ecl/CMakeLists.txt @@ -178,7 +178,6 @@ if(ECL_ASAN) set(CMAKE_EXE_LINKER_FLAGS ${CMAKE_EXE_LINKER_FLAGS} -fsanitize=address) endif() -add_subdirectory(airdata) add_subdirectory(EKF) add_subdirectory(geo) diff --git a/src/lib/ecl/airdata/CMakeLists.txt b/src/lib/ecl/airdata/CMakeLists.txt deleted file mode 100644 index 38fda5b8df..0000000000 --- a/src/lib/ecl/airdata/CMakeLists.txt +++ /dev/null @@ -1,39 +0,0 @@ -############################################################################ -# -# Copyright (c) 2018 ECL 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 ECL 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. -# -############################################################################ - -add_library(ecl_airdata - WindEstimator.cpp - ) -add_dependencies(ecl_airdata prebuild_targets) -target_compile_definitions(ecl_airdata PRIVATE -DMODULE_NAME="ecl/airdata") -target_include_directories(ecl_airdata PUBLIC ${ECL_SOURCE_DIR}) \ No newline at end of file diff --git a/src/lib/airspeed_validator/CMakeLists.txt b/src/lib/wind_estimator/CMakeLists.txt similarity index 85% rename from src/lib/airspeed_validator/CMakeLists.txt rename to src/lib/wind_estimator/CMakeLists.txt index 5d015e2bcb..78c94f104c 100644 --- a/src/lib/airspeed_validator/CMakeLists.txt +++ b/src/lib/wind_estimator/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2019 PX4 Development Team. All rights reserved. +# Copyright (c) 2018-2021 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 @@ -31,11 +31,8 @@ # ############################################################################ -px4_add_library(AirspeedValidator AirspeedValidator.cpp) - -target_include_directories(AirspeedValidator - PUBLIC - ${CMAKE_CURRENT_SOURCE_DIR} +add_library(wind_estimator + WindEstimator.cpp + WindEstimator.hpp ) - -target_link_libraries(AirspeedValidator PUBLIC ecl_airdata) +add_dependencies(wind_estimator prebuild_targets) diff --git a/src/lib/ecl/airdata/WindEstimator.cpp b/src/lib/wind_estimator/WindEstimator.cpp similarity index 95% rename from src/lib/ecl/airdata/WindEstimator.cpp rename to src/lib/wind_estimator/WindEstimator.cpp index f755893b58..df840080f0 100644 --- a/src/lib/ecl/airdata/WindEstimator.cpp +++ b/src/lib/wind_estimator/WindEstimator.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2018-2021 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 @@ -163,7 +163,8 @@ WindEstimator::fuse_airspeed(uint64_t time_now, const float true_airspeed, const const float k_tas = _state(INDEX_TAS_SCALE); // compute kalman gain K - const float HH0 = sqrtf(v_d * v_d + (v_e - _state(INDEX_W_E)) * (v_e - _state(INDEX_W_E)) + (v_n - _state(INDEX_W_N)) * (v_n - _state(INDEX_W_N))); + const float HH0 = sqrtf(v_d * v_d + (v_e - _state(INDEX_W_E)) * (v_e - _state(INDEX_W_E)) + (v_n - _state( + INDEX_W_N)) * (v_n - _state(INDEX_W_N))); const float HH1 = k_tas / HH0; matrix::Matrix H_tas; @@ -175,20 +176,22 @@ WindEstimator::fuse_airspeed(uint64_t time_now, const float true_airspeed, const const matrix::Matrix S = H_tas * _P * H_tas.transpose() + _tas_var; - K /= S(0,0); + K /= S(0, 0); // compute innovation - const float airspeed_pred = k_tas * sqrtf((v_n - _state(INDEX_W_N)) * (v_n - _state(INDEX_W_N)) + (v_e - _state(INDEX_W_E)) * + const float airspeed_pred = k_tas * sqrtf((v_n - _state(INDEX_W_N)) * (v_n - _state(INDEX_W_N)) + (v_e - _state( + INDEX_W_E)) * (v_e - _state(INDEX_W_E)) + v_d * v_d); _tas_innov = true_airspeed - airspeed_pred; // innovation variance - _tas_innov_var = S(0,0); + _tas_innov_var = S(0, 0); bool reinit_filter = false; bool meas_is_rejected = false; - meas_is_rejected = check_if_meas_is_rejected(time_now, _tas_innov, _tas_innov_var, _tas_gate, _time_rejected_tas, reinit_filter); + meas_is_rejected = check_if_meas_is_rejected(time_now, _tas_innov, _tas_innov_var, _tas_gate, _time_rejected_tas, + reinit_filter); reinit_filter |= _tas_innov_var < 0.0f; @@ -261,7 +264,7 @@ WindEstimator::fuse_beta(uint64_t time_now, const matrix::Vector3f &velI, const const matrix::Matrix S = H_beta * _P * H_beta.transpose() + _beta_var; - K /= S(0,0); + K /= S(0, 0); // compute predicted side slip angle matrix::Vector3f rel_wind(velI(0) - _state(INDEX_W_N), velI(1) - _state(INDEX_W_E), velI(2)); @@ -276,7 +279,7 @@ WindEstimator::fuse_beta(uint64_t time_now, const matrix::Vector3f &velI, const const float beta_pred = rel_wind(1) / rel_wind(0); _beta_innov = 0.0f - beta_pred; - _beta_innov_var = S(0,0); + _beta_innov_var = S(0, 0); bool reinit_filter = false; bool meas_is_rejected = false; @@ -325,7 +328,7 @@ WindEstimator::run_sanity_checks() } } - if (!ISFINITE(_state(INDEX_W_N)) || !ISFINITE(_state(INDEX_W_E)) || !ISFINITE(_state(INDEX_TAS_SCALE))) { + if (!PX4_ISFINITE(_state(INDEX_W_N)) || !PX4_ISFINITE(_state(INDEX_W_E)) || !PX4_ISFINITE(_state(INDEX_TAS_SCALE))) { _initialised = false; return; } @@ -335,6 +338,7 @@ WindEstimator::run_sanity_checks() // to be computed once for a perticular installation. if (_enforced_airspeed_scale < 0) { _state(INDEX_TAS_SCALE) = math::max(0.0f, _state(INDEX_TAS_SCALE)); + } else { _state(INDEX_TAS_SCALE) = _enforced_airspeed_scale; } diff --git a/src/lib/ecl/airdata/WindEstimator.hpp b/src/lib/wind_estimator/WindEstimator.hpp similarity index 98% rename from src/lib/ecl/airdata/WindEstimator.hpp rename to src/lib/wind_estimator/WindEstimator.hpp index 0f38a41e87..d90e242937 100644 --- a/src/lib/ecl/airdata/WindEstimator.hpp +++ b/src/lib/wind_estimator/WindEstimator.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2018-2021 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 @@ -38,7 +38,6 @@ #pragma once -#include #include #include diff --git a/src/lib/ecl/airdata/python/wind_est_derivation.py b/src/lib/wind_estimator/python/wind_est_derivation.py similarity index 100% rename from src/lib/ecl/airdata/python/wind_est_derivation.py rename to src/lib/wind_estimator/python/wind_est_derivation.py diff --git a/src/lib/airspeed_validator/AirspeedValidator.cpp b/src/modules/airspeed_selector/AirspeedValidator.cpp similarity index 100% rename from src/lib/airspeed_validator/AirspeedValidator.cpp rename to src/modules/airspeed_selector/AirspeedValidator.cpp diff --git a/src/lib/airspeed_validator/AirspeedValidator.hpp b/src/modules/airspeed_selector/AirspeedValidator.hpp similarity index 99% rename from src/lib/airspeed_validator/AirspeedValidator.hpp rename to src/modules/airspeed_selector/AirspeedValidator.hpp index 8e1bc5e4d5..0fc5e1ac9d 100644 --- a/src/lib/airspeed_validator/AirspeedValidator.hpp +++ b/src/modules/airspeed_selector/AirspeedValidator.hpp @@ -39,7 +39,7 @@ #pragma once #include -#include +#include #include diff --git a/src/modules/airspeed_selector/CMakeLists.txt b/src/modules/airspeed_selector/CMakeLists.txt index e87cf3c056..fb3d24f869 100644 --- a/src/modules/airspeed_selector/CMakeLists.txt +++ b/src/modules/airspeed_selector/CMakeLists.txt @@ -35,7 +35,8 @@ px4_add_module( MAIN airspeed_selector SRCS airspeed_selector_main.cpp + AirspeedValidator.cpp + AirspeedValidator.hpp DEPENDS - ecl_airdata - AirspeedValidator + wind_estimator ) diff --git a/src/modules/airspeed_selector/airspeed_selector_main.cpp b/src/modules/airspeed_selector/airspeed_selector_main.cpp index d347734169..6ee2476ade 100644 --- a/src/modules/airspeed_selector/airspeed_selector_main.cpp +++ b/src/modules/airspeed_selector/airspeed_selector_main.cpp @@ -31,8 +31,10 @@ * ****************************************************************************/ +#include "AirspeedValidator.hpp" + #include -#include +#include #include #include #include @@ -40,8 +42,7 @@ #include #include #include -#include -#include +#include #include #include