zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_GPS: Force the buadrate on SBF units

c415-sdk
Michael du Breuil 4 years ago committed by WickedShell
parent
01eb64ce73
commit
4d42996068
2 changed files with 64 additions and 40 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 85
      libraries/AP_GPS/AP_GPS_SBF.cpp
  2. 19
      libraries/AP_GPS/AP_GPS_SBF.h

85
libraries/AP_GPS/AP_GPS_SBF.cpp
Unescape View File

@ -71,7 +71,7 @@ AP_GPS_SBF::AP_GPS_SBF(AP_GPS &_gps, AP_GPS::GPS_State &_state, @@ -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) {
free(_initial_sso);
free(config_string);
}
// Process all bytes available from the stream
@ -87,7 +87,7 @@ AP_GPS_SBF::read(void) @@ -87,7 +87,7 @@ AP_GPS_SBF::read(void)
}
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();
if (now > _init_blob_time) {
if (now > _config_last_ack_time + 2000) {
@ -96,24 +96,38 @@ AP_GPS_SBF::read(void) @@ -96,24 +96,38 @@ AP_GPS_SBF::read(void)
port->write((const uint8_t*)_port_enable, strlen(_port_enable));
_config_last_ack_time = now;
} else if (readyForCommand) {
char *init_str = nullptr;
if (!_validated_initial_sso) {
if (_initial_sso == nullptr) {
if (asprintf(&_initial_sso, "sso, Stream%d, COM%d%s",
(int)GPS_SBF_STREAM_NUMBER,
(int)gps._com_port[state.instance],
_sso_normal) == -1) {
_initial_sso = nullptr;
}
if (config_string == nullptr) {
switch (config_step) {
case Config_State::Baud_Rate:
if (asprintf(&config_string, "scs,COM%d,baud%d,bits8,No,bit1,%s\n",
(int)gps._com_port[state.instance],
230400,
port->get_flow_control() != AP_HAL::UARTDriver::flow_control::FLOW_CONTROL_ENABLE ? "none" : "RTS|CTS") == -1) {
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) {
Debug("SBF sending init string: %s", init_str);
port->write((const uint8_t*)init_str, strlen(init_str));
if (config_string != nullptr) {
Debug("SBF sending init string: %s", config_string);
port->write((const uint8_t*)config_string, strlen(config_string));
readyForCommand = false;
}
}
@ -277,24 +291,29 @@ AP_GPS_SBF::parse(uint8_t temp) @@ -277,24 +291,29 @@ AP_GPS_SBF::parse(uint8_t temp)
if (sbf_msg.data.bytes[0] == ':') {
// valid command, determine if it was the one we were trying
// to send in the configuration sequence
const char * reference_blob = nullptr;
if (!_validated_initial_sso) {
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),
if (config_string != nullptr) {
if (!strncmp(config_string, (char *)(sbf_msg.data.bytes + 2),
sbf_msg.read - SBF_EXCESS_COMMAND_BYTES)) {
Debug("SBF Ack Command: %s\n", sbf_msg.data.bytes);
if (!_validated_initial_sso) {
free(_initial_sso);
_initial_sso = nullptr;
_validated_initial_sso = true;
} else {
_init_blob_index++;
free(config_string);
config_string = nullptr;
switch (config_step) {
case Config_State::Baud_Rate:
config_step = Config_State::SSO;
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();
} else {

19
libraries/AP_GPS/AP_GPS_SBF.h
Unescape View File

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

Write Preview
Loading…
Cancel
Save