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

EKF: fix initialization of local position validity 2 (#820)

Browse Source 
The `_deadreckon_time_exceeded` flag is used in
`local_position_is_valid()`. This means that
`_params.valid_timeout_max` after startup, in my observed case 5
seconds, the local position switche from valid to invalid and then after
a while back to valid again.

With this fix, the local position is flagged invalid from boot and gets validated after the first aiding event.

Co-authored-by: Julian Oes <julian@oes.ch>
This commit is contained in:
kritz
2020-05-20 08:24:23 +02:00
committed by GitHub
parent 716caa5168
commit bf78044bcd
2 changed files with 5 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/ekf_helper.cpp
+2 -2
View File
@@ -1215,8 +1215,8 @@ void Ekf::update_deadreckoning_status()
_time_last_aiding = _time_last_imu - _params.no_aid_timeout_max; _time_last_aiding = _time_last_imu - _params.no_aid_timeout_max;
} }
// report if we have been deadreckoning for too long // report if we have been deadreckoning for too long, initial state is deadreckoning until aiding is present
_deadreckon_time_exceeded = isTimedOut(_time_last_aiding, (uint64_t)_params.valid_timeout_max); _deadreckon_time_exceeded = (_time_last_aiding == 0) || isTimedOut(_time_last_aiding, (uint64_t)_params.valid_timeout_max);
} }
// calculate the inverse rotation matrix from a quaternion rotation // calculate the inverse rotation matrix from a quaternion rotation
test/test_EKF_fusionLogic.cpp
+3 -5
View File
@@ -71,11 +71,9 @@ class EkfFusionLogicTest : public ::testing::Test {
TEST_F(EkfFusionLogicTest, doNoFusion) TEST_F(EkfFusionLogicTest, doNoFusion)
{ {
// GIVEN: a tilt and heading aligned filter // GIVEN: a tilt and heading aligned filter
// WHEN: having no aiding source EKF should not have a valid position estimate // WHEN: having no aiding source
// THEN: EKF should not have a valid position estimate
// TODO: for the first 5 second it still has some valid local position EXPECT_FALSE(_ekf->local_position_is_valid());
// that needs to change
EXPECT_TRUE(_ekf->local_position_is_valid());
_sensor_simulator.runSeconds(4); _sensor_simulator.runSeconds(4);