diff options
author | Lorenz Meier <lm@qgroundcontrol.org> | 2014-10-13 13:03:42 +0200 |
---|---|---|
committer | Lorenz Meier <lm@qgroundcontrol.org> | 2014-10-13 13:03:42 +0200 |
commit | fd983ed8e61dad21fbe6cac4efbe439ad7bb6e72 (patch) | |
tree | fac4f2cbed98267f827610a37cfceb5951fcaa6f | |
parent | 87cef55005c227e6d5b8f8a6cbec0e5da0efc558 (diff) | |
parent | 686d3f4c7989f9b883b54fc26bb1974d10df98d3 (diff) | |
download | px4-firmware-fd983ed8e61dad21fbe6cac4efbe439ad7bb6e72.tar.gz px4-firmware-fd983ed8e61dad21fbe6cac4efbe439ad7bb6e72.tar.bz2 px4-firmware-fd983ed8e61dad21fbe6cac4efbe439ad7bb6e72.zip |
Merge pull request #1384 from dyeldandi/issue_1382
Fixed issue #1382
-rw-r--r-- | src/drivers/gps/ashtech.cpp | 52 | ||||
-rw-r--r-- | src/drivers/gps/ashtech.h | 2 |
2 files changed, 42 insertions, 12 deletions
diff --git a/src/drivers/gps/ashtech.cpp b/src/drivers/gps/ashtech.cpp index 1b706b5b2..a2e292de2 100644 --- a/src/drivers/gps/ashtech.cpp +++ b/src/drivers/gps/ashtech.cpp @@ -178,11 +178,20 @@ int ASHTECH::handle_message(int len) _gps_position->alt = alt * 1000; _rate_count_lat_lon++; - if ((lat == 0.0) && (lon == 0.0) && (alt == 0.0)) { + if (fix_quality <= 0) { _gps_position->fix_type = 0; } else { - _gps_position->fix_type = 3 + fix_quality; + /* + * in this NMEA message float integers (value 5) mode has higher value than fixed integers (value 4), whereas it provides lower quality, + * and since value 3 is not being used, I "moved" value 5 to 3 to add it to _gps_position->fix_type + */ + if (fix_quality == 5) { fix_quality = 3; } + + /* + * fix quality 1 means just a normal 3D fix, so I'm subtracting 1 here. This way we'll have 3 for auto, 4 for DGPS, 5 for floats, 6 for fixed. + */ + _gps_position->fix_type = 3 + fix_quality - 1; } _gps_position->timestamp_position = hrt_absolute_time(); @@ -229,6 +238,11 @@ int ASHTECH::handle_message(int len) *cc Checksum */ bufptr = (char *)(_rx_buffer + 10); + + /* + * Ashtech would return empty space as coordinate (lat, lon or alt) if it doesn't have a fix yet + */ + int coordinatesFound = 0; double ashtech_time __attribute__((unused)) = 0.0, lat = 0.0, lon = 0.0, alt = 0.0; int num_of_sv __attribute__((unused)) = 0, fix_quality = 0; double track_true = 0.0, ground_speed = 0.0 , age_of_corr __attribute__((unused)) = 0.0; @@ -241,15 +255,31 @@ int ASHTECH::handle_message(int len) if (bufptr && *(++bufptr) != ',') { ashtech_time = strtod(bufptr, &endp); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { lat = strtod(bufptr, &endp); bufptr = endp; } + if (bufptr && *(++bufptr) != ',') { + /* + * if a coordinate is skipped (i.e. no fix), it either won't get into this block (two commas in a row) + * or strtod won't find anything and endp will point exactly where bufptr is. The same is for lon and alt. + */ + lat = strtod(bufptr, &endp); + if (bufptr != endp) {coordinatesFound++;} + bufptr = endp; + } if (bufptr && *(++bufptr) != ',') { ns = *(bufptr++); } - if (bufptr && *(++bufptr) != ',') { lon = strtod(bufptr, &endp); bufptr = endp; } + if (bufptr && *(++bufptr) != ',') { + lon = strtod(bufptr, &endp); + if (bufptr != endp) {coordinatesFound++;} + bufptr = endp; + } if (bufptr && *(++bufptr) != ',') { ew = *(bufptr++); } - if (bufptr && *(++bufptr) != ',') { alt = strtod(bufptr, &endp); bufptr = endp; } + if (bufptr && *(++bufptr) != ',') { + alt = strtod(bufptr, &endp); + if (bufptr != endp) {coordinatesFound++;} + bufptr = endp; + } if (bufptr && *(++bufptr) != ',') { age_of_corr = strtod(bufptr, &endp); bufptr = endp; } @@ -280,7 +310,7 @@ int ASHTECH::handle_message(int len) _gps_position->alt = alt * 1000; _rate_count_lat_lon++; - if ((lat == 0.0) && (lon == 0.0) && (alt == 0.0)) { + if (coordinatesFound < 3) { _gps_position->fix_type = 0; } else { @@ -461,12 +491,12 @@ int ASHTECH::receive(unsigned timeout) uint64_t time_started = hrt_absolute_time(); int j = 0; - ssize_t count = 0; + ssize_t bytes_count = 0; while (true) { /* pass received bytes to the packet decoder */ - while (j < count) { + while (j < bytes_count) { int l = 0; if ((l = parse_char(buf[j])) > 0) { @@ -486,7 +516,7 @@ int ASHTECH::receive(unsigned timeout) } /* everything is read */ - j = count = 0; + j = bytes_count = 0; /* then poll for new data */ int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), timeout * 2); @@ -507,7 +537,7 @@ int ASHTECH::receive(unsigned timeout) * won't block even on a blocking device. If more bytes are * available, we'll go back to poll() again... */ - count = ::read(_fd, buf, sizeof(buf)); + bytes_count = ::read(_fd, buf, sizeof(buf)); } } } @@ -600,7 +630,7 @@ int ASHTECH::configure(unsigned &baudrate) const unsigned baudrates_to_try[] = {9600, 38400, 19200, 57600, 115200}; - for (int baud_i = 0; baud_i < sizeof(baudrates_to_try) / sizeof(baudrates_to_try[0]); baud_i++) { + for (unsigned int baud_i = 0; baud_i < sizeof(baudrates_to_try) / sizeof(baudrates_to_try[0]); baud_i++) { baudrate = baudrates_to_try[baud_i]; set_baudrate(_fd, baudrate); write(_fd, (uint8_t *)comm, sizeof(comm)); diff --git a/src/drivers/gps/ashtech.h b/src/drivers/gps/ashtech.h index 6e65d0b2b..6ba522b9c 100644 --- a/src/drivers/gps/ashtech.h +++ b/src/drivers/gps/ashtech.h @@ -68,7 +68,7 @@ class ASHTECH : public GPS_Helper char *_parse_pos; /** parse position */ bool _gsv_in_progress; /**< Indicates that gsv data parsing is in progress */ - /* int _satellites_count; /**< Number of satellites info parsed. */ + /* int _satellites_count; **< Number of satellites info parsed. */ uint8_t count; /**< Number of satellites in satellite info */ uint8_t svid[SAT_INFO_MAX_SATELLITES]; /**< Space vehicle ID [1..255], see scheme below */ uint8_t used[SAT_INFO_MAX_SATELLITES]; /**< 0: Satellite not used, 1: used for navigation */ |