From f08a4af7510e02f8b3aa3710da3b5fef04031196 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Fri, 6 Nov 2015 17:35:12 +1100 Subject: [PATCH] AP_NavEKF2: Remove redundant if statement Thank you to OXINARF for picking up this one --- libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp | 13 +++++-------- 1 file changed, 5 insertions(+), 8 deletions(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp index b583de891d..d8fbeb2d97 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp @@ -437,14 +437,11 @@ void NavEKF2_core::readGpsData() // Set the EKF origin and magnetic field declination if not previously set and GPS checks have passed if (gpsGoodToAlign && !validOrigin) { - // Set the NE origin to the current GPS position if not previously set - if (!validOrigin) { - setOrigin(); - // Now we know the location we have an estimate for the magnetic field declination and adjust the earth field accordingly - alignMagStateDeclination(); - // Set the height of the NED origin to ‘height of baro height datum relative to GPS height datum' - EKF_origin.alt = gpsloc.alt - baroDataNew.hgt; - } + setOrigin(); + // Now we know the location we have an estimate for the magnetic field declination and adjust the earth field accordingly + alignMagStateDeclination(); + // Set the height of the NED origin to ‘height of baro height datum relative to GPS height datum' + EKF_origin.alt = gpsloc.alt - baroDataNew.hgt; } // convert GPS measurements to local NED and save to buffer to be fused later if we have a valid origin