fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 05:04:08 +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-14 19:25:36 +11:00 committed by Lorenz Meier
parent 460c9e5250
commit a776b2c549
1 changed files with 7 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
EKF/control.cpp
View File

@ -329,21 +329,21 @@ void Ekf::controlGpsFusion()
// If the heading is valid start using gps aiding
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
// otherwise we only need to reset the position
_control_status.flags.gps = true;
if (!_control_status.flags.opt_flow) {
if (resetPosition() && resetVelocity()) {
_control_status.flags.gps = true;
} else {
if (!resetPosition() || !resetVelocity()) {
_control_status.flags.gps = false;
}
} else if (!resetPosition()) {
_control_status.flags.gps = false;
}
if (_control_status.flags.gps) {
ECL_INFO("EKF commencing GPS aiding");
_time_last_gps = _time_last_imu;
}
}