Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-23 22:58:10 +08:00
Code Issues Actions 15 Packages Projects Releases Wiki Activity

ekf2: use estimator_aid_src for all yaw sources (mag, gnss, ev)

Browse Source
This commit is contained in:
Daniel Agar
2022-07-20 21:37:00 -04:00
parent d5d88cba5b
commit a397c09e59
16 changed files with 334 additions and 155 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF2.cpp
+10 -4
View File
@@ -650,10 +650,19 @@ 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);
// GNSS velocity & position
// EV yaw
PublishAidSourceStatus(_ekf.aid_src_ev_yaw(), _status_ev_yaw_pub_last, _estimator_aid_src_ev_yaw_pub);
// GNSS yaw/velocity/position
PublishAidSourceStatus(_ekf.aid_src_gnss_yaw(), _status_gnss_yaw_pub_last, _estimator_aid_src_gnss_yaw_pub);
PublishAidSourceStatus(_ekf.aid_src_gnss_vel(), _status_gnss_vel_pub_last, _estimator_aid_src_gnss_vel_pub);
PublishAidSourceStatus(_ekf.aid_src_gnss_pos(), _status_gnss_pos_pub_last, _estimator_aid_src_gnss_pos_pub);
// mag heading
PublishAidSourceStatus(_ekf.aid_src_mag_heading(), _status_mag_heading_pub_last, _estimator_aid_src_mag_heading_pub);
// mag 3d
PublishAidSourceStatus(_ekf.aid_src_mag(), _status_mag_pub_last, _estimator_aid_src_mag_pub);
}
void EKF2::PublishAttitude(const hrt_abstime &timestamp)
@@ -1368,9 +1377,6 @@ void EKF2::PublishStatusFlags(const hrt_abstime &timestamp)
status_flags.reject_ver_vel = _ekf.innov_check_fail_status_flags().reject_ver_vel;
status_flags.reject_hor_pos = _ekf.innov_check_fail_status_flags().reject_hor_pos;
status_flags.reject_ver_pos = _ekf.innov_check_fail_status_flags().reject_ver_pos;
status_flags.reject_mag_x = _ekf.innov_check_fail_status_flags().reject_mag_x;
status_flags.reject_mag_y = _ekf.innov_check_fail_status_flags().reject_mag_y;
status_flags.reject_mag_z = _ekf.innov_check_fail_status_flags().reject_mag_z;
status_flags.reject_yaw = _ekf.innov_check_fail_status_flags().reject_yaw;
status_flags.reject_airspeed = _ekf.innov_check_fail_status_flags().reject_airspeed;
status_flags.reject_sideslip = _ekf.innov_check_fail_status_flags().reject_sideslip;