Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-05 23:43:58 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_GPS_SBF: add sd logging by default, improve init

Browse Source
This commit is contained in:
Michael Oborne 2015-12-07 21:45:06 +09:00 committed by Randy Mackay
parent 0bcc6e324a
commit 1c1eea6119
2 changed files with 32 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
libraries/AP_GPS/AP_GPS_SBF.cpp
View File

@ -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
{

20
libraries/AP_GPS/AP_GPS_SBF.h
View File

@ -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
{