diff --git a/src/modules/ekf2/test/CMakeLists.txt b/src/modules/ekf2/test/CMakeLists.txt index 5c2c850c59..63cb634b79 100644 --- a/src/modules/ekf2/test/CMakeLists.txt +++ b/src/modules/ekf2/test/CMakeLists.txt @@ -51,6 +51,7 @@ px4_add_unit_gtest(SRC test_EKF_gnss_yaw_generated.cpp LINKLIBS ecl_EKF ecl_test px4_add_unit_gtest(SRC test_EKF_height_fusion.cpp LINKLIBS ecl_EKF ecl_sensor_sim ecl_test_helper) px4_add_unit_gtest(SRC test_EKF_imuSampling.cpp LINKLIBS ecl_EKF ecl_sensor_sim) px4_add_unit_gtest(SRC test_EKF_initialization.cpp LINKLIBS ecl_EKF ecl_sensor_sim) +px4_add_unit_gtest(SRC test_EKF_mag.cpp LINKLIBS ecl_EKF ecl_sensor_sim) px4_add_unit_gtest(SRC test_EKF_mag_3d_fusion_generated.cpp LINKLIBS ecl_EKF ecl_test_helper) px4_add_unit_gtest(SRC test_EKF_mag_declination_generated.cpp LINKLIBS ecl_EKF ecl_test_helper) px4_add_unit_gtest(SRC test_EKF_measurementSampling.cpp LINKLIBS ecl_EKF ecl_sensor_sim) diff --git a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp index 7f5a46fe80..b32b08ccae 100644 --- a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp +++ b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp @@ -195,6 +195,11 @@ void EkfWrapper::setMagFuseTypeNone() _ekf_params->mag_fusion_type = MagFuseType::NONE; } +void EkfWrapper::enableMagStrengthCheck() +{ + _ekf_params->check_mag_strength = 1; +} + bool EkfWrapper::isWindVelocityEstimated() const { return _ekf->control_status_flags().wind; diff --git a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h index f13a772188..df8cf5c87a 100644 --- a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h +++ b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h @@ -97,6 +97,7 @@ public: bool isIntendingMagHeadingFusion() const; bool isIntendingMag3DFusion() const; void setMagFuseTypeNone(); + void enableMagStrengthCheck(); bool isWindVelocityEstimated() const; diff --git a/src/modules/ekf2/test/test_EKF_mag.cpp b/src/modules/ekf2/test/test_EKF_mag.cpp new file mode 100644 index 0000000000..299266f112 --- /dev/null +++ b/src/modules/ekf2/test/test_EKF_mag.cpp @@ -0,0 +1,129 @@ +/**************************************************************************** + * + * Copyright (c) 2023 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 mag fusion + */ + +#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 EkfMagTest : public ::testing::Test +{ +public: + + EkfMagTest(): ::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 + { + // run briefly to init, then manually set in air and at rest (default for a real vehicle) + _ekf->init(0); + _sensor_simulator.runSeconds(0.1); + _ekf->set_in_air_status(false); + _ekf->set_vehicle_at_rest(true); + } + + const uint32_t _init_duration_s{4}; +}; + +TEST_F(EkfMagTest, fusionStartWithReset) +{ + // GIVEN: some meaningful mag data + const float mag_heading = M_PI_F / 3.f; + const Vector3f mag_data(0.2f * cosf(mag_heading), -0.2f * sinf(mag_heading), 0.4f); + _sensor_simulator._mag.setData(mag_data); + + const int initial_quat_reset_counter = _ekf_wrapper.getQuaternionResetCounter(); + _sensor_simulator.runSeconds(_init_duration_s); + + // THEN: the fusion initializes using the mag data and runs normally + EXPECT_NEAR(_ekf_wrapper.getYawAngle(), mag_heading, radians(0.1f)); + EXPECT_TRUE(_ekf_wrapper.isIntendingMagHeadingFusion()); + EXPECT_FALSE(_ekf_wrapper.isIntendingMag3DFusion()); + + EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter + 1); +} + +TEST_F(EkfMagTest, noInitLargeStrength) +{ + // GIVEN: a really large magnetic field + _ekf_wrapper.enableMagStrengthCheck(); + const Vector3f mag_data(1.f, 1.f, 1.f); + _sensor_simulator._mag.setData(mag_data); + + const int initial_quat_reset_counter = _ekf_wrapper.getQuaternionResetCounter(); + _sensor_simulator.runSeconds(_init_duration_s); + + // THEN: the fusion shouldn't start + EXPECT_FALSE(_ekf_wrapper.isIntendingMagHeadingFusion()); + EXPECT_FALSE(_ekf_wrapper.isIntendingMag3DFusion()); + EXPECT_EQ(0, (int) _ekf->control_status_flags().yaw_align); + EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter); +} + +TEST_F(EkfMagTest, suddenLargeStrength) +{ + _ekf_wrapper.enableMagStrengthCheck(); + + // GIVEN: some meaningful mag data + const float mag_heading = -M_PI_F / 7.f; + Vector3f mag_data(0.2f * cosf(mag_heading), -0.2f * sinf(mag_heading), 0.4f); + _sensor_simulator._mag.setData(mag_data); + + _sensor_simulator.runSeconds(_init_duration_s); + + // THEN: the fusion initializes using the mag data and runs normally + EXPECT_NEAR(_ekf_wrapper.getYawAngle(), mag_heading, radians(0.1f)); + EXPECT_TRUE(_ekf_wrapper.isIntendingMagHeadingFusion()); + EXPECT_FALSE(_ekf_wrapper.isIntendingMag3DFusion()); + + // BUT WHEN: the mag field norm is suddenly too large + mag_data *= 5.f; + _sensor_simulator._mag.setData(mag_data); + _sensor_simulator.runSeconds(6.f); + + // THEN: the mag fusion should stop after some time + EXPECT_FALSE(_ekf_wrapper.isIntendingMagHeadingFusion()); + EXPECT_FALSE(_ekf_wrapper.isIntendingMag3DFusion()); +}