fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-23 06:03:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2_terrain: handle height reset

Browse Source
This commit is contained in:
bresch
2023-09-25 09:34:14 -04:00
committed by Daniel Agar
parent 6cb2c176d5
commit 514e0330e5
5 changed files with 38 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/test/CMakeLists.txt
+1 -1
View File
@@ -58,7 +58,7 @@ px4_add_unit_gtest(SRC test_EKF_mag_declination_generated.cpp LINKLIBS ecl_EKF e
px4_add_unit_gtest(SRC test_EKF_measurementSampling.cpp LINKLIBS ecl_EKF ecl_sensor_sim)
px4_add_unit_gtest(SRC test_EKF_ringbuffer.cpp LINKLIBS ecl_EKF ecl_sensor_sim)
px4_add_unit_gtest(SRC test_EKF_sideslip_fusion_generated.cpp LINKLIBS ecl_EKF ecl_test_helper)
px4_add_unit_gtest(SRC test_EKF_terrain_estimator.cpp LINKLIBS ecl_EKF ecl_sensor_sim)
px4_add_unit_gtest(SRC test_EKF_terrain_estimator.cpp LINKLIBS ecl_EKF ecl_sensor_sim ecl_test_helper)
px4_add_unit_gtest(SRC test_EKF_utils.cpp LINKLIBS ecl_EKF ecl_sensor_sim)
px4_add_unit_gtest(SRC test_EKF_withReplayData.cpp LINKLIBS ecl_EKF ecl_sensor_sim)
px4_add_unit_gtest(SRC test_EKF_yaw_estimator.cpp LINKLIBS ecl_EKF ecl_sensor_sim ecl_test_helper)
src/modules/ekf2/test/test_EKF_terrain_estimator.cpp
+28
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 EkfTerrainTest : public ::testing::Test
{
@@ -182,3 +183,30 @@ TEST_F(EkfTerrainTest, testRngForTerrainFusion)
const float estimated_distance_to_ground = _ekf->getTerrainVertPos();
EXPECT_NEAR(estimated_distance_to_ground, rng_height, 0.01f);
}
TEST_F(EkfTerrainTest, testHeightReset)
{
// GIVEN: rng for terrain but not flow
_ekf_wrapper.disableTerrainFlowFusion();
_ekf_wrapper.enableTerrainRngFusion();
const float rng_height = 1.f;
const float flow_height = 1.f;
runFlowAndRngScenario(rng_height, flow_height);
const float estimated_distance_to_ground = _ekf->getTerrainVertPos() - _ekf->getPosition()(2);
ResetLoggingChecker reset_logging_checker(_ekf);
reset_logging_checker.capturePreResetState();
// WHEN: the baro height is suddenly changed to trigger a height reset
const float new_baro_height = _sensor_simulator._baro.getData() + 50.f;
_sensor_simulator._baro.setData(new_baro_height);
_sensor_simulator.stopGps(); // prevent from switching to GNSS height
_sensor_simulator.runSeconds(6);
// THEN: a height reset occured and the estimated distance to the ground remains constant
reset_logging_checker.capturePostResetState();
EXPECT_TRUE(reset_logging_checker.isVerticalPositionResetCounterIncreasedBy(1));
EXPECT_NEAR(estimated_distance_to_ground, _ekf->getTerrainVertPos() - _ekf->getPosition()(2), 1e-3f);
}