From 5b02310f439a64e1dae39dab12b2e91c21d6fd70 Mon Sep 17 00:00:00 2001 From: kamilritz Date: Thu, 9 Jan 2020 17:03:47 +0100 Subject: [PATCH] Test Airspeed fusion --- test/CMakeLists.txt | 1 + test/sensor_simulator/ekf_wrapper.cpp | 7 ++ test/sensor_simulator/ekf_wrapper.h | 1 + test/test_EKF_airspeed.cpp | 102 ++++++++++++++++++++++++++ 4 files changed, 111 insertions(+) create mode 100644 test/test_EKF_airspeed.cpp diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index e172a2e403..78e1237fb5 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -44,6 +44,7 @@ set(SRCS test_EKF_fusionLogic.cpp test_EKF_initialization.cpp test_EKF_externalVision.cpp + test_EKF_airspeed.cpp ) add_executable(ECL_GTESTS ${SRCS}) diff --git a/test/sensor_simulator/ekf_wrapper.cpp b/test/sensor_simulator/ekf_wrapper.cpp index 840dfadb3e..df296938e2 100644 --- a/test/sensor_simulator/ekf_wrapper.cpp +++ b/test/sensor_simulator/ekf_wrapper.cpp @@ -163,6 +163,13 @@ Vector3f EkfWrapper::getVelocityVariance() const return Vector3f(_ekf->velocity_covariances().diag()); } +Vector2f EkfWrapper::getWindVelocity() const +{ + float temp[2]; + _ekf->get_wind_velocity(temp); + return Vector2f(temp); +} + Quatf EkfWrapper::getVisionAlignmentQuaternion() const { float temp[4]; diff --git a/test/sensor_simulator/ekf_wrapper.h b/test/sensor_simulator/ekf_wrapper.h index e6930accbf..a5da3c879d 100644 --- a/test/sensor_simulator/ekf_wrapper.h +++ b/test/sensor_simulator/ekf_wrapper.h @@ -80,6 +80,7 @@ public: matrix::Vector getQuaternionVariance() const; Vector3f getPositionVariance() const; Vector3f getVelocityVariance() const; + Vector2f getWindVelocity() const; Quatf getVisionAlignmentQuaternion() const; diff --git a/test/test_EKF_airspeed.cpp b/test/test_EKF_airspeed.cpp new file mode 100644 index 0000000000..849467e373 --- /dev/null +++ b/test/test_EKF_airspeed.cpp @@ -0,0 +1,102 @@ +/**************************************************************************** + * + * Copyright (c) 2019 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 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. + * + ****************************************************************************/ + +/** + * Test the fusion of airspeed data + * @author Kamil Ritz + */ + +#include +#include "EKF/ekf.h" +#include "sensor_simulator/sensor_simulator.h" +#include "sensor_simulator/ekf_wrapper.h" + + +class EkfAirspeedTest : public ::testing::Test { + public: + + EkfAirspeedTest(): ::testing::Test(), + _ekf{std::make_shared()}, + _sensor_simulator(_ekf), + _ekf_wrapper(_ekf) {}; + + std::shared_ptr _ekf; + SensorSimulator _sensor_simulator; + EkfWrapper _ekf_wrapper; + + // Setup the Ekf with synthetic measurements + void SetUp() override + { + _ekf->init(0); + _sensor_simulator.runSeconds(2); + } + + // Use this method to clean up any memory, network etc. after each test + void TearDown() override + { + } +}; + +TEST_F(EkfAirspeedTest, temp) +{ + const Vector3f simulated_velocity(1.5f,0.0f,0.0f); + _ekf_wrapper.enableExternalVisionVelocityFusion(); + _sensor_simulator._vio.setVelocity(simulated_velocity); + _sensor_simulator.startExternalVision(); + + _ekf->set_in_air_status(true); + _sensor_simulator.startAirspeedSensor(); + _sensor_simulator._airspeed.setData(0.1f,0.1f); + + _sensor_simulator.runSeconds(40); + + + filter_control_status_u control_status; + _ekf->get_control_mode(&control_status.value); + EXPECT_TRUE(control_status.flags.wind); + + EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionPositionFusion()); + EXPECT_TRUE(_ekf_wrapper.isIntendingExternalVisionVelocityFusion()); + EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionHeadingFusion()); + + EXPECT_TRUE(_ekf->local_position_is_valid()); + EXPECT_FALSE(_ekf->global_position_is_valid()); + + const Vector3f vel = _ekf_wrapper.getVelocity(); + const Vector2f vel_wind = _ekf_wrapper.getWindVelocity(); + + + EXPECT_TRUE(matrix::isEqual(vel, simulated_velocity)); + EXPECT_TRUE(matrix::isEqual(vel_wind, Vector2f{1.4f, 0.0f})); + +}