From bf6778277ea06e335d6bb7eee7ccb5881fd9213c Mon Sep 17 00:00:00 2001 From: "Dr.-Ing. Amilcar Do Carmo Lucas" Date: Wed, 19 Jul 2017 14:12:04 +0200 Subject: [PATCH] AP_GPS: implement GPS*_RTK mavlink messages --- libraries/AP_GPS/AP_GPS.h | 8 ++++++ libraries/AP_GPS/GPS_Backend.cpp | 42 ++++++++++++++++++++++++++++++++ 2 files changed, 50 insertions(+) diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h index b021a6312c..3cc0eed732 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/libraries/AP_GPS/AP_GPS.h @@ -149,8 +149,16 @@ public: uint32_t last_gps_time_ms; ///< the system time we got the last GPS timestamp, milliseconds // all the following fields must only all be filled by RTK capable backend drivers + uint32_t rtk_time_week_ms; ///< GPS Time of Week of last baseline in milliseconds + uint16_t rtk_week_number; ///< GPS Week Number of last baseline uint32_t rtk_age_ms; ///< GPS age of last baseline correction in milliseconds (0 when no corrections, 0xFFFFFFFF indicates overflow) uint8_t rtk_num_sats; ///< Current number of satellites used for RTK calculation + uint8_t rtk_baseline_coords_type; ///< Coordinate system of baseline. 0 == ECEF, 1 == NED + int32_t rtk_baseline_x_mm; ///< Current baseline in ECEF x or NED north component in mm + int32_t rtk_baseline_y_mm; ///< Current baseline in ECEF y or NED east component in mm + int32_t rtk_baseline_z_mm; ///< Current baseline in ECEF z or NED down component in mm + uint32_t rtk_accuracy; ///< Current estimate of 3D baseline accuracy (receiver dependent, typical 0 to 9999) + int32_t rtk_iar_num_hypotheses; ///< Current number of integer ambiguity hypotheses }; /// Startup initialisation. diff --git a/libraries/AP_GPS/GPS_Backend.cpp b/libraries/AP_GPS/GPS_Backend.cpp index 733428ad72..5742915a2e 100644 --- a/libraries/AP_GPS/GPS_Backend.cpp +++ b/libraries/AP_GPS/GPS_Backend.cpp @@ -184,3 +184,45 @@ bool AP_GPS_Backend::should_df_log() const } return true; } + + +void AP_GPS_Backend::send_mavlink_gps_rtk(mavlink_channel_t chan) +{ + const uint8_t instance = state.instance; + // send status + switch (instance) { + case 0: + mavlink_msg_gps_rtk_send(chan, + 0, // Not implemented yet + 0, // Not implemented yet + state.rtk_week_number, + state.rtk_time_week_ms, + 0, // Not implemented yet + 0, // Not implemented yet + state.rtk_num_sats, + state.rtk_baseline_coords_type, + state.rtk_baseline_x_mm, + state.rtk_baseline_y_mm, + state.rtk_baseline_z_mm, + state.rtk_accuracy, + state.rtk_iar_num_hypotheses); + break; + case 1: + mavlink_msg_gps2_rtk_send(chan, + 0, // Not implemented yet + 0, // Not implemented yet + state.rtk_week_number, + state.rtk_time_week_ms, + 0, // Not implemented yet + 0, // Not implemented yet + state.rtk_num_sats, + state.rtk_baseline_coords_type, + state.rtk_baseline_x_mm, + state.rtk_baseline_y_mm, + state.rtk_baseline_z_mm, + state.rtk_accuracy, + state.rtk_iar_num_hypotheses); + break; + } +} +