From 5f505a0f37ad58d9cef9ad66afaffb9017ae1ff7 Mon Sep 17 00:00:00 2001 From: Michael Oborne Date: Sat, 12 Nov 2016 10:48:33 +0800 Subject: [PATCH] AP_GPS_SBF: fix accuracy reported by driver --- libraries/AP_GPS/AP_GPS_SBF.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS_SBF.cpp b/libraries/AP_GPS/AP_GPS_SBF.cpp index cbd2e631b1..0be401cada 100644 --- a/libraries/AP_GPS/AP_GPS_SBF.cpp +++ b/libraries/AP_GPS/AP_GPS_SBF.cpp @@ -226,9 +226,10 @@ 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.horizontal_accuracy = (float)temp.HAccuracy * 0.01f; - state.vertical_accuracy = (float)temp.VAccuracy * 0.01f; + + // 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; state.have_horizontal_accuracy = true; state.have_vertical_accuracy = true; }