AP_GPS: support GPS_RTK and GPS2_RTK messages in Emlid reach modules.…

… Requires ERB version > 0.2.0 (not released yet)
ArduPilot · Aug 21, 2017 · 03c47be · 03c47be
1 parent 022ae07
commit 03c47be
Show file tree

Hide file tree

Showing 2 changed files with 30 additions and 0 deletions.
diff --git a/libraries/AP_GPS/AP_GPS_ERB.cpp b/libraries/AP_GPS/AP_GPS_ERB.cpp
@@ -189,6 +189,25 @@ AP_GPS_ERB::_parse_gps(void)
             state.time_week_ms    = _buffer.stat.time;
             state.time_week       = _buffer.stat.week;
         }
+        if (   _buffer.ver.ver_high > 0
+           ||  _buffer.ver.ver_medium >= 2) {
+            state.rtk_baseline_coords_type = 1;
+            state.rtk_num_sats      = _buffer.stat.base_num_sats;
+            if (_buffer.stat.age_cs == 0xFFFF) {
+                state.rtk_age_ms    = 0xFFFFFFFF;
+            } else {
+                state.rtk_age_ms    = _buffer.stat.age_cs * 10;
+            }
+            state.rtk_baseline_x_mm = _buffer.stat.baseline_N_mm;
+            state.rtk_baseline_y_mm = _buffer.stat.baseline_E_mm;
+            state.rtk_baseline_z_mm = _buffer.stat.baseline_D_mm;
+            state.rtk_accuracy      = _buffer.stat.ar_ratio;
+
+            state.rtk_week_number   = _buffer.stat.week;
+            state.rtk_time_week_ms  = _buffer.stat.time;
+            state.rtk_iar_num_hypotheses = _buffer.stat.iar_num_hypotheses;
+            // state.num_leap_seconds = _buffer.stat.num_leap_seconds; // leap seconds (0x7F indicates invalid)
+        }
         break;
     case MSG_DOPS:
         Debug("Message DOPS");

diff --git a/libraries/AP_GPS/AP_GPS_ERB.h b/libraries/AP_GPS/AP_GPS_ERB.h
@@ -66,6 +66,17 @@ class AP_GPS_ERB : public AP_GPS_Backend
         uint8_t fix_type;   ///< see erb_fix_type enum
         uint8_t fix_status;
         uint8_t satellites;
+        // Introduced in ERB version 0.2.0
+        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
+        int32_t iar_num_hypotheses;  ///< Current number of integer ambiguity hypotheses.
+        int8_t num_leap_seconds;     ///< leap seconds (0x7F indicates invalid)
     };
     struct PACKED erb_dops {
         uint32_t time;      ///< GPS time of week of the navigation epoch [ms]