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

convert range kin consistency into kin valid

Browse Source
This commit is contained in:
Marco Hauswirth
2024-05-30 15:42:34 +02:00
parent 42f4e02d7e
commit b7a1aba598
4 changed files with 6 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
msg/EstimatorStatusFlags.msg
+1 -1
View File
@@ -35,7 +35,7 @@ bool cs_gps_yaw_fault # 27 - true when the GNSS heading has been declare
bool cs_rng_fault # 28 - true when the range finder has been declared faulty and is no longer being used
bool cs_inertial_dead_reckoning # 29 - true if we are no longer fusing measurements that constrain horizontal velocity drift
bool cs_wind_dead_reckoning # 30 - true if we are navigationg reliant on wind relative measurements
bool cs_rng_kin_consistent # 31 - true when the range finder kinematic consistency check is passing
bool cs_rng_kin_valid # 31 - true when the range finder data is healthy and the kinematic consistency check is passing
bool cs_fake_pos # 32 - true when fake position measurements are being fused
bool cs_fake_hgt # 33 - true when fake height measurements are being fused
bool cs_gravity_vector # 34 - true when gravity vector measurements are being fused
src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp
+3 -1
View File
@@ -86,7 +86,9 @@ void Ekf::controlRangeHeightFusion()
}
}
_control_status.flags.rng_kin_consistent = _rng_consistency_check.isKinematicallyConsistent();
if (_range_sensor.isDataReady()) {
_control_status.flags.rng_kin_valid = _rng_consistency_check.isKinematicallyConsistent() && _range_sensor.isHealthy();
}
} else {
return;
src/modules/ekf2/EKF/common.h
+1 -1
View File
@@ -578,7 +578,7 @@ union filter_control_status_u {
uint64_t rng_fault : 1; ///< 28 - true when the range finder has been declared faulty and is no longer being used
uint64_t inertial_dead_reckoning : 1; ///< 29 - true if we are no longer fusing measurements that constrain horizontal velocity drift
uint64_t wind_dead_reckoning : 1; ///< 30 - true if we are navigationg reliant on wind relative measurements
uint64_t rng_kin_consistent : 1; ///< 31 - true when the range finder kinematic consistency check is passing
uint64_t rng_kin_valid : 1; ///< 31 - true when the range finder data is healthy and the kinematic consistency check is passing
uint64_t fake_pos : 1; ///< 32 - true when fake position measurements are being fused
uint64_t fake_hgt : 1; ///< 33 - true when fake height measurements are being fused
uint64_t gravity_vector : 1; ///< 34 - true when gravity vector measurements are being fused
src/modules/ekf2/EKF2.cpp
+1 -1
View File
@@ -1908,7 +1908,7 @@ void EKF2::PublishStatusFlags(const hrt_abstime &timestamp)
status_flags.cs_rng_fault = _ekf.control_status_flags().rng_fault;
status_flags.cs_inertial_dead_reckoning = _ekf.control_status_flags().inertial_dead_reckoning;
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_rng_kin_valid = _ekf.control_status_flags().rng_kin_valid;
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;