From e0c2a45eec133be7ccf32c9289ebfd800642ba51 Mon Sep 17 00:00:00 2001
From: "Dr.-Ing. Amilcar Do Carmo Lucas" <amilcar.lucas@iav.de>
Date: Wed, 5 Jul 2017 18:53:55 +0200
Subject: [PATCH] AP_GPS: support GPS_RTK and GPS2_RTK messages in Emlid reach
 modules. Requires Reach FW >= 2.7.4

---
 libraries/AP_GPS/AP_GPS_ERB.cpp | 17 +++++++++++++++++
 libraries/AP_GPS/AP_GPS_ERB.h   | 12 ++++++++++++
 2 files changed, 29 insertions(+)

diff --git a/libraries/AP_GPS/AP_GPS_ERB.cpp b/libraries/AP_GPS/AP_GPS_ERB.cpp
index 90fca59c44..ab789c432b 100644
--- a/libraries/AP_GPS/AP_GPS_ERB.cpp
+++ b/libraries/AP_GPS/AP_GPS_ERB.cpp
@@ -209,6 +209,23 @@ AP_GPS_ERB::_parse_gps(void)
         state.speed_accuracy = _buffer.vel.speed_accuracy * 0.01f;
         _new_speed = true;
         break;
+    case MSG_RTK:
+        Debug("Message RTK");
+        state.rtk_baseline_coords_type = RTK_BASELINE_COORDINATE_SYSTEM_NED;
+        state.rtk_num_sats      = _buffer.rtk.base_num_sats;
+        if (_buffer.rtk.age_cs == 0xFFFF) {
+            state.rtk_age_ms    = 0xFFFFFFFF;
+        } else {
+            state.rtk_age_ms    = _buffer.rtk.age_cs * 10;
+        }
+        state.rtk_baseline_x_mm = _buffer.rtk.baseline_N_mm;
+        state.rtk_baseline_y_mm = _buffer.rtk.baseline_E_mm;
+        state.rtk_baseline_z_mm = _buffer.rtk.baseline_D_mm;
+        state.rtk_accuracy      = _buffer.rtk.ar_ratio;
+
+        state.rtk_week_number   = _buffer.rtk.base_week_number;
+        state.rtk_time_week_ms  = _buffer.rtk.base_time_week_ms;
+        break;
     default:
         Debug("Unexpected message 0x%02x", (unsigned)_msg_id);
         return false;
diff --git a/libraries/AP_GPS/AP_GPS_ERB.h b/libraries/AP_GPS/AP_GPS_ERB.h
index 121a59f85c..c5895f4599 100644
--- a/libraries/AP_GPS/AP_GPS_ERB.h
+++ b/libraries/AP_GPS/AP_GPS_ERB.h
@@ -83,6 +83,16 @@ private:
         int32_t heading_2d; ///< Heading of motion 2-D [1e5 deg]
         uint32_t speed_accuracy; ///< Speed accuracy Estimate [cm/s]
     };
+    struct PACKED erb_rtk {
+        uint8_t base_num_sats;       ///< Current number of satellites used for RTK calculation
+        uint16_t age_cs;             ///< Age of the corrections in centiseconds (0 when no corrections, 0xFFFF indicates overflow)
+        int32_t baseline_N_mm;       ///< distance between base and rover along the north axis in millimeters
+        int32_t baseline_E_mm;       ///< distance between base and rover along the east axis in millimeters
+        int32_t baseline_D_mm;       ///< distance between base and rover along the down axis in millimeters
+        uint16_t ar_ratio;           ///< AR ratio multiplied by 10
+        uint16_t base_week_number;   ///< GPS Week Number of last baseline
+        uint32_t base_time_week_ms;  ///< GPS Time of Week of last baseline in milliseconds
+    };
 
     // Receive buffer
     union PACKED {
@@ -92,6 +102,7 @@ private:
         erb_stat stat;
         erb_dops dops;
         erb_vel vel;
+        erb_rtk rtk;
     } _buffer;
 
     enum erb_protocol_bytes {
@@ -102,6 +113,7 @@ private:
         MSG_STAT = 0x03,
         MSG_DOPS = 0x04,
         MSG_VEL = 0x05,
+        MSG_RTK = 0x07,
     };
 
     enum erb_fix_type {