fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-17 02:10:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: new gravity observation (#21038)

Browse Source 
Signed-off-by: Daniel M. Sahu <danielmohansahu@gmail.com>
This commit is contained in:
Daniel Sahu
2023-02-07 13:28:58 -05:00
committed by GitHub
parent 3e149ee6c5
commit fa6fda6cce
15 changed files with 456 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF2.cpp
+8
View File
@@ -131,6 +131,7 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
_param_ekf2_eva_noise(_params->ev_att_noise),
_param_ekf2_evv_gate(_params->ev_vel_innov_gate),
_param_ekf2_evp_gate(_params->ev_pos_innov_gate),
_param_ekf2_grav_noise(_params->gravity_noise),
_param_ekf2_of_n_min(_params->flow_noise),
_param_ekf2_of_n_max(_params->flow_noise_qual_min),
_param_ekf2_of_qmin(_params->flow_qual_min),
@@ -804,6 +805,9 @@ void EKF2::PublishAidSourceStatus(const hrt_abstime &timestamp)
// mag 3d
PublishAidSourceStatus(_ekf.aid_src_mag(), _status_mag_pub_last, _estimator_aid_src_mag_pub);
// gravity
PublishAidSourceStatus(_ekf.aid_src_gravity(), _status_gravity_pub_last, _estimator_aid_src_gravity_pub);
// aux velocity
PublishAidSourceStatus(_ekf.aid_src_aux_vel(), _status_aux_vel_pub_last, _estimator_aid_src_aux_vel_pub);
@@ -1098,6 +1102,7 @@ void EKF2::PublishInnovations(const hrt_abstime &timestamp)
_ekf.getHaglInnov(innovations.hagl);
_ekf.getHaglRateInnov(innovations.hagl_rate);
_ekf.getTerrainFlowInnov(innovations.terr_flow);
_ekf.getGravityInnov(innovations.gravity);
// Not yet supported
innovations.aux_vvel = NAN;
@@ -1148,6 +1153,7 @@ void EKF2::PublishInnovationTestRatios(const hrt_abstime &timestamp)
_ekf.getHaglInnovRatio(test_ratios.hagl);
_ekf.getHaglRateInnovRatio(test_ratios.hagl_rate);
_ekf.getTerrainFlowInnovRatio(test_ratios.terr_flow[0]);
_ekf.getGravityInnovRatio(test_ratios.gravity[0]);
// Not yet supported
test_ratios.aux_vvel = NAN;
@@ -1174,6 +1180,7 @@ void EKF2::PublishInnovationVariances(const hrt_abstime &timestamp)
_ekf.getHaglInnovVar(variances.hagl);
_ekf.getHaglRateInnovVar(variances.hagl_rate);
_ekf.getTerrainFlowInnovVar(variances.terr_flow);
_ekf.getGravityInnovVar(variances.gravity);
// Not yet supported
variances.aux_vvel = NAN;
@@ -1507,6 +1514,7 @@ void EKF2::PublishStatusFlags(const hrt_abstime &timestamp)
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.cs_gravity_vector = _ekf.control_status_flags().gravity_vector;
status_flags.fault_status_changes = _filter_fault_status_changes;
status_flags.fs_bad_mag_x = _ekf.fault_status_flags().bad_mag_x;