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:
bresch
2024-08-23 11:17:21 +02:00
committed by Mathieu Bresciani
parent 64b0586dad
commit 1a0f97ebbd
13 changed files with 215 additions and 60 deletions
+9 -1
View File
@@ -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