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

GNSS yaw unit test: test fallback to non yaw aiding mode

Browse Source
This commit is contained in:
bresch
2021-05-11 17:38:41 +02:00
committed by Paul Riseborough
parent a4fe2b1e72
commit f7c687e433
3 changed files with 17 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/sensor_simulator/ekf_wrapper.cpp
+5
View File
@@ -165,6 +165,11 @@ bool EkfWrapper::isIntendingMag3DFusion() const
return _ekf->control_status_flags().mag_3D; return _ekf->control_status_flags().mag_3D;
} }
void EkfWrapper::setMagFuseTypeNone()
{
_ekf_params->mag_fusion_type = MAG_FUSE_TYPE_NONE;
}
bool EkfWrapper::isWindVelocityEstimated() const bool EkfWrapper::isWindVelocityEstimated() const
{ {
return _ekf->control_status_flags().wind; return _ekf->control_status_flags().wind;
test/sensor_simulator/ekf_wrapper.h
+1
View File
@@ -87,6 +87,7 @@ public:
bool isIntendingMagHeadingFusion() const; bool isIntendingMagHeadingFusion() const;
bool isIntendingMag3DFusion() const; bool isIntendingMag3DFusion() const;
void setMagFuseTypeNone();
void enableExternalVisionAlignment(); void enableExternalVisionAlignment();
void disableExternalVisionAlignment(); void disableExternalVisionAlignment();
test/test_EKF_gps_yaw.cpp
+11
View File
@@ -257,4 +257,15 @@ TEST_F(EkfGpsHeadingTest, stop_on_ground)
// should stop as well because the yaw is not aligned anymore // should stop as well because the yaw is not aligned anymore
EXPECT_FALSE(_ekf_wrapper.isIntendingGpsHeadingFusion()); EXPECT_FALSE(_ekf_wrapper.isIntendingGpsHeadingFusion());
EXPECT_FALSE(_ekf_wrapper.isIntendingGpsFusion()); EXPECT_FALSE(_ekf_wrapper.isIntendingGpsFusion());
// AND IF: the mag fusion type is set to NONE
_ekf_wrapper.setMagFuseTypeNone();
// WHEN: running without yaw aiding
const matrix::Vector<float, 4> quat_variance_before = _ekf_wrapper.getQuaternionVariance();
_sensor_simulator.runSeconds(20.0);
const matrix::Vector<float, 4> quat_variance_after = _ekf_wrapper.getQuaternionVariance();
// THEN: the yaw variance is constrained by fusing constant data
EXPECT_LT(quat_variance_after(3), quat_variance_before(3));
} }