forked from Archive/PX4-Autopilot
EKF: fix initialization of local position validity 2 (#820)
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:
parent
716caa5168
commit
bf78044bcd
|
@ -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
|
||||||
|
|
|
@ -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);
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue