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

ekf2: GNSS velocity control should own vertical velocity reset if height faiing

Browse Source 
- GNSS height control using the velocity sample directly is ignoring
   potential position offsets
This commit is contained in:
Daniel Agar
2024-07-10 13:09:33 -04:00
parent 9bbfc8715e
commit 177613eb68
2 changed files with 11 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp
-9
View File
@@ -108,15 +108,6 @@ void Ekf::controlGnssHeightFusion(const gnssSample &gps_sample)
resetVerticalPositionTo(-(measurement - bias_est.getBias()), measurement_var);
bias_est.setBias(_state.pos(2) + measurement);
// reset vertical velocity
if (PX4_ISFINITE(gps_sample.vel(2)) && (_params.gnss_ctrl & static_cast<int32_t>(GnssCtrl::VEL))) {
// use 1.5 as a typical ratio of vacc/hacc
resetVerticalVelocityTo(gps_sample.vel(2), sq(math::max(1.5f * gps_sample.sacc, _params.gps_vel_noise)));
} else {
resetVerticalVelocityToZero();
}
aid_src.time_last_fuse = _time_delayed_us;
} else if (is_fusion_failing) {
src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp
+11 -5
View File
@@ -132,17 +132,23 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
}
if (do_vel_pos_reset) {
ECL_WARN("GPS fusion timeout, resetting velocity / position");
ECL_WARN("GPS fusion timeout, resetting");
}
if (gnss_vel_enabled) {
if (gnss_vel_enabled) {
if (do_vel_pos_reset) {
resetVelocityToGnss(_aid_src_gnss_vel);
}
if (gnss_pos_enabled) {
resetHorizontalPositionToGnss(_aid_src_gnss_pos);
} else if (isHeightResetRequired()) {
// reset vertical velocity if height is failing
resetVerticalVelocityTo(_aid_src_gnss_vel.observation[2], _aid_src_gnss_vel.observation_variance[2]);
}
}
if (gnss_pos_enabled && do_vel_pos_reset) {
resetHorizontalPositionToGnss(_aid_src_gnss_pos);
}
} else {
stopGpsFusion();
}