AP_NavEKF2: Don't correct GPS for zero position offset

libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp

@@ -192,28 +192,26 @@ void NavEKF2_core::SelectVelPosFusion()
         // Don't fuse velocity data if GPS doesn't support it
         if (frontend->_fusionModeGPS <= 1) {
             fuseVelData = true;
-
         } else {
             fuseVelData = false;
         }
         fusePosData = true;
 
-        // correct GPS velocity data for position offset relative to the IMU
+        // correct GPS data for position offset of antenna phase centre relative to the IMU
-        // TOTO use a filtered angular rate with a group delay that matches the GPS delay
         Vector3f posOffsetBody = gpsDataDelayed.body_offset - accelPosOffset;
-        if (fuseVelData) {
+        if (!posOffsetBody.is_zero()) {
-            Vector3f angRate = imuDataDelayed.delAng * (1.0f/imuDataDelayed.delAngDT);
+            if (fuseVelData) {
-            Vector3f velOffsetBody = angRate % posOffsetBody;
+                // TODO use a filtered angular rate with a group delay that matches the GPS delay
-            Vector3f velOffsetEarth = prevTnb.mul_transpose(velOffsetBody);
+                Vector3f angRate = imuDataDelayed.delAng * (1.0f/imuDataDelayed.delAngDT);
-            gpsDataDelayed.vel -= velOffsetEarth;
+                Vector3f velOffsetBody = angRate % posOffsetBody;
+                Vector3f velOffsetEarth = prevTnb.mul_transpose(velOffsetBody);
+                gpsDataDelayed.vel -= velOffsetEarth;
+            }
+            Vector3f posOffsetEarth = prevTnb.mul_transpose(posOffsetBody);
+            gpsDataDelayed.pos.x -= posOffsetEarth.x;
+            gpsDataDelayed.pos.y -= posOffsetEarth.y;
+            gpsDataDelayed.hgt += posOffsetEarth.z;
         }
-
-        // correct GPS position data for position offset relative to the IMU
-        Vector3f posOffsetEarth = prevTnb.mul_transpose(posOffsetBody);
-        gpsDataDelayed.pos.x -= posOffsetEarth.x;
-        gpsDataDelayed.pos.y -= posOffsetEarth.y;
-        gpsDataDelayed.hgt += posOffsetEarth.z;
-
     } else {
         fuseVelData = false;
         fusePosData = false;