From 82e7e44cc3133f922f9da24bc7cb8b3b3a85ef2c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 18 Jul 2017 10:57:36 +1000 Subject: [PATCH] AP_GPS: fixed UAVCAN as 2nd GPS This fixes the issue here: https://discuss.ardupilot.org/t/ac3-6-dev-dual-gps-issues/19172 thanks to Francisco for spotting the issue this is tested with UAVCAN as 2nd GPS, ublox as primary --- libraries/AP_GPS/AP_GPS.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index b60f897b1c..88a8a57c5b 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -399,6 +399,11 @@ void AP_GPS::detect_instance(uint8_t instance) struct detect_state *dstate = &detect_state[instance]; uint32_t now = AP_HAL::millis(); + state[instance].instance = instance; + state[instance].status = NO_GPS; + state[instance].hdop = GPS_UNKNOWN_DOP; + state[instance].vdop = GPS_UNKNOWN_DOP; + switch (_type[instance]) { #if CONFIG_HAL_BOARD == HAL_BOARD_QURT case GPS_TYPE_QURT: @@ -456,11 +461,6 @@ void AP_GPS::detect_instance(uint8_t instance) return; } - state[instance].instance = instance; - state[instance].status = NO_GPS; - state[instance].hdop = GPS_UNKNOWN_DOP; - state[instance].vdop = GPS_UNKNOWN_DOP; - // all remaining drivers automatically cycle through baud rates to detect // the correct baud rate, and should have the selected baud broadcast dstate->auto_detected_baud = true;