dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

gps_hgt: add and improve unit tests

This commit is contained in:
bresch 2022-02-16 15:57:26 +01:00 committed by Mathieu Bresciani
parent edabfd2f0e
commit 4f0a959244
2 changed files with 48 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/modules/ekf2/test/CMakeLists.txt
View File

@ -40,7 +40,7 @@ px4_add_unit_gtest(SRC test_EKF_airspeed.cpp LINKLIBS ecl_EKF ecl_sensor_sim)
px4_add_unit_gtest(SRC test_EKF_basics.cpp LINKLIBS ecl_EKF ecl_sensor_sim)
px4_add_unit_gtest(SRC test_EKF_externalVision.cpp LINKLIBS ecl_EKF ecl_sensor_sim ecl_test_helper)
px4_add_unit_gtest(SRC test_EKF_flow.cpp LINKLIBS ecl_EKF ecl_sensor_sim ecl_test_helper)
px4_add_unit_gtest(SRC test_EKF_fusionLogic.cpp LINKLIBS ecl_EKF ecl_sensor_sim)
px4_add_unit_gtest(SRC test_EKF_fusionLogic.cpp LINKLIBS ecl_EKF ecl_sensor_sim ecl_test_helper)
px4_add_unit_gtest(SRC test_EKF_gps.cpp LINKLIBS ecl_EKF ecl_sensor_sim ecl_test_helper)
px4_add_unit_gtest(SRC test_EKF_gps_yaw.cpp LINKLIBS ecl_EKF ecl_sensor_sim)
px4_add_unit_gtest(SRC test_EKF_imuSampling.cpp LINKLIBS ecl_EKF ecl_sensor_sim)

53
src/modules/ekf2/test/test_EKF_fusionLogic.cpp
View File

@ -40,6 +40,7 @@
#include "EKF/ekf.h"
#include "sensor_simulator/sensor_simulator.h"
#include "sensor_simulator/ekf_wrapper.h"
#include "test_helper/reset_logging_checker.h"
class EkfFusionLogicTest : public ::testing::Test
@ -344,7 +345,9 @@ TEST_F(EkfFusionLogicTest, doVisionHeadingFusion)
TEST_F(EkfFusionLogicTest, doBaroHeightFusion)
{
// GIVEN: EKF that receives baro data
// GIVEN: EKF that receives baro and GPS data
_sensor_simulator.startGps();
_sensor_simulator.runSeconds(11);
// THEN: EKF should intend to fuse baro by default
EXPECT_TRUE(_ekf_wrapper.isIntendingBaroHeightFusion());
@ -353,11 +356,49 @@ TEST_F(EkfFusionLogicTest, doBaroHeightFusion)
_sensor_simulator.stopBaro();
_sensor_simulator.runSeconds(6);
// THEN: EKF should stop to intend to use baro hgt
// TODO: We have no fall back in balce
EXPECT_TRUE(_ekf_wrapper.isIntendingBaroHeightFusion()); // TODO: Needs to change
// THEN: EKF should stop to intend to use baro hgt and use GPS as a fallback
EXPECT_FALSE(_ekf_wrapper.isIntendingBaroHeightFusion());
EXPECT_TRUE(_ekf_wrapper.isIntendingGpsHeightFusion());
}
TEST_F(EkfFusionLogicTest, doBaroHeightFusionTimeout)
{
// GIVEN: EKF that receives baro data
// THEN: EKF should intend to fuse baro by default
EXPECT_TRUE(_ekf_wrapper.isIntendingBaroHeightFusion());
// WHEN: the baro data jumps by a lot
ResetLoggingChecker reset_logging_checker(_ekf);
reset_logging_checker.capturePreResetState();
_sensor_simulator._baro.setData(100.f);
_sensor_simulator.runSeconds(6);
reset_logging_checker.capturePostResetState();
// THEN: EKF should reset to the measurement
EXPECT_TRUE(_ekf_wrapper.isIntendingBaroHeightFusion());
EXPECT_TRUE(reset_logging_checker.isVerticalVelocityResetCounterIncreasedBy(1));
EXPECT_TRUE(reset_logging_checker.isVerticalPositionResetCounterIncreasedBy(1));
// BUT WHEN: GPS height data is also available
_sensor_simulator.startGps();
_sensor_simulator.runSeconds(11);
reset_logging_checker.capturePostResetState();
EXPECT_TRUE(reset_logging_checker.isVerticalVelocityResetCounterIncreasedBy(2));
// AND: the baro data jumps by a lot
_sensor_simulator._baro.setData(800.f);
_sensor_simulator.runSeconds(6);
reset_logging_checker.capturePostResetState();
// THEN: EKF should fallback to GPS height
EXPECT_FALSE(_ekf_wrapper.isIntendingBaroHeightFusion());
EXPECT_TRUE(_ekf_wrapper.isIntendingGpsHeightFusion());
EXPECT_TRUE(reset_logging_checker.isVerticalVelocityResetCounterIncreasedBy(3));
EXPECT_TRUE(reset_logging_checker.isVerticalPositionResetCounterIncreasedBy(2));
}
TEST_F(EkfFusionLogicTest, doGpsHeightFusion)
{
@ -371,13 +412,13 @@ TEST_F(EkfFusionLogicTest, doGpsHeightFusion)
// WHEN: stop sending gps data
_sensor_simulator.stopGps();
_sensor_simulator.runSeconds(11); // TODO: We have to wait way too long
_sensor_simulator.runSeconds(5);
// THEN: EKF should stop to intend to use gps height
EXPECT_FALSE(_ekf_wrapper.isIntendingGpsHeightFusion());
EXPECT_TRUE(_ekf_wrapper.isIntendingBaroHeightFusion());
}
TEST_F(EkfFusionLogicTest, doRangeHeightFusion)
{
// WHEN: commanding range height and sending range data