wind_estimator: move from ecl and consolidate airspeed selector

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)

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)
 

src/lib/ecl/airdata/CMakeLists.txt

@@ -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})

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)

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<float, 1, 3> H_tas;
@@ -175,20 +176,22 @@ WindEstimator::fuse_airspeed(uint64_t time_now, const float true_airspeed, const
 
 	const matrix::Matrix<float, 1, 1> 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<float, 1, 1> 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;
 	}

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 <ecl.h>
 #include <mathlib/mathlib.h>
 #include <matrix/math.hpp>
 

src/modules/airspeed_selector/AirspeedValidator.hpp

@@ -39,7 +39,7 @@
 #pragma once
 
 #include <airspeed/airspeed.h>
-#include <ecl/airdata/WindEstimator.hpp>
+#include <lib/wind_estimator/WindEstimator.hpp>
 #include <uORB/topics/airspeed_wind.h>
 
 

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
 )

src/modules/airspeed_selector/airspeed_selector_main.cpp

@@ -31,8 +31,10 @@
  *
  ****************************************************************************/
 
+#include "AirspeedValidator.hpp"
+
 #include <drivers/drv_hrt.h>
-#include <ecl/airdata/WindEstimator.hpp>
+#include <lib/wind_estimator/WindEstimator.hpp>
 #include <matrix/math.hpp>
 #include <parameters/param.h>
 #include <perf/perf_counter.h>
@@ -40,8 +42,7 @@
 #include <px4_platform_common/module_params.h>
 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
 #include <lib/airspeed/airspeed.h>
-#include <AirspeedValidator.hpp>
-#include <systemlib/mavlink_log.h>
+#include <lib/systemlib/mavlink_log.h>
 
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionMultiArray.hpp>