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

EKF: Fix bug preventing reset to GPS position when using optical flow

Browse Source
This commit is contained in:
Paul Riseborough
2016-11-26 10:28:58 +01:00
committed by Lorenz Meier
parent 460c9e5250
commit a776b2c549
1 changed files with 7 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+7 -7
View File
@@ -329,21 +329,21 @@ void Ekf::controlGpsFusion()
// If the heading is valid start using gps aiding // If the heading is valid start using gps aiding
if (_control_status.flags.yaw_align) { if (_control_status.flags.yaw_align) {
_control_status.flags.gps = true;
_time_last_gps = _time_last_imu;
// if we are not already aiding with optical flow, then we need to reset the position and velocity // if we are not already aiding with optical flow, then we need to reset the position and velocity
// otherwise we only need to reset the position
_control_status.flags.gps = true;
if (!_control_status.flags.opt_flow) { if (!_control_status.flags.opt_flow) {
if (resetPosition() && resetVelocity()) { if (!resetPosition() || !resetVelocity()) {
_control_status.flags.gps = true;
} else {
_control_status.flags.gps = false; _control_status.flags.gps = false;
} }
} else if (!resetPosition()) {
_control_status.flags.gps = false;
} }
if (_control_status.flags.gps) { if (_control_status.flags.gps) {
ECL_INFO("EKF commencing GPS aiding"); ECL_INFO("EKF commencing GPS aiding");
_time_last_gps = _time_last_imu;
} }
} }