Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-03 04:03:59 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_GPS: SBF send port enable string on start

Browse Source 
Also prohibts SBF, GSOF, NOVA drivers from getting config strings not
meant for them
This commit is contained in:
Michael du Breuil 2017-09-21 19:49:41 -07:00 committed by Francisco Ferreira
parent 4f83d39b5e
commit 561acb372e
3 changed files with 18 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_GPS/AP_GPS.cpp
View File

@ -501,12 +501,12 @@ void AP_GPS::detect_instance(uint8_t instance)
_port[instance]->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE); _port[instance]->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE);
dstate->last_baud_change_ms = now; 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)); 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); send_blob_update(instance);
} }

18
libraries/AP_GPS/AP_GPS_SBF.cpp
View File

@ -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; 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 // 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]; const char *init_str = _initialisation_blob[_init_blob_index];
if (now > _init_blob_time) { if (now > _init_blob_time) {
Debug("SBF sending init blob: %s\n", init_str); if (now > _config_last_ack_time + 2500) {
port->write((const uint8_t*)init_str, strlen(init_str)); // try to enable input on the GPS port if we have not made progress on configuring it
// if this is too low a race condition on start occurs and the GPS isn't detected Debug("SBF Sending port enable");
_init_blob_time = now + 2000; 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)) { sbf_msg.read - SBF_EXCESS_COMMAND_BYTES)) {
Debug("SBF Ack Command: %s\n", sbf_msg.data.bytes); Debug("SBF Ack Command: %s\n", sbf_msg.data.bytes);
_init_blob_index++; _init_blob_index++;
_config_last_ack_time = AP_HAL::millis();
} else { } else {
Debug("SBF Ack command (unexpected): %s\n", sbf_msg.data.bytes); Debug("SBF Ack command (unexpected): %s\n", sbf_msg.data.bytes);
} }

3
libraries/AP_GPS/AP_GPS_SBF.h
View File

@ -65,6 +65,9 @@ private:
"sem, PVT, 5\n", "sem, PVT, 5\n",
"spm, Rover, all\n", "spm, Rover, all\n",
"sso, Stream2, Dsk1, postprocess+event, msec100\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 crc_error_counter = 0;
uint32_t last_injected_data_ms = 0; uint32_t last_injected_data_ms = 0;