fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #1384 from dyeldandi/issue_1382

Browse Source 
Fixed issue #1382
This commit is contained in:
Lorenz Meier 2014-10-13 13:03:42 +02:00
commit fd983ed8e6
2 changed files with 42 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

52
src/drivers/gps/ashtech.cpp
View File

@ -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));

2
src/drivers/gps/ashtech.h
View File

@ -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 */