fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-22 07:37:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF2: gps_yaw_reset_timeout_max

Browse Source 
Signed-off-by: Silvan <silvan@auterion.com>
This commit is contained in:
Silvan
2025-11-12 18:13:26 +01:00
committed by Claudio Chies
parent 6901bc6a01
commit 2bafb76334
3 changed files with 4 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/aid_sources/gnss/gnss_yaw_control.cpp
+2 -2
View File
@@ -75,7 +75,7 @@ void Ekf::controlGnssYawFusion(const gnssSample &gnss_sample)
fuseGnssYaw(gnss_sample.yaw_offset);
const bool is_fusion_failing = isTimedOut(_aid_src_gnss_yaw.time_last_fuse, _params.reset_timeout_max);
const bool is_fusion_failing = isTimedOut(_aid_src_gnss_yaw.time_last_fuse, _params.reset_timeout_gps_yaw_max);
if (is_fusion_failing) {
stopGnssYawFusion();
@@ -123,7 +123,7 @@ void Ekf::controlGnssYawFusion(const gnssSample &gnss_sample)
}
} else if (_control_status.flags.gnss_yaw
&& !isNewestSampleRecent(_time_last_gnss_yaw_buffer_push, _params.reset_timeout_max)) {
&& !isNewestSampleRecent(_time_last_gnss_yaw_buffer_push, _params.reset_timeout_gps_yaw_max)) {
// No yaw data in the message anymore. Stop until it comes back.
stopGnssYawFusion();
src/modules/ekf2/EKF/common.h
+1
View File
@@ -480,6 +480,7 @@ struct parameters {
const unsigned reset_timeout_max{7'000'000}; ///< maximum time we allow horizontal inertial dead reckoning before attempting to reset the states to the measurement or change _control_status if the data is unavailable (uSec)
const unsigned no_aid_timeout_max{1'000'000}; ///< maximum lapsed time from last fusion of a measurement that constrains horizontal velocity drift before the EKF will determine that the sensor is no longer contributing to aiding (uSec)
const unsigned hgt_fusion_timeout_max{5'000'000}; ///< maximum time we allow height fusion to fail before attempting a reset or stopping the fusion aiding (uSec)
const unsigned reset_timeout_gps_yaw_max{3'000'000}; ///< maximum time we allow without GPS yaw
int32_t ekf2_noaid_tout{5'000'000}; ///< amount of time spent inertial dead reckoning before the estimator reports the state estimates as invalid (uSec)
src/modules/ekf2/test/test_EKF_gnss_yaw.cpp
+1 -1
View File
@@ -273,7 +273,7 @@ TEST_F(EkfGpsHeadingTest, yawJmpOnGround)
const int initial_quat_reset_counter = _ekf_wrapper.getQuaternionResetCounter();
gps_heading = matrix::wrap_pi(_ekf_wrapper.getYawAngle() + math::radians(45.f));
_sensor_simulator._gps.setYaw(gps_heading);
_sensor_simulator.runSeconds(8);
_sensor_simulator.runSeconds(4);
// THEN: the fusion should stop, reset to mag
EXPECT_FALSE(_ekf_wrapper.isIntendingGpsHeadingFusion());