diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 02ed3f6919..ae0203bc30 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -45,6 +45,7 @@ set(SRCS test_AlphaFilter.cpp test_EKF_fusionLogic.cpp test_EKF_initialization.cpp + test_EKF_gps_yaw.cpp test_EKF_gps.cpp test_EKF_externalVision.cpp test_EKF_airspeed.cpp diff --git a/test/sensor_simulator/ekf_wrapper.cpp b/test/sensor_simulator/ekf_wrapper.cpp index 56f57ca314..25886a9841 100644 --- a/test/sensor_simulator/ekf_wrapper.cpp +++ b/test/sensor_simulator/ekf_wrapper.cpp @@ -75,6 +75,23 @@ bool EkfWrapper::isIntendingGpsFusion() const return control_status.flags.gps; } +void EkfWrapper::enableGpsHeadingFusion() +{ + _ekf_params->fusion_mode |= MASK_USE_GPSYAW; +} + +void EkfWrapper::disableGpsHeadingFusion() +{ + _ekf_params->fusion_mode &= ~MASK_USE_GPSYAW; +} + +bool EkfWrapper::isIntendingGpsHeadingFusion() const +{ + filter_control_status_u control_status; + _ekf->get_control_mode(&control_status.value); + return control_status.flags.gps_yaw; +} + void EkfWrapper::enableFlowFusion() { _ekf_params->fusion_mode |= MASK_USE_OF; @@ -165,7 +182,21 @@ Eulerf EkfWrapper::getEulerAngles() const return Eulerf(_ekf->getQuaternion()); } +float EkfWrapper::getYawAngle() const +{ + const Eulerf euler(_ekf->getQuaternion()); + return euler(2); +} + matrix::Vector EkfWrapper::getQuaternionVariance() const { return matrix::Vector(_ekf->orientation_covariances().diag()); } + +int EkfWrapper::getQuaternionResetCounter() const +{ + float tmp[4]; + uint8_t counter; + _ekf->get_quat_reset(tmp, &counter); + return static_cast(counter); +} diff --git a/test/sensor_simulator/ekf_wrapper.h b/test/sensor_simulator/ekf_wrapper.h index 09805fa291..c74763c13a 100644 --- a/test/sensor_simulator/ekf_wrapper.h +++ b/test/sensor_simulator/ekf_wrapper.h @@ -64,6 +64,10 @@ public: void disableGpsFusion(); bool isIntendingGpsFusion() const; + void enableGpsHeadingFusion(); + void disableGpsHeadingFusion(); + bool isIntendingGpsHeadingFusion() const; + void enableFlowFusion(); void disableFlowFusion(); bool isIntendingFlowFusion() const; @@ -86,7 +90,9 @@ public: bool isWindVelocityEstimated() const; Eulerf getEulerAngles() const; + float getYawAngle() const; matrix::Vector getQuaternionVariance() const; + int getQuaternionResetCounter() const; private: std::shared_ptr _ekf; diff --git a/test/sensor_simulator/gps.cpp b/test/sensor_simulator/gps.cpp index 84ee3c0331..d98a10a72f 100644 --- a/test/sensor_simulator/gps.cpp +++ b/test/sensor_simulator/gps.cpp @@ -44,6 +44,10 @@ void Gps::setVelocity(const Vector3f& vel) _gps_data.vel_ned = vel; } +void Gps::setYaw(float yaw) { + _gps_data.yaw = yaw; +} + void Gps::setFixType(int n) { _gps_data.fix_type = n; diff --git a/test/sensor_simulator/gps.h b/test/sensor_simulator/gps.h index 021e439acc..944132638d 100644 --- a/test/sensor_simulator/gps.h +++ b/test/sensor_simulator/gps.h @@ -57,6 +57,7 @@ public: void setLatitude(int32_t lat); void setLongitude(int32_t lon); void setVelocity(const Vector3f& vel); + void setYaw(float yaw); void setFixType(int n); void setNumberOfSatellites(int n); void setPdop(float pdop); diff --git a/test/test_EKF_gps_yaw.cpp b/test/test_EKF_gps_yaw.cpp new file mode 100644 index 0000000000..86b227873c --- /dev/null +++ b/test/test_EKF_gps_yaw.cpp @@ -0,0 +1,113 @@ +/**************************************************************************** + * + * Copyright (c) 2020 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 gps yaw fusion + * @author Kamil Ritz + */ + +#include +#include "EKF/ekf.h" +#include "sensor_simulator/sensor_simulator.h" +#include "sensor_simulator/ekf_wrapper.h" +#include "test_helper/reset_logging_checker.h" + +class EkfGpsHeadingTest : public ::testing::Test { + public: + + EkfGpsHeadingTest(): ::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); + _ekf_wrapper.enableGpsFusion(); + _sensor_simulator.startGps(); + _sensor_simulator.runSeconds(11); + } +}; + +TEST_F(EkfGpsHeadingTest, fusionStartWithReset) +{ + // GIVEN:EKF that fuses GPS + + // WHEN: enabling GPS heading fusion and heading difference is bigger than 15 degrees + const float gps_heading = _ekf_wrapper.getYawAngle() + math::radians(20.f); + _sensor_simulator._gps.setYaw(gps_heading); + _ekf_wrapper.enableGpsHeadingFusion(); + const int initial_quat_reset_counter = _ekf_wrapper.getQuaternionResetCounter(); + _sensor_simulator.runSeconds(0.2); + + // THEN: GPS heading fusion should have started; + EXPECT_TRUE(_ekf_wrapper.isIntendingGpsHeadingFusion()); + + // AND: a reset to GPS heading is performed + EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter + 1); + EXPECT_NEAR(_ekf_wrapper.getYawAngle(), gps_heading, 0.001); + + // WHEN: GPS heading is disabled + _sensor_simulator._gps.stop(); + _sensor_simulator.runSeconds(11); + + // THEN: after a while the fusion should be stopped + // TODO: THIS IS NOT HAPPENING + EXPECT_FALSE(_ekf_wrapper.isIntendingGpsHeadingFusion()); +} + +TEST_F(EkfGpsHeadingTest, fusionStartWithoutReset) +{ + // GIVEN:EKF that fuses GPS + + // WHEN: enabling GPS heading fusion and heading difference is smaller than 15 degrees + const float gps_heading = _ekf_wrapper.getYawAngle() + math::radians(10.f); + _sensor_simulator._gps.setYaw(gps_heading); + _ekf_wrapper.enableGpsHeadingFusion(); + const int initial_quat_reset_counter = _ekf_wrapper.getQuaternionResetCounter(); + _sensor_simulator.runSeconds(0.2); + + // THEN: GPS heading fusion should have started; + EXPECT_TRUE(_ekf_wrapper.isIntendingGpsHeadingFusion()); + + // AND: no reset to GPS heading should be performed + EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter); + + // TODO: investigate why heading is not converging exactly to GPS heading +}