diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index fc6274cd93..54c37e0e81 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -501,12 +501,12 @@ void AP_GPS::detect_instance(uint8_t instance) _port[instance]->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE); dstate->last_baud_change_ms = now; - if (_auto_config == GPS_AUTO_CONFIG_ENABLE) { + if (_auto_config == GPS_AUTO_CONFIG_ENABLE && new_gps == nullptr) { send_blob_start(instance, _initialisation_blob, sizeof(_initialisation_blob)); } } - if (_auto_config == GPS_AUTO_CONFIG_ENABLE) { + if (_auto_config == GPS_AUTO_CONFIG_ENABLE && new_gps == nullptr) { send_blob_update(instance); } diff --git a/libraries/AP_GPS/AP_GPS_SBF.cpp b/libraries/AP_GPS/AP_GPS_SBF.cpp index 702a18b6d3..76e5be8a7f 100644 --- a/libraries/AP_GPS/AP_GPS_SBF.cpp +++ b/libraries/AP_GPS/AP_GPS_SBF.cpp @@ -55,7 +55,8 @@ AP_GPS_SBF::AP_GPS_SBF(AP_GPS &_gps, AP_GPS::GPS_State &_state, { sbf_msg.sbf_state = sbf_msg_parser_t::PREAMBLE1; - port->write((const uint8_t*)_initialisation_blob[0], strlen(_initialisation_blob[0])); + port->write((const uint8_t*)_port_enable, strlen(_port_enable)); + _config_last_ack_time = AP_HAL::millis(); } // Process all bytes available from the stream @@ -70,10 +71,16 @@ AP_GPS_SBF::read(void) const char *init_str = _initialisation_blob[_init_blob_index]; if (now > _init_blob_time) { - Debug("SBF sending init blob: %s\n", init_str); - port->write((const uint8_t*)init_str, strlen(init_str)); - // if this is too low a race condition on start occurs and the GPS isn't detected - _init_blob_time = now + 2000; + if (now > _config_last_ack_time + 2500) { + // try to enable input on the GPS port if we have not made progress on configuring it + Debug("SBF Sending port enable"); + port->write((const uint8_t*)_port_enable, strlen(_port_enable)); + _config_last_ack_time = now; + } else { + Debug("SBF sending init string: %s", init_str); + port->write((const uint8_t*)init_str, strlen(init_str)); + } + _init_blob_time = now + 1000; } } @@ -194,6 +201,7 @@ AP_GPS_SBF::parse(uint8_t temp) sbf_msg.read - SBF_EXCESS_COMMAND_BYTES)) { Debug("SBF Ack Command: %s\n", sbf_msg.data.bytes); _init_blob_index++; + _config_last_ack_time = AP_HAL::millis(); } else { Debug("SBF Ack command (unexpected): %s\n", sbf_msg.data.bytes); } diff --git a/libraries/AP_GPS/AP_GPS_SBF.h b/libraries/AP_GPS/AP_GPS_SBF.h index 9665e54ed9..0126c0782b 100644 --- a/libraries/AP_GPS/AP_GPS_SBF.h +++ b/libraries/AP_GPS/AP_GPS_SBF.h @@ -65,6 +65,9 @@ private: "sem, PVT, 5\n", "spm, Rover, all\n", "sso, Stream2, Dsk1, postprocess+event, msec100\n"}; + uint32_t _config_last_ack_time; + + const char* _port_enable = "\nSSSSSSSSSS\n"; uint32_t crc_error_counter = 0; uint32_t last_injected_data_ms = 0;