#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 / 1.9438445; /** knots to m/s */ 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,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"; 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; }