fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 23:43:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: estimator aid source status (baro height)

Browse Source
This commit is contained in:
Daniel Agar
2022-05-25 09:25:12 -04:00
parent 4dbe6f0a1c
commit 1e25aee6fa
13 changed files with 218 additions and 156 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/control.cpp
+20 -10
View File
@@ -779,16 +779,16 @@ void Ekf::checkVerticalAccelerationHealth()
bool are_vertical_pos_and_vel_independant = false;
if (_control_status.flags.gps) {
// velocity
auto &gps_vel = _aid_src_gnss_vel;
// GNSS velocity
const auto &gps_vel = _aid_src_gnss_vel;
if (gps_vel.time_last_fuse[2] > _vert_vel_fuse_time_us) {
_vert_vel_fuse_time_us = gps_vel.time_last_fuse[2];
_vert_vel_innov_ratio = gps_vel.innovation[2] / sqrtf(gps_vel.innovation_variance[2]);
}
// position
auto &gps_pos = _aid_src_gnss_pos;
// GNSS position
const auto &gps_pos = _aid_src_gnss_pos;
if (gps_pos.time_last_fuse[2] > _vert_pos_fuse_attempt_time_us) {
_vert_pos_fuse_attempt_time_us = gps_pos.time_last_fuse[2];
@@ -796,6 +796,16 @@ void Ekf::checkVerticalAccelerationHealth()
}
}
if (_control_status.flags.baro_hgt) {
// baro height
const auto &baro_hgt = _aid_src_baro_hgt;
if (baro_hgt.time_last_fuse > _vert_pos_fuse_attempt_time_us) {
_vert_pos_fuse_attempt_time_us = baro_hgt.time_last_fuse;
_vert_pos_innov_ratio = baro_hgt.innovation / sqrtf(baro_hgt.innovation_variance);
}
}
if (isRecent(_vert_pos_fuse_attempt_time_us, 1000000)) {
if (isRecent(_vert_vel_fuse_time_us, 1000000)) {
// If vertical position and velocity come from independent sensors then we can
@@ -941,15 +951,15 @@ void Ekf::controlHeightFusion()
updateBaroHgtOffset();
updateGroundEffect();
if (_control_status.flags.baro_hgt) {
if (_baro_data_ready) {
updateBaroHgt(_baro_sample_delayed, _aid_src_baro_hgt);
if (_baro_data_ready && !_baro_hgt_faulty) {
fuseBaroHgt();
if (_control_status.flags.baro_hgt && !_baro_hgt_faulty) {
fuseBaroHgt(_aid_src_baro_hgt);
}
}
} else if (_control_status.flags.gps_hgt) {
// handled by gps fusion
} else if (_control_status.flags.rng_hgt) {
if (_control_status.flags.rng_hgt) {
if (_range_sensor.isDataHealthy()) {
fuseRngHgt();