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

EKF2: add validity flags to global pos message (#23787)

Browse Source 
Co-authored-by: Mathieu Bresciani <brescianimathieu@gmail.com>
This commit is contained in:
Marco Hauswirth
2024-10-23 10:19:04 +02:00
committed by GitHub
co-authored by Mathieu Bresciani
parent 808153b049
commit 0c451552c7
16 changed files with 87 additions and 102 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/test/test_EKF_airspeed.cpp
+3 -3
View File
@@ -207,7 +207,7 @@ TEST_F(EkfAirspeedTest, testAirspeedDeadReckoning)
EXPECT_NEAR(vel_wind_earth(0), 0.f, .1f);
EXPECT_NEAR(vel_wind_earth(1), 0.f, .1f);
EXPECT_TRUE(_ekf->global_position_is_valid());
EXPECT_TRUE(_ekf->isGlobalHorizontalPositionValid());
}
TEST_F(EkfAirspeedTest, testAirspeedDeadReckoningLatLonAltReset)
@@ -239,7 +239,7 @@ TEST_F(EkfAirspeedTest, testAirspeedDeadReckoningLatLonAltReset)
_sensor_simulator.runSeconds(10.f);
EXPECT_TRUE(_ekf_wrapper.isIntendingAirspeedFusion());
EXPECT_TRUE(_ekf->global_position_is_valid());
EXPECT_TRUE(_ekf->isGlobalHorizontalPositionValid());
// WHEN: an external position reset is sent
ResetLoggingChecker reset_logging_checker(_ekf);
@@ -260,7 +260,7 @@ TEST_F(EkfAirspeedTest, testAirspeedDeadReckoningLatLonAltReset)
EXPECT_NEAR(altitude_est, altitude_new, 0.01f);
EXPECT_NEAR(latitude_est, latitude_new, 1e-3f);
EXPECT_NEAR(longitude_est, longitude_new, 1e-3f);
EXPECT_TRUE(_ekf->global_position_is_valid());
EXPECT_TRUE(_ekf->isGlobalHorizontalPositionValid());
reset_logging_checker.capturePostResetState();
EXPECT_TRUE(reset_logging_checker.isVerticalVelocityResetCounterIncreasedBy(0));
src/modules/ekf2/test/test_EKF_basics.cpp
+9 -9
View File
@@ -283,7 +283,7 @@ TEST_F(EkfBasicsTest, reset_ekf_global_origin_gps_uninitialized)
// Global origin has been initialized but since there is no position aiding, the global
// position is still not valid
EXPECT_TRUE(_ekf->global_origin_valid());
EXPECT_FALSE(_ekf->global_position_is_valid());
EXPECT_FALSE(_ekf->isGlobalHorizontalPositionValid());
_sensor_simulator.runSeconds(1);
@@ -307,9 +307,9 @@ TEST_F(EkfBasicsTest, global_position_from_local_ev)
_sensor_simulator.runSeconds(1);
// THEN; since there is no origin, only the local position can be valid
EXPECT_TRUE(_ekf->local_position_is_valid());
EXPECT_TRUE(_ekf->isLocalHorizontalPositionValid());
EXPECT_FALSE(_ekf->global_origin_valid());
EXPECT_FALSE(_ekf->global_position_is_valid());
EXPECT_FALSE(_ekf->isGlobalHorizontalPositionValid());
_latitude_new = 45.0000005;
_longitude_new = 111.0000005;
@@ -320,8 +320,8 @@ TEST_F(EkfBasicsTest, global_position_from_local_ev)
// THEN: local and global positions are valid
EXPECT_TRUE(_ekf->global_origin_valid());
EXPECT_TRUE(_ekf->global_position_is_valid());
EXPECT_TRUE(_ekf->local_position_is_valid());
EXPECT_TRUE(_ekf->isGlobalHorizontalPositionValid());
EXPECT_TRUE(_ekf->isLocalHorizontalPositionValid());
}
TEST_F(EkfBasicsTest, global_position_from_opt_flow)
@@ -338,9 +338,9 @@ TEST_F(EkfBasicsTest, global_position_from_opt_flow)
_sensor_simulator.runSeconds(1);
// THEN; since there is no origin, only the local position can be valid
EXPECT_TRUE(_ekf->local_position_is_valid());
EXPECT_TRUE(_ekf->isLocalHorizontalPositionValid());
EXPECT_FALSE(_ekf->global_origin_valid());
EXPECT_FALSE(_ekf->global_position_is_valid());
EXPECT_FALSE(_ekf->isGlobalHorizontalPositionValid());
_latitude_new = 45.0000005;
_longitude_new = 111.0000005;
@@ -351,8 +351,8 @@ TEST_F(EkfBasicsTest, global_position_from_opt_flow)
// THEN: local and global positions are valid
EXPECT_TRUE(_ekf->global_origin_valid());
EXPECT_TRUE(_ekf->global_position_is_valid());
EXPECT_TRUE(_ekf->local_position_is_valid());
EXPECT_TRUE(_ekf->isGlobalHorizontalPositionValid());
EXPECT_TRUE(_ekf->isLocalHorizontalPositionValid());
}
// TODO: Add sampling tests
src/modules/ekf2/test/test_EKF_externalVision.cpp
+6 -6
View File
@@ -85,8 +85,8 @@ TEST_F(EkfExternalVisionTest, checkVisionFusionLogic)
EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionVelocityFusion());
EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionHeadingFusion());
EXPECT_TRUE(_ekf->local_position_is_valid());
EXPECT_FALSE(_ekf->global_position_is_valid());
EXPECT_TRUE(_ekf->isLocalHorizontalPositionValid());
EXPECT_FALSE(_ekf->isGlobalHorizontalPositionValid());
_ekf_wrapper.enableExternalVisionVelocityFusion();
_sensor_simulator.runSeconds(2);
@@ -95,8 +95,8 @@ TEST_F(EkfExternalVisionTest, checkVisionFusionLogic)
EXPECT_TRUE(_ekf_wrapper.isIntendingExternalVisionVelocityFusion());
EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionHeadingFusion());
EXPECT_TRUE(_ekf->local_position_is_valid());
EXPECT_FALSE(_ekf->global_position_is_valid());
EXPECT_TRUE(_ekf->isLocalHorizontalPositionValid());
EXPECT_FALSE(_ekf->isGlobalHorizontalPositionValid());
_ekf_wrapper.enableExternalVisionHeadingFusion();
_sensor_simulator.runSeconds(2);
@@ -105,8 +105,8 @@ TEST_F(EkfExternalVisionTest, checkVisionFusionLogic)
EXPECT_TRUE(_ekf_wrapper.isIntendingExternalVisionVelocityFusion());
EXPECT_TRUE(_ekf_wrapper.isIntendingExternalVisionHeadingFusion());
EXPECT_TRUE(_ekf->local_position_is_valid());
EXPECT_FALSE(_ekf->global_position_is_valid());
EXPECT_TRUE(_ekf->isLocalHorizontalPositionValid());
EXPECT_FALSE(_ekf->isGlobalHorizontalPositionValid());
}
TEST_F(EkfExternalVisionTest, visionVelocityReset)
src/modules/ekf2/test/test_EKF_fake_pos.cpp
+1 -1
View File
@@ -117,5 +117,5 @@ TEST_F(EkfFakePosTest, testValidFakePosValidLocalPos)
_sensor_simulator.runSeconds(60);
EXPECT_EQ(1, (int) _ekf->control_status_flags().valid_fake_pos);
EXPECT_TRUE(_ekf->local_position_is_valid());
EXPECT_TRUE(_ekf->isLocalHorizontalPositionValid());
}
src/modules/ekf2/test/test_EKF_fusionLogic.cpp
+29 -29
View File
@@ -84,13 +84,13 @@ TEST_F(EkfFusionLogicTest, doNoFusion)
// GIVEN: a tilt and heading aligned filter
// WHEN: having no aiding source
// THEN: EKF should not have a valid position estimate
EXPECT_FALSE(_ekf->local_position_is_valid());
EXPECT_FALSE(_ekf->isLocalHorizontalPositionValid());
_sensor_simulator.runSeconds(4);
// THEN: Local and global position should not be valid
EXPECT_FALSE(_ekf->local_position_is_valid());
EXPECT_FALSE(_ekf->global_position_is_valid());
EXPECT_FALSE(_ekf->isLocalHorizontalPositionValid());
EXPECT_FALSE(_ekf->isGlobalHorizontalPositionValid());
}
TEST_F(EkfFusionLogicTest, doGpsFusion)
@@ -104,8 +104,8 @@ TEST_F(EkfFusionLogicTest, doGpsFusion)
// THEN: EKF should intend to fuse GPS
EXPECT_TRUE(_ekf_wrapper.isIntendingGpsFusion());
// THEN: Local and global position should be valid
EXPECT_TRUE(_ekf->local_position_is_valid());
EXPECT_TRUE(_ekf->global_position_is_valid());
EXPECT_TRUE(_ekf->isLocalHorizontalPositionValid());
EXPECT_TRUE(_ekf->isGlobalHorizontalPositionValid());
// WHEN: GPS data is not send for 11s
_sensor_simulator.stopGps();
@@ -113,8 +113,8 @@ TEST_F(EkfFusionLogicTest, doGpsFusion)
// THEN: EKF should stop to intend to fuse GPS
EXPECT_FALSE(_ekf_wrapper.isIntendingGpsFusion());
EXPECT_FALSE(_ekf->local_position_is_valid());
EXPECT_FALSE(_ekf->global_position_is_valid());
EXPECT_FALSE(_ekf->isLocalHorizontalPositionValid());
EXPECT_FALSE(_ekf->isGlobalHorizontalPositionValid());
// WHEN: GPS data is send again for 11s
_sensor_simulator.startGps();
@@ -122,8 +122,8 @@ TEST_F(EkfFusionLogicTest, doGpsFusion)
// THEN: EKF should to intend to fuse GPS
EXPECT_TRUE(_ekf_wrapper.isIntendingGpsFusion());
EXPECT_TRUE(_ekf->local_position_is_valid());
EXPECT_TRUE(_ekf->global_position_is_valid());
EXPECT_TRUE(_ekf->isLocalHorizontalPositionValid());
EXPECT_TRUE(_ekf->isGlobalHorizontalPositionValid());
// WHEN: clients decides to stop GPS fusion
_ekf_wrapper.disableGpsFusion();
@@ -230,8 +230,8 @@ TEST_F(EkfFusionLogicTest, doFlowFusion)
// THEN: EKF should not intend to fuse flow measurements
EXPECT_FALSE(_ekf_wrapper.isIntendingFlowFusion());
// THEN: Local and global position should not be valid
EXPECT_FALSE(_ekf->local_position_is_valid());
EXPECT_FALSE(_ekf->global_position_is_valid());
EXPECT_FALSE(_ekf->isLocalHorizontalPositionValid());
EXPECT_FALSE(_ekf->isGlobalHorizontalPositionValid());
// WHEN: Flow data is not send and we enable flow fusion
_sensor_simulator.stopFlow();
@@ -242,8 +242,8 @@ TEST_F(EkfFusionLogicTest, doFlowFusion)
// THEN: EKF should not intend to fuse flow
EXPECT_FALSE(_ekf_wrapper.isIntendingFlowFusion());
// THEN: Local and global position should not be valid
EXPECT_FALSE(_ekf->local_position_is_valid());
EXPECT_FALSE(_ekf->global_position_is_valid());
EXPECT_FALSE(_ekf->isLocalHorizontalPositionValid());
EXPECT_FALSE(_ekf->isGlobalHorizontalPositionValid());
// WHEN: Flow data is sent and we enable flow fusion
_sensor_simulator.startFlow();
@@ -253,8 +253,8 @@ TEST_F(EkfFusionLogicTest, doFlowFusion)
// THEN: EKF should intend to fuse flow
EXPECT_TRUE(_ekf_wrapper.isIntendingFlowFusion());
// THEN: Local and global position should be valid
EXPECT_TRUE(_ekf->local_position_is_valid());
EXPECT_FALSE(_ekf->global_position_is_valid());
EXPECT_TRUE(_ekf->isLocalHorizontalPositionValid());
EXPECT_FALSE(_ekf->isGlobalHorizontalPositionValid());
// WHEN: Stop sending flow data
_sensor_simulator.stopFlow();
@@ -263,8 +263,8 @@ TEST_F(EkfFusionLogicTest, doFlowFusion)
// THEN: EKF should not intend to fuse flow measurements
EXPECT_FALSE(_ekf_wrapper.isIntendingFlowFusion());
// THEN: Local and global position should not be valid
EXPECT_FALSE(_ekf->local_position_is_valid());
EXPECT_FALSE(_ekf->global_position_is_valid());
EXPECT_FALSE(_ekf->isLocalHorizontalPositionValid());
EXPECT_FALSE(_ekf->isGlobalHorizontalPositionValid());
}
TEST_F(EkfFusionLogicTest, doVisionPositionFusion)
@@ -279,8 +279,8 @@ TEST_F(EkfFusionLogicTest, doVisionPositionFusion)
EXPECT_TRUE(_ekf_wrapper.isIntendingExternalVisionPositionFusion());
EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionVelocityFusion());
EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionHeadingFusion());
EXPECT_TRUE(_ekf->local_position_is_valid());
EXPECT_FALSE(_ekf->global_position_is_valid());
EXPECT_TRUE(_ekf->isLocalHorizontalPositionValid());
EXPECT_FALSE(_ekf->isGlobalHorizontalPositionValid());
// WHEN: stop sending vision data
_sensor_simulator.stopExternalVision();
@@ -291,8 +291,8 @@ TEST_F(EkfFusionLogicTest, doVisionPositionFusion)
EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionPositionFusion());
EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionVelocityFusion());
EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionHeadingFusion());
EXPECT_FALSE(_ekf->local_position_is_valid());
EXPECT_FALSE(_ekf->global_position_is_valid());
EXPECT_FALSE(_ekf->isLocalHorizontalPositionValid());
EXPECT_FALSE(_ekf->isGlobalHorizontalPositionValid());
}
TEST_F(EkfFusionLogicTest, doVisionVelocityFusion)
@@ -307,8 +307,8 @@ TEST_F(EkfFusionLogicTest, doVisionVelocityFusion)
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());
EXPECT_TRUE(_ekf->isLocalHorizontalPositionValid());
EXPECT_FALSE(_ekf->isGlobalHorizontalPositionValid());
// WHEN: stop sending vision data
_sensor_simulator.stopExternalVision();
@@ -319,8 +319,8 @@ TEST_F(EkfFusionLogicTest, doVisionVelocityFusion)
EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionPositionFusion());
EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionVelocityFusion());
EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionHeadingFusion());
EXPECT_FALSE(_ekf->local_position_is_valid());
EXPECT_FALSE(_ekf->global_position_is_valid());
EXPECT_FALSE(_ekf->isLocalHorizontalPositionValid());
EXPECT_FALSE(_ekf->isGlobalHorizontalPositionValid());
}
TEST_F(EkfFusionLogicTest, doVisionHeadingFusion)
@@ -336,8 +336,8 @@ TEST_F(EkfFusionLogicTest, doVisionHeadingFusion)
EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionPositionFusion());
EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionVelocityFusion());
EXPECT_TRUE(_ekf_wrapper.isIntendingExternalVisionHeadingFusion());
EXPECT_FALSE(_ekf->local_position_is_valid());
EXPECT_FALSE(_ekf->global_position_is_valid());
EXPECT_FALSE(_ekf->isLocalHorizontalPositionValid());
EXPECT_FALSE(_ekf->isGlobalHorizontalPositionValid());
// THEN: Yaw state should be reset to vision
EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter + 1);
@@ -350,8 +350,8 @@ TEST_F(EkfFusionLogicTest, doVisionHeadingFusion)
EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionPositionFusion());
EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionVelocityFusion());
EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionHeadingFusion());
EXPECT_FALSE(_ekf->local_position_is_valid());
EXPECT_FALSE(_ekf->global_position_is_valid());
EXPECT_FALSE(_ekf->isLocalHorizontalPositionValid());
EXPECT_FALSE(_ekf->isGlobalHorizontalPositionValid());
// THEN: Yaw state shoud be reset to mag
EXPECT_TRUE(_ekf_wrapper.isIntendingMagHeadingFusion());
EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter + 2);
src/modules/ekf2/test/test_EKF_gps.cpp
+1 -1
View File
@@ -107,7 +107,7 @@ TEST_F(EkfGpsTest, gpsFixLoss)
// THEN: after dead-reconing for a couple of seconds, the local position gets invalidated
_sensor_simulator.runSeconds(6);
EXPECT_TRUE(_ekf->control_status_flags().inertial_dead_reckoning);
EXPECT_FALSE(_ekf->local_position_is_valid());
EXPECT_FALSE(_ekf->isLocalHorizontalPositionValid());
// The control logic takes a bit more time to deactivate the GNSS fusion completely
_sensor_simulator.runSeconds(5);
src/modules/ekf2/test/test_EKF_yaw_estimator.cpp
+2 -2
View File
@@ -111,6 +111,6 @@ TEST_F(EKFYawEstimatorTest, inAirYawAlignment)
EXPECT_TRUE(reset_logging_checker.isHorizontalVelocityResetCounterIncreasedBy(1));
EXPECT_TRUE(reset_logging_checker.isHorizontalPositionResetCounterIncreasedBy(1));
EXPECT_TRUE(_ekf->local_position_is_valid());
EXPECT_TRUE(_ekf->global_position_is_valid());
EXPECT_TRUE(_ekf->isLocalHorizontalPositionValid());
EXPECT_TRUE(_ekf->isGlobalHorizontalPositionValid());
}