From e0c2a45eec133be7ccf32c9289ebfd800642ba51 Mon Sep 17 00:00:00 2001 From: "Dr.-Ing. Amilcar Do Carmo Lucas" 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 {