From b791fef7cbed2022a5282aef1fe7636e1adf6cd9 Mon Sep 17 00:00:00 2001 From: "Dr.-Ing. Amilcar Do Carmo Lucas" Date: Wed, 5 Jul 2017 18:19:19 +0200 Subject: [PATCH] AP_GPS: Publish the number of satellites in the RTK correction messages, and the age of the RTK correction messages --- libraries/AP_GPS/AP_GPS_NOVA.cpp | 2 ++ libraries/AP_GPS/AP_GPS_SBF.cpp | 3 ++- 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/libraries/AP_GPS/AP_GPS_NOVA.cpp b/libraries/AP_GPS/AP_GPS_NOVA.cpp index 225bdb1f47..33d6dae3bf 100644 --- a/libraries/AP_GPS/AP_GPS_NOVA.cpp +++ b/libraries/AP_GPS/AP_GPS_NOVA.cpp @@ -206,6 +206,8 @@ AP_GPS_NOVA::process_message(void) state.vertical_accuracy = (float) bestposu.hgtsdev; state.have_horizontal_accuracy = true; state.have_vertical_accuracy = true; + state.rtk_age_ms = bestposu.diffage * 1000; + state.rtk_num_sats = bestposu.svsused; if (bestposu.solstat == 0) // have a solution { diff --git a/libraries/AP_GPS/AP_GPS_SBF.cpp b/libraries/AP_GPS/AP_GPS_SBF.cpp index 4369d40997..8f35a054e9 100644 --- a/libraries/AP_GPS/AP_GPS_SBF.cpp +++ b/libraries/AP_GPS/AP_GPS_SBF.cpp @@ -223,7 +223,8 @@ AP_GPS_SBF::process_message(void) state.ground_speed = (float)safe_sqrt(ground_vector_sq); state.ground_course = wrap_360(degrees(atan2f(state.velocity[1], state.velocity[0]))); - + state.rtk_age_ms = temp.MeanCorrAge * 10; + // value is expressed as twice the rms error = int16 * 0.01/2 state.horizontal_accuracy = (float)temp.HAccuracy * 0.005f; state.vertical_accuracy = (float)temp.VAccuracy * 0.005f;