From 213f4aadbdd089c3f793ba7ac13e0bc3b9b46fee Mon Sep 17 00:00:00 2001 From: Denis Yeldandi Date: Tue, 30 Sep 2014 15:44:47 +0400 Subject: Ashtech GPS driver --- src/drivers/drv_gps.h | 3 +- src/drivers/gps/ashtech.cpp | 616 +++++++++++++++++++++++++ src/drivers/gps/ashtech.h | 96 ++++ src/drivers/gps/gps.cpp | 18 + src/drivers/gps/module.mk | 1 + src/modules/uORB/topics/vehicle_gps_position.h | 2 +- 6 files changed, 734 insertions(+), 2 deletions(-) create mode 100644 src/drivers/gps/ashtech.cpp create mode 100644 src/drivers/gps/ashtech.h diff --git a/src/drivers/drv_gps.h b/src/drivers/drv_gps.h index e14f4e00d..76a211000 100644 --- a/src/drivers/drv_gps.h +++ b/src/drivers/drv_gps.h @@ -57,7 +57,8 @@ typedef enum { GPS_DRIVER_MODE_NONE = 0, GPS_DRIVER_MODE_UBX, - GPS_DRIVER_MODE_MTK + GPS_DRIVER_MODE_MTK, + GPS_DRIVER_MODE_ASHTECH } gps_driver_mode_t; diff --git a/src/drivers/gps/ashtech.cpp b/src/drivers/gps/ashtech.cpp new file mode 100644 index 000000000..d338ff8e5 --- /dev/null +++ b/src/drivers/gps/ashtech.cpp @@ -0,0 +1,616 @@ +#include "ashtech.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +typedef double float64_t; +typedef float float32_t; + +char* str_scanDec(const char *pos, int8_t sign, int8_t n_max_digit, int32_t *result) +{ + int8_t n=0; + int32_t d=0; + int8_t neg = 0; + + if(*pos == '-') { neg = 1; pos++; } + else if(*pos == '+') { pos++; } + else if(sign) return(NULL); + while(*pos >= '0' && *pos <= '9') + { + d = d*10 + (*(pos++) - '0'); + n++; + if(n_max_digit>0 && n==n_max_digit) break; + } + if(n==0 || n>10) return(NULL); + if(neg) *result = -d; + else *result = d; + return((char *)pos); +} + +char* scanFloat64(const char *pos, int8_t sign, int8_t n_max_int, int8_t n_max_frac, + float64_t *result) +{ + float64_t f=0.0, div=1.0; + int32_t d_int; + int8_t n=0, isneg= 0; + + if( *pos == '-' ) isneg= 1; + if((pos = str_scanDec(pos, sign, n_max_int, &d_int)) == NULL) return(NULL); + if(*(pos) == '.') + { + pos++; + while(*pos >= '0' && *pos <= '9') + { + f = f*(10.0) + (float64_t)(*(pos++) - '0'); + div *= (0.1); + n++; + if(n_max_frac>0 && n==n_max_frac) break; + } + } + else if(n_max_frac > 0) return(NULL); + if( isneg ) *result = (float64_t)d_int - f*div; + else *result = (float64_t)d_int + f*div; + return((char *)pos); +} + + + +ASHTECH::ASHTECH(const int &fd, struct vehicle_gps_position_s *gps_position, struct satellite_info_s *satellite_info): +_fd(fd), +_satellite_info(satellite_info), +_gps_position(gps_position) +{ + decode_init(); + _decode_state = NME_DECODE_UNINIT; + _rx_buffer_bytes = 0; +} + +ASHTECH::~ASHTECH(){ +} +//All NMEA descriptions are taken from +//http://www.trimble.com/OEM_ReceiverHelp/V4.44/en/NMEA-0183messages_MessageOverview.html +int ASHTECH::handle_message(int len){ + if(len < 7) return 0; + int uiCalcComma = 0; + for(int i = 0 ; i < len; i++){ + if(_rx_buffer[i] == ',')uiCalcComma++; + } + char* bufptr = (char*)( _rx_buffer+6); + + if((memcmp(_rx_buffer+3, "ZDA,",3) == 0)&&(uiCalcComma == 6)){ + /* + UTC day, month, and year, and local time zone offset + An example of the ZDA message string is: + + $GPZDA,172809.456,12,07,1996,00,00*45 + + ZDA message fields + Field Meaning + 0 Message ID $GPZDA + 1 UTC + 2 Day, ranging between 01 and 31 + 3 Month, ranging between 01 and 12 + 4 Year + 5 Local time zone offset from GMT, ranging from 00 through �13 hours + 6 Local time zone offset from GMT, ranging from 00 through 59 minutes + 7 The checksum data, always begins with * + Fields 5 and 6 together yield the total offset. For example, if field 5 is -5 and field 6 is +15, local time is 5 hours and 15 minutes earlier than GMT. + */ + float64_t ashtech_time = 0.0; + int day = 0, month = 0,year = 0,local_time_off_hour = 0,local_time_off_min = 0; + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&ashtech_time); + if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &day); + if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &month); + if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &year); + if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &local_time_off_hour); + if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &local_time_off_min); + + + int ashtech_hour = ashtech_time/10000; + int ashtech_minute = (ashtech_time - ashtech_hour*10000)/100; + float64_t ashtech_sec = ashtech_time - ashtech_hour*10000 - ashtech_minute*100; + //convert to unix timestamp + struct tm timeinfo; + timeinfo.tm_year = year - 1900; + timeinfo.tm_mon = month - 1; + timeinfo.tm_mday = day; + timeinfo.tm_hour = ashtech_hour; + timeinfo.tm_min = ashtech_minute; + timeinfo.tm_sec = int(ashtech_sec); + time_t epoch = mktime(&timeinfo); + + _gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this + _gps_position->time_gps_usec += (uint64_t)((ashtech_sec - int(ashtech_sec)) * 1e6); + _gps_position->timestamp_time = hrt_absolute_time(); + } + + else if((memcmp(_rx_buffer+3, "GGA,",3) == 0)&&(uiCalcComma == 14)){ + /* + Time, position, and fix related data + An example of the GBS message string is: + + $GPGGA,172814.0,3723.46587704,N,12202.26957864,W,2,6,1.2,18.893,M,-25.669,M,2.0,0031*4F + + Note - The data string exceeds the ASHTECH standard length. + GGA message fields + Field Meaning + 0 Message ID $GPGGA + 1 UTC of position fix + 2 Latitude + 3 Direction of latitude: + N: North + S: South + 4 Longitude + 5 Direction of longitude: + E: East + W: West + 6 GPS Quality indicator: + 0: Fix not valid + 1: GPS fix + 2: Differential GPS fix, OmniSTAR VBS + 4: Real-Time Kinematic, fixed integers + 5: Real-Time Kinematic, float integers, OmniSTAR XP/HP or Location RTK + 7 Number of SVs in use, range from 00 through to 24+ + 8 HDOP + 9 Orthometric height (MSL reference) + 10 M: unit of measure for orthometric height is meters + 11 Geoid separation + 12 M: geoid separation measured in meters + 13 Age of differential GPS data record, Type 1 or Type 9. Null field when DGPS is not used. + 14 Reference station ID, range 0000-4095. A null field when any reference station ID is selected and no corrections are received1. + 15 + The checksum data, always begins with * + Note - If a user-defined geoid model, or an inclined + */ + float64_t ashtech_time = 0.0, lat = 0.0, lon = 0.0, alt = 0.0; + int num_of_sv = 0, fix_quality=0; + float64_t hdop = 99.9; + char ns = '?', ew = '?'; + + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&ashtech_time); + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&lat); + if(bufptr && *(++bufptr) != ',') ns = *(bufptr++); + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&lon); + if(bufptr && *(++bufptr) != ',') ew = *(bufptr++); + if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &fix_quality); + if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &num_of_sv); + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&hdop); + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&alt); + + if(ns == 'S') + lat = -lat; + if(ew == 'W') + lon = -lon; + + _gps_position->lat = (int(lat*0.01) + (lat*0.01 - int(lat*0.01))*100.0/60.0)*10000000; + _gps_position->lon = (int(lon*0.01) + (lon*0.01 - int(lon*0.01))*100.0/60.0)*10000000; + _gps_position->alt = alt*1000; + _rate_count_lat_lon++; + + if((lat == 0.0) && (lon == 0.0) && (alt == 0.0)){ + _gps_position->fix_type = 0; + } + else + _gps_position->fix_type = 3 + fix_quality; + + _gps_position->timestamp_position = hrt_absolute_time(); + + _gps_position->vel_m_s = 0; /**< GPS ground speed (m/s) */ + _gps_position->vel_n_m_s = 0; /**< GPS ground speed in m/s */ + _gps_position->vel_e_m_s = 0; /**< GPS ground speed in m/s */ + _gps_position->vel_d_m_s = 0; /**< GPS ground speed in m/s */ + _gps_position->cog_rad = 0; /**< Course over ground (NOT heading, but direction of movement) in rad, -PI..PI */ + _gps_position->vel_ned_valid = true; /**< Flag to indicate if NED speed is valid */ + _gps_position->c_variance_rad = 0.1; + _gps_position->timestamp_velocity = hrt_absolute_time(); + return 1; + } + else if ((memcmp(_rx_buffer, "$PASHR,POS,", 11) == 0) && (uiCalcComma == 18)) { + /* + Example $PASHR,POS,2,10,125410.00,5525.8138702,N,03833.9587380,E,131.555,1.0,0.0,0.007,-0.001,2.0,1.0,1.7,1.0,*34 + + $PASHR,POS,d1,d2,m3,m4,c5,m6,c7,f8,f9,f10,f11,f12,f13,f14,f15,f16,s17*cc + Parameter Description Range + d1 Position mode 0: standalone + 1: differential + 2: RTK float + 3: RTK fixed + 5: Dead reckoning + 9: SBAS (see NPT setting) + d2 Number of satellite used in position fix 0-99 + m3 Current UTC time of position fix (hhmmss.ss) 000000.00-235959.99 + m4 Latitude of position (ddmm.mmmmmm) 0-90 degrees 00-59.9999999 minutes + c5 Latitude sector N, S + m6 Longitude of position (dddmm.mmmmmm) 0-180 degrees 00-59.9999999 minutes + c7 Longitude sector E,W + f8 Altitude above ellipsoid +9999.000 + f9 Differential age (data link age), seconds 0.0-600.0 + f10 True track/course over ground in degrees 0.0-359.9 + f11 Speed over ground in knots 0.0-999.9 + f12 Vertical velocity in decimeters per second +999.9 + f13 PDOP 0-99.9 + f14 HDOP 0-99.9 + f15 VDOP 0-99.9 + f16 TDOP 0-99.9 + s17 Reserved no data + *cc Checksum + */ + bufptr = (char*)( _rx_buffer+10); + float64_t ashtech_time = 0.0, lat = 0.0, lon = 0.0, alt = 0.0; + int num_of_sv = 0, fix_quality=0; + float64_t track_true = 0.0, ground_speed = 0.0 , age_of_corr = 0.0; + float64_t hdop = 99.9, vdop = 99.9, pdop = 99.9, tdop = 99.9,vertic_vel = 0.0; + char ns = '?', ew = '?'; + + if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &fix_quality); + if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &num_of_sv); + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&ashtech_time); + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&lat); + if(bufptr && *(++bufptr) != ',') ns = *(bufptr++); + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&lon); + if(bufptr && *(++bufptr) != ',') ew = *(bufptr++); + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&alt); + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&age_of_corr); + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&track_true); + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&ground_speed); + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&vertic_vel); + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&pdop); + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&hdop); + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&vdop); + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&tdop); + + if(ns == 'S') + lat = -lat; + if(ew == 'W') + lon = -lon; + + _gps_position->lat = (int(lat*0.01) + (lat*0.01 - int(lat*0.01))*100.0/60.0)*10000000; + _gps_position->lon = (int(lon*0.01) + (lon*0.01 - int(lon*0.01))*100.0/60.0)*10000000; + _gps_position->alt = alt*1000; + _rate_count_lat_lon++; + + if((lat == 0.0) && (lon == 0.0) && (alt == 0.0)){ + _gps_position->fix_type = 0; + } + else + _gps_position->fix_type = 3 + fix_quality; + + _gps_position->timestamp_position = hrt_absolute_time(); + + const float64_t m_pi = 3.14159265; + + double track_rad = track_true * m_pi / 180.0; + + double velocity_ms = ground_speed / 3.6; + double velocity_north = velocity_ms * cos(track_rad); + double velocity_east = velocity_ms * sin(track_rad); + + _gps_position->vel_m_s = velocity_ms; /**< GPS ground speed (m/s) */ + _gps_position->vel_n_m_s = velocity_north; /**< GPS ground speed in m/s */ + _gps_position->vel_e_m_s = velocity_east; /**< GPS ground speed in m/s */ + _gps_position->vel_d_m_s = -vertic_vel; /**< GPS ground speed in m/s */ + _gps_position->cog_rad = track_rad; /**< Course over ground (NOT heading, but direction of movement) in rad, -PI..PI */ + _gps_position->vel_ned_valid = true; /**< Flag to indicate if NED speed is valid */ + _gps_position->c_variance_rad = 0.1; + _gps_position->timestamp_velocity = hrt_absolute_time(); + return 1; + + } + else if((memcmp(_rx_buffer+3, "GST,",3) == 0)&&(uiCalcComma == 8)){ + /* + Position error statistics + An example of the GST message string is: + + $GPGST,172814.0,0.006,0.023,0.020,273.6,0.023,0.020,0.031*6A + + The Talker ID ($--) will vary depending on the satellite system used for the position solution: + + $GP - GPS only + $GL - GLONASS only + $GN - Combined + GST message fields + Field Meaning + 0 Message ID $GPGST + 1 UTC of position fix + 2 RMS value of the pseudorange residuals; includes carrier phase residuals during periods of RTK (float) and RTK (fixed) processing + 3 Error ellipse semi-major axis 1 sigma error, in meters + 4 Error ellipse semi-minor axis 1 sigma error, in meters + 5 Error ellipse orientation, degrees from true north + 6 Latitude 1 sigma error, in meters + 7 Longitude 1 sigma error, in meters + 8 Height 1 sigma error, in meters + 9 The checksum data, always begins with * + */ + float64_t ashtech_time = 0.0, lat_err = 0.0, lon_err = 0.0, alt_err = 0.0; + float64_t min_err = 0.0, maj_err = 0.0, deg_from_north = 0.0, rms_err = 0.0; + + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&ashtech_time); + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&rms_err); + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&maj_err); + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&min_err); + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,°_from_north); + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&lat_err); + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&lon_err); + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&alt_err); + + _gps_position->eph = sqrt(lat_err*lat_err +lon_err*lon_err); + _gps_position->epv = alt_err; + + _gps_position->s_variance_m_s = 0; + _gps_position->timestamp_variance = hrt_absolute_time(); + } + else if((memcmp(_rx_buffer+3, "VTG,",3) == 0)&&(uiCalcComma == 9)){ + /* + Track made good and speed over ground + An example of the VTG message string is: + + $GPVTG,054.7,T,034.4,M,005.5,N,010.2,K*48 + + VTG message fields + Field Meaning + 0 Message ID $GPVTG + 1 Track made good (degrees true) + 2 T: track made good is relative to true north + 3 Track made good (degrees magnetic) + 4 M: track made good is relative to magnetic north + 5 Speed, in knots + 6 N: speed is measured in knots + 7 Speed over ground in kilometers/hour (kph) + 8 K: speed over ground is measured in kph + 9 The checksum data, always begins with * + */ + /*float64_t track_true = 0.0, speed = 0.0, track_magnetic = 0.0, ground_speed = 0.0; + char true_north = '?', magnetic_north = '?', speed_in_knots = '?', speed_in_kph = '?'; + + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&track_true); + if(bufptr && *(++bufptr) != ',') true_north = *(bufptr++); + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&track_magnetic); + if(bufptr && *(++bufptr) != ',') magnetic_north = *(bufptr++); + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&speed); + if(bufptr && *(++bufptr) != ',') speed_in_knots = *(bufptr++); + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&ground_speed); + if(bufptr && *(++bufptr) != ',') speed_in_kph = *(bufptr++); + + const float64_t dPI = 3.14159265; + float64_t tan_C = tan(track_true * dPI / 180.0); + float64_t lat_ = sqrt(ground_speed*ground_speed/ (1+tan_C)); //km/hour + float64_t lon_ = lat_*tan_C; // //km/hour*/ + } + else if((memcmp(_rx_buffer+3, "GSV,",3) == 0)){ + /* + The GSV message string identifies the number of SVs in view, the PRN numbers, elevations, azimuths, and SNR values. An example of the GSV message string is: + + $GPGSV,4,1,13,02,02,213,,03,-3,000,,11,00,121,,14,13,172,05*67 + + GSV message fields + Field Meaning + 0 Message ID $GPGSV + 1 Total number of messages of this type in this cycle + 2 Message number + 3 Total number of SVs visible + 4 SV PRN number + 5 Elevation, in degrees, 90� maximum + 6 Azimuth, degrees from True North, 000� through 359� + 7 SNR, 00 through 99 dB (null when not tracking) + 8-11 Information about second SV, same format as fields 4 through 7 + 12-15 Information about third SV, same format as fields 4 through 7 + 16-19 Information about fourth SV, same format as fields 4 through 7 + 20 The checksum data, always begins with * + */ +// currently process only gps, because do not know what +// Global satellite ID I should use for non GPS sats + bool bGPS = false; + if(memcmp(_rx_buffer, "$GP",3) != 0) + return 0; + else + bGPS = true; + + int all_msg_num, this_msg_num, tot_sv_visible; + struct gsv_sat{ + int svid; + int elevation; + int azimuth; + int snr; + } sat[4]; + memset(sat, 0, sizeof(sat)); + + if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &all_msg_num); + if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &this_msg_num); + if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &tot_sv_visible); + if((this_msg_num<1) || (this_msg_num>all_msg_num)){ + return 0; + } + if((this_msg_num == 0)&&(bGPS == true)){ + memset(_satellite_info->svid, 0,sizeof(_satellite_info->svid)); + memset(_satellite_info->used, 0,sizeof(_satellite_info->used)); + memset(_satellite_info->snr, 0,sizeof(_satellite_info->snr)); + memset(_satellite_info->elevation,0,sizeof(_satellite_info->elevation)); + memset(_satellite_info->azimuth, 0,sizeof(_satellite_info->azimuth)); + } + + int end = 4; + if(this_msg_num == all_msg_num){ + end = tot_sv_visible - (this_msg_num-1)*4; + _gps_position->satellites_used = tot_sv_visible; + _satellite_info->count = SAT_INFO_MAX_SATELLITES; + _satellite_info->timestamp = hrt_absolute_time(); + } + for(int y = 0 ; y < end ;y++){ + if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &sat[y].svid); + if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &sat[y].elevation); + if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &sat[y].azimuth); + if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &sat[y].snr); + + _satellite_info->svid[y+(this_msg_num-1)*4] = sat[y].svid; + _satellite_info->used[y+(this_msg_num-1)*4] = ((sat[y].snr>0)? true: false); + _satellite_info->snr[y+(this_msg_num-1)*4] = sat[y].snr; + _satellite_info->elevation[y+(this_msg_num-1)*4] = sat[y].elevation; + _satellite_info->azimuth[y+(this_msg_num-1)*4] = sat[y].azimuth; + } + } + + return 0; +} + + +int ASHTECH::receive(unsigned timeout){ +{ + /* poll descriptor */ + pollfd fds[1]; + fds[0].fd = _fd; + fds[0].events = POLLIN; + + uint8_t buf[32]; + + /* timeout additional to poll */ + uint64_t time_started = hrt_absolute_time(); + + int j = 0; + ssize_t count = 0; + + while (true) { + + /* pass received bytes to the packet decoder */ + while (j < count) { + int l=0; + if ((l = parse_char(buf[j])) > 0) { + /* return to configure during configuration or to the gps driver during normal work + * if a packet has arrived */ + if (handle_message(l) > 0) + return 1; + } + /* in case we keep trying but only get crap from GPS */ + if (time_started + timeout*1000*2 < hrt_absolute_time() ) { + return -1; + } + j++; + } + + /* everything is read */ + j = count = 0; + + /* then poll for new data */ + int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), timeout*2); + + if (ret < 0) { + /* something went wrong when polling */ + return -1; + + } else if (ret == 0) { + /* Timeout */ + return -1; + + } else if (ret > 0) { + /* if we have new data from GPS, go handle it */ + if (fds[0].revents & POLLIN) { + /* + * We are here because poll says there is some data, so this + * 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)); + } + } + } +} + +} +#define HEXDIGIT_CHAR(d) ((char)((d) + (((d) < 0xA) ? '0' : 'A'-0xA))) + +int ASHTECH::parse_char(uint8_t b) +{ + int iRet = 0; + + switch (_decode_state) { + /* First, look for sync1 */ + case NME_DECODE_UNINIT: + if (b == '$') { + _decode_state = NME_DECODE_GOT_SYNC1; + _rx_buffer_bytes = 0; + _rx_buffer[_rx_buffer_bytes++] = b; + } + + break; + + case NME_DECODE_GOT_SYNC1: + if (b == '$') { + _decode_state = NME_DECODE_GOT_SYNC1; + _rx_buffer_bytes = 0; + + } else if (b == '*') { + _decode_state = NME_DECODE_GOT_ASTERIKS; + } + + if (_rx_buffer_bytes >= (sizeof(_rx_buffer) - 5)) { + _decode_state = NME_DECODE_UNINIT; + _rx_buffer_bytes = 0; + + } else + _rx_buffer[_rx_buffer_bytes++] = b; + + break; + + case NME_DECODE_GOT_ASTERIKS: + _rx_buffer[_rx_buffer_bytes++] = b; + _decode_state = NME_DECODE_GOT_FIRST_CS_BYTE; + break; + + case NME_DECODE_GOT_FIRST_CS_BYTE: + _rx_buffer[_rx_buffer_bytes++] = b; + uint8_t checksum = 0; + uint8_t *buffer = _rx_buffer + 1; + uint8_t *bufend = _rx_buffer + _rx_buffer_bytes - 3; + + for (; buffer < bufend; buffer++) checksum ^= *buffer; + + if ((HEXDIGIT_CHAR(checksum >> 4) == *(_rx_buffer + _rx_buffer_bytes - 2)) && + (HEXDIGIT_CHAR(checksum & 0x0F) == *(_rx_buffer + _rx_buffer_bytes - 1))) { + iRet = _rx_buffer_bytes; + } + + _decode_state = NME_DECODE_UNINIT; + _rx_buffer_bytes = 0; + break; + } + + return iRet; +} + +void ASHTECH::decode_init(void){ + +} + +//ashtech boad configuration script +//char comm[] = "$PASHS,NME,GGA,A,ON,0.1\r\n" +char comm[] = "$PASHS,NME,ZDA,B,ON,3\r\n"\ + "$PASHS,NME,GGA,B,OFF\r\n"\ + "$PASHS,NME,GST,B,ON,3\r\n"\ + "$PASHS,NME,POS,B,ON,0.1\r\n"\ + "$PASHS,NME,GSV,B,ON,3\r\n"\ + "$PASHS,SPD,A,8\r\n"\ + "$PASHS,SPD,B,7\r\n"; // default baud is 7 + +int ASHTECH::configure(unsigned &baudrate){ + /* try different baudrates */ + 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++) { + baudrate = baudrates_to_try[baud_i]; + set_baudrate(_fd, baudrate); + write(_fd, (uint8_t*)comm, sizeof(comm)); + } + set_baudrate(_fd, 38400); + return 0; +} diff --git a/src/drivers/gps/ashtech.h b/src/drivers/gps/ashtech.h new file mode 100644 index 000000000..e8c5ffb4c --- /dev/null +++ b/src/drivers/gps/ashtech.h @@ -0,0 +1,96 @@ +/**************************************************************************** + * + * Copyright (C) 2013. All rights reserved. + * Author: Boriskin Aleksey + * Kistanov Alexander + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* @file ASHTECH protocol definitions */ + +#ifndef ASHTECH_H_ +#define ASHTECH_H_ + +#include "gps_helper.h" + +#ifndef RECV_BUFFER_SIZE +#define RECV_BUFFER_SIZE 512 + +#define SAT_INFO_MAX_SATELLITES 20 +#endif + + +class ASHTECH : public GPS_Helper +{ + enum ashtech_decode_state_t { + NME_DECODE_UNINIT, + NME_DECODE_GOT_SYNC1, + NME_DECODE_GOT_ASTERIKS, + NME_DECODE_GOT_FIRST_CS_BYTE + }; + + int _fd; + struct satellite_info_s *_satellite_info; + struct vehicle_gps_position_s *_gps_position; + int ashtechlog_fd;//miklm + + ashtech_decode_state_t _decode_state; + uint8_t _rx_buffer[RECV_BUFFER_SIZE]; + uint16_t _rx_buffer_bytes; + bool _parse_error; // parse error flag + 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. + 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 */ + uint8_t elevation[SAT_INFO_MAX_SATELLITES]; /**< Elevation (0: right on top of receiver, 90: on the horizon) of satellite */ + uint8_t azimuth[SAT_INFO_MAX_SATELLITES]; /**< Direction of satellite, 0: 0 deg, 255: 360 deg. */ + uint8_t snr[SAT_INFO_MAX_SATELLITES]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99, zero when not tracking this satellite. */ + +public: + ASHTECH(const int &fd, struct vehicle_gps_position_s *gps_position, struct satellite_info_s *satellite_info); + ~ASHTECH(); + int receive(unsigned timeout); + int configure(unsigned &baudrate); + void decode_init(void); + int handle_message(int len); + int parse_char(uint8_t b); + /** Read int ASHTECH parameter */ + int32_t read_int(); + /** Read float ASHTECH parameter */ + double read_float(); + /** Read char ASHTECH parameter */ + char read_char(); + +}; + +#endif /* ASHTECH_H_ */ diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 34dd63086..2547cda97 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -69,6 +69,7 @@ #include "ubx.h" #include "mtk.h" +#include "ashtech.h" #define TIMEOUT_5HZ 500 @@ -341,6 +342,11 @@ GPS::task_main() _Helper = new MTK(_serial_fd, &_report_gps_pos); break; + case GPS_DRIVER_MODE_ASHTECH: + + _Helper = new ASHTECH(_serial_fd, &_report_gps_pos, _p_report_sat_info); + break; + default: break; } @@ -402,6 +408,10 @@ GPS::task_main() mode_str = "MTK"; break; + case GPS_DRIVER_MODE_ASHTECH: + mode_str = "ASHTECH"; + break; + default: break; } @@ -429,6 +439,10 @@ GPS::task_main() break; case GPS_DRIVER_MODE_MTK: + _mode = GPS_DRIVER_MODE_ASHTECH; + break; + + case GPS_DRIVER_MODE_ASHTECH: _mode = GPS_DRIVER_MODE_UBX; break; @@ -475,6 +489,10 @@ GPS::print_info() warnx("protocol: MTK"); break; + case GPS_DRIVER_MODE_ASHTECH: + warnx("protocol: ASHTECH"); + break; + default: break; } diff --git a/src/drivers/gps/module.mk b/src/drivers/gps/module.mk index b00818424..4f99b0d3b 100644 --- a/src/drivers/gps/module.mk +++ b/src/drivers/gps/module.mk @@ -40,6 +40,7 @@ MODULE_COMMAND = gps SRCS = gps.cpp \ gps_helper.cpp \ mtk.cpp \ + ashtech.cpp \ ubx.cpp MODULE_STACKSIZE = 1200 diff --git a/src/modules/uORB/topics/vehicle_gps_position.h b/src/modules/uORB/topics/vehicle_gps_position.h index 80d65cd69..43909e2fa 100644 --- a/src/modules/uORB/topics/vehicle_gps_position.h +++ b/src/modules/uORB/topics/vehicle_gps_position.h @@ -62,7 +62,7 @@ struct vehicle_gps_position_s { uint64_t timestamp_variance; float s_variance_m_s; /**< speed accuracy estimate m/s */ float c_variance_rad; /**< course accuracy estimate rad */ - uint8_t fix_type; /**< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. */ + uint8_t fix_type; /**< 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: Real-Time Kinematic, fixed integers, 5: Real-Time Kinematic, float integers, OmniSTAR XP/HP or Location RTK. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. */ float eph; /**< GPS HDOP horizontal dilution of position in m */ float epv; /**< GPS VDOP horizontal dilution of position in m */ -- cgit v1.2.3 From a960fcbdef475e5ab264760568b15867d55b7774 Mon Sep 17 00:00:00 2001 From: Denis Yeldandi Date: Thu, 2 Oct 2014 14:34:20 +0400 Subject: Increased ashtech POS frequency and increased baudrate --- src/drivers/gps/ashtech.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/drivers/gps/ashtech.cpp b/src/drivers/gps/ashtech.cpp index d338ff8e5..614aade00 100644 --- a/src/drivers/gps/ashtech.cpp +++ b/src/drivers/gps/ashtech.cpp @@ -597,20 +597,22 @@ void ASHTECH::decode_init(void){ char comm[] = "$PASHS,NME,ZDA,B,ON,3\r\n"\ "$PASHS,NME,GGA,B,OFF\r\n"\ "$PASHS,NME,GST,B,ON,3\r\n"\ - "$PASHS,NME,POS,B,ON,0.1\r\n"\ + "$PASHS,NME,POS,B,ON,0.05\r\n"\ "$PASHS,NME,GSV,B,ON,3\r\n"\ "$PASHS,SPD,A,8\r\n"\ - "$PASHS,SPD,B,7\r\n"; // default baud is 7 + "$PASHS,SPD,B,9\r\n"; // default baud is 7 int ASHTECH::configure(unsigned &baudrate){ /* try different baudrates */ 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++) { baudrate = baudrates_to_try[baud_i]; set_baudrate(_fd, baudrate); write(_fd, (uint8_t*)comm, sizeof(comm)); } - set_baudrate(_fd, 38400); + + set_baudrate(_fd, 115200); return 0; } -- cgit v1.2.3 From 04ceb3c95d6c713dfb4f673d3a4cafa7c27c13ca Mon Sep 17 00:00:00 2001 From: Denis Yeldandi Date: Sat, 4 Oct 2014 09:21:47 +0400 Subject: Fixed km/h -> knots in POS --- src/drivers/gps/ashtech.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/gps/ashtech.cpp b/src/drivers/gps/ashtech.cpp index d338ff8e5..69c22ab28 100644 --- a/src/drivers/gps/ashtech.cpp +++ b/src/drivers/gps/ashtech.cpp @@ -293,7 +293,7 @@ int ASHTECH::handle_message(int len){ double track_rad = track_true * m_pi / 180.0; - double velocity_ms = ground_speed / 3.6; + double velocity_ms = ground_speed / 1.9438445; /** knots to m/s */ double velocity_north = velocity_ms * cos(track_rad); double velocity_east = velocity_ms * sin(track_rad); -- cgit v1.2.3 From 6bf005e9019939d266da80f60aeeb126b66935de Mon Sep 17 00:00:00 2001 From: Denis Yeldandi Date: Mon, 6 Oct 2014 17:56:56 +0400 Subject: PASHS,POP,20 is needed for 20Hz configuration --- src/drivers/gps/ashtech.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/drivers/gps/ashtech.cpp b/src/drivers/gps/ashtech.cpp index b37f70ad1..d40b12721 100644 --- a/src/drivers/gps/ashtech.cpp +++ b/src/drivers/gps/ashtech.cpp @@ -594,7 +594,8 @@ void ASHTECH::decode_init(void){ //ashtech boad configuration script //char comm[] = "$PASHS,NME,GGA,A,ON,0.1\r\n" -char comm[] = "$PASHS,NME,ZDA,B,ON,3\r\n"\ +char comm[] = "$PASHS,POP,20\r\n"\ + "$PASHS,NME,ZDA,B,ON,3\r\n"\ "$PASHS,NME,GGA,B,OFF\r\n"\ "$PASHS,NME,GST,B,ON,3\r\n"\ "$PASHS,NME,POS,B,ON,0.05\r\n"\ -- cgit v1.2.3 From 1bf1cec5672a7f30656e3fe44535ab0390bbd2d0 Mon Sep 17 00:00:00 2001 From: Denis Yeldandi Date: Wed, 8 Oct 2014 23:37:41 +0400 Subject: Corrected gps_fix values description --- src/modules/uORB/topics/vehicle_gps_position.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/uORB/topics/vehicle_gps_position.h b/src/modules/uORB/topics/vehicle_gps_position.h index 43909e2fa..31e616f4f 100644 --- a/src/modules/uORB/topics/vehicle_gps_position.h +++ b/src/modules/uORB/topics/vehicle_gps_position.h @@ -62,7 +62,7 @@ struct vehicle_gps_position_s { uint64_t timestamp_variance; float s_variance_m_s; /**< speed accuracy estimate m/s */ float c_variance_rad; /**< course accuracy estimate rad */ - uint8_t fix_type; /**< 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: Real-Time Kinematic, fixed integers, 5: Real-Time Kinematic, float integers, OmniSTAR XP/HP or Location RTK. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. */ + uint8_t fix_type; /**< 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: RTCM code differential, 5: Real-Time Kinematic, float, 6: Real-Time Kinematic, fixed, 8: Extrapolated. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. */ float eph; /**< GPS HDOP horizontal dilution of position in m */ float epv; /**< GPS VDOP horizontal dilution of position in m */ -- cgit v1.2.3 From 09b5206404116b263feb50de18c54d53544c9114 Mon Sep 17 00:00:00 2001 From: Denis Yeldandi Date: Thu, 9 Oct 2014 11:15:11 +0400 Subject: Code style fix --- src/drivers/gps/ashtech.cpp | 1103 +++++++++++++++++++++++-------------------- src/drivers/gps/ashtech.h | 4 +- src/drivers/gps/gps.cpp | 13 +- 3 files changed, 605 insertions(+), 515 deletions(-) diff --git a/src/drivers/gps/ashtech.cpp b/src/drivers/gps/ashtech.cpp index d40b12721..7342d01d1 100644 --- a/src/drivers/gps/ashtech.cpp +++ b/src/drivers/gps/ashtech.cpp @@ -18,282 +18,343 @@ typedef double float64_t; typedef float float32_t; -char* str_scanDec(const char *pos, int8_t sign, int8_t n_max_digit, int32_t *result) +char *str_scanDec(const char *pos, int8_t sign, int8_t n_max_digit, int32_t *result) { - int8_t n=0; - int32_t d=0; - int8_t neg = 0; - - if(*pos == '-') { neg = 1; pos++; } - else if(*pos == '+') { pos++; } - else if(sign) return(NULL); - while(*pos >= '0' && *pos <= '9') - { - d = d*10 + (*(pos++) - '0'); - n++; - if(n_max_digit>0 && n==n_max_digit) break; - } - if(n==0 || n>10) return(NULL); - if(neg) *result = -d; - else *result = d; - return((char *)pos); + int8_t n = 0; + int32_t d = 0; + int8_t neg = 0; + + if (*pos == '-') { neg = 1; pos++; } + + else if (*pos == '+') { pos++; } + + else if (sign) { return (NULL); } + + while (*pos >= '0' && *pos <= '9') { + d = d * 10 + (*(pos++) - '0'); + n++; + + if (n_max_digit > 0 && n == n_max_digit) { break; } + } + + if (n == 0 || n > 10) { return (NULL); } + + if (neg) { *result = -d; } + + else { *result = d; } + + return ((char *)pos); } -char* scanFloat64(const char *pos, int8_t sign, int8_t n_max_int, int8_t n_max_frac, - float64_t *result) +char *scanFloat64(const char *pos, int8_t sign, int8_t n_max_int, int8_t n_max_frac, + float64_t *result) { - float64_t f=0.0, div=1.0; - int32_t d_int; - int8_t n=0, isneg= 0; - - if( *pos == '-' ) isneg= 1; - if((pos = str_scanDec(pos, sign, n_max_int, &d_int)) == NULL) return(NULL); - if(*(pos) == '.') - { - pos++; - while(*pos >= '0' && *pos <= '9') - { - f = f*(10.0) + (float64_t)(*(pos++) - '0'); - div *= (0.1); - n++; - if(n_max_frac>0 && n==n_max_frac) break; - } - } - else if(n_max_frac > 0) return(NULL); - if( isneg ) *result = (float64_t)d_int - f*div; - else *result = (float64_t)d_int + f*div; - return((char *)pos); + float64_t f = 0.0, div = 1.0; + int32_t d_int; + int8_t n = 0, isneg = 0; + + if (*pos == '-') { isneg = 1; } + + if ((pos = str_scanDec(pos, sign, n_max_int, &d_int)) == NULL) { return (NULL); } + + if (*(pos) == '.') { + pos++; + + while (*pos >= '0' && *pos <= '9') { + f = f * (10.0) + (float64_t)(*(pos++) - '0'); + div *= (0.1); + n++; + + if (n_max_frac > 0 && n == n_max_frac) { break; } + } + + } else if (n_max_frac > 0) { return (NULL); } + + if (isneg) { *result = (float64_t)d_int - f * div; } + + else { *result = (float64_t)d_int + f * div; } + + return ((char *)pos); } ASHTECH::ASHTECH(const int &fd, struct vehicle_gps_position_s *gps_position, struct satellite_info_s *satellite_info): -_fd(fd), -_satellite_info(satellite_info), -_gps_position(gps_position) + _fd(fd), + _satellite_info(satellite_info), + _gps_position(gps_position) { - decode_init(); - _decode_state = NME_DECODE_UNINIT; - _rx_buffer_bytes = 0; + decode_init(); + _decode_state = NME_DECODE_UNINIT; + _rx_buffer_bytes = 0; } -ASHTECH::~ASHTECH(){ +ASHTECH::~ASHTECH() +{ } -//All NMEA descriptions are taken from -//http://www.trimble.com/OEM_ReceiverHelp/V4.44/en/NMEA-0183messages_MessageOverview.html -int ASHTECH::handle_message(int len){ - if(len < 7) return 0; - int uiCalcComma = 0; - for(int i = 0 ; i < len; i++){ - if(_rx_buffer[i] == ',')uiCalcComma++; - } - char* bufptr = (char*)( _rx_buffer+6); - - if((memcmp(_rx_buffer+3, "ZDA,",3) == 0)&&(uiCalcComma == 6)){ - /* - UTC day, month, and year, and local time zone offset - An example of the ZDA message string is: - - $GPZDA,172809.456,12,07,1996,00,00*45 - - ZDA message fields - Field Meaning - 0 Message ID $GPZDA - 1 UTC - 2 Day, ranging between 01 and 31 - 3 Month, ranging between 01 and 12 - 4 Year - 5 Local time zone offset from GMT, ranging from 00 through �13 hours - 6 Local time zone offset from GMT, ranging from 00 through 59 minutes - 7 The checksum data, always begins with * - Fields 5 and 6 together yield the total offset. For example, if field 5 is -5 and field 6 is +15, local time is 5 hours and 15 minutes earlier than GMT. - */ - float64_t ashtech_time = 0.0; - int day = 0, month = 0,year = 0,local_time_off_hour = 0,local_time_off_min = 0; - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&ashtech_time); - if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &day); - if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &month); - if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &year); - if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &local_time_off_hour); - if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &local_time_off_min); - - - int ashtech_hour = ashtech_time/10000; - int ashtech_minute = (ashtech_time - ashtech_hour*10000)/100; - float64_t ashtech_sec = ashtech_time - ashtech_hour*10000 - ashtech_minute*100; - //convert to unix timestamp - struct tm timeinfo; - timeinfo.tm_year = year - 1900; - timeinfo.tm_mon = month - 1; - timeinfo.tm_mday = day; - timeinfo.tm_hour = ashtech_hour; - timeinfo.tm_min = ashtech_minute; - timeinfo.tm_sec = int(ashtech_sec); - time_t epoch = mktime(&timeinfo); - - _gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this - _gps_position->time_gps_usec += (uint64_t)((ashtech_sec - int(ashtech_sec)) * 1e6); - _gps_position->timestamp_time = hrt_absolute_time(); - } - - else if((memcmp(_rx_buffer+3, "GGA,",3) == 0)&&(uiCalcComma == 14)){ - /* - Time, position, and fix related data - An example of the GBS message string is: - - $GPGGA,172814.0,3723.46587704,N,12202.26957864,W,2,6,1.2,18.893,M,-25.669,M,2.0,0031*4F - - Note - The data string exceeds the ASHTECH standard length. - GGA message fields - Field Meaning - 0 Message ID $GPGGA - 1 UTC of position fix - 2 Latitude - 3 Direction of latitude: - N: North - S: South - 4 Longitude - 5 Direction of longitude: - E: East - W: West - 6 GPS Quality indicator: - 0: Fix not valid - 1: GPS fix - 2: Differential GPS fix, OmniSTAR VBS - 4: Real-Time Kinematic, fixed integers - 5: Real-Time Kinematic, float integers, OmniSTAR XP/HP or Location RTK - 7 Number of SVs in use, range from 00 through to 24+ - 8 HDOP - 9 Orthometric height (MSL reference) - 10 M: unit of measure for orthometric height is meters - 11 Geoid separation - 12 M: geoid separation measured in meters - 13 Age of differential GPS data record, Type 1 or Type 9. Null field when DGPS is not used. - 14 Reference station ID, range 0000-4095. A null field when any reference station ID is selected and no corrections are received1. - 15 - The checksum data, always begins with * - Note - If a user-defined geoid model, or an inclined - */ - float64_t ashtech_time = 0.0, lat = 0.0, lon = 0.0, alt = 0.0; - int num_of_sv = 0, fix_quality=0; - float64_t hdop = 99.9; - char ns = '?', ew = '?'; - - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&ashtech_time); - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&lat); - if(bufptr && *(++bufptr) != ',') ns = *(bufptr++); - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&lon); - if(bufptr && *(++bufptr) != ',') ew = *(bufptr++); - if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &fix_quality); - if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &num_of_sv); - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&hdop); - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&alt); - - if(ns == 'S') - lat = -lat; - if(ew == 'W') - lon = -lon; - - _gps_position->lat = (int(lat*0.01) + (lat*0.01 - int(lat*0.01))*100.0/60.0)*10000000; - _gps_position->lon = (int(lon*0.01) + (lon*0.01 - int(lon*0.01))*100.0/60.0)*10000000; - _gps_position->alt = alt*1000; - _rate_count_lat_lon++; - - if((lat == 0.0) && (lon == 0.0) && (alt == 0.0)){ - _gps_position->fix_type = 0; - } - else - _gps_position->fix_type = 3 + fix_quality; - - _gps_position->timestamp_position = hrt_absolute_time(); - - _gps_position->vel_m_s = 0; /**< GPS ground speed (m/s) */ - _gps_position->vel_n_m_s = 0; /**< GPS ground speed in m/s */ - _gps_position->vel_e_m_s = 0; /**< GPS ground speed in m/s */ - _gps_position->vel_d_m_s = 0; /**< GPS ground speed in m/s */ - _gps_position->cog_rad = 0; /**< Course over ground (NOT heading, but direction of movement) in rad, -PI..PI */ - _gps_position->vel_ned_valid = true; /**< Flag to indicate if NED speed is valid */ - _gps_position->c_variance_rad = 0.1; - _gps_position->timestamp_velocity = hrt_absolute_time(); - return 1; - } - else if ((memcmp(_rx_buffer, "$PASHR,POS,", 11) == 0) && (uiCalcComma == 18)) { - /* - Example $PASHR,POS,2,10,125410.00,5525.8138702,N,03833.9587380,E,131.555,1.0,0.0,0.007,-0.001,2.0,1.0,1.7,1.0,*34 - - $PASHR,POS,d1,d2,m3,m4,c5,m6,c7,f8,f9,f10,f11,f12,f13,f14,f15,f16,s17*cc - Parameter Description Range - d1 Position mode 0: standalone - 1: differential - 2: RTK float - 3: RTK fixed - 5: Dead reckoning - 9: SBAS (see NPT setting) - d2 Number of satellite used in position fix 0-99 - m3 Current UTC time of position fix (hhmmss.ss) 000000.00-235959.99 - m4 Latitude of position (ddmm.mmmmmm) 0-90 degrees 00-59.9999999 minutes - c5 Latitude sector N, S - m6 Longitude of position (dddmm.mmmmmm) 0-180 degrees 00-59.9999999 minutes - c7 Longitude sector E,W - f8 Altitude above ellipsoid +9999.000 - f9 Differential age (data link age), seconds 0.0-600.0 - f10 True track/course over ground in degrees 0.0-359.9 - f11 Speed over ground in knots 0.0-999.9 - f12 Vertical velocity in decimeters per second +999.9 - f13 PDOP 0-99.9 - f14 HDOP 0-99.9 - f15 VDOP 0-99.9 - f16 TDOP 0-99.9 - s17 Reserved no data - *cc Checksum - */ - bufptr = (char*)( _rx_buffer+10); - float64_t ashtech_time = 0.0, lat = 0.0, lon = 0.0, alt = 0.0; - int num_of_sv = 0, fix_quality=0; - float64_t track_true = 0.0, ground_speed = 0.0 , age_of_corr = 0.0; - float64_t hdop = 99.9, vdop = 99.9, pdop = 99.9, tdop = 99.9,vertic_vel = 0.0; - char ns = '?', ew = '?'; - - if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &fix_quality); - if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &num_of_sv); - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&ashtech_time); - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&lat); - if(bufptr && *(++bufptr) != ',') ns = *(bufptr++); - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&lon); - if(bufptr && *(++bufptr) != ',') ew = *(bufptr++); - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&alt); - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&age_of_corr); - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&track_true); - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&ground_speed); - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&vertic_vel); - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&pdop); - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&hdop); - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&vdop); - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&tdop); - - if(ns == 'S') - lat = -lat; - if(ew == 'W') - lon = -lon; - - _gps_position->lat = (int(lat*0.01) + (lat*0.01 - int(lat*0.01))*100.0/60.0)*10000000; - _gps_position->lon = (int(lon*0.01) + (lon*0.01 - int(lon*0.01))*100.0/60.0)*10000000; - _gps_position->alt = alt*1000; - _rate_count_lat_lon++; - - if((lat == 0.0) && (lon == 0.0) && (alt == 0.0)){ - _gps_position->fix_type = 0; - } - else - _gps_position->fix_type = 3 + fix_quality; - - _gps_position->timestamp_position = hrt_absolute_time(); - - const float64_t m_pi = 3.14159265; - - double track_rad = track_true * m_pi / 180.0; - - double velocity_ms = ground_speed / 1.9438445; /** knots to m/s */ + +/* + * All NMEA descriptions are taken from + * http://www.trimble.com/OEM_ReceiverHelp/V4.44/en/NMEA-0183messages_MessageOverview.html + */ + +int ASHTECH::handle_message(int len) +{ + if (len < 7) { return 0; } + + int uiCalcComma = 0; + + for (int i = 0 ; i < len; i++) { + if (_rx_buffer[i] == ',') { uiCalcComma++; } + } + + char *bufptr = (char *)(_rx_buffer + 6); + + if ((memcmp(_rx_buffer + 3, "ZDA,", 3) == 0) && (uiCalcComma == 6)) { + /* + UTC day, month, and year, and local time zone offset + An example of the ZDA message string is: + + $GPZDA,172809.456,12,07,1996,00,00*45 + + ZDA message fields + Field Meaning + 0 Message ID $GPZDA + 1 UTC + 2 Day, ranging between 01 and 31 + 3 Month, ranging between 01 and 12 + 4 Year + 5 Local time zone offset from GMT, ranging from 00 through �13 hours + 6 Local time zone offset from GMT, ranging from 00 through 59 minutes + 7 The checksum data, always begins with * + Fields 5 and 6 together yield the total offset. For example, if field 5 is -5 and field 6 is +15, local time is 5 hours and 15 minutes earlier than GMT. + */ + float64_t ashtech_time = 0.0; + int day = 0, month = 0, year = 0, local_time_off_hour = 0, local_time_off_min = 0; + + if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &ashtech_time); } + + if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &day); } + + if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &month); } + + if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &year); } + + if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &local_time_off_hour); } + + if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &local_time_off_min); } + + + int ashtech_hour = ashtech_time / 10000; + int ashtech_minute = (ashtech_time - ashtech_hour * 10000) / 100; + float64_t ashtech_sec = ashtech_time - ashtech_hour * 10000 - ashtech_minute * 100; + /* + * convert to unix timestamp + */ + struct tm timeinfo; + timeinfo.tm_year = year - 1900; + timeinfo.tm_mon = month - 1; + timeinfo.tm_mday = day; + timeinfo.tm_hour = ashtech_hour; + timeinfo.tm_min = ashtech_minute; + timeinfo.tm_sec = int(ashtech_sec); + time_t epoch = mktime(&timeinfo); + + _gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this + _gps_position->time_gps_usec += (uint64_t)((ashtech_sec - int(ashtech_sec)) * 1e6); + _gps_position->timestamp_time = hrt_absolute_time(); + } + + else if ((memcmp(_rx_buffer + 3, "GGA,", 3) == 0) && (uiCalcComma == 14)) { + /* + Time, position, and fix related data + An example of the GBS message string is: + + $GPGGA,172814.0,3723.46587704,N,12202.26957864,W,2,6,1.2,18.893,M,-25.669,M,2.0,0031*4F + + Note - The data string exceeds the ASHTECH standard length. + GGA message fields + Field Meaning + 0 Message ID $GPGGA + 1 UTC of position fix + 2 Latitude + 3 Direction of latitude: + N: North + S: South + 4 Longitude + 5 Direction of longitude: + E: East + W: West + 6 GPS Quality indicator: + 0: Fix not valid + 1: GPS fix + 2: Differential GPS fix, OmniSTAR VBS + 4: Real-Time Kinematic, fixed integers + 5: Real-Time Kinematic, float integers, OmniSTAR XP/HP or Location RTK + 7 Number of SVs in use, range from 00 through to 24+ + 8 HDOP + 9 Orthometric height (MSL reference) + 10 M: unit of measure for orthometric height is meters + 11 Geoid separation + 12 M: geoid separation measured in meters + 13 Age of differential GPS data record, Type 1 or Type 9. Null field when DGPS is not used. + 14 Reference station ID, range 0000-4095. A null field when any reference station ID is selected and no corrections are received1. + 15 + The checksum data, always begins with * + Note - If a user-defined geoid model, or an inclined + */ + float64_t ashtech_time = 0.0, lat = 0.0, lon = 0.0, alt = 0.0; + int num_of_sv = 0, fix_quality = 0; + float64_t hdop = 99.9; + char ns = '?', ew = '?'; + + if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &ashtech_time); } + + if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &lat); } + + if (bufptr && *(++bufptr) != ',') { ns = *(bufptr++); } + + if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &lon); } + + if (bufptr && *(++bufptr) != ',') { ew = *(bufptr++); } + + if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &fix_quality); } + + if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &num_of_sv); } + + if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &hdop); } + + if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &alt); } + + if (ns == 'S') { + lat = -lat; + } + + if (ew == 'W') { + lon = -lon; + } + + _gps_position->lat = (int(lat * 0.01) + (lat * 0.01 - int(lat * 0.01)) * 100.0 / 60.0) * 10000000; + _gps_position->lon = (int(lon * 0.01) + (lon * 0.01 - int(lon * 0.01)) * 100.0 / 60.0) * 10000000; + _gps_position->alt = alt * 1000; + _rate_count_lat_lon++; + + if ((lat == 0.0) && (lon == 0.0) && (alt == 0.0)) { + _gps_position->fix_type = 0; + + } else { + _gps_position->fix_type = 3 + fix_quality; + } + + _gps_position->timestamp_position = hrt_absolute_time(); + + _gps_position->vel_m_s = 0; /**< GPS ground speed (m/s) */ + _gps_position->vel_n_m_s = 0; /**< GPS ground speed in m/s */ + _gps_position->vel_e_m_s = 0; /**< GPS ground speed in m/s */ + _gps_position->vel_d_m_s = 0; /**< GPS ground speed in m/s */ + _gps_position->cog_rad = + 0; /**< Course over ground (NOT heading, but direction of movement) in rad, -PI..PI */ + _gps_position->vel_ned_valid = true; /**< Flag to indicate if NED speed is valid */ + _gps_position->c_variance_rad = 0.1; + _gps_position->timestamp_velocity = hrt_absolute_time(); + return 1; + + } else if ((memcmp(_rx_buffer, "$PASHR,POS,", 11) == 0) && (uiCalcComma == 18)) { + /* + Example $PASHR,POS,2,10,125410.00,5525.8138702,N,03833.9587380,E,131.555,1.0,0.0,0.007,-0.001,2.0,1.0,1.7,1.0,*34 + + $PASHR,POS,d1,d2,m3,m4,c5,m6,c7,f8,f9,f10,f11,f12,f13,f14,f15,f16,s17*cc + Parameter Description Range + d1 Position mode 0: standalone + 1: differential + 2: RTK float + 3: RTK fixed + 5: Dead reckoning + 9: SBAS (see NPT setting) + d2 Number of satellite used in position fix 0-99 + m3 Current UTC time of position fix (hhmmss.ss) 000000.00-235959.99 + m4 Latitude of position (ddmm.mmmmmm) 0-90 degrees 00-59.9999999 minutes + c5 Latitude sector N, S + m6 Longitude of position (dddmm.mmmmmm) 0-180 degrees 00-59.9999999 minutes + c7 Longitude sector E,W + f8 Altitude above ellipsoid +9999.000 + f9 Differential age (data link age), seconds 0.0-600.0 + f10 True track/course over ground in degrees 0.0-359.9 + f11 Speed over ground in knots 0.0-999.9 + f12 Vertical velocity in decimeters per second +999.9 + f13 PDOP 0-99.9 + f14 HDOP 0-99.9 + f15 VDOP 0-99.9 + f16 TDOP 0-99.9 + s17 Reserved no data + *cc Checksum + */ + bufptr = (char *)(_rx_buffer + 10); + float64_t ashtech_time = 0.0, lat = 0.0, lon = 0.0, alt = 0.0; + int num_of_sv = 0, fix_quality = 0; + float64_t track_true = 0.0, ground_speed = 0.0 , age_of_corr = 0.0; + float64_t hdop = 99.9, vdop = 99.9, pdop = 99.9, tdop = 99.9, vertic_vel = 0.0; + char ns = '?', ew = '?'; + + if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &fix_quality); } + + if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &num_of_sv); } + + if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &ashtech_time); } + + if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &lat); } + + if (bufptr && *(++bufptr) != ',') { ns = *(bufptr++); } + + if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &lon); } + + if (bufptr && *(++bufptr) != ',') { ew = *(bufptr++); } + + if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &alt); } + + if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &age_of_corr); } + + if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &track_true); } + + if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &ground_speed); } + + if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &vertic_vel); } + + if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &pdop); } + + if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &hdop); } + + if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &vdop); } + + if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &tdop); } + + if (ns == 'S') { + lat = -lat; + } + + if (ew == 'W') { + lon = -lon; + } + + _gps_position->lat = (int(lat * 0.01) + (lat * 0.01 - int(lat * 0.01)) * 100.0 / 60.0) * 10000000; + _gps_position->lon = (int(lon * 0.01) + (lon * 0.01 - int(lon * 0.01)) * 100.0 / 60.0) * 10000000; + _gps_position->alt = alt * 1000; + _rate_count_lat_lon++; + + if ((lat == 0.0) && (lon == 0.0) && (alt == 0.0)) { + _gps_position->fix_type = 0; + + } else { + _gps_position->fix_type = 3 + fix_quality; + } + + _gps_position->timestamp_position = hrt_absolute_time(); + + const float64_t m_pi = 3.14159265; + + double track_rad = track_true * m_pi / 180.0; + + double velocity_ms = ground_speed / 1.9438445; /** knots to m/s */ double velocity_north = velocity_ms * cos(track_rad); double velocity_east = velocity_ms * sin(track_rad); @@ -301,230 +362,255 @@ int ASHTECH::handle_message(int len){ _gps_position->vel_n_m_s = velocity_north; /**< GPS ground speed in m/s */ _gps_position->vel_e_m_s = velocity_east; /**< GPS ground speed in m/s */ _gps_position->vel_d_m_s = -vertic_vel; /**< GPS ground speed in m/s */ - _gps_position->cog_rad = track_rad; /**< Course over ground (NOT heading, but direction of movement) in rad, -PI..PI */ + _gps_position->cog_rad = + track_rad; /**< Course over ground (NOT heading, but direction of movement) in rad, -PI..PI */ _gps_position->vel_ned_valid = true; /**< Flag to indicate if NED speed is valid */ _gps_position->c_variance_rad = 0.1; _gps_position->timestamp_velocity = hrt_absolute_time(); - return 1; - - } - else if((memcmp(_rx_buffer+3, "GST,",3) == 0)&&(uiCalcComma == 8)){ - /* - Position error statistics - An example of the GST message string is: - - $GPGST,172814.0,0.006,0.023,0.020,273.6,0.023,0.020,0.031*6A - - The Talker ID ($--) will vary depending on the satellite system used for the position solution: - - $GP - GPS only - $GL - GLONASS only - $GN - Combined - GST message fields - Field Meaning - 0 Message ID $GPGST - 1 UTC of position fix - 2 RMS value of the pseudorange residuals; includes carrier phase residuals during periods of RTK (float) and RTK (fixed) processing - 3 Error ellipse semi-major axis 1 sigma error, in meters - 4 Error ellipse semi-minor axis 1 sigma error, in meters - 5 Error ellipse orientation, degrees from true north - 6 Latitude 1 sigma error, in meters - 7 Longitude 1 sigma error, in meters - 8 Height 1 sigma error, in meters - 9 The checksum data, always begins with * - */ - float64_t ashtech_time = 0.0, lat_err = 0.0, lon_err = 0.0, alt_err = 0.0; - float64_t min_err = 0.0, maj_err = 0.0, deg_from_north = 0.0, rms_err = 0.0; - - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&ashtech_time); - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&rms_err); - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&maj_err); - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&min_err); - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,°_from_north); - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&lat_err); - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&lon_err); - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&alt_err); - - _gps_position->eph = sqrt(lat_err*lat_err +lon_err*lon_err); - _gps_position->epv = alt_err; - - _gps_position->s_variance_m_s = 0; - _gps_position->timestamp_variance = hrt_absolute_time(); - } - else if((memcmp(_rx_buffer+3, "VTG,",3) == 0)&&(uiCalcComma == 9)){ - /* - Track made good and speed over ground - An example of the VTG message string is: - - $GPVTG,054.7,T,034.4,M,005.5,N,010.2,K*48 - - VTG message fields - Field Meaning - 0 Message ID $GPVTG - 1 Track made good (degrees true) - 2 T: track made good is relative to true north - 3 Track made good (degrees magnetic) - 4 M: track made good is relative to magnetic north - 5 Speed, in knots - 6 N: speed is measured in knots - 7 Speed over ground in kilometers/hour (kph) - 8 K: speed over ground is measured in kph - 9 The checksum data, always begins with * - */ - /*float64_t track_true = 0.0, speed = 0.0, track_magnetic = 0.0, ground_speed = 0.0; - char true_north = '?', magnetic_north = '?', speed_in_knots = '?', speed_in_kph = '?'; - - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&track_true); - if(bufptr && *(++bufptr) != ',') true_north = *(bufptr++); - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&track_magnetic); - if(bufptr && *(++bufptr) != ',') magnetic_north = *(bufptr++); - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&speed); - if(bufptr && *(++bufptr) != ',') speed_in_knots = *(bufptr++); - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&ground_speed); - if(bufptr && *(++bufptr) != ',') speed_in_kph = *(bufptr++); - - const float64_t dPI = 3.14159265; - float64_t tan_C = tan(track_true * dPI / 180.0); - float64_t lat_ = sqrt(ground_speed*ground_speed/ (1+tan_C)); //km/hour - float64_t lon_ = lat_*tan_C; // //km/hour*/ - } - else if((memcmp(_rx_buffer+3, "GSV,",3) == 0)){ - /* - The GSV message string identifies the number of SVs in view, the PRN numbers, elevations, azimuths, and SNR values. An example of the GSV message string is: - - $GPGSV,4,1,13,02,02,213,,03,-3,000,,11,00,121,,14,13,172,05*67 - - GSV message fields - Field Meaning - 0 Message ID $GPGSV - 1 Total number of messages of this type in this cycle - 2 Message number - 3 Total number of SVs visible - 4 SV PRN number - 5 Elevation, in degrees, 90� maximum - 6 Azimuth, degrees from True North, 000� through 359� - 7 SNR, 00 through 99 dB (null when not tracking) - 8-11 Information about second SV, same format as fields 4 through 7 - 12-15 Information about third SV, same format as fields 4 through 7 - 16-19 Information about fourth SV, same format as fields 4 through 7 - 20 The checksum data, always begins with * - */ -// currently process only gps, because do not know what -// Global satellite ID I should use for non GPS sats - bool bGPS = false; - if(memcmp(_rx_buffer, "$GP",3) != 0) - return 0; - else - bGPS = true; - - int all_msg_num, this_msg_num, tot_sv_visible; - struct gsv_sat{ - int svid; - int elevation; - int azimuth; - int snr; - } sat[4]; - memset(sat, 0, sizeof(sat)); - - if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &all_msg_num); - if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &this_msg_num); - if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &tot_sv_visible); - if((this_msg_num<1) || (this_msg_num>all_msg_num)){ - return 0; - } - if((this_msg_num == 0)&&(bGPS == true)){ - memset(_satellite_info->svid, 0,sizeof(_satellite_info->svid)); - memset(_satellite_info->used, 0,sizeof(_satellite_info->used)); - memset(_satellite_info->snr, 0,sizeof(_satellite_info->snr)); - memset(_satellite_info->elevation,0,sizeof(_satellite_info->elevation)); - memset(_satellite_info->azimuth, 0,sizeof(_satellite_info->azimuth)); - } - - int end = 4; - if(this_msg_num == all_msg_num){ - end = tot_sv_visible - (this_msg_num-1)*4; - _gps_position->satellites_used = tot_sv_visible; - _satellite_info->count = SAT_INFO_MAX_SATELLITES; - _satellite_info->timestamp = hrt_absolute_time(); - } - for(int y = 0 ; y < end ;y++){ - if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &sat[y].svid); - if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &sat[y].elevation); - if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &sat[y].azimuth); - if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &sat[y].snr); - - _satellite_info->svid[y+(this_msg_num-1)*4] = sat[y].svid; - _satellite_info->used[y+(this_msg_num-1)*4] = ((sat[y].snr>0)? true: false); - _satellite_info->snr[y+(this_msg_num-1)*4] = sat[y].snr; - _satellite_info->elevation[y+(this_msg_num-1)*4] = sat[y].elevation; - _satellite_info->azimuth[y+(this_msg_num-1)*4] = sat[y].azimuth; - } - } - - return 0; + return 1; + + } else if ((memcmp(_rx_buffer + 3, "GST,", 3) == 0) && (uiCalcComma == 8)) { + /* + Position error statistics + An example of the GST message string is: + + $GPGST,172814.0,0.006,0.023,0.020,273.6,0.023,0.020,0.031*6A + + The Talker ID ($--) will vary depending on the satellite system used for the position solution: + + $GP - GPS only + $GL - GLONASS only + $GN - Combined + GST message fields + Field Meaning + 0 Message ID $GPGST + 1 UTC of position fix + 2 RMS value of the pseudorange residuals; includes carrier phase residuals during periods of RTK (float) and RTK (fixed) processing + 3 Error ellipse semi-major axis 1 sigma error, in meters + 4 Error ellipse semi-minor axis 1 sigma error, in meters + 5 Error ellipse orientation, degrees from true north + 6 Latitude 1 sigma error, in meters + 7 Longitude 1 sigma error, in meters + 8 Height 1 sigma error, in meters + 9 The checksum data, always begins with * + */ + float64_t ashtech_time = 0.0, lat_err = 0.0, lon_err = 0.0, alt_err = 0.0; + float64_t min_err = 0.0, maj_err = 0.0, deg_from_north = 0.0, rms_err = 0.0; + + if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &ashtech_time); } + + if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &rms_err); } + + if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &maj_err); } + + if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &min_err); } + + if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, °_from_north); } + + if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &lat_err); } + + if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &lon_err); } + + if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &alt_err); } + + _gps_position->eph = sqrt(lat_err * lat_err + lon_err * lon_err); + _gps_position->epv = alt_err; + + _gps_position->s_variance_m_s = 0; + _gps_position->timestamp_variance = hrt_absolute_time(); + + } else if ((memcmp(_rx_buffer + 3, "VTG,", 3) == 0) && (uiCalcComma == 9)) { + /* + Track made good and speed over ground + An example of the VTG message string is: + + $GPVTG,054.7,T,034.4,M,005.5,N,010.2,K*48 + + VTG message fields + Field Meaning + 0 Message ID $GPVTG + 1 Track made good (degrees true) + 2 T: track made good is relative to true north + 3 Track made good (degrees magnetic) + 4 M: track made good is relative to magnetic north + 5 Speed, in knots + 6 N: speed is measured in knots + 7 Speed over ground in kilometers/hour (kph) + 8 K: speed over ground is measured in kph + 9 The checksum data, always begins with * + */ + /*float64_t track_true = 0.0, speed = 0.0, track_magnetic = 0.0, ground_speed = 0.0; + char true_north = '?', magnetic_north = '?', speed_in_knots = '?', speed_in_kph = '?'; + + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&track_true); + if(bufptr && *(++bufptr) != ',') true_north = *(bufptr++); + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&track_magnetic); + if(bufptr && *(++bufptr) != ',') magnetic_north = *(bufptr++); + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&speed); + if(bufptr && *(++bufptr) != ',') speed_in_knots = *(bufptr++); + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&ground_speed); + if(bufptr && *(++bufptr) != ',') speed_in_kph = *(bufptr++); + + const float64_t dPI = 3.14159265; + float64_t tan_C = tan(track_true * dPI / 180.0); + float64_t lat_ = sqrt(ground_speed*ground_speed/ (1+tan_C)); //km/hour + float64_t lon_ = lat_*tan_C; // //km/hour*/ + } else if ((memcmp(_rx_buffer + 3, "GSV,", 3) == 0)) { + /* + The GSV message string identifies the number of SVs in view, the PRN numbers, elevations, azimuths, and SNR values. An example of the GSV message string is: + + $GPGSV,4,1,13,02,02,213,,03,-3,000,,11,00,121,,14,13,172,05*67 + + GSV message fields + Field Meaning + 0 Message ID $GPGSV + 1 Total number of messages of this type in this cycle + 2 Message number + 3 Total number of SVs visible + 4 SV PRN number + 5 Elevation, in degrees, 90� maximum + 6 Azimuth, degrees from True North, 000� through 359� + 7 SNR, 00 through 99 dB (null when not tracking) + 8-11 Information about second SV, same format as fields 4 through 7 + 12-15 Information about third SV, same format as fields 4 through 7 + 16-19 Information about fourth SV, same format as fields 4 through 7 + 20 The checksum data, always begins with * + */ + /* + * currently process only gps, because do not know what + * Global satellite ID I should use for non GPS sats + */ + bool bGPS = false; + + if (memcmp(_rx_buffer, "$GP", 3) != 0) { + return 0; + + } else { + bGPS = true; + } + + int all_msg_num, this_msg_num, tot_sv_visible; + struct gsv_sat { + int svid; + int elevation; + int azimuth; + int snr; + } sat[4]; + memset(sat, 0, sizeof(sat)); + + if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &all_msg_num); } + + if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &this_msg_num); } + + if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &tot_sv_visible); } + + if ((this_msg_num < 1) || (this_msg_num > all_msg_num)) { + return 0; + } + + if ((this_msg_num == 0) && (bGPS == true)) { + memset(_satellite_info->svid, 0, sizeof(_satellite_info->svid)); + memset(_satellite_info->used, 0, sizeof(_satellite_info->used)); + memset(_satellite_info->snr, 0, sizeof(_satellite_info->snr)); + memset(_satellite_info->elevation, 0, sizeof(_satellite_info->elevation)); + memset(_satellite_info->azimuth, 0, sizeof(_satellite_info->azimuth)); + } + + int end = 4; + + if (this_msg_num == all_msg_num) { + end = tot_sv_visible - (this_msg_num - 1) * 4; + _gps_position->satellites_used = tot_sv_visible; + _satellite_info->count = SAT_INFO_MAX_SATELLITES; + _satellite_info->timestamp = hrt_absolute_time(); + } + + for (int y = 0 ; y < end ; y++) { + if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &sat[y].svid); } + + if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &sat[y].elevation); } + + if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &sat[y].azimuth); } + + if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &sat[y].snr); } + + _satellite_info->svid[y + (this_msg_num - 1) * 4] = sat[y].svid; + _satellite_info->used[y + (this_msg_num - 1) * 4] = ((sat[y].snr > 0) ? true : false); + _satellite_info->snr[y + (this_msg_num - 1) * 4] = sat[y].snr; + _satellite_info->elevation[y + (this_msg_num - 1) * 4] = sat[y].elevation; + _satellite_info->azimuth[y + (this_msg_num - 1) * 4] = sat[y].azimuth; + } + } + + return 0; } -int ASHTECH::receive(unsigned timeout){ +int ASHTECH::receive(unsigned timeout) { - /* poll descriptor */ - pollfd fds[1]; - fds[0].fd = _fd; - fds[0].events = POLLIN; - - uint8_t buf[32]; - - /* timeout additional to poll */ - uint64_t time_started = hrt_absolute_time(); - - int j = 0; - ssize_t count = 0; - - while (true) { - - /* pass received bytes to the packet decoder */ - while (j < count) { - int l=0; - if ((l = parse_char(buf[j])) > 0) { - /* return to configure during configuration or to the gps driver during normal work - * if a packet has arrived */ - if (handle_message(l) > 0) - return 1; - } - /* in case we keep trying but only get crap from GPS */ - if (time_started + timeout*1000*2 < hrt_absolute_time() ) { - return -1; - } - j++; - } - - /* everything is read */ - j = count = 0; - - /* then poll for new data */ - int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), timeout*2); - - if (ret < 0) { - /* something went wrong when polling */ - return -1; - - } else if (ret == 0) { - /* Timeout */ - return -1; - - } else if (ret > 0) { - /* if we have new data from GPS, go handle it */ - if (fds[0].revents & POLLIN) { - /* - * We are here because poll says there is some data, so this - * 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)); - } - } - } -} + { + /* poll descriptor */ + pollfd fds[1]; + fds[0].fd = _fd; + fds[0].events = POLLIN; + + uint8_t buf[32]; + + /* timeout additional to poll */ + uint64_t time_started = hrt_absolute_time(); + + int j = 0; + ssize_t count = 0; + + while (true) { + + /* pass received bytes to the packet decoder */ + while (j < count) { + int l = 0; + + if ((l = parse_char(buf[j])) > 0) { + /* return to configure during configuration or to the gps driver during normal work + * if a packet has arrived */ + if (handle_message(l) > 0) { + return 1; + } + } + + /* in case we keep trying but only get crap from GPS */ + if (time_started + timeout * 1000 * 2 < hrt_absolute_time()) { + return -1; + } + + j++; + } + + /* everything is read */ + j = count = 0; + + /* then poll for new data */ + int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), timeout * 2); + + if (ret < 0) { + /* something went wrong when polling */ + return -1; + + } else if (ret == 0) { + /* Timeout */ + return -1; + + } else if (ret > 0) { + /* if we have new data from GPS, go handle it */ + if (fds[0].revents & POLLIN) { + /* + * We are here because poll says there is some data, so this + * 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)); + } + } + } + } } #define HEXDIGIT_CHAR(d) ((char)((d) + (((d) < 0xA) ? '0' : 'A'-0xA))) @@ -557,8 +643,9 @@ int ASHTECH::parse_char(uint8_t b) _decode_state = NME_DECODE_UNINIT; _rx_buffer_bytes = 0; - } else + } else { _rx_buffer[_rx_buffer_bytes++] = b; + } break; @@ -573,7 +660,7 @@ int ASHTECH::parse_char(uint8_t b) uint8_t *buffer = _rx_buffer + 1; uint8_t *bufend = _rx_buffer + _rx_buffer_bytes - 3; - for (; buffer < bufend; buffer++) checksum ^= *buffer; + for (; buffer < bufend; buffer++) { checksum ^= *buffer; } if ((HEXDIGIT_CHAR(checksum >> 4) == *(_rx_buffer + _rx_buffer_bytes - 2)) && (HEXDIGIT_CHAR(checksum & 0x0F) == *(_rx_buffer + _rx_buffer_bytes - 1))) { @@ -588,32 +675,36 @@ int ASHTECH::parse_char(uint8_t b) return iRet; } -void ASHTECH::decode_init(void){ +void ASHTECH::decode_init(void) +{ } -//ashtech boad configuration script -//char comm[] = "$PASHS,NME,GGA,A,ON,0.1\r\n" +/* + * ashtech boad configuration script + */ + char comm[] = "$PASHS,POP,20\r\n"\ - "$PASHS,NME,ZDA,B,ON,3\r\n"\ - "$PASHS,NME,GGA,B,OFF\r\n"\ - "$PASHS,NME,GST,B,ON,3\r\n"\ - "$PASHS,NME,POS,B,ON,0.05\r\n"\ - "$PASHS,NME,GSV,B,ON,3\r\n"\ - "$PASHS,SPD,A,8\r\n"\ - "$PASHS,SPD,B,9\r\n"; // default baud is 7 - -int ASHTECH::configure(unsigned &baudrate){ - /* try different baudrates */ - 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++) { - baudrate = baudrates_to_try[baud_i]; - set_baudrate(_fd, baudrate); - write(_fd, (uint8_t*)comm, sizeof(comm)); - } - - set_baudrate(_fd, 115200); - return 0; + "$PASHS,NME,ZDA,B,ON,3\r\n"\ + "$PASHS,NME,GGA,B,OFF\r\n"\ + "$PASHS,NME,GST,B,ON,3\r\n"\ + "$PASHS,NME,POS,B,ON,0.05\r\n"\ + "$PASHS,NME,GSV,B,ON,3\r\n"\ + "$PASHS,SPD,A,8\r\n"\ + "$PASHS,SPD,B,9\r\n"; + +int ASHTECH::configure(unsigned &baudrate) +{ + /* try different baudrates */ + 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++) { + baudrate = baudrates_to_try[baud_i]; + set_baudrate(_fd, baudrate); + write(_fd, (uint8_t *)comm, sizeof(comm)); + } + + set_baudrate(_fd, 115200); + return 0; } diff --git a/src/drivers/gps/ashtech.h b/src/drivers/gps/ashtech.h index e8c5ffb4c..56f8571a4 100644 --- a/src/drivers/gps/ashtech.h +++ b/src/drivers/gps/ashtech.h @@ -67,8 +67,8 @@ class ASHTECH : public GPS_Helper bool _parse_error; // parse error flag 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. + bool _gsv_in_progress; /**< Indicates that gsv data parsing is in progress */ + //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 */ diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 2547cda97..5fb4b9ff8 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -343,7 +343,6 @@ GPS::task_main() break; case GPS_DRIVER_MODE_ASHTECH: - _Helper = new ASHTECH(_serial_fd, &_report_gps_pos, _p_report_sat_info); break; @@ -408,9 +407,9 @@ GPS::task_main() mode_str = "MTK"; break; - case GPS_DRIVER_MODE_ASHTECH: - mode_str = "ASHTECH"; - break; + case GPS_DRIVER_MODE_ASHTECH: + mode_str = "ASHTECH"; + break; default: break; @@ -439,10 +438,10 @@ GPS::task_main() break; case GPS_DRIVER_MODE_MTK: - _mode = GPS_DRIVER_MODE_ASHTECH; - break; + _mode = GPS_DRIVER_MODE_ASHTECH; + break; - case GPS_DRIVER_MODE_ASHTECH: + case GPS_DRIVER_MODE_ASHTECH: _mode = GPS_DRIVER_MODE_UBX; break; -- cgit v1.2.3 From 9f33555f53d253a04de1ca97d1306b763c9d2b6e Mon Sep 17 00:00:00 2001 From: Denis Yeldandi Date: Thu, 9 Oct 2014 11:36:04 +0400 Subject: More code style fixes --- src/drivers/gps/ashtech.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/drivers/gps/ashtech.h b/src/drivers/gps/ashtech.h index 56f8571a4..6e65d0b2b 100644 --- a/src/drivers/gps/ashtech.h +++ b/src/drivers/gps/ashtech.h @@ -59,16 +59,16 @@ class ASHTECH : public GPS_Helper int _fd; struct satellite_info_s *_satellite_info; struct vehicle_gps_position_s *_gps_position; - int ashtechlog_fd;//miklm + int ashtechlog_fd; ashtech_decode_state_t _decode_state; uint8_t _rx_buffer[RECV_BUFFER_SIZE]; uint16_t _rx_buffer_bytes; - bool _parse_error; // parse error flag - char *_parse_pos; // parse position + bool _parse_error; /** parse error flag */ + 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 */ -- cgit v1.2.3 From d3875eabf2ce0ac538954b0d5747f1155cf2ef33 Mon Sep 17 00:00:00 2001 From: Denis Yeldandi Date: Thu, 9 Oct 2014 11:37:22 +0400 Subject: Removed VTG message parsing --- src/drivers/gps/ashtech.cpp | 36 ------------------------------------ 1 file changed, 36 deletions(-) diff --git a/src/drivers/gps/ashtech.cpp b/src/drivers/gps/ashtech.cpp index 7342d01d1..077a6ddf4 100644 --- a/src/drivers/gps/ashtech.cpp +++ b/src/drivers/gps/ashtech.cpp @@ -419,42 +419,6 @@ int ASHTECH::handle_message(int len) _gps_position->s_variance_m_s = 0; _gps_position->timestamp_variance = hrt_absolute_time(); - } else if ((memcmp(_rx_buffer + 3, "VTG,", 3) == 0) && (uiCalcComma == 9)) { - /* - Track made good and speed over ground - An example of the VTG message string is: - - $GPVTG,054.7,T,034.4,M,005.5,N,010.2,K*48 - - VTG message fields - Field Meaning - 0 Message ID $GPVTG - 1 Track made good (degrees true) - 2 T: track made good is relative to true north - 3 Track made good (degrees magnetic) - 4 M: track made good is relative to magnetic north - 5 Speed, in knots - 6 N: speed is measured in knots - 7 Speed over ground in kilometers/hour (kph) - 8 K: speed over ground is measured in kph - 9 The checksum data, always begins with * - */ - /*float64_t track_true = 0.0, speed = 0.0, track_magnetic = 0.0, ground_speed = 0.0; - char true_north = '?', magnetic_north = '?', speed_in_knots = '?', speed_in_kph = '?'; - - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&track_true); - if(bufptr && *(++bufptr) != ',') true_north = *(bufptr++); - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&track_magnetic); - if(bufptr && *(++bufptr) != ',') magnetic_north = *(bufptr++); - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&speed); - if(bufptr && *(++bufptr) != ',') speed_in_knots = *(bufptr++); - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&ground_speed); - if(bufptr && *(++bufptr) != ',') speed_in_kph = *(bufptr++); - - const float64_t dPI = 3.14159265; - float64_t tan_C = tan(track_true * dPI / 180.0); - float64_t lat_ = sqrt(ground_speed*ground_speed/ (1+tan_C)); //km/hour - float64_t lon_ = lat_*tan_C; // //km/hour*/ } else if ((memcmp(_rx_buffer + 3, "GSV,", 3) == 0)) { /* The GSV message string identifies the number of SVs in view, the PRN numbers, elevations, azimuths, and SNR values. An example of the GSV message string is: -- cgit v1.2.3 From e37b25fd5868d00a3a6b39c1112f30bfe37a8a47 Mon Sep 17 00:00:00 2001 From: Denis Yeldandi Date: Thu, 9 Oct 2014 11:47:20 +0400 Subject: Non-ascii characters cleanup --- src/drivers/gps/ashtech.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/drivers/gps/ashtech.cpp b/src/drivers/gps/ashtech.cpp index 077a6ddf4..71ddb28cc 100644 --- a/src/drivers/gps/ashtech.cpp +++ b/src/drivers/gps/ashtech.cpp @@ -124,7 +124,7 @@ int ASHTECH::handle_message(int len) 2 Day, ranging between 01 and 31 3 Month, ranging between 01 and 12 4 Year - 5 Local time zone offset from GMT, ranging from 00 through �13 hours + 5 Local time zone offset from GMT, ranging from 00 through 13 hours 6 Local time zone offset from GMT, ranging from 00 through 59 minutes 7 The checksum data, always begins with * Fields 5 and 6 together yield the total offset. For example, if field 5 is -5 and field 6 is +15, local time is 5 hours and 15 minutes earlier than GMT. @@ -432,8 +432,8 @@ int ASHTECH::handle_message(int len) 2 Message number 3 Total number of SVs visible 4 SV PRN number - 5 Elevation, in degrees, 90� maximum - 6 Azimuth, degrees from True North, 000� through 359� + 5 Elevation, in degrees, 90 maximum + 6 Azimuth, degrees from True North, 000 through 359 7 SNR, 00 through 99 dB (null when not tracking) 8-11 Information about second SV, same format as fields 4 through 7 12-15 Information about third SV, same format as fields 4 through 7 -- cgit v1.2.3 From c4e934c13311fac2eb85e5e3f1a161af78be7118 Mon Sep 17 00:00:00 2001 From: Denis Yeldandi Date: Thu, 9 Oct 2014 12:01:09 +0400 Subject: Multiple fixes: - Fixed boad - board typo - Ashtech initialization string is const char* now - Using standard M_PI constant instead of locally defined one - Removed float32_t and float64_t in favor of standard float and double --- src/drivers/gps/ashtech.cpp | 39 +++++++++++++++++---------------------- 1 file changed, 17 insertions(+), 22 deletions(-) diff --git a/src/drivers/gps/ashtech.cpp b/src/drivers/gps/ashtech.cpp index 71ddb28cc..503e56dae 100644 --- a/src/drivers/gps/ashtech.cpp +++ b/src/drivers/gps/ashtech.cpp @@ -15,9 +15,6 @@ #include #include -typedef double float64_t; -typedef float float32_t; - char *str_scanDec(const char *pos, int8_t sign, int8_t n_max_digit, int32_t *result) { int8_t n = 0; @@ -47,9 +44,9 @@ char *str_scanDec(const char *pos, int8_t sign, int8_t n_max_digit, int32_t *re } char *scanFloat64(const char *pos, int8_t sign, int8_t n_max_int, int8_t n_max_frac, - float64_t *result) + double *result) { - float64_t f = 0.0, div = 1.0; + double f = 0.0, div = 1.0; int32_t d_int; int8_t n = 0, isneg = 0; @@ -61,7 +58,7 @@ char *scanFloat64(const char *pos, int8_t sign, int8_t n_max_int, int8_t n_max_ pos++; while (*pos >= '0' && *pos <= '9') { - f = f * (10.0) + (float64_t)(*(pos++) - '0'); + f = f * (10.0) + (double)(*(pos++) - '0'); div *= (0.1); n++; @@ -70,9 +67,9 @@ char *scanFloat64(const char *pos, int8_t sign, int8_t n_max_int, int8_t n_max_ } else if (n_max_frac > 0) { return (NULL); } - if (isneg) { *result = (float64_t)d_int - f * div; } + if (isneg) { *result = (double)d_int - f * div; } - else { *result = (float64_t)d_int + f * div; } + else { *result = (double)d_int + f * div; } return ((char *)pos); } @@ -129,7 +126,7 @@ int ASHTECH::handle_message(int len) 7 The checksum data, always begins with * Fields 5 and 6 together yield the total offset. For example, if field 5 is -5 and field 6 is +15, local time is 5 hours and 15 minutes earlier than GMT. */ - float64_t ashtech_time = 0.0; + double ashtech_time = 0.0; int day = 0, month = 0, year = 0, local_time_off_hour = 0, local_time_off_min = 0; if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &ashtech_time); } @@ -147,7 +144,7 @@ int ASHTECH::handle_message(int len) int ashtech_hour = ashtech_time / 10000; int ashtech_minute = (ashtech_time - ashtech_hour * 10000) / 100; - float64_t ashtech_sec = ashtech_time - ashtech_hour * 10000 - ashtech_minute * 100; + double ashtech_sec = ashtech_time - ashtech_hour * 10000 - ashtech_minute * 100; /* * convert to unix timestamp */ @@ -203,9 +200,9 @@ int ASHTECH::handle_message(int len) The checksum data, always begins with * Note - If a user-defined geoid model, or an inclined */ - float64_t ashtech_time = 0.0, lat = 0.0, lon = 0.0, alt = 0.0; + double ashtech_time = 0.0, lat = 0.0, lon = 0.0, alt = 0.0; int num_of_sv = 0, fix_quality = 0; - float64_t hdop = 99.9; + double hdop = 99.9; char ns = '?', ew = '?'; if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &ashtech_time); } @@ -290,10 +287,10 @@ int ASHTECH::handle_message(int len) *cc Checksum */ bufptr = (char *)(_rx_buffer + 10); - float64_t ashtech_time = 0.0, lat = 0.0, lon = 0.0, alt = 0.0; + double ashtech_time = 0.0, lat = 0.0, lon = 0.0, alt = 0.0; int num_of_sv = 0, fix_quality = 0; - float64_t track_true = 0.0, ground_speed = 0.0 , age_of_corr = 0.0; - float64_t hdop = 99.9, vdop = 99.9, pdop = 99.9, tdop = 99.9, vertic_vel = 0.0; + double track_true = 0.0, ground_speed = 0.0 , age_of_corr = 0.0; + double hdop = 99.9, vdop = 99.9, pdop = 99.9, tdop = 99.9, vertic_vel = 0.0; char ns = '?', ew = '?'; if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &fix_quality); } @@ -350,9 +347,7 @@ int ASHTECH::handle_message(int len) _gps_position->timestamp_position = hrt_absolute_time(); - const float64_t m_pi = 3.14159265; - - double track_rad = track_true * m_pi / 180.0; + double track_rad = track_true * M_PI / 180.0; double velocity_ms = ground_speed / 1.9438445; /** knots to m/s */ double velocity_north = velocity_ms * cos(track_rad); @@ -394,8 +389,8 @@ int ASHTECH::handle_message(int len) 8 Height 1 sigma error, in meters 9 The checksum data, always begins with * */ - float64_t ashtech_time = 0.0, lat_err = 0.0, lon_err = 0.0, alt_err = 0.0; - float64_t min_err = 0.0, maj_err = 0.0, deg_from_north = 0.0, rms_err = 0.0; + double ashtech_time = 0.0, lat_err = 0.0, lon_err = 0.0, alt_err = 0.0; + double min_err = 0.0, maj_err = 0.0, deg_from_north = 0.0, rms_err = 0.0; if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &ashtech_time); } @@ -645,10 +640,10 @@ void ASHTECH::decode_init(void) } /* - * ashtech boad configuration script + * ashtech board configuration script */ -char comm[] = "$PASHS,POP,20\r\n"\ +const char comm[] = "$PASHS,POP,20\r\n"\ "$PASHS,NME,ZDA,B,ON,3\r\n"\ "$PASHS,NME,GGA,B,OFF\r\n"\ "$PASHS,NME,GST,B,ON,3\r\n"\ -- cgit v1.2.3 From 5bbca777961e0e0b109d954772ff0176c65277f5 Mon Sep 17 00:00:00 2001 From: Denis Yeldandi Date: Thu, 9 Oct 2014 14:11:18 +0400 Subject: Got rid of str_scanDec and scanFloat64. - Replaced str_scanDec and scanFloat64 with strtol and strtod. - Added __attribute__ ((unused)) to yet unused variables - Added initialization for a few variables --- src/drivers/gps/ashtech.cpp | 170 +++++++++++++++----------------------------- 1 file changed, 56 insertions(+), 114 deletions(-) diff --git a/src/drivers/gps/ashtech.cpp b/src/drivers/gps/ashtech.cpp index 503e56dae..1b706b5b2 100644 --- a/src/drivers/gps/ashtech.cpp +++ b/src/drivers/gps/ashtech.cpp @@ -1,6 +1,7 @@ #include "ashtech.h" #include +#include #include #include #include @@ -15,67 +16,6 @@ #include #include -char *str_scanDec(const char *pos, int8_t sign, int8_t n_max_digit, int32_t *result) -{ - int8_t n = 0; - int32_t d = 0; - int8_t neg = 0; - - if (*pos == '-') { neg = 1; pos++; } - - else if (*pos == '+') { pos++; } - - else if (sign) { return (NULL); } - - while (*pos >= '0' && *pos <= '9') { - d = d * 10 + (*(pos++) - '0'); - n++; - - if (n_max_digit > 0 && n == n_max_digit) { break; } - } - - if (n == 0 || n > 10) { return (NULL); } - - if (neg) { *result = -d; } - - else { *result = d; } - - return ((char *)pos); -} - -char *scanFloat64(const char *pos, int8_t sign, int8_t n_max_int, int8_t n_max_frac, - double *result) -{ - double f = 0.0, div = 1.0; - int32_t d_int; - int8_t n = 0, isneg = 0; - - if (*pos == '-') { isneg = 1; } - - if ((pos = str_scanDec(pos, sign, n_max_int, &d_int)) == NULL) { return (NULL); } - - if (*(pos) == '.') { - pos++; - - while (*pos >= '0' && *pos <= '9') { - f = f * (10.0) + (double)(*(pos++) - '0'); - div *= (0.1); - n++; - - if (n_max_frac > 0 && n == n_max_frac) { break; } - } - - } else if (n_max_frac > 0) { return (NULL); } - - if (isneg) { *result = (double)d_int - f * div; } - - else { *result = (double)d_int + f * div; } - - return ((char *)pos); -} - - - ASHTECH::ASHTECH(const int &fd, struct vehicle_gps_position_s *gps_position, struct satellite_info_s *satellite_info): _fd(fd), _satellite_info(satellite_info), @@ -97,6 +37,8 @@ ASHTECH::~ASHTECH() int ASHTECH::handle_message(int len) { + char * endp; + if (len < 7) { return 0; } int uiCalcComma = 0; @@ -127,19 +69,19 @@ int ASHTECH::handle_message(int len) Fields 5 and 6 together yield the total offset. For example, if field 5 is -5 and field 6 is +15, local time is 5 hours and 15 minutes earlier than GMT. */ double ashtech_time = 0.0; - int day = 0, month = 0, year = 0, local_time_off_hour = 0, local_time_off_min = 0; + int day = 0, month = 0, year = 0, local_time_off_hour __attribute__((unused)) = 0, local_time_off_min __attribute__((unused)) = 0; - if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &ashtech_time); } + if (bufptr && *(++bufptr) != ',') { ashtech_time = strtod(bufptr, &endp); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &day); } + if (bufptr && *(++bufptr) != ',') { day = strtol(bufptr, &endp, 10); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &month); } + if (bufptr && *(++bufptr) != ',') { month = strtol(bufptr, &endp, 10); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &year); } + if (bufptr && *(++bufptr) != ',') { year = strtol(bufptr, &endp, 10); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &local_time_off_hour); } + if (bufptr && *(++bufptr) != ',') { local_time_off_hour = strtol(bufptr, &endp, 10); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &local_time_off_min); } + if (bufptr && *(++bufptr) != ',') { local_time_off_min = strtol(bufptr, &endp, 10); bufptr = endp; } int ashtech_hour = ashtech_time / 10000; @@ -200,28 +142,28 @@ int ASHTECH::handle_message(int len) The checksum data, always begins with * Note - If a user-defined geoid model, or an inclined */ - double ashtech_time = 0.0, lat = 0.0, lon = 0.0, alt = 0.0; - int num_of_sv = 0, fix_quality = 0; - double hdop = 99.9; + 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 hdop __attribute__((unused)) = 99.9; char ns = '?', ew = '?'; - if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &ashtech_time); } + if (bufptr && *(++bufptr) != ',') { ashtech_time = strtod(bufptr, &endp); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &lat); } + if (bufptr && *(++bufptr) != ',') { lat = strtod(bufptr, &endp); bufptr = endp; } if (bufptr && *(++bufptr) != ',') { ns = *(bufptr++); } - if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &lon); } + if (bufptr && *(++bufptr) != ',') { lon = strtod(bufptr, &endp); bufptr = endp; } if (bufptr && *(++bufptr) != ',') { ew = *(bufptr++); } - if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &fix_quality); } + if (bufptr && *(++bufptr) != ',') { fix_quality = strtol(bufptr, &endp, 10); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &num_of_sv); } + if (bufptr && *(++bufptr) != ',') { num_of_sv = strtol(bufptr, &endp, 10); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &hdop); } + if (bufptr && *(++bufptr) != ',') { hdop = strtod(bufptr, &endp); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &alt); } + if (bufptr && *(++bufptr) != ',') { alt = strtod(bufptr, &endp); bufptr = endp; } if (ns == 'S') { lat = -lat; @@ -287,43 +229,43 @@ int ASHTECH::handle_message(int len) *cc Checksum */ bufptr = (char *)(_rx_buffer + 10); - double ashtech_time = 0.0, lat = 0.0, lon = 0.0, alt = 0.0; - int num_of_sv = 0, fix_quality = 0; - double track_true = 0.0, ground_speed = 0.0 , age_of_corr = 0.0; - double hdop = 99.9, vdop = 99.9, pdop = 99.9, tdop = 99.9, vertic_vel = 0.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; + double hdop __attribute__((unused)) = 99.9, vdop __attribute__((unused)) = 99.9, pdop __attribute__((unused)) = 99.9, tdop __attribute__((unused)) = 99.9, vertic_vel = 0.0; char ns = '?', ew = '?'; - if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &fix_quality); } + if (bufptr && *(++bufptr) != ',') { fix_quality = strtol(bufptr, &endp, 10); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &num_of_sv); } + if (bufptr && *(++bufptr) != ',') { num_of_sv = strtol(bufptr, &endp, 10); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &ashtech_time); } + if (bufptr && *(++bufptr) != ',') { ashtech_time = strtod(bufptr, &endp); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &lat); } + if (bufptr && *(++bufptr) != ',') { lat = strtod(bufptr, &endp); bufptr = endp; } if (bufptr && *(++bufptr) != ',') { ns = *(bufptr++); } - if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &lon); } + if (bufptr && *(++bufptr) != ',') { lon = strtod(bufptr, &endp); bufptr = endp; } if (bufptr && *(++bufptr) != ',') { ew = *(bufptr++); } - if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &alt); } + if (bufptr && *(++bufptr) != ',') { alt = strtod(bufptr, &endp); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &age_of_corr); } + if (bufptr && *(++bufptr) != ',') { age_of_corr = strtod(bufptr, &endp); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &track_true); } + if (bufptr && *(++bufptr) != ',') { track_true = strtod(bufptr, &endp); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &ground_speed); } + if (bufptr && *(++bufptr) != ',') { ground_speed = strtod(bufptr, &endp); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &vertic_vel); } + if (bufptr && *(++bufptr) != ',') { vertic_vel = strtod(bufptr, &endp); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &pdop); } + if (bufptr && *(++bufptr) != ',') { pdop = strtod(bufptr, &endp); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &hdop); } + if (bufptr && *(++bufptr) != ',') { hdop = strtod(bufptr, &endp); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &vdop); } + if (bufptr && *(++bufptr) != ',') { vdop = strtod(bufptr, &endp); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &tdop); } + if (bufptr && *(++bufptr) != ',') { tdop = strtod(bufptr, &endp); bufptr = endp; } if (ns == 'S') { lat = -lat; @@ -389,24 +331,24 @@ int ASHTECH::handle_message(int len) 8 Height 1 sigma error, in meters 9 The checksum data, always begins with * */ - double ashtech_time = 0.0, lat_err = 0.0, lon_err = 0.0, alt_err = 0.0; - double min_err = 0.0, maj_err = 0.0, deg_from_north = 0.0, rms_err = 0.0; + double ashtech_time __attribute__((unused)) = 0.0, lat_err = 0.0, lon_err = 0.0, alt_err = 0.0; + double min_err __attribute__((unused)) = 0.0, maj_err __attribute__((unused)) = 0.0, deg_from_north __attribute__((unused)) = 0.0, rms_err __attribute__((unused)) = 0.0; - if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &ashtech_time); } + if (bufptr && *(++bufptr) != ',') { ashtech_time = strtod(bufptr, &endp); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &rms_err); } + if (bufptr && *(++bufptr) != ',') { rms_err = strtod(bufptr, &endp); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &maj_err); } + if (bufptr && *(++bufptr) != ',') { maj_err = strtod(bufptr, &endp); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &min_err); } + if (bufptr && *(++bufptr) != ',') { min_err = strtod(bufptr, &endp); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, °_from_north); } + if (bufptr && *(++bufptr) != ',') { deg_from_north = strtod(bufptr, &endp); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &lat_err); } + if (bufptr && *(++bufptr) != ',') { lat_err = strtod(bufptr, &endp); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &lon_err); } + if (bufptr && *(++bufptr) != ',') { lon_err = strtod(bufptr, &endp); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &alt_err); } + if (bufptr && *(++bufptr) != ',') { alt_err = strtod(bufptr, &endp); bufptr = endp; } _gps_position->eph = sqrt(lat_err * lat_err + lon_err * lon_err); _gps_position->epv = alt_err; @@ -448,7 +390,7 @@ int ASHTECH::handle_message(int len) bGPS = true; } - int all_msg_num, this_msg_num, tot_sv_visible; + int all_msg_num = 0, this_msg_num = 0, tot_sv_visible = 0; struct gsv_sat { int svid; int elevation; @@ -457,11 +399,11 @@ int ASHTECH::handle_message(int len) } sat[4]; memset(sat, 0, sizeof(sat)); - if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &all_msg_num); } + if (bufptr && *(++bufptr) != ',') { all_msg_num = strtol(bufptr, &endp, 10); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &this_msg_num); } + if (bufptr && *(++bufptr) != ',') { this_msg_num = strtol(bufptr, &endp, 10); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &tot_sv_visible); } + if (bufptr && *(++bufptr) != ',') { tot_sv_visible = strtol(bufptr, &endp, 10); bufptr = endp; } if ((this_msg_num < 1) || (this_msg_num > all_msg_num)) { return 0; @@ -485,13 +427,13 @@ int ASHTECH::handle_message(int len) } for (int y = 0 ; y < end ; y++) { - if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &sat[y].svid); } + if (bufptr && *(++bufptr) != ',') { sat[y].svid = strtol(bufptr, &endp, 10); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &sat[y].elevation); } + if (bufptr && *(++bufptr) != ',') { sat[y].elevation = strtol(bufptr, &endp, 10); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &sat[y].azimuth); } + if (bufptr && *(++bufptr) != ',') { sat[y].azimuth = strtol(bufptr, &endp, 10); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &sat[y].snr); } + if (bufptr && *(++bufptr) != ',') { sat[y].snr = strtol(bufptr, &endp, 10); bufptr = endp; } _satellite_info->svid[y + (this_msg_num - 1) * 4] = sat[y].svid; _satellite_info->used[y + (this_msg_num - 1) * 4] = ((sat[y].snr > 0) ? true : false); -- cgit v1.2.3