diff --git a/test/sensor_simulator/ekf_wrapper.cpp b/test/sensor_simulator/ekf_wrapper.cpp index afeabb0191..ed8890eb0a 100644 --- a/test/sensor_simulator/ekf_wrapper.cpp +++ b/test/sensor_simulator/ekf_wrapper.cpp @@ -165,6 +165,11 @@ bool EkfWrapper::isIntendingMag3DFusion() const return _ekf->control_status_flags().mag_3D; } +void EkfWrapper::setMagFuseTypeNone() +{ + _ekf_params->mag_fusion_type = MAG_FUSE_TYPE_NONE; +} + bool EkfWrapper::isWindVelocityEstimated() const { return _ekf->control_status_flags().wind; diff --git a/test/sensor_simulator/ekf_wrapper.h b/test/sensor_simulator/ekf_wrapper.h index 75a8b97837..c7ef60ec4d 100644 --- a/test/sensor_simulator/ekf_wrapper.h +++ b/test/sensor_simulator/ekf_wrapper.h @@ -87,6 +87,7 @@ public: bool isIntendingMagHeadingFusion() const; bool isIntendingMag3DFusion() const; + void setMagFuseTypeNone(); void enableExternalVisionAlignment(); void disableExternalVisionAlignment(); diff --git a/test/test_EKF_gps_yaw.cpp b/test/test_EKF_gps_yaw.cpp index 4cbe4021e6..ecf6acf91a 100644 --- a/test/test_EKF_gps_yaw.cpp +++ b/test/test_EKF_gps_yaw.cpp @@ -257,4 +257,15 @@ TEST_F(EkfGpsHeadingTest, stop_on_ground) // should stop as well because the yaw is not aligned anymore EXPECT_FALSE(_ekf_wrapper.isIntendingGpsHeadingFusion()); 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 quat_variance_before = _ekf_wrapper.getQuaternionVariance(); + _sensor_simulator.runSeconds(20.0); + const matrix::Vector 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)); }