diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index 31d2af635e..7209fdfe94 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -203,7 +203,7 @@ AP_GPS::detect_instance(uint8_t instance) dstate->detect_started_ms = now; } - if (now - dstate->last_baud_change_ms > 1200) { + if (now - dstate->last_baud_change_ms > GPS_BAUD_TIME_MS) { // try the next baud rate dstate->last_baud++; if (dstate->last_baud == ARRAY_SIZE(_baudrates)) { @@ -218,7 +218,8 @@ AP_GPS::detect_instance(uint8_t instance) send_blob_update(instance); - while (_port[instance]->available() > 0 && new_gps == NULL) { + while (initblob_state[instance].remaining == 0 && _port[instance]->available() > 0 + && new_gps == NULL) { uint8_t data = _port[instance]->read(); /* running a uBlox at less than 38400 will lead to packet @@ -257,7 +258,7 @@ AP_GPS::detect_instance(uint8_t instance) hal.console->print_P(PSTR(" SIRF ")); new_gps = new AP_GPS_SIRF(*this, state[instance], _port[instance]); } - else if (now - dstate->detect_started_ms > 5000) { + else if (now - dstate->detect_started_ms > (ARRAY_SIZE(_baudrates) * GPS_BAUD_TIME_MS)) { // prevent false detection of NMEA mode in // a MTK or UBLOX which has booted in NMEA mode if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_NMEA) && diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h index 3f6b39c3ca..7dc7fa9bf2 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/libraries/AP_GPS/AP_GPS.h @@ -412,6 +412,8 @@ private: void update_instance(uint8_t instance); }; +#define GPS_BAUD_TIME_MS 1200 + #include #include #include diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.h b/libraries/AP_GPS/AP_GPS_UBLOX.h index f26e1305d8..8b8bb6c745 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.h +++ b/libraries/AP_GPS/AP_GPS_UBLOX.h @@ -56,6 +56,8 @@ public: // Methods bool read(); + AP_GPS::GPS_Status highest_supported_status(void) { return AP_GPS::GPS_OK_FIX_3D_DGPS; } + static bool _detect(struct UBLOX_detect_state &state, uint8_t data); private: