mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-11-22 20:53:20 +08:00
ekf2-fake pos: add valid fake position fusion
This is similar to fake pos but is only used when the ekf has an external information telling it that the vehicle is not changing position. This information can then be used to keep a valid local position even when the vehicle isn't exactly at rest.
This commit is contained in:
committed by
Mathieu Bresciani
parent
64b0586dad
commit
1a0f97ebbd
@@ -642,7 +642,8 @@ uint16_t Ekf::get_ekf_soln_status() const
|
||||
#endif // CONFIG_EKF2_TERRAIN
|
||||
|
||||
// 128 ESTIMATOR_CONST_POS_MODE True if the EKF is in a constant position mode and is not using external measurements (eg GPS or optical flow)
|
||||
soln_status.flags.const_pos_mode = _control_status.flags.fake_pos || _control_status.flags.vehicle_at_rest;
|
||||
soln_status.flags.const_pos_mode = _control_status.flags.fake_pos || _control_status.flags.valid_fake_pos
|
||||
|| _control_status.flags.vehicle_at_rest;
|
||||
|
||||
// 256 ESTIMATOR_PRED_POS_HORIZ_REL True if the EKF has sufficient data to enter a mode that will provide a (relative) position estimate
|
||||
soln_status.flags.pred_pos_horiz_rel = isHorizontalAidingActive();
|
||||
@@ -793,6 +794,13 @@ void Ekf::updateHorizontalDeadReckoningstatus()
|
||||
}
|
||||
}
|
||||
|
||||
if (_control_status.flags.valid_fake_pos && isRecent(_aid_src_fake_pos.time_last_fuse, _params.no_aid_timeout_max)) {
|
||||
// only respect as a valid aiding source now if we expect to have another valid source once in air
|
||||
if (aiding_expected_in_air) {
|
||||
inertial_dead_reckoning = false;
|
||||
}
|
||||
}
|
||||
|
||||
if (inertial_dead_reckoning) {
|
||||
if (isTimedOut(_time_last_horizontal_aiding, (uint64_t)_params.valid_timeout_max)) {
|
||||
// deadreckon time exceeded
|
||||
|
||||
Reference in New Issue
Block a user