AP_NavEKF2: Add GPS antenna position offset data to data buffer

libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp

@@ -400,6 +400,9 @@ void NavEKF2_core::readGpsData()
             // Prevent time delay exceeding age of oldest IMU data in the buffer
             gpsDataNew.time_ms = MAX(gpsDataNew.time_ms,imuDataDelayed.time_ms);
 
+            // Get the GPS position offset in body frame
+            gpsDataNew.body_offset = _ahrs->get_gps().get_antenna_offset();
+
             // read the NED velocity from the GPS
             gpsDataNew.vel = _ahrs->get_gps().velocity();
 
@@ -478,6 +481,7 @@ void NavEKF2_core::readGpsData()
             }
 
             // convert GPS measurements to local NED and save to buffer to be fused later if we have a valid origin
+            // correct for position offset of antenna in body frame
             if (validOrigin) {
                 gpsDataNew.pos = location_diff(EKF_origin, gpsloc);
                 gpsDataNew.hgt = 0.01f * (gpsloc.alt - EKF_origin.alt);

libraries/AP_NavEKF2/AP_NavEKF2_core.h

@@ -374,6 +374,7 @@ private:
         float       hgt;         // 2
         Vector3f    vel;         // 3..5
         uint32_t    time_ms;     // 6
+        Vector3f    body_offset; // 7..9
     };
 
     struct mag_elements {