From b296bc1d003a5a4bacfab08fc44ce1b7fdae2381 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 8 Dec 2016 09:37:03 +0900 Subject: [PATCH] GPS: MAV driver fix for sanity checks of cog, sat count --- libraries/AP_GPS/AP_GPS_MAV.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS_MAV.cpp b/libraries/AP_GPS/AP_GPS_MAV.cpp index 9d9a703ae2..e49a26bc8e 100644 --- a/libraries/AP_GPS/AP_GPS_MAV.cpp +++ b/libraries/AP_GPS/AP_GPS_MAV.cpp @@ -130,13 +130,15 @@ void AP_GPS_MAV::handle_msg(const mavlink_message_t *msg) } Vector3f vel(packet.vn/100.0f, packet.ve/100.0f, packet.vd/100.0f); state.velocity = vel; - if (packet.cog < 65535) { + if (packet.cog < 36000) { state.ground_course = packet.cog / 100.0f; } state.have_speed_accuracy = 0; state.have_horizontal_accuracy = 0; state.have_vertical_accuracy = 0; - state.num_sats = packet.satellites_visible; + if (packet.satellites_visible < 255) { + state.num_sats = packet.satellites_visible; + } state.last_gps_time_ms = AP_HAL::millis(); _new_data = true; break;