Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_GPS: Force the buadrate on SBF units

This commit is contained in:
Michael du Breuil 2021-02-11 16:14:37 -07:00 committed by WickedShell
parent 01eb64ce73
commit 4d42996068
2 changed files with 64 additions and 40 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

85
libraries/AP_GPS/AP_GPS_SBF.cpp
View File

@ -71,7 +71,7 @@ AP_GPS_SBF::AP_GPS_SBF(AP_GPS &_gps, AP_GPS::GPS_State &_state,
} }
AP_GPS_SBF::~AP_GPS_SBF (void) { AP_GPS_SBF::~AP_GPS_SBF (void) {
free(_initial_sso); free(config_string);
} }
// Process all bytes available from the stream // Process all bytes available from the stream
@ -87,7 +87,7 @@ AP_GPS_SBF::read(void)
} }
if (gps._auto_config != AP_GPS::GPS_AUTO_CONFIG_DISABLE) { if (gps._auto_config != AP_GPS::GPS_AUTO_CONFIG_DISABLE) {
if (_init_blob_index < ARRAY_SIZE(_initialisation_blob)) { if (config_step != Config_State::Complete) {
uint32_t now = AP_HAL::millis(); uint32_t now = AP_HAL::millis();
if (now > _init_blob_time) { if (now > _init_blob_time) {
if (now > _config_last_ack_time + 2000) { if (now > _config_last_ack_time + 2000) {
@ -96,24 +96,38 @@ AP_GPS_SBF::read(void)
port->write((const uint8_t*)_port_enable, strlen(_port_enable)); port->write((const uint8_t*)_port_enable, strlen(_port_enable));
_config_last_ack_time = now; _config_last_ack_time = now;
} else if (readyForCommand) { } else if (readyForCommand) {
char *init_str = nullptr; if (config_string == nullptr) {
if (!_validated_initial_sso) { switch (config_step) {
if (_initial_sso == nullptr) { case Config_State::Baud_Rate:
if (asprintf(&_initial_sso, "sso, Stream%d, COM%d%s", if (asprintf(&config_string, "scs,COM%d,baud%d,bits8,No,bit1,%s\n",
(int)GPS_SBF_STREAM_NUMBER, (int)gps._com_port[state.instance],
(int)gps._com_port[state.instance], 230400,
_sso_normal) == -1) { port->get_flow_control() != AP_HAL::UARTDriver::flow_control::FLOW_CONTROL_ENABLE ? "none" : "RTS|CTS") == -1) {
_initial_sso = nullptr; config_string = nullptr;
} }
break;
case Config_State::SSO:
if (asprintf(&config_string, "sso,Stream%d,COM%d,PVTGeodetic+DOP+ReceiverStatus+VelCovGeodetic+BaseVectorGeod,msec100\n",
(int)GPS_SBF_STREAM_NUMBER,
(int)gps._com_port[state.instance]) == -1) {
config_string = nullptr;
}
break;
case Config_State::Blob:
if (asprintf(&config_string,"%s\n", (char *)_initialisation_blob[_init_blob_index]) == -1) {
config_string = nullptr;
}
break;
case Config_State::Complete:
// should never reach here, why search for a config if we have fully configured already
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
break;
} }
init_str = _initial_sso;
} else {
init_str = (char *)_initialisation_blob[_init_blob_index];
} }
if (init_str != nullptr) { if (config_string != nullptr) {
Debug("SBF sending init string: %s", init_str); Debug("SBF sending init string: %s", config_string);
port->write((const uint8_t*)init_str, strlen(init_str)); port->write((const uint8_t*)config_string, strlen(config_string));
readyForCommand = false; readyForCommand = false;
} }
} }
@ -277,24 +291,29 @@ AP_GPS_SBF::parse(uint8_t temp)
if (sbf_msg.data.bytes[0] == ':') { if (sbf_msg.data.bytes[0] == ':') {
// valid command, determine if it was the one we were trying // valid command, determine if it was the one we were trying
// to send in the configuration sequence // to send in the configuration sequence
const char * reference_blob = nullptr; if (config_string != nullptr) {
if (!_validated_initial_sso) { if (!strncmp(config_string, (char *)(sbf_msg.data.bytes + 2),
reference_blob = _initial_sso;
} else {
if (_init_blob_index < ARRAY_SIZE(_initialisation_blob)) {
reference_blob = _initialisation_blob[_init_blob_index];
}
}
if (reference_blob != nullptr) {
if (!strncmp(reference_blob, (char *)(sbf_msg.data.bytes + 2),
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);
if (!_validated_initial_sso) { free(config_string);
free(_initial_sso); config_string = nullptr;
_initial_sso = nullptr; switch (config_step) {
_validated_initial_sso = true; case Config_State::Baud_Rate:
} else { config_step = Config_State::SSO;
_init_blob_index++; break;
case Config_State::SSO:
config_step = Config_State::Blob;
break;
case Config_State::Blob:
_init_blob_index++;
if (_init_blob_index >= ARRAY_SIZE(_initialisation_blob)) {
config_step = Config_State::Complete;
}
break;
case Config_State::Complete:
// should never reach here, this implies that we validated a config string when we hadn't sent any
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
break;
} }
_config_last_ack_time = AP_HAL::millis(); _config_last_ack_time = AP_HAL::millis();
} else { } else {

19
libraries/AP_GPS/AP_GPS_SBF.h
View File

@ -63,16 +63,21 @@ private:
static const uint8_t SBF_PREAMBLE1 = '$'; static const uint8_t SBF_PREAMBLE1 = '$';
static const uint8_t SBF_PREAMBLE2 = '@'; static const uint8_t SBF_PREAMBLE2 = '@';
bool _validated_initial_sso;
uint8_t _init_blob_index; uint8_t _init_blob_index;
uint32_t _init_blob_time; uint32_t _init_blob_time;
char *_initial_sso; enum class Config_State {
const char* _sso_normal = ", PVTGeodetic+DOP+ReceiverStatus+VelCovGeodetic+BaseVectorGeod, msec100\n"; Baud_Rate,
SSO,
Blob,
Complete
};
Config_State config_step;
char *config_string;
static constexpr const char* _initialisation_blob[] = { static constexpr const char* _initialisation_blob[] = {
"srd, Moderate, UAV\n", "srd,Moderate,UAV",
"sem, PVT, 5\n", "sem,PVT,5",
"spm, Rover, all\n", "spm,Rover,all",
"sso, Stream2, Dsk1, postprocess+event+comment+ReceiverStatus, msec100\n", "sso,Stream2,Dsk1,postprocess+event+comment+ReceiverStatus,msec100",
#if defined (GPS_SBF_EXTRA_CONFIG) #if defined (GPS_SBF_EXTRA_CONFIG)
GPS_SBF_EXTRA_CONFIG GPS_SBF_EXTRA_CONFIG
#endif #endif