diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index e2e0d623d8..e8b7c9b04e 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -553,6 +553,32 @@ void AP_GPS::send_blob_start(uint8_t instance, const char *_blob, uint16_t size) initblob_state[instance].remaining = size; } +/* + initialise state for sending binary blob initialisation strings + */ +void AP_GPS::send_blob_start(uint8_t instance) +{ + if (_type[instance] == GPS_TYPE_UBLOX && (_driver_options & AP_GPS_Backend::DriverOptions::UBX_Use115200)) { + static const char blob[] = UBLOX_SET_BINARY_115200; + send_blob_start(instance, blob, sizeof(blob)); + } else if ((_type[instance] == GPS_TYPE_UBLOX_RTK_BASE || + _type[instance] == GPS_TYPE_UBLOX_RTK_ROVER) && + ((_driver_options.get() & AP_GPS_Backend::DriverOptions::UBX_MBUseUart2) == 0)) { + // we use 460800 when doing moving baseline as we need + // more bandwidth. We don't do this if using UART2, as + // in that case the RTCMv3 data doesn't go over the + // link to the flight controller + static const char blob[] = UBLOX_SET_BINARY_460800; + send_blob_start(instance, blob, sizeof(blob)); +#if AP_GPS_NMEA_ENABLED + } else if (_type[instance] == GPS_TYPE_HEMI) { + send_blob_start(instance, AP_GPS_NMEA_HEMISPHERE_INIT_STRING, strlen(AP_GPS_NMEA_HEMISPHERE_INIT_STRING)); +#endif // AP_GPS_NMEA_ENABLED + } else { + send_blob_start(instance, _initialisation_blob, sizeof(_initialisation_blob)); + } +} + /* send some more initialisation string bytes if there is room in the UART transmit buffer @@ -684,25 +710,7 @@ void AP_GPS::detect_instance(uint8_t instance) dstate->last_baud_change_ms = now; if (_auto_config >= GPS_AUTO_CONFIG_ENABLE_SERIAL_ONLY && new_gps == nullptr) { - if (_type[instance] == GPS_TYPE_UBLOX && (_driver_options & AP_GPS_Backend::DriverOptions::UBX_Use115200)) { - static const char blob[] = UBLOX_SET_BINARY_115200; - send_blob_start(instance, blob, sizeof(blob)); - } else if ((_type[instance] == GPS_TYPE_UBLOX_RTK_BASE || - _type[instance] == GPS_TYPE_UBLOX_RTK_ROVER) && - ((_driver_options.get() & AP_GPS_Backend::DriverOptions::UBX_MBUseUart2) == 0)) { - // we use 460800 when doing moving baseline as we need - // more bandwidth. We don't do this if using UART2, as - // in that case the RTCMv3 data doesn't go over the - // link to the flight controller - static const char blob[] = UBLOX_SET_BINARY_460800; - send_blob_start(instance, blob, sizeof(blob)); -#if AP_GPS_NMEA_ENABLED - } else if (_type[instance] == GPS_TYPE_HEMI) { - send_blob_start(instance, AP_GPS_NMEA_HEMISPHERE_INIT_STRING, strlen(AP_GPS_NMEA_HEMISPHERE_INIT_STRING)); -#endif // AP_GPS_NMEA_ENABLED - } else { - send_blob_start(instance, _initialisation_blob, sizeof(_initialisation_blob)); - } + send_blob_start(instance); } } diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h index 65c37b2a68..cf73c3985d 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/libraries/AP_GPS/AP_GPS.h @@ -473,6 +473,7 @@ public: bool blend_health_check() const; // handle sending of initialisation strings to the GPS - only used by backends + void send_blob_start(uint8_t instance); void send_blob_start(uint8_t instance, const char *_blob, uint16_t size); void send_blob_update(uint8_t instance);