dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

ekf: add logging for rng kinematic consistency check

This commit is contained in:
bresch 2022-03-21 15:31:43 +01:00 committed by Mathieu Bresciani
parent 8693ad15a7
commit d903613c9c
4 changed files with 5 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
msg/estimator_status_flags.msg
View File

@ -35,6 +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
# fault status
uint32 fault_status_changes # number of filter fault status (fs) changes

1
src/modules/ekf2/EKF/common.h
View File

@ -493,6 +493,7 @@ union filter_control_status_u {
uint32_t rng_fault : 1; ///< 28 - true when the range finder has been declared faulty and is no longer being used
uint32_t inertial_dead_reckoning : 1; ///< 29 - true if we are no longer fusing measurements that constrain horizontal velocity drift
uint32_t wind_dead_reckoning : 1; ///< 30 - true if we are navigationg reliant on wind relative measurements
uint32_t rng_kin_consistent : 1; ///< 31 - true when the range finder kinematic consistency check is passing
} flags;
uint32_t value;
};

2
src/modules/ekf2/EKF/control.cpp
View File

@ -142,6 +142,8 @@ void Ekf::controlFusionModes()
_range_sensor.setRange(_range_sensor.getRange() + pos_offset_earth(2) / _range_sensor.getCosTilt());
_rng_consistency_check.update(_range_sensor.getDistBottom(), getRngHeightVariance(), _state.vel(2), P(6, 6), _time_last_imu);
}
_control_status.flags.rng_kin_consistent = _rng_consistency_check.isKinematicallyConsistent();
}
if (_flow_buffer) {

1
src/modules/ekf2/EKF2.cpp
View File

@ -1304,6 +1304,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.fault_status_changes = _filter_fault_status_changes;
status_flags.fs_bad_mag_x = _ekf.fault_status_flags().bad_mag_x;