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

ekf2: add fake height fusion logic

Browse Source 
When there is no vertical aiding, fake height is started to constrain
the vertical channel of the EKF
This commit is contained in:
bresch
2022-08-08 16:55:51 +02:00
committed by Daniel Agar
parent aa716936bf
commit a54fa7b9b1
15 changed files with 177 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF2.cpp
+2
View File
@@ -722,6 +722,7 @@ void EKF2::PublishAidSourceStatus(const hrt_abstime &timestamp)
// fake position
PublishAidSourceStatus(_ekf.aid_src_fake_pos(), _status_fake_pos_pub_last, _estimator_aid_src_fake_pos_pub);
PublishAidSourceStatus(_ekf.aid_src_fake_hgt(), _status_fake_hgt_pub_last, _estimator_aid_src_fake_hgt_pub);
// EV yaw
PublishAidSourceStatus(_ekf.aid_src_ev_yaw(), _status_ev_yaw_pub_last, _estimator_aid_src_ev_yaw_pub);
@@ -1429,6 +1430,7 @@ void EKF2::PublishStatusFlags(const hrt_abstime &timestamp)
status_flags.cs_wind_dead_reckoning = _ekf.control_status_flags().wind_dead_reckoning;
status_flags.cs_rng_kin_consistent = _ekf.control_status_flags().rng_kin_consistent;
status_flags.cs_fake_pos = _ekf.control_status_flags().fake_pos;
status_flags.cs_fake_hgt = _ekf.control_status_flags().fake_hgt;
status_flags.fault_status_changes = _filter_fault_status_changes;
status_flags.fs_bad_mag_x = _ekf.fault_status_flags().bad_mag_x;