From 109e0e6dfcfe012fe06db301e316f2ed470baf0c Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 10 Mar 2016 15:14:50 +1100 Subject: [PATCH] EKF: When commencing GPS aiding, don't reset local position and velocity if using optical flow --- EKF/control.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index dc180d1aca..e2583f5af3 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -121,12 +121,15 @@ void Ekf::controlFusionModes() _control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag); } - // If the heading is valid, reset the positon and velocity and start using gps aiding + // If the heading is valid start using gps aiding if (_control_status.flags.yaw_align) { - resetPosition(); - resetVelocity(); _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 (!_control_status.flags.opt_flow) { + _control_status.flags.gps = resetPosition(); + _control_status.flags.gps = resetVelocity(); + } } }