diff --git a/libraries/AP_GPS/AP_GPS_SBF.cpp b/libraries/AP_GPS/AP_GPS_SBF.cpp index 72155b9cab..8479a7fcdb 100644 --- a/libraries/AP_GPS/AP_GPS_SBF.cpp +++ b/libraries/AP_GPS/AP_GPS_SBF.cpp @@ -53,16 +53,21 @@ AP_GPS_SBF::AP_GPS_SBF(AP_GPS &_gps, AP_GPS::GPS_State &_state, bool AP_GPS_SBF::read(void) { - uint32_t now = AP_HAL::millis(); - - if (_init_blob_index < (sizeof(_initialisation_blob) / sizeof(_initialisation_blob[0]))) { - if (now > _init_blob_time) { - port->write((const uint8_t*)_initialisation_blob[_init_blob_index], strlen(_initialisation_blob[_init_blob_index])); - _init_blob_time = now + 70; - _init_blob_index++; - } - } - + uint32_t now = AP_HAL::millis(); + + if (_init_blob_index < (sizeof(_initialisation_blob) / sizeof(_initialisation_blob[0]))) { + if (validcommand) { + _init_blob_index++; + validcommand = false; + _init_blob_time = 0; + } + + if (now > _init_blob_time) { + port->write((const uint8_t*)_initialisation_blob[_init_blob_index], strlen(_initialisation_blob[_init_blob_index])); + _init_blob_time = now + 1000; + } + } + bool ret = false; while (port->available() > 0) { uint8_t temp = port->read(); @@ -82,11 +87,17 @@ AP_GPS_SBF::parse(uint8_t temp) if (temp == SBF_PREAMBLE1) { sbf_msg.sbf_state = sbf_msg_parser_t::PREAMBLE2; sbf_msg.read = 0; + } else if (temp == '$') { + // this is a command response + sbf_msg.sbf_state = sbf_msg_parser_t::PREAMBLE2; } break; case sbf_msg_parser_t::PREAMBLE2: if (temp == SBF_PREAMBLE2) { sbf_msg.sbf_state = sbf_msg_parser_t::CRC1; + } else if (temp == 'R') { + validcommand = true; + sbf_msg.sbf_state = sbf_msg_parser_t::PREAMBLE1; } else { diff --git a/libraries/AP_GPS/AP_GPS_SBF.h b/libraries/AP_GPS/AP_GPS_SBF.h index 32463d7aaf..8a30759edb 100644 --- a/libraries/AP_GPS/AP_GPS_SBF.h +++ b/libraries/AP_GPS/AP_GPS_SBF.h @@ -45,18 +45,20 @@ private: static const uint8_t SBF_PREAMBLE1 = '$'; static const uint8_t SBF_PREAMBLE2 = '@'; - - uint8_t _init_blob_index = 0; - uint32_t _init_blob_time = 0; - const char* _initialisation_blob[4] = { - "sso, Stream1, COM1, PVTGeodetic+DOP+ExtEventPVTGeodetic, msec100\n", - "srd, Moderate, UAV\n", - "sem, PVT, 5\n", - "spm, Rover, StandAlone+DGPS+RTK\n"}; + + uint8_t _init_blob_index = 0; + uint32_t _init_blob_time = 0; + const char* _initialisation_blob[5] = { + "sso, Stream1, COM1, PVTGeodetic+DOP+ExtEventPVTGeodetic, msec100\n", + "srd, Moderate, UAV\n", + "sem, PVT, 5\n", + "spm, Rover, StandAlone+SBAS+DGPS+RTK\n", + "sso, Stream2, Dsk1, Rinex+Event+RawData, msec100\n"}; uint32_t last_hdop = 999; uint32_t crc_error_counter = 0; - uint32_t last_injected_data_ms = 0; + uint32_t last_injected_data_ms = 0; + bool validcommand = false; struct PACKED msg4007 {