Skip to content

Commit

Permalink
nmea: fix style, remove unused code, more robust parsing
Browse files Browse the repository at this point in the history 
- fix time data type
- prefer number of used satellites from GGA msg (which we're sure is not
  the number of visible sats)
  • Loading branch information
@bkueng
bkueng committed Aug 17, 2021
1 parent 24345b3 commit 0480f57
Show file tree
Hide file tree
Showing 2 changed files with 42 additions and 73 deletions.
97 changes: 39 additions & 58 deletions src/nmea.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -279,6 +279,7 @@ int GPSDriverNMEA::handleMessage(int len)
*/
_gps_position->fix_type = 3 + fix_quality - 1;
}

if (!_POS_received && (_last_POS_timeUTC < utc_time)) {
_last_POS_timeUTC = utc_time;
_POS_received = true;
Expand All @@ -291,8 +292,6 @@ int GPSDriverNMEA::handleMessage(int len)
_gps_position->c_variance_rad = 0.1f;
_gps_position->timestamp = gps_absolute_time();

// mavlink_log_info(&mavlink_log_pub, "GGA time->>> %d ",(int)(utc_time));

} else if (memcmp(_rx_buffer + 3, "HDT,", 4) == 0 && uiCalcComma == 2) {
/*
Heading message
Expand Down Expand Up @@ -401,6 +400,7 @@ int GPSDriverNMEA::handleMessage(int len)
_last_POS_timeUTC = utc_time;
_POS_received = true;
}

_ALT_received = true;
_SVNUM_received = true;

Expand Down Expand Up @@ -544,13 +544,13 @@ int GPSDriverNMEA::handleMessage(int len)
_last_POS_timeUTC = utc_time;
_POS_received = true;
}

if (!_VEL_received && (_last_VEL_timeUTC < utc_time)) {
_last_VEL_timeUTC = utc_time;
_VEL_received = true;
}
_TIME_received = true;

// mavlink_log_info(&mavlink_log_pub, "RMC time->>> %d ",(int)(utc_time));
_TIME_received = true;

} else if ((memcmp(_rx_buffer + 3, "GST,", 4) == 0) && (uiCalcComma == 8)) {

Expand Down Expand Up @@ -612,8 +612,6 @@ int GPSDriverNMEA::handleMessage(int len)
_EPH_received = true;
_last_FIX_timeUTC = utc_time;

// mavlink_log_info(&mavlink_log_pub, "gst data->>>eph %.2f---epv %.2f ",(double)(_gps_position->eph),(double)(_gps_position->epv));

} else if ((memcmp(_rx_buffer + 3, "GSA,", 4) == 0) && (uiCalcComma >= 17)) {

/*
Expand Down Expand Up @@ -837,19 +835,16 @@ int GPSDriverNMEA::handleMessage(int len)
if (!_VEL_received) {
_VEL_received = true;
}
// mavlink_log_info(&mavlink_log_pub, "get VTG data ");
}

if (_SVNUM_received &&
_SVINFO_received &&
_FIX_received
) {
if (_sat_num_gga > 0) {
_gps_position->satellites_used = _sat_num_gga;

_sat_num_gsv = _sat_num_gpgsv + _sat_num_glgsv + _sat_num_gagsv
+ _sat_num_gbgsv + _sat_num_bdgsv;
} else if (_SVNUM_received && _SVINFO_received && _FIX_received) {

_sat_num_gsv = _sat_num_gpgsv + _sat_num_glgsv + _sat_num_gagsv
+ _sat_num_gbgsv + _sat_num_bdgsv;
_gps_position->satellites_used = MAX(_sat_num_gns, _sat_num_gsv);
_gps_position->satellites_used = MAX(_gps_position->satellites_used, _sat_num_gga);
}

if (_VEL_received && _POS_received) {
Expand All @@ -870,51 +865,36 @@ int GPSDriverNMEA::receive(unsigned timeout)
uint8_t buf[GPS_READ_BUFFER_SIZE];

/* timeout additional to poll */
uint64_t time_started = gps_absolute_time();
gps_abstime time_started = gps_absolute_time();

int j = 0;
int bytes_count = 0;
int handled = 0;

while (true) {
int ret = read(buf, sizeof(buf), timeout);

if (ret < 0) {
/* something went wrong when polling or reading */
GPS_WARN("poll_or_read err");
return -1;

/* pass received bytes to the packet decoder */
while (j < bytes_count) {
int l = 0;
} else if (ret != 0) {

if ((l = parseChar(buf[j])) > 0) {
/* return to configure during configuration or to the gps driver during normal work
* if a packet has arrived */
int ret = handleMessage(l);
/* pass received bytes to the packet decoder */
for (int i = 0; i < ret; i++) {
int l = parseChar(buf[i]);

if (ret > 0) {
return ret;
if (l > 0) {
handled |= handleMessage(l);
}
}

j++;
}

/* everything is read */
j = bytes_count = 0;

/* then poll or read for new data */
int ret = read(buf, sizeof(buf), timeout * 2);

if (ret < 0) {
/* something went wrong when polling */
return -1;

} else if (ret == 0) {
/* Timeout while polling or just nothing read if reading, let's
* stay here, and use timeout below. */

} else if (ret > 0) {
/* if we have new data from GPS, go handle it */
bytes_count = ret;
if (handled > 0) {
return handled;
}
}

/* in case we get crap from GPS or time out */
if (time_started + timeout * 1000 * 2 < gps_absolute_time()) {
/* abort after timeout if no useful packets received */
if (time_started + timeout * 1000 < gps_absolute_time()) {
return -1;
}
}
Expand Down Expand Up @@ -993,15 +973,16 @@ int GPSDriverNMEA::configure(unsigned &baudrate, const GPSConfig &config)
GPS_WARN("NMEA: Unsupported Output Mode %i", (int)config.output_mode);
return -1;
}

// If a baudrate is defined, we test this first
if (baudrate > 0) {
setBaudrate(baudrate);
decodeInit();
receive(400);
int ret = receive(400);
gps_usleep(2000);

// If a valid POS message is recived we have GPS
if (_POS_received) {
// If a valid POS message is received we have GPS
if (_POS_received || ret > 0) {
return 0;
}
}
Expand All @@ -1010,16 +991,17 @@ int GPSDriverNMEA::configure(unsigned &baudrate, const GPSConfig &config)
const unsigned baudrates_to_try[] = {9600, 19200, 38400, 57600, 115200};
unsigned test_baudrate;

for (unsigned int baud_i = 0; !_POS_received && baud_i < sizeof(baudrates_to_try) / sizeof(baudrates_to_try[0]); baud_i++) {
for (unsigned int baud_i = 0; !_POS_received
&& baud_i < sizeof(baudrates_to_try) / sizeof(baudrates_to_try[0]); baud_i++) {

test_baudrate = baudrates_to_try[baud_i];
setBaudrate(test_baudrate);
decodeInit();
receive(400);
int ret = receive(400);
gps_usleep(2000);

// If a valid POS message is recived we have GPS
if (_POS_received) {
// If a valid POS message is received we have GPS
if (_POS_received || ret > 0) {
return 0;
}
}
Expand All @@ -1028,7 +1010,6 @@ int GPSDriverNMEA::configure(unsigned &baudrate, const GPSConfig &config)
if (baudrate > 0) {
return setBaudrate(baudrate);
}
else {
return setBaudrate(NMEA_DEFAULT_BAUDRATE);
}

return setBaudrate(NMEA_DEFAULT_BAUDRATE);
}
18 changes: 3 additions & 15 deletions src/nmea.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -66,18 +66,6 @@ class GPSDriverNMEA : public GPSHelper
int configure(unsigned &baudrate, const GPSConfig &config) override;

private:
enum class NMEADecodeFlags : int {
got_zda = (1 << 0),
got_gga = (1 << 1),
got_hdt = (1 << 2),
got_gns = (1 << 3),
got_rmc = (1 << 4),
got_gst = (1 << 5),
got_gsa = (1 << 6),
got_gsv = (1 << 7),
got_vtg = (1 << 8)
};

enum class NMEADecodeState {
uninit,
got_sync1,
Expand All @@ -95,9 +83,9 @@ class GPSDriverNMEA : public GPSHelper

sensor_gps_s *_gps_position {nullptr};
satellite_info_s *_satellite_info {nullptr};
uint32_t _last_POS_timeUTC{0};
uint32_t _last_VEL_timeUTC{0};
uint32_t _last_FIX_timeUTC{0};
double _last_POS_timeUTC{0};
double _last_VEL_timeUTC{0};
double _last_FIX_timeUTC{0};
uint64_t _last_timestamp_time{0};

uint8_t _sat_num_gga{0};
Expand Down

0 comments on commit 0480f57

Please sign in to comment.