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

ekf2: move estimator_status test ratios to filtered values

Browse Source
This commit is contained in:
Daniel Agar
2024-06-17 18:09:48 -04:00
parent e4fc3022f2
commit dcb1103299
9 changed files with 142 additions and 84 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF2.cpp
+7 -9
View File
@@ -1802,15 +1802,13 @@ void EKF2::PublishStatus(const hrt_abstime &timestamp)
status.control_mode_flags = _ekf.control_status().value;
status.filter_fault_flags = _ekf.fault_status().value;
uint16_t innov_check_flags_temp = 0;
_ekf.get_innovation_test_status(innov_check_flags_temp, status.mag_test_ratio,
status.vel_test_ratio, status.pos_test_ratio,
status.hgt_test_ratio, status.tas_test_ratio,
status.hagl_test_ratio, status.beta_test_ratio);
// Bit mismatch between ecl and Firmware, combine the 2 first bits to preserve msg definition
// TODO: legacy use only, those flags are also in estimator_status_flags
status.innovation_check_flags = (innov_check_flags_temp >> 1) | (innov_check_flags_temp & 0x1);
status.mag_test_ratio = _ekf.getHeadingInnovationTestRatio();
status.vel_test_ratio = _ekf.getVelocityInnovationTestRatio();
status.pos_test_ratio = _ekf.getHorizontalPositionInnovationTestRatio();
status.hgt_test_ratio = _ekf.getVerticalPositionInnovationTestRatio();
status.tas_test_ratio = _ekf.getAirspeedInnovationTestRatio();
status.hagl_test_ratio = _ekf.getHeightAboveGroundInnovationTestRatio();
status.beta_test_ratio = _ekf.getSyntheticSideslipInnovationTestRatio();
_ekf.get_ekf_lpos_accuracy(&status.pos_horiz_accuracy, &status.pos_vert_accuracy);
_ekf.get_ekf_soln_status(&status.solution_status_flags);