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

ekf2: gps control use adjusted velocity and position for reset

Browse Source
This commit is contained in:
Daniel Agar
2023-03-01 10:53:48 -05:00
parent 95754876ed
commit 3fac85369e
1 changed files with 8 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/gps_control.cpp
+8 -2
View File
@@ -176,10 +176,16 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
if (do_vel_pos_reset) {
ECL_WARN("GPS fusion timeout, resetting velocity and position");
// reset velocity
_information_events.flags.reset_vel_to_gps = true;
resetVelocityTo(velocity, vel_obs_var);
_aid_src_gnss_vel.time_last_fuse = _time_delayed_us;
// reset position
_information_events.flags.reset_pos_to_gps = true;
resetVelocityTo(gps_sample.vel, vel_obs_var);
resetHorizontalPositionTo(gps_sample.pos, pos_obs_var);
resetHorizontalPositionTo(position, pos_obs_var);
_aid_src_gnss_pos.time_last_fuse = _time_delayed_us;
}
} else {