From b51c9dea6cc260d830653a422b2e4127bc808341 Mon Sep 17 00:00:00 2001
From: priseborough <p_riseborough@live.com.au>
Date: Sat, 8 Oct 2016 09:04:14 +1100
Subject: [PATCH] AP_GPS: Add parameters defining antenna offset in body frame

---
 libraries/AP_GPS/AP_GPS.cpp | 38 +++++++++++++++++++++++++++++++++++++
 libraries/AP_GPS/AP_GPS.h   | 11 ++++++++++-
 2 files changed, 48 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp
index 99d4100fcb..55dd083b2b 100644
--- a/libraries/AP_GPS/AP_GPS.cpp
+++ b/libraries/AP_GPS/AP_GPS.cpp
@@ -153,6 +153,44 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = {
     // @User: Advanced
     AP_GROUPINFO("RATE_MS2", 15, AP_GPS, _rate_ms[1], 200),
 
+    // @Param: POS1_X
+    // @DisplayName: Antenna X position offset
+    // @Description: X position of the first GPS antenna in body frame. Use antenna phase centroid location if provided by the manufacturer.
+    // @Units: m
+    // @User: Advanced
+
+    // @Param: POS1_Y
+    // @DisplayName: Antenna Y position offset
+    // @Description: Y position of the first GPS antenna in body frame. Use antenna phase centroid location if provided by the manufacturer.
+    // @Units: m
+    // @User: Advanced
+
+    // @Param: POS1_Z
+    // @DisplayName: Antenna Z position offset
+    // @Description: Z position of the first GPS antenna in body frame. Use antenna phase centroid location if provided by the manufacturer.
+    // @Units: m
+    // @User: Advanced
+    AP_GROUPINFO("POS1", 16, AP_GPS, _antenna_offset[0], 0.0f),
+
+    // @Param: POS2_X
+    // @DisplayName: Antenna X position offset
+    // @Description: X position of the second GPS antenna in body frame. Use antenna phase centroid location if provided by the manufacturer.
+    // @Units: m
+    // @User: Advanced
+
+    // @Param: POS2_Y
+    // @DisplayName: Antenna Y position offset
+    // @Description: Y position of the second GPS antenna in body frame. Use antenna phase centroid location if provided by the manufacturer.
+    // @Units: m
+    // @User: Advanced
+
+    // @Param: POS2_Z
+    // @DisplayName: Antenna Z position offset
+    // @Description: Z position of the second GPS antenna in body frame. Use antenna phase centroid location if provided by the manufacturer.
+    // @Units: m
+    // @User: Advanced
+    AP_GROUPINFO("POS2", 17, AP_GPS, _antenna_offset[1], 0.0f),
+
     AP_GROUPEND
 };
 
diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h
index fd05c9dcaf..3381840297 100644
--- a/libraries/AP_GPS/AP_GPS.h
+++ b/libraries/AP_GPS/AP_GPS.h
@@ -312,6 +312,14 @@ public:
     // the expected lag (in seconds) in the position and velocity readings from the gps
     float get_lag() const { return 0.2f; }
 
+    // return a 3D vector defining the offset of the GPS antenna in metres relative to the body frame origin
+    const Vector3f get_antenna_offset(uint8_t instance) const {
+        return _antenna_offset[instance];
+    }
+    const Vector3f get_antenna_offset(void) const {
+        return _antenna_offset[primary_instance];
+    }
+
     // set position for HIL
     void setHIL(uint8_t instance, GPS_Status status, uint64_t time_epoch_ms, 
                 const Location &location, const Vector3f &velocity, uint8_t num_sats,
@@ -340,7 +348,8 @@ public:
     AP_Int16 _rate_ms[2];
     AP_Int8 _save_config;
     AP_Int8 _auto_config;
-    
+    AP_Vector3f _antenna_offset[2];
+
     // handle sending of initialisation strings to the GPS
     void send_blob_start(uint8_t instance, const char *_blob, uint16_t size);
     void send_blob_update(uint8_t instance);