diff --git a/src/drivers/gps/ashtech.cpp b/src/drivers/gps/ashtech.cpp deleted file mode 100644 index 8f7cb65c97..0000000000 --- a/src/drivers/gps/ashtech.cpp +++ /dev/null @@ -1,683 +0,0 @@ -#include "ashtech.h" - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#ifdef __PX4_QURT -#include -#endif - -#include - -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) -{ - char *endp; - - 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. - */ - double ashtech_time = 0.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) != ',') { ashtech_time = strtod(bufptr, &endp); bufptr = endp; } - - if (bufptr && *(++bufptr) != ',') { day = strtol(bufptr, &endp, 10); bufptr = endp; } - - if (bufptr && *(++bufptr) != ',') { month = strtol(bufptr, &endp, 10); bufptr = endp; } - - if (bufptr && *(++bufptr) != ',') { year = strtol(bufptr, &endp, 10); bufptr = endp; } - - if (bufptr && *(++bufptr) != ',') { local_time_off_hour = strtol(bufptr, &endp, 10); bufptr = endp; } - - if (bufptr && *(++bufptr) != ',') { local_time_off_min = strtol(bufptr, &endp, 10); bufptr = endp; } - - - int ashtech_hour = static_cast(ashtech_time / 10000); - int ashtech_minute = static_cast((ashtech_time - ashtech_hour * 10000) / 100); - double ashtech_sec = static_cast(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); - - // TODO: this functionality is not available on the Snapdragon yet -#ifndef __PX4_QURT - time_t epoch = mktime(&timeinfo); - - if (epoch > GPS_EPOCH_SECS) { - uint64_t usecs = static_cast((ashtech_sec - static_cast(ashtech_sec))) * 1000000; - - // FMUv2+ boards have a hardware RTC, but GPS helps us to configure it - // and control its drift. Since we rely on the HRT for our monotonic - // clock, updating it from time to time is safe. - - timespec ts; - ts.tv_sec = epoch; - ts.tv_nsec = usecs * 1000; - - if (px4_clock_settime(CLOCK_REALTIME, &ts)) { - warn("failed setting clock"); - } - - _gps_position->time_utc_usec = static_cast(epoch) * 1000000ULL; - _gps_position->time_utc_usec += usecs; - - } else { - _gps_position->time_utc_usec = 0; - } - -#else - _gps_position->time_utc_usec = 0; -#endif - - _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 - */ - 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) != ',') { ashtech_time = strtod(bufptr, &endp); bufptr = endp; } - - if (bufptr && *(++bufptr) != ',') { lat = strtod(bufptr, &endp); bufptr = endp; } - - if (bufptr && *(++bufptr) != ',') { ns = *(bufptr++); } - - if (bufptr && *(++bufptr) != ',') { lon = strtod(bufptr, &endp); bufptr = endp; } - - if (bufptr && *(++bufptr) != ',') { ew = *(bufptr++); } - - if (bufptr && *(++bufptr) != ',') { fix_quality = strtol(bufptr, &endp, 10); bufptr = endp; } - - if (bufptr && *(++bufptr) != ',') { num_of_sv = strtol(bufptr, &endp, 10); bufptr = endp; } - - if (bufptr && *(++bufptr) != ',') { hdop = strtod(bufptr, &endp); bufptr = endp; } - - if (bufptr && *(++bufptr) != ',') { alt = strtod(bufptr, &endp); bufptr = endp; } - - if (ns == 'S') { - lat = -lat; - } - - if (ew == 'W') { - lon = -lon; - } - - /* convert from degrees, minutes and seconds to degrees * 1e7 */ - _gps_position->lat = static_cast((int(lat * 0.01) + (lat * 0.01 - int(lat * 0.01)) * 100.0 / 60.0) * 10000000); - _gps_position->lon = static_cast((int(lon * 0.01) + (lon * 0.01 - int(lon * 0.01)) * 100.0 / 60.0) * 10000000); - _gps_position->alt = static_cast(alt * 1000); - _rate_count_lat_lon++; - - if (fix_quality <= 0) { - _gps_position->fix_type = 0; - - } else { - /* - * in this NMEA message float integers (value 5) mode has higher value than fixed integers (value 4), whereas it provides lower quality, - * and since value 3 is not being used, I "moved" value 5 to 3 to add it to _gps_position->fix_type - */ - if (fix_quality == 5) { fix_quality = 3; } - - /* - * fix quality 1 means just a normal 3D fix, so I'm subtracting 1 here. This way we'll have 3 for auto, 4 for DGPS, 5 for floats, 6 for fixed. - */ - _gps_position->fix_type = 3 + fix_quality - 1; - } - - _gps_position->timestamp_position = hrt_absolute_time(); - - _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.1f; - _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); - - /* - * Ashtech would return empty space as coordinate (lat, lon or alt) if it doesn't have a fix yet - */ - int coordinatesFound = 0; - double ashtech_time __attribute__((unused)) = 0.0, lat = 0.0, lon = 0.0, alt = 0.0; - int num_of_sv __attribute__((unused)) = 0, fix_quality = 0; - double track_true = 0.0, ground_speed = 0.0 , age_of_corr __attribute__((unused)) = 0.0; - double hdop = 99.9, vdop = 99.9, pdop __attribute__((unused)) = 99.9, - tdop __attribute__((unused)) = 99.9, vertic_vel = 0.0; - char ns = '?', ew = '?'; - - if (bufptr && *(++bufptr) != ',') { fix_quality = strtol(bufptr, &endp, 10); bufptr = endp; } - - if (bufptr && *(++bufptr) != ',') { num_of_sv = strtol(bufptr, &endp, 10); bufptr = endp; } - - if (bufptr && *(++bufptr) != ',') { ashtech_time = strtod(bufptr, &endp); bufptr = endp; } - - if (bufptr && *(++bufptr) != ',') { - /* - * if a coordinate is skipped (i.e. no fix), it either won't get into this block (two commas in a row) - * or strtod won't find anything and endp will point exactly where bufptr is. The same is for lon and alt. - */ - lat = strtod(bufptr, &endp); - - if (bufptr != endp) {coordinatesFound++;} - - bufptr = endp; - } - - if (bufptr && *(++bufptr) != ',') { ns = *(bufptr++); } - - if (bufptr && *(++bufptr) != ',') { - lon = strtod(bufptr, &endp); - - if (bufptr != endp) {coordinatesFound++;} - - bufptr = endp; - } - - if (bufptr && *(++bufptr) != ',') { ew = *(bufptr++); } - - if (bufptr && *(++bufptr) != ',') { - alt = strtod(bufptr, &endp); - - if (bufptr != endp) {coordinatesFound++;} - - bufptr = endp; - } - - if (bufptr && *(++bufptr) != ',') { age_of_corr = strtod(bufptr, &endp); bufptr = endp; } - - if (bufptr && *(++bufptr) != ',') { track_true = strtod(bufptr, &endp); bufptr = endp; } - - if (bufptr && *(++bufptr) != ',') { ground_speed = strtod(bufptr, &endp); bufptr = endp; } - - if (bufptr && *(++bufptr) != ',') { vertic_vel = strtod(bufptr, &endp); bufptr = endp; } - - if (bufptr && *(++bufptr) != ',') { pdop = strtod(bufptr, &endp); bufptr = endp; } - - if (bufptr && *(++bufptr) != ',') { hdop = strtod(bufptr, &endp); bufptr = endp; } - - if (bufptr && *(++bufptr) != ',') { vdop = strtod(bufptr, &endp); bufptr = endp; } - - if (bufptr && *(++bufptr) != ',') { tdop = strtod(bufptr, &endp); bufptr = endp; } - - if (ns == 'S') { - lat = -lat; - } - - if (ew == 'W') { - lon = -lon; - } - - _gps_position->lat = static_cast((int(lat * 0.01) + (lat * 0.01 - int(lat * 0.01)) * 100.0 / 60.0) * 10000000); - _gps_position->lon = static_cast((int(lon * 0.01) + (lon * 0.01 - int(lon * 0.01)) * 100.0 / 60.0) * 10000000); - _gps_position->alt = static_cast(alt * 1000); - _gps_position->hdop = (float)hdop / 100.0f; - _gps_position->vdop = (float)vdop / 100.0f; - _rate_count_lat_lon++; - - if (coordinatesFound < 3) { - _gps_position->fix_type = 0; - - } else { - _gps_position->fix_type = 3 + fix_quality; - } - - _gps_position->timestamp_position = hrt_absolute_time(); - - float track_rad = static_cast(track_true) * M_PI_F / 180.0f; - - float velocity_ms = static_cast(ground_speed) / 1.9438445f; /** knots to m/s */ - float velocity_north = static_cast(velocity_ms) * cosf(track_rad); - float velocity_east = static_cast(velocity_ms) * sinf(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 = static_cast(-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.1f; - _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 * - */ - 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) != ',') { ashtech_time = strtod(bufptr, &endp); bufptr = endp; } - - if (bufptr && *(++bufptr) != ',') { rms_err = strtod(bufptr, &endp); bufptr = endp; } - - if (bufptr && *(++bufptr) != ',') { maj_err = strtod(bufptr, &endp); bufptr = endp; } - - if (bufptr && *(++bufptr) != ',') { min_err = strtod(bufptr, &endp); bufptr = endp; } - - if (bufptr && *(++bufptr) != ',') { deg_from_north = strtod(bufptr, &endp); bufptr = endp; } - - if (bufptr && *(++bufptr) != ',') { lat_err = strtod(bufptr, &endp); bufptr = endp; } - - if (bufptr && *(++bufptr) != ',') { lon_err = strtod(bufptr, &endp); bufptr = endp; } - - if (bufptr && *(++bufptr) != ',') { alt_err = strtod(bufptr, &endp); bufptr = endp; } - - _gps_position->eph = sqrtf(static_cast(lat_err) * static_cast(lat_err) - + static_cast(lon_err) * static_cast(lon_err)); - _gps_position->epv = static_cast(alt_err); - - _gps_position->s_variance_m_s = 0; - _gps_position->timestamp_variance = hrt_absolute_time(); - - } 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 = 0, this_msg_num = 0, tot_sv_visible = 0; - struct gsv_sat { - int svid; - int elevation; - int azimuth; - int snr; - } sat[4]; - memset(sat, 0, sizeof(sat)); - - if (bufptr && *(++bufptr) != ',') { all_msg_num = strtol(bufptr, &endp, 10); bufptr = endp; } - - if (bufptr && *(++bufptr) != ',') { this_msg_num = strtol(bufptr, &endp, 10); bufptr = endp; } - - 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; - } - - if (this_msg_num == 0 && bGPS && _satellite_info) { - 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; - - if (_satellite_info) { - _satellite_info->count = satellite_info_s::SAT_INFO_MAX_SATELLITES; - _satellite_info->timestamp = hrt_absolute_time(); - } - } - - if (_satellite_info) { - for (int y = 0 ; y < end ; y++) { - if (bufptr && *(++bufptr) != ',') { sat[y].svid = strtol(bufptr, &endp, 10); bufptr = endp; } - - if (bufptr && *(++bufptr) != ',') { sat[y].elevation = strtol(bufptr, &endp, 10); bufptr = endp; } - - if (bufptr && *(++bufptr) != ',') { sat[y].azimuth = strtol(bufptr, &endp, 10); bufptr = endp; } - - 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); - _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) -{ - { - - uint8_t buf[32]; - - /* timeout additional to poll */ - uint64_t time_started = hrt_absolute_time(); - - int j = 0; - ssize_t bytes_count = 0; - - while (true) { - - /* pass received bytes to the packet decoder */ - while (j < bytes_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; - } - } - - j++; - } - - /* everything is read */ - j = bytes_count = 0; - - /* then poll or read for new data */ - int ret = poll_or_read(_fd, buf, sizeof(buf), timeout * 2); - - if (ret < 0) { - /* something went wrong when polling */ - return -1; - - } else if (ret == 0) { - /* Timeout while polling or just nothing read if reading, let's - * stay here, and use timeout below. */ - - } else if (ret > 0) { - /* if we have new data from GPS, go handle it */ - bytes_count = ret; - } - - /* in case we get crap from GPS or time out */ - if (time_started + timeout * 1000 * 2 < hrt_absolute_time()) { - return -1; - } - } - } - -} -#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 board configuration script - */ - -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"\ - "$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 (unsigned int baud_i = 0; baud_i < sizeof(baudrates_to_try) / sizeof(baudrates_to_try[0]); baud_i++) { - baudrate = baudrates_to_try[baud_i]; - set_baudrate(_fd, baudrate); - - if (write(_fd, comm, sizeof(comm)) != sizeof(comm)) { - return 1; - } - } - - set_baudrate(_fd, 115200); - return 0; -} diff --git a/src/drivers/gps/ashtech.h b/src/drivers/gps/ashtech.h deleted file mode 100644 index e7b9e40328..0000000000 --- a/src/drivers/gps/ashtech.h +++ /dev/null @@ -1,96 +0,0 @@ -/**************************************************************************** - * - * 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 -#endif - -#include - - -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; - - 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[satellite_info_s::SAT_INFO_MAX_SATELLITES]; /**< Space vehicle ID [1..255], see scheme below */ - uint8_t used[satellite_info_s::SAT_INFO_MAX_SATELLITES]; /**< 0: Satellite not used, 1: used for navigation */ - uint8_t elevation[satellite_info_s::SAT_INFO_MAX_SATELLITES]; /**< Elevation (0: right on top of receiver, 90: on the horizon) of satellite */ - uint8_t azimuth[satellite_info_s::SAT_INFO_MAX_SATELLITES]; /**< Direction of satellite, 0: 0 deg, 255: 360 deg. */ - uint8_t snr[satellite_info_s::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 3f117fc197..31dfac7b0f 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -639,6 +639,7 @@ GPS::task_main() } else { _report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos); } + last_rate_count++; } diff --git a/src/drivers/gps/gps_helper.cpp b/src/drivers/gps/gps_helper.cpp deleted file mode 100644 index 7ac6dceda5..0000000000 --- a/src/drivers/gps/gps_helper.cpp +++ /dev/null @@ -1,210 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012-2016 PX4 Development Team. All rights reserved. - * - * 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. - * - ****************************************************************************/ - -#ifndef __PX4_QURT -#include -#include -#else -#include -#include -#endif - -#include -#include -#include -#include - -#include "gps_helper.h" - -/** - * @file gps_helper.cpp - * - * @author Thomas Gubler - * @author Julian Oes - */ - -#define GPS_WAIT_BEFORE_READ 20 // ms, wait before reading to save read() calls - - -float -GPS_Helper::get_position_update_rate() -{ - return _rate_lat_lon; -} - -float -GPS_Helper::get_velocity_update_rate() -{ - return _rate_vel; -} - -void -GPS_Helper::reset_update_rates() -{ - _rate_count_vel = 0; - _rate_count_lat_lon = 0; - _interval_rate_start = hrt_absolute_time(); -} - -void -GPS_Helper::store_update_rates() -{ - _rate_vel = _rate_count_vel / (((float)(hrt_absolute_time() - _interval_rate_start)) / 1000000.0f); - _rate_lat_lon = _rate_count_lat_lon / (((float)(hrt_absolute_time() - _interval_rate_start)) / 1000000.0f); -} - -int -GPS_Helper::set_baudrate(const int &fd, unsigned baud) -{ - -#if __PX4_QURT - // TODO: currently QURT does not support configuration with termios. - dspal_serial_ioctl_data_rate data_rate; - - switch (baud) { - case 9600: data_rate.bit_rate = DSPAL_SIO_BITRATE_9600; break; - - case 19200: data_rate.bit_rate = DSPAL_SIO_BITRATE_19200; break; - - case 38400: data_rate.bit_rate = DSPAL_SIO_BITRATE_38400; break; - - case 57600: data_rate.bit_rate = DSPAL_SIO_BITRATE_57600; break; - - case 115200: data_rate.bit_rate = DSPAL_SIO_BITRATE_115200; break; - - default: - PX4_ERR("ERR: unknown baudrate: %d", baud); - return -EINVAL; - } - - int ret = ::ioctl(fd, SERIAL_IOCTL_SET_DATA_RATE, (void *)&data_rate); - - if (ret != 0) { - - return ret; - } - -#else - /* process baud rate */ - int speed; - - switch (baud) { - case 9600: speed = B9600; break; - - case 19200: speed = B19200; break; - - case 38400: speed = B38400; break; - - case 57600: speed = B57600; break; - - case 115200: speed = B115200; break; - - default: - PX4_ERR("ERR: unknown baudrate: %d", baud); - return -EINVAL; - } - - struct termios uart_config; - - int termios_state; - - /* fill the struct for the new configuration */ - tcgetattr(fd, &uart_config); - - /* clear ONLCR flag (which appends a CR for every LF) */ - uart_config.c_oflag &= ~ONLCR; - /* no parity, one stop bit */ - uart_config.c_cflag &= ~(CSTOPB | PARENB); - - /* set baud rate */ - if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) { - warnx("ERR: %d (cfsetispeed)\n", termios_state); - return -1; - } - - if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) { - warnx("ERR: %d (cfsetospeed)\n", termios_state); - return -1; - } - - if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) { - warnx("ERR: %d (tcsetattr)\n", termios_state); - return -1; - } - -#endif - return 0; -} - -int -GPS_Helper::poll_or_read(int fd, uint8_t *buf, size_t buf_length, uint64_t timeout) -{ - -#ifndef __PX4_QURT - - /* For non QURT, use the usual polling. */ - - pollfd fds[1]; - fds[0].fd = fd; - fds[0].events = POLLIN; - - /* Poll for new data, */ - int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), timeout); - - 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. But don't read immediately - * by 1-2 bytes, wait for some more data to save expensive read() calls. - * If more bytes are available, we'll go back to poll() again. - */ - usleep(GPS_WAIT_BEFORE_READ * 1000); - return ::read(fd, buf, buf_length); - - } else { - return -1; - } - - } else { - return ret; - } - -#else - /* For QURT, just use read for now, since this doesn't block, we need to slow it down - * just a bit. */ - usleep(10000); - return ::read(fd, buf, buf_length); -#endif -} diff --git a/src/drivers/gps/gps_helper.h b/src/drivers/gps/gps_helper.h deleted file mode 100644 index e30313a893..0000000000 --- a/src/drivers/gps/gps_helper.h +++ /dev/null @@ -1,88 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. - * - * 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 gps_helper.h - * @author Thomas Gubler - * @author Julian Oes - */ - -#ifndef GPS_HELPER_H -#define GPS_HELPER_H - -#include -#include - -// TODO: this number seems wrong -#define GPS_EPOCH_SECS 1234567890ULL - -class GPS_Helper -{ -public: - - GPS_Helper() {}; - virtual ~GPS_Helper() {}; - - virtual int configure(unsigned &baud) = 0; - virtual int receive(unsigned timeout) = 0; - int set_baudrate(const int &fd, unsigned baud); - float get_position_update_rate(); - float get_velocity_update_rate(); - void reset_update_rates(); - void store_update_rates(); - - /* This is an abstraction for the poll on serial used. The - * implementation is different for QURT than for POSIX and - * NuttX. - * - * @param fd: serial file descriptor - * @param buf: pointer to read buffer - * @param buf_length: size of read buffer - * @param timeout: timeout time in us - * @return: 0 for nothing read, or poll timed out - * < 0 for error - * > 0 number of bytes read - */ - int poll_or_read(int fd, uint8_t *buf, size_t buf_length, uint64_t timeout); - -protected: - uint8_t _rate_count_lat_lon; - uint8_t _rate_count_vel; - - float _rate_lat_lon = 0.0f; - float _rate_vel = 0.0f; - - uint64_t _interval_rate_start; -}; - -#endif /* GPS_HELPER_H */ diff --git a/src/drivers/gps/mtk.cpp b/src/drivers/gps/mtk.cpp deleted file mode 100644 index ea3b33618d..0000000000 --- a/src/drivers/gps/mtk.cpp +++ /dev/null @@ -1,308 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. - * - * 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 mtk.cpp - * - * @author Thomas Gubler - * @author Julian Oes - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "mtk.h" - - -MTK::MTK(const int &fd, struct vehicle_gps_position_s *gps_position) : - _fd(fd), - _gps_position(gps_position), - _mtk_revision(0) -{ - decode_init(); -} - -MTK::~MTK() -{ -} - -int -MTK::configure(unsigned &baudrate) -{ - /* set baudrate first */ - if (GPS_Helper::set_baudrate(_fd, MTK_BAUDRATE) != 0) { - return -1; - } - - baudrate = MTK_BAUDRATE; - - /* Write config messages, don't wait for an answer */ - if (strlen(MTK_OUTPUT_5HZ) != write(_fd, MTK_OUTPUT_5HZ, strlen(MTK_OUTPUT_5HZ))) { - goto errout; - } - - usleep(10000); - - if (strlen(MTK_SET_BINARY) != write(_fd, MTK_SET_BINARY, strlen(MTK_SET_BINARY))) { - goto errout; - } - - usleep(10000); - - if (strlen(SBAS_ON) != write(_fd, SBAS_ON, strlen(SBAS_ON))) { - goto errout; - } - - usleep(10000); - - if (strlen(WAAS_ON) != write(_fd, WAAS_ON, strlen(WAAS_ON))) { - goto errout; - } - - usleep(10000); - - if (strlen(MTK_NAVTHRES_OFF) != write(_fd, MTK_NAVTHRES_OFF, strlen(MTK_NAVTHRES_OFF))) { - goto errout; - } - - return 0; - -errout: - warnx("mtk: config write failed"); - return -1; -} - -int -MTK::receive(unsigned timeout) -{ - uint8_t buf[32]; - gps_mtk_packet_t packet; - - /* timeout additional to poll */ - uint64_t time_started = hrt_absolute_time(); - - int j = 0; - - while (true) { - - int ret = poll_or_read(_fd, buf, sizeof(buf), timeout); - - if (ret > 0) { - /* first read whatever is left */ - if (j < ret) { - /* pass received bytes to the packet decoder */ - while (j < ret) { - if (parse_char(buf[j], packet) > 0) { - handle_message(packet); - return 1; - } - - j++; - } - - /* everything is read */ - j = 0; - } - - } else { - usleep(20000); - } - - /* in case we keep trying but only get crap from GPS */ - if (time_started + timeout * 1000 < hrt_absolute_time()) { - return -1; - } - } -} - -void -MTK::decode_init(void) -{ - _rx_ck_a = 0; - _rx_ck_b = 0; - _rx_count = 0; - _decode_state = MTK_DECODE_UNINIT; -} -int -MTK::parse_char(uint8_t b, gps_mtk_packet_t &packet) -{ - int ret = 0; - - if (_decode_state == MTK_DECODE_UNINIT) { - - if (b == MTK_SYNC1_V16) { - _decode_state = MTK_DECODE_GOT_CK_A; - _mtk_revision = 16; - - } else if (b == MTK_SYNC1_V19) { - _decode_state = MTK_DECODE_GOT_CK_A; - _mtk_revision = 19; - } - - } else if (_decode_state == MTK_DECODE_GOT_CK_A) { - if (b == MTK_SYNC2) { - _decode_state = MTK_DECODE_GOT_CK_B; - - } else { - // Second start symbol was wrong, reset state machine - decode_init(); - } - - } else if (_decode_state == MTK_DECODE_GOT_CK_B) { - // Add to checksum - if (_rx_count < 33) { - add_byte_to_checksum(b); - } - - // Fill packet buffer - ((uint8_t *)(&packet))[_rx_count] = b; - _rx_count++; - - /* Packet size minus checksum, XXX ? */ - if (_rx_count >= sizeof(packet)) { - /* Compare checksum */ - if (_rx_ck_a == packet.ck_a && _rx_ck_b == packet.ck_b) { - ret = 1; - - } else { - ret = -1; - } - - // Reset state machine to decode next packet - decode_init(); - } - } - - return ret; -} - -void -MTK::handle_message(gps_mtk_packet_t &packet) -{ - if (_mtk_revision == 16) { - _gps_position->lat = packet.latitude * 10; // from degrees*1e6 to degrees*1e7 - _gps_position->lon = packet.longitude * 10; // from degrees*1e6 to degrees*1e7 - - } else if (_mtk_revision == 19) { - _gps_position->lat = packet.latitude; // both degrees*1e7 - _gps_position->lon = packet.longitude; // both degrees*1e7 - - } else { - warnx("mtk: unknown revision"); - _gps_position->lat = 0; - _gps_position->lon = 0; - - // Indicate this data is not usable and bail out - _gps_position->eph = 1000.0f; - _gps_position->epv = 1000.0f; - _gps_position->fix_type = 0; - return; - } - - _gps_position->alt = (int32_t)(packet.msl_altitude * 10); // from cm to mm - _gps_position->fix_type = packet.fix_type; - _gps_position->eph = packet.hdop / 100.0f; // from cm to m - _gps_position->epv = _gps_position->eph; // unknown in mtk custom mode, so we cheat with eph - _gps_position->hdop = packet.hdop / 100.0f; - _gps_position->vdop = _gps_position->hdop; - _gps_position->vel_m_s = ((float)packet.ground_speed) / 100.0f; // from cm/s to m/s - _gps_position->cog_rad = ((float)packet.heading) * M_DEG_TO_RAD_F * 1e-2f; //from deg *100 to rad - _gps_position->satellites_used = packet.satellites; - - /* convert time and date information to unix timestamp */ - struct tm timeinfo; - uint32_t timeinfo_conversion_temp; - - timeinfo.tm_mday = packet.date / 10000; - timeinfo_conversion_temp = packet.date - timeinfo.tm_mday * 10000; - timeinfo.tm_mon = (timeinfo_conversion_temp / 100) - 1; - timeinfo.tm_year = (timeinfo_conversion_temp - (timeinfo.tm_mon + 1) * 100) + 100; - - timeinfo.tm_hour = (packet.utc_time / 10000000); - timeinfo_conversion_temp = packet.utc_time - timeinfo.tm_hour * 10000000; - timeinfo.tm_min = timeinfo_conversion_temp / 100000; - timeinfo_conversion_temp -= timeinfo.tm_min * 100000; - timeinfo.tm_sec = timeinfo_conversion_temp / 1000; - timeinfo_conversion_temp -= timeinfo.tm_sec * 1000; - - // TODO: this functionality is not available on the Snapdragon yet -#ifndef __PX4_QURT - - time_t epoch = mktime(&timeinfo); - - if (epoch > GPS_EPOCH_SECS) { - // FMUv2+ boards have a hardware RTC, but GPS helps us to configure it - // and control its drift. Since we rely on the HRT for our monotonic - // clock, updating it from time to time is safe. - - timespec ts; - ts.tv_sec = epoch; - ts.tv_nsec = timeinfo_conversion_temp * 1000000ULL; - - if (px4_clock_settime(CLOCK_REALTIME, &ts)) { - warn("failed setting clock"); - } - - _gps_position->time_utc_usec = static_cast(epoch) * 1000000ULL; - _gps_position->time_utc_usec += timeinfo_conversion_temp * 1000ULL; - - } else { - _gps_position->time_utc_usec = 0; - } - -#else - _gps_position->time_utc_usec = 0; -#endif - - _gps_position->timestamp_position = _gps_position->timestamp_time = hrt_absolute_time(); - - // Position and velocity update always at the same time - _rate_count_vel++; - _rate_count_lat_lon++; - - return; -} - -void -MTK::add_byte_to_checksum(uint8_t b) -{ - _rx_ck_a = _rx_ck_a + b; - _rx_ck_b = _rx_ck_b + _rx_ck_a; -} diff --git a/src/drivers/gps/mtk.h b/src/drivers/gps/mtk.h deleted file mode 100644 index 9ff894e69b..0000000000 --- a/src/drivers/gps/mtk.h +++ /dev/null @@ -1,126 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012-2016 PX4 Development Team. All rights reserved. - * - * 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 mtk.cpp - * - * @author Thomas Gubler - * @author Julian Oes - */ - -#ifndef MTK_H_ -#define MTK_H_ - -#include "gps_helper.h" - -#define MTK_SYNC1_V16 0xd0 -#define MTK_SYNC1_V19 0xd1 -#define MTK_SYNC2 0xdd - -#define MTK_OUTPUT_5HZ "$PMTK220,200*2C\r\n" -#define MTK_SET_BINARY "$PGCMD,16,0,0,0,0,0*6A\r\n" -#define SBAS_ON "$PMTK313,1*2E\r\n" -#define WAAS_ON "$PMTK301,2*2E\r\n" -#define MTK_NAVTHRES_OFF "$PMTK397,0*23\r\n" - -#define MTK_TIMEOUT_5HZ 400 -#define MTK_BAUDRATE 38400 - -typedef enum { - MTK_DECODE_UNINIT = 0, - MTK_DECODE_GOT_CK_A = 1, - MTK_DECODE_GOT_CK_B = 2 -} mtk_decode_state_t; - -/** the structures of the binary packets */ -#pragma pack(push, 1) - -typedef struct { - uint8_t payload; ///< Number of payload bytes - int32_t latitude; ///< Latitude in degrees * 10^7 - int32_t longitude; ///< Longitude in degrees * 10^7 - uint32_t msl_altitude; ///< MSL altitude in meters * 10^2 - uint32_t ground_speed; ///< velocity in m/s - int32_t heading; ///< heading in degrees * 10^2 - uint8_t satellites; ///< number of satellites used - uint8_t fix_type; ///< fix type: XXX correct for that - uint32_t date; - uint32_t utc_time; - uint16_t hdop; ///< horizontal dilution of position (without unit) - uint8_t ck_a; - uint8_t ck_b; -} gps_mtk_packet_t; - -#pragma pack(pop) - -#define MTK_RECV_BUFFER_SIZE 40 - -class MTK : public GPS_Helper -{ -public: - MTK(const int &fd, struct vehicle_gps_position_s *gps_position); - ~MTK(); - int receive(unsigned timeout); - int configure(unsigned &baudrate); - -private: - /** - * Parse the binary MTK packet - */ - int parse_char(uint8_t b, gps_mtk_packet_t &packet); - - /** - * Handle the package once it has arrived - */ - void handle_message(gps_mtk_packet_t &packet); - - /** - * Reset the parse state machine for a fresh start - */ - void decode_init(void); - - /** - * While parsing add every byte (except the sync bytes) to the checksum - */ - void add_byte_to_checksum(uint8_t); - - int _fd; - struct vehicle_gps_position_s *_gps_position; - mtk_decode_state_t _decode_state; - uint8_t _mtk_revision; - unsigned _rx_count; - uint8_t _rx_ck_a; - uint8_t _rx_ck_b; -}; - -#endif /* MTK_H_ */ diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp deleted file mode 100644 index e331a70166..0000000000 --- a/src/drivers/gps/ubx.cpp +++ /dev/null @@ -1,1189 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. - * - * 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 ubx.cpp - * - * U-Blox protocol implementation. Following u-blox 6/7/8 Receiver Description - * including Prototol Specification. - * - * @author Thomas Gubler - * @author Julian Oes - * @author Anton Babushkin - * - * @author Hannes Delago - * (rework, add ubx7+ compatibility) - * - * @see https://www2.u-blox.com/images/downloads/Product_Docs/u-blox6-GPS-GLONASS-QZSS-V14_ReceiverDescriptionProtocolSpec_Public_(GPS.G6-SW-12013).pdf - * @see https://www.u-blox.com/sites/default/files/products/documents/u-bloxM8_ReceiverDescrProtSpec_%28UBX-13003221%29_Public.pdf - */ - -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include - -#include "ubx.h" - -#define UBX_CONFIG_TIMEOUT 200 // ms, timeout for waiting ACK -#define UBX_PACKET_TIMEOUT 2 // ms, if now data during this delay assume that full update received -#define DISABLE_MSG_INTERVAL 1000000 // us, try to disable message with this interval - -#define MIN(X,Y) ((X) < (Y) ? (X) : (Y)) -#define SWAP16(X) ((((X) >> 8) & 0x00ff) | (((X) << 8) & 0xff00)) - -#define FNV1_32_INIT ((uint32_t)0x811c9dc5) // init value for FNV1 hash algorithm -#define FNV1_32_PRIME ((uint32_t)0x01000193) // magic prime for FNV1 hash algorithm - - -/**** Trace macros, disable for production builds */ -#define UBX_TRACE_PARSER(s, ...) {/*PX4_INFO(s, ## __VA_ARGS__);*/} /* decoding progress in parse_char() */ -#define UBX_TRACE_RXMSG(s, ...) {/*PX4_INFO(s, ## __VA_ARGS__);*/} /* Rx msgs in payload_rx_done() */ -#define UBX_TRACE_SVINFO(s, ...) {/*PX4_INFO(s, ## __VA_ARGS__);*/} /* NAV-SVINFO processing (debug use only, will cause rx buffer overflows) */ - -/**** Warning macros, disable to save memory */ -#define UBX_WARN(s, ...) {PX4_WARN(s, ## __VA_ARGS__);} -#define UBX_DEBUG(s, ...) {/*PX4_WARN(s, ## __VA_ARGS__);*/} - -UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position, struct satellite_info_s *satellite_info) : - _fd(fd), - _gps_position(gps_position), - _satellite_info(satellite_info), - _configured(false), - _ack_state(UBX_ACK_IDLE), - _got_posllh(false), - _got_velned(false), - _disable_cmd_last(0), - _ack_waiting_msg(0), - _ubx_version(0), - _use_nav_pvt(false) -{ - decode_init(); -} - -UBX::~UBX() -{ -} - -int -UBX::configure(unsigned &baudrate) -{ - _configured = false; - /* try different baudrates */ - const unsigned baudrates[] = {9600, 38400, 19200, 57600, 115200}; - - unsigned baud_i; - ubx_payload_tx_cfg_prt_t cfg_prt[2]; - - for (baud_i = 0; baud_i < sizeof(baudrates) / sizeof(baudrates[0]); baud_i++) { - baudrate = baudrates[baud_i]; - set_baudrate(_fd, baudrate); - - /* flush input and wait for at least 20 ms silence */ - decode_init(); - receive(20); - decode_init(); - - /* Send a CFG-PRT message to set the UBX protocol for in and out - * and leave the baudrate as it is, we just want an ACK-ACK for this */ - memset(cfg_prt, 0, 2 * sizeof(ubx_payload_tx_cfg_prt_t)); - cfg_prt[0].portID = UBX_TX_CFG_PRT_PORTID; - cfg_prt[0].mode = UBX_TX_CFG_PRT_MODE; - cfg_prt[0].baudRate = baudrate; - cfg_prt[0].inProtoMask = UBX_TX_CFG_PRT_INPROTOMASK; - cfg_prt[0].outProtoMask = UBX_TX_CFG_PRT_OUTPROTOMASK; - cfg_prt[1].portID = UBX_TX_CFG_PRT_PORTID_USB; - cfg_prt[1].mode = UBX_TX_CFG_PRT_MODE; - cfg_prt[1].baudRate = baudrate; - cfg_prt[1].inProtoMask = UBX_TX_CFG_PRT_INPROTOMASK; - cfg_prt[1].outProtoMask = UBX_TX_CFG_PRT_OUTPROTOMASK; - - if (!send_message(UBX_MSG_CFG_PRT, (uint8_t *)cfg_prt, 2 * sizeof(ubx_payload_tx_cfg_prt_t))) { - continue; - } - - if (wait_for_ack(UBX_MSG_CFG_PRT, UBX_CONFIG_TIMEOUT, false) < 0) { - /* try next baudrate */ - continue; - } - - /* Send a CFG-PRT message again, this time change the baudrate */ - memset(cfg_prt, 0, 2 * sizeof(ubx_payload_tx_cfg_prt_t)); - cfg_prt[0].portID = UBX_TX_CFG_PRT_PORTID; - cfg_prt[0].mode = UBX_TX_CFG_PRT_MODE; - cfg_prt[0].baudRate = UBX_TX_CFG_PRT_BAUDRATE; - cfg_prt[0].inProtoMask = UBX_TX_CFG_PRT_INPROTOMASK; - cfg_prt[0].outProtoMask = UBX_TX_CFG_PRT_OUTPROTOMASK; - cfg_prt[1].portID = UBX_TX_CFG_PRT_PORTID_USB; - cfg_prt[1].mode = UBX_TX_CFG_PRT_MODE; - cfg_prt[1].baudRate = UBX_TX_CFG_PRT_BAUDRATE; - cfg_prt[1].inProtoMask = UBX_TX_CFG_PRT_INPROTOMASK; - cfg_prt[1].outProtoMask = UBX_TX_CFG_PRT_OUTPROTOMASK; - - if (!send_message(UBX_MSG_CFG_PRT, (uint8_t *)cfg_prt, 2 * sizeof(ubx_payload_tx_cfg_prt_t))) { - continue; - } - - /* no ACK is expected here, but read the buffer anyway in case we actually get an ACK */ - wait_for_ack(UBX_MSG_CFG_PRT, UBX_CONFIG_TIMEOUT, false); - - if (UBX_TX_CFG_PRT_BAUDRATE != baudrate) { - set_baudrate(_fd, UBX_TX_CFG_PRT_BAUDRATE); - baudrate = UBX_TX_CFG_PRT_BAUDRATE; - } - - /* at this point we have correct baudrate on both ends */ - break; - } - - if (baud_i >= sizeof(baudrates) / sizeof(baudrates[0])) { - return 1; // connection and/or baudrate detection failed - } - - /* Send a CFG-RATE message to define update rate */ - memset(&_buf.payload_tx_cfg_rate, 0, sizeof(_buf.payload_tx_cfg_rate)); - _buf.payload_tx_cfg_rate.measRate = UBX_TX_CFG_RATE_MEASINTERVAL; - _buf.payload_tx_cfg_rate.navRate = UBX_TX_CFG_RATE_NAVRATE; - _buf.payload_tx_cfg_rate.timeRef = UBX_TX_CFG_RATE_TIMEREF; - - if (!send_message(UBX_MSG_CFG_RATE, (uint8_t *)&_buf, sizeof(_buf.payload_tx_cfg_rate))) { - return 1; - } - - if (wait_for_ack(UBX_MSG_CFG_RATE, UBX_CONFIG_TIMEOUT, true) < 0) { - return 1; - } - - /* send a NAV5 message to set the options for the internal filter */ - memset(&_buf.payload_tx_cfg_nav5, 0, sizeof(_buf.payload_tx_cfg_nav5)); - _buf.payload_tx_cfg_nav5.mask = UBX_TX_CFG_NAV5_MASK; - _buf.payload_tx_cfg_nav5.dynModel = UBX_TX_CFG_NAV5_DYNMODEL; - _buf.payload_tx_cfg_nav5.fixMode = UBX_TX_CFG_NAV5_FIXMODE; - - if (!send_message(UBX_MSG_CFG_NAV5, (uint8_t *)&_buf, sizeof(_buf.payload_tx_cfg_nav5))) { - return 1; - } - - if (wait_for_ack(UBX_MSG_CFG_NAV5, UBX_CONFIG_TIMEOUT, true) < 0) { - return 1; - } - -#ifdef UBX_CONFIGURE_SBAS - /* send a SBAS message to set the SBAS options */ - memset(&_buf.payload_tx_cfg_sbas, 0, sizeof(_buf.payload_tx_cfg_sbas)); - _buf.payload_tx_cfg_sbas.mode = UBX_TX_CFG_SBAS_MODE; - - if (!send_message(UBX_MSG_CFG_SBAS, (uint8_t *)&_buf, sizeof(_buf.payload_tx_cfg_sbas))) { - return 1; - } - - if (wait_for_ack(UBX_MSG_CFG_SBAS, UBX_CONFIG_TIMEOUT, true) < 0) { - return 1; - } - -#endif - - /* configure message rates */ - /* the last argument is divisor for measurement rate (set by CFG RATE), i.e. 1 means 5Hz */ - - /* try to set rate for NAV-PVT */ - /* (implemented for ubx7+ modules only, use NAV-SOL, NAV-POSLLH, NAV-VELNED and NAV-TIMEUTC for ubx6) */ - if (!configure_message_rate(UBX_MSG_NAV_PVT, 1)) { - return 1; - } - - if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) { - _use_nav_pvt = false; - - } else { - _use_nav_pvt = true; - } - - UBX_DEBUG("%susing NAV-PVT", _use_nav_pvt ? "" : "not "); - - if (!_use_nav_pvt) { - if (!configure_message_rate(UBX_MSG_NAV_TIMEUTC, 5)) { - return 1; - } - - if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) { - return 1; - } - - if (!configure_message_rate(UBX_MSG_NAV_POSLLH, 1)) { - return 1; - } - - if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) { - return 1; - } - - if (!configure_message_rate(UBX_MSG_NAV_SOL, 1)) { - return 1; - } - - if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) { - return 1; - } - - if (!configure_message_rate(UBX_MSG_NAV_VELNED, 1)) { - return 1; - } - - if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) { - return 1; - } - } - - if (!configure_message_rate(UBX_MSG_NAV_DOP, 1)) { - return 1; - } - - if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) { - return 1; - } - - if (!configure_message_rate(UBX_MSG_NAV_SVINFO, (_satellite_info != nullptr) ? 5 : 0)) { - return 1; - } - - if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) { - return 1; - } - - if (!configure_message_rate(UBX_MSG_MON_HW, 1)) { - return 1; - } - - if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) { - return 1; - } - - /* request module version information by sending an empty MON-VER message */ - if (!send_message(UBX_MSG_MON_VER, nullptr, 0)) { - return 1; - } - - _configured = true; - return 0; -} - -int // -1 = NAK, error or timeout, 0 = ACK -UBX::wait_for_ack(const uint16_t msg, const unsigned timeout, const bool report) -{ - int ret = -1; - - _ack_state = UBX_ACK_WAITING; - _ack_waiting_msg = msg; // memorize sent msg class&ID for ACK check - - hrt_abstime time_started = hrt_absolute_time(); - - while ((_ack_state == UBX_ACK_WAITING) && (hrt_absolute_time() < time_started + timeout * 1000)) { - receive(timeout); - } - - if (_ack_state == UBX_ACK_GOT_ACK) { - ret = 0; // ACK received ok - - } else if (report) { - if (_ack_state == UBX_ACK_GOT_NAK) { - UBX_DEBUG("ubx msg 0x%04x NAK", SWAP16((unsigned)msg)); - - } else { - UBX_DEBUG("ubx msg 0x%04x ACK timeout", SWAP16((unsigned)msg)); - } - } - - _ack_state = UBX_ACK_IDLE; - return ret; -} - -int // -1 = error, 0 = no message handled, 1 = message handled, 2 = sat info message handled -UBX::receive(const unsigned timeout) -{ - uint8_t buf[128]; - - /* timeout additional to poll */ - uint64_t time_started = hrt_absolute_time(); - - int handled = 0; - - while (true) { - bool ready_to_return = _configured ? (_got_posllh && _got_velned) : handled; - - /* Wait for only UBX_PACKET_TIMEOUT if something already received. */ - int ret = poll_or_read(_fd, buf, sizeof(buf), ready_to_return ? UBX_PACKET_TIMEOUT : timeout); - - if (ret < 0) { - /* something went wrong when polling or reading */ - UBX_WARN("ubx poll_or_read err"); - return -1; - - } else if (ret == 0) { - /* return success if ready */ - if (ready_to_return) { - _got_posllh = false; - _got_velned = false; - return handled; - } - - } else { - //UBX_DEBUG("read %d bytes", ret); - - /* pass received bytes to the packet decoder */ - for (int i = 0; i < ret; i++) { - handled |= parse_char(buf[i]); - //UBX_DEBUG("parsed %d: 0x%x", i, buf[i]); - } - } - - /* abort after timeout if no useful packets received */ - if (time_started + timeout * 1000 < hrt_absolute_time()) { - UBX_DEBUG("timed out, returning"); - return -1; - } - } -} - -int // 0 = decoding, 1 = message handled, 2 = sat info message handled -UBX::parse_char(const uint8_t b) -{ - int ret = 0; - - switch (_decode_state) { - - /* Expecting Sync1 */ - case UBX_DECODE_SYNC1: - if (b == UBX_SYNC1) { // Sync1 found --> expecting Sync2 - UBX_TRACE_PARSER("A"); - _decode_state = UBX_DECODE_SYNC2; - } - - break; - - /* Expecting Sync2 */ - case UBX_DECODE_SYNC2: - if (b == UBX_SYNC2) { // Sync2 found --> expecting Class - UBX_TRACE_PARSER("B"); - _decode_state = UBX_DECODE_CLASS; - - } else { // Sync1 not followed by Sync2: reset parser - decode_init(); - } - - break; - - /* Expecting Class */ - case UBX_DECODE_CLASS: - UBX_TRACE_PARSER("C"); - add_byte_to_checksum(b); // checksum is calculated for everything except Sync and Checksum bytes - _rx_msg = b; - _decode_state = UBX_DECODE_ID; - break; - - /* Expecting ID */ - case UBX_DECODE_ID: - UBX_TRACE_PARSER("D"); - add_byte_to_checksum(b); - _rx_msg |= b << 8; - _decode_state = UBX_DECODE_LENGTH1; - break; - - /* Expecting first length byte */ - case UBX_DECODE_LENGTH1: - UBX_TRACE_PARSER("E"); - add_byte_to_checksum(b); - _rx_payload_length = b; - _decode_state = UBX_DECODE_LENGTH2; - break; - - /* Expecting second length byte */ - case UBX_DECODE_LENGTH2: - UBX_TRACE_PARSER("F"); - add_byte_to_checksum(b); - _rx_payload_length |= b << 8; // calculate payload size - - if (payload_rx_init() != 0) { // start payload reception - // payload will not be handled, discard message - decode_init(); - - } else { - _decode_state = (_rx_payload_length > 0) ? UBX_DECODE_PAYLOAD : UBX_DECODE_CHKSUM1; - } - - break; - - /* Expecting payload */ - case UBX_DECODE_PAYLOAD: - UBX_TRACE_PARSER("."); - add_byte_to_checksum(b); - - switch (_rx_msg) { - case UBX_MSG_NAV_SVINFO: - ret = payload_rx_add_nav_svinfo(b); // add a NAV-SVINFO payload byte - break; - - case UBX_MSG_MON_VER: - ret = payload_rx_add_mon_ver(b); // add a MON-VER payload byte - break; - - default: - ret = payload_rx_add(b); // add a payload byte - break; - } - - if (ret < 0) { - // payload not handled, discard message - decode_init(); - - } else if (ret > 0) { - // payload complete, expecting checksum - _decode_state = UBX_DECODE_CHKSUM1; - - } else { - // expecting more payload, stay in state UBX_DECODE_PAYLOAD - } - - ret = 0; - break; - - /* Expecting first checksum byte */ - case UBX_DECODE_CHKSUM1: - if (_rx_ck_a != b) { - UBX_WARN("ubx checksum err"); - decode_init(); - - } else { - _decode_state = UBX_DECODE_CHKSUM2; - } - - break; - - /* Expecting second checksum byte */ - case UBX_DECODE_CHKSUM2: - if (_rx_ck_b != b) { - UBX_WARN("ubx checksum err"); - - } else { - ret = payload_rx_done(); // finish payload processing - } - - decode_init(); - break; - - default: - break; - } - - return ret; -} - -/** - * Start payload rx - */ -int // -1 = abort, 0 = continue -UBX::payload_rx_init() -{ - int ret = 0; - - _rx_state = UBX_RXMSG_HANDLE; // handle by default - - switch (_rx_msg) { - case UBX_MSG_NAV_PVT: - if ((_rx_payload_length != UBX_PAYLOAD_RX_NAV_PVT_SIZE_UBX7) /* u-blox 7 msg format */ - && (_rx_payload_length != UBX_PAYLOAD_RX_NAV_PVT_SIZE_UBX8)) { /* u-blox 8+ msg format */ - _rx_state = UBX_RXMSG_ERROR_LENGTH; - - } else if (!_configured) { - _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured - - } else if (!_use_nav_pvt) { - _rx_state = UBX_RXMSG_DISABLE; // disable if not using NAV-PVT - } - - break; - - case UBX_MSG_NAV_POSLLH: - if (_rx_payload_length != sizeof(ubx_payload_rx_nav_posllh_t)) { - _rx_state = UBX_RXMSG_ERROR_LENGTH; - - } else if (!_configured) { - _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured - - } else if (_use_nav_pvt) { - _rx_state = UBX_RXMSG_DISABLE; // disable if using NAV-PVT instead - } - - break; - - case UBX_MSG_NAV_SOL: - if (_rx_payload_length != sizeof(ubx_payload_rx_nav_sol_t)) { - _rx_state = UBX_RXMSG_ERROR_LENGTH; - - } else if (!_configured) { - _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured - - } else if (_use_nav_pvt) { - _rx_state = UBX_RXMSG_DISABLE; // disable if using NAV-PVT instead - } - - break; - - case UBX_MSG_NAV_DOP: - if (_rx_payload_length != sizeof(ubx_payload_rx_nav_dop_t)) { - _rx_state = UBX_RXMSG_ERROR_LENGTH; - - } else if (!_configured) { - _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured - - } - - break; - - case UBX_MSG_NAV_TIMEUTC: - if (_rx_payload_length != sizeof(ubx_payload_rx_nav_timeutc_t)) { - _rx_state = UBX_RXMSG_ERROR_LENGTH; - - } else if (!_configured) { - _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured - - } else if (_use_nav_pvt) { - _rx_state = UBX_RXMSG_DISABLE; // disable if using NAV-PVT instead - } - - break; - - case UBX_MSG_NAV_SVINFO: - if (_satellite_info == nullptr) { - _rx_state = UBX_RXMSG_DISABLE; // disable if sat info not requested - - } else if (!_configured) { - _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured - - } else { - memset(_satellite_info, 0, sizeof(*_satellite_info)); // initialize sat info - } - - break; - - case UBX_MSG_NAV_VELNED: - if (_rx_payload_length != sizeof(ubx_payload_rx_nav_velned_t)) { - _rx_state = UBX_RXMSG_ERROR_LENGTH; - - } else if (!_configured) { - _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured - - } else if (_use_nav_pvt) { - _rx_state = UBX_RXMSG_DISABLE; // disable if using NAV-PVT instead - } - - break; - - case UBX_MSG_MON_VER: - break; // unconditionally handle this message - - case UBX_MSG_MON_HW: - if ((_rx_payload_length != sizeof(ubx_payload_rx_mon_hw_ubx6_t)) /* u-blox 6 msg format */ - && (_rx_payload_length != sizeof(ubx_payload_rx_mon_hw_ubx7_t))) { /* u-blox 7+ msg format */ - _rx_state = UBX_RXMSG_ERROR_LENGTH; - - } else if (!_configured) { - _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured - } - - break; - - case UBX_MSG_ACK_ACK: - if (_rx_payload_length != sizeof(ubx_payload_rx_ack_ack_t)) { - _rx_state = UBX_RXMSG_ERROR_LENGTH; - - } else if (_configured) { - _rx_state = UBX_RXMSG_IGNORE; // ignore if _configured - } - - break; - - case UBX_MSG_ACK_NAK: - if (_rx_payload_length != sizeof(ubx_payload_rx_ack_nak_t)) { - _rx_state = UBX_RXMSG_ERROR_LENGTH; - - } else if (_configured) { - _rx_state = UBX_RXMSG_IGNORE; // ignore if _configured - } - - break; - - default: - _rx_state = UBX_RXMSG_DISABLE; // disable all other messages - break; - } - - switch (_rx_state) { - case UBX_RXMSG_HANDLE: // handle message - case UBX_RXMSG_IGNORE: // ignore message but don't report error - ret = 0; - break; - - case UBX_RXMSG_DISABLE: // disable unexpected messages - UBX_DEBUG("ubx msg 0x%04x len %u unexpected", SWAP16((unsigned)_rx_msg), (unsigned)_rx_payload_length); - - { - hrt_abstime t = hrt_absolute_time(); - - if (t > _disable_cmd_last + DISABLE_MSG_INTERVAL) { - /* don't attempt for every message to disable, some might not be disabled */ - _disable_cmd_last = t; - UBX_DEBUG("ubx disabling msg 0x%04x", SWAP16((unsigned)_rx_msg)); - - if (!configure_message_rate(_rx_msg, 0)) { - ret = -1; - } - } - } - - ret = -1; // return error, abort handling this message - break; - - case UBX_RXMSG_ERROR_LENGTH: // error: invalid length - UBX_WARN("ubx msg 0x%04x invalid len %u", SWAP16((unsigned)_rx_msg), (unsigned)_rx_payload_length); - ret = -1; // return error, abort handling this message - break; - - default: // invalid message state - UBX_WARN("ubx internal err1"); - ret = -1; // return error, abort handling this message - break; - } - - return ret; -} - -/** - * Add payload rx byte - */ -int // -1 = error, 0 = ok, 1 = payload completed -UBX::payload_rx_add(const uint8_t b) -{ - int ret = 0; - uint8_t *p_buf = (uint8_t *)&_buf; - - p_buf[_rx_payload_index] = b; - - if (++_rx_payload_index >= _rx_payload_length) { - ret = 1; // payload received completely - } - - return ret; -} - -/** - * Add NAV-SVINFO payload rx byte - */ -int // -1 = error, 0 = ok, 1 = payload completed -UBX::payload_rx_add_nav_svinfo(const uint8_t b) -{ - int ret = 0; - uint8_t *p_buf = (uint8_t *)&_buf; - - if (_rx_payload_index < sizeof(ubx_payload_rx_nav_svinfo_part1_t)) { - // Fill Part 1 buffer - p_buf[_rx_payload_index] = b; - - } else { - if (_rx_payload_index == sizeof(ubx_payload_rx_nav_svinfo_part1_t)) { - // Part 1 complete: decode Part 1 buffer - _satellite_info->count = MIN(_buf.payload_rx_nav_svinfo_part1.numCh, satellite_info_s::SAT_INFO_MAX_SATELLITES); - UBX_TRACE_SVINFO("SVINFO len %u numCh %u", (unsigned)_rx_payload_length, - (unsigned)_buf.payload_rx_nav_svinfo_part1.numCh); - } - - if (_rx_payload_index < sizeof(ubx_payload_rx_nav_svinfo_part1_t) + _satellite_info->count * sizeof( - ubx_payload_rx_nav_svinfo_part2_t)) { - // Still room in _satellite_info: fill Part 2 buffer - unsigned buf_index = (_rx_payload_index - sizeof(ubx_payload_rx_nav_svinfo_part1_t)) % sizeof( - ubx_payload_rx_nav_svinfo_part2_t); - p_buf[buf_index] = b; - - if (buf_index == sizeof(ubx_payload_rx_nav_svinfo_part2_t) - 1) { - // Part 2 complete: decode Part 2 buffer - unsigned sat_index = (_rx_payload_index - sizeof(ubx_payload_rx_nav_svinfo_part1_t)) / sizeof( - ubx_payload_rx_nav_svinfo_part2_t); - _satellite_info->used[sat_index] = (uint8_t)(_buf.payload_rx_nav_svinfo_part2.flags & 0x01); - _satellite_info->snr[sat_index] = (uint8_t)(_buf.payload_rx_nav_svinfo_part2.cno); - _satellite_info->elevation[sat_index] = (uint8_t)(_buf.payload_rx_nav_svinfo_part2.elev); - _satellite_info->azimuth[sat_index] = (uint8_t)((float)_buf.payload_rx_nav_svinfo_part2.azim * 255.0f / 360.0f); - _satellite_info->svid[sat_index] = (uint8_t)(_buf.payload_rx_nav_svinfo_part2.svid); - UBX_TRACE_SVINFO("SVINFO #%02u used %u snr %3u elevation %3u azimuth %3u svid %3u", - (unsigned)sat_index + 1, - (unsigned)_satellite_info->used[sat_index], - (unsigned)_satellite_info->snr[sat_index], - (unsigned)_satellite_info->elevation[sat_index], - (unsigned)_satellite_info->azimuth[sat_index], - (unsigned)_satellite_info->svid[sat_index] - ); - } - } - } - - if (++_rx_payload_index >= _rx_payload_length) { - ret = 1; // payload received completely - } - - return ret; -} - -/** - * Add MON-VER payload rx byte - */ -int // -1 = error, 0 = ok, 1 = payload completed -UBX::payload_rx_add_mon_ver(const uint8_t b) -{ - int ret = 0; - uint8_t *p_buf = (uint8_t *)&_buf; - - if (_rx_payload_index < sizeof(ubx_payload_rx_mon_ver_part1_t)) { - // Fill Part 1 buffer - p_buf[_rx_payload_index] = b; - - } else { - if (_rx_payload_index == sizeof(ubx_payload_rx_mon_ver_part1_t)) { - // Part 1 complete: decode Part 1 buffer and calculate hash for SW&HW version strings - _ubx_version = fnv1_32_str(_buf.payload_rx_mon_ver_part1.swVersion, FNV1_32_INIT); - _ubx_version = fnv1_32_str(_buf.payload_rx_mon_ver_part1.hwVersion, _ubx_version); - UBX_DEBUG("VER hash 0x%08x", _ubx_version); - UBX_DEBUG("VER hw \"%10s\"", _buf.payload_rx_mon_ver_part1.hwVersion); - UBX_DEBUG("VER sw \"%30s\"", _buf.payload_rx_mon_ver_part1.swVersion); - } - - // fill Part 2 buffer - unsigned buf_index = (_rx_payload_index - sizeof(ubx_payload_rx_mon_ver_part1_t)) % sizeof( - ubx_payload_rx_mon_ver_part2_t); - p_buf[buf_index] = b; - - if (buf_index == sizeof(ubx_payload_rx_mon_ver_part2_t) - 1) { - // Part 2 complete: decode Part 2 buffer - UBX_DEBUG("VER ext \" %30s\"", _buf.payload_rx_mon_ver_part2.extension); - } - } - - if (++_rx_payload_index >= _rx_payload_length) { - ret = 1; // payload received completely - } - - return ret; -} - -/** - * Finish payload rx - */ -int // 0 = no message handled, 1 = message handled, 2 = sat info message handled -UBX::payload_rx_done(void) -{ - int ret = 0; - - // return if no message handled - if (_rx_state != UBX_RXMSG_HANDLE) { - return ret; - } - - // handle message - switch (_rx_msg) { - - case UBX_MSG_NAV_PVT: - UBX_TRACE_RXMSG("Rx NAV-PVT"); - - //Check if position fix flag is good - if ((_buf.payload_rx_nav_pvt.flags & UBX_RX_NAV_PVT_FLAGS_GNSSFIXOK) == 1) { - _gps_position->fix_type = _buf.payload_rx_nav_pvt.fixType; - _gps_position->vel_ned_valid = true; - - } else { - _gps_position->fix_type = 0; - _gps_position->vel_ned_valid = false; - } - - _gps_position->satellites_used = _buf.payload_rx_nav_pvt.numSV; - - _gps_position->lat = _buf.payload_rx_nav_pvt.lat; - _gps_position->lon = _buf.payload_rx_nav_pvt.lon; - _gps_position->alt = _buf.payload_rx_nav_pvt.hMSL; - - _gps_position->eph = (float)_buf.payload_rx_nav_pvt.hAcc * 1e-3f; - _gps_position->epv = (float)_buf.payload_rx_nav_pvt.vAcc * 1e-3f; - _gps_position->s_variance_m_s = (float)_buf.payload_rx_nav_pvt.sAcc * 1e-3f; - - _gps_position->vel_m_s = (float)_buf.payload_rx_nav_pvt.gSpeed * 1e-3f; - - _gps_position->vel_n_m_s = (float)_buf.payload_rx_nav_pvt.velN * 1e-3f; - _gps_position->vel_e_m_s = (float)_buf.payload_rx_nav_pvt.velE * 1e-3f; - _gps_position->vel_d_m_s = (float)_buf.payload_rx_nav_pvt.velD * 1e-3f; - - _gps_position->cog_rad = (float)_buf.payload_rx_nav_pvt.headMot * M_DEG_TO_RAD_F * 1e-5f; - _gps_position->c_variance_rad = (float)_buf.payload_rx_nav_pvt.headAcc * M_DEG_TO_RAD_F * 1e-5f; - - //Check if time and date fix flags are good - if ((_buf.payload_rx_nav_pvt.valid & UBX_RX_NAV_PVT_VALID_VALIDDATE) - && (_buf.payload_rx_nav_pvt.valid & UBX_RX_NAV_PVT_VALID_VALIDTIME) - && (_buf.payload_rx_nav_pvt.valid & UBX_RX_NAV_PVT_VALID_FULLYRESOLVED)) { - /* convert to unix timestamp */ - struct tm timeinfo; - timeinfo.tm_year = _buf.payload_rx_nav_pvt.year - 1900; - timeinfo.tm_mon = _buf.payload_rx_nav_pvt.month - 1; - timeinfo.tm_mday = _buf.payload_rx_nav_pvt.day; - timeinfo.tm_hour = _buf.payload_rx_nav_pvt.hour; - timeinfo.tm_min = _buf.payload_rx_nav_pvt.min; - timeinfo.tm_sec = _buf.payload_rx_nav_pvt.sec; - - // TODO: this functionality is not available on the Snapdragon yet -#ifndef __PX4_QURT - time_t epoch = mktime(&timeinfo); - - if (epoch > GPS_EPOCH_SECS) { - // FMUv2+ boards have a hardware RTC, but GPS helps us to configure it - // and control its drift. Since we rely on the HRT for our monotonic - // clock, updating it from time to time is safe. - - timespec ts; - ts.tv_sec = epoch; - ts.tv_nsec = _buf.payload_rx_nav_pvt.nano; - - if (px4_clock_settime(CLOCK_REALTIME, &ts)) { - warn("failed setting clock"); - } - - _gps_position->time_utc_usec = static_cast(epoch) * 1000000ULL; - _gps_position->time_utc_usec += _buf.payload_rx_nav_timeutc.nano / 1000; - - } else { - _gps_position->time_utc_usec = 0; - } - -#else - _gps_position->time_utc_usec = 0; -#endif - } - - _gps_position->timestamp_time = hrt_absolute_time(); - _gps_position->timestamp_velocity = hrt_absolute_time(); - _gps_position->timestamp_variance = hrt_absolute_time(); - _gps_position->timestamp_position = hrt_absolute_time(); - - _rate_count_vel++; - _rate_count_lat_lon++; - - _got_posllh = true; - _got_velned = true; - - ret = 1; - break; - - case UBX_MSG_NAV_POSLLH: - UBX_TRACE_RXMSG("Rx NAV-POSLLH"); - - _gps_position->lat = _buf.payload_rx_nav_posllh.lat; - _gps_position->lon = _buf.payload_rx_nav_posllh.lon; - _gps_position->alt = _buf.payload_rx_nav_posllh.hMSL; - _gps_position->eph = (float)_buf.payload_rx_nav_posllh.hAcc * 1e-3f; // from mm to m - _gps_position->epv = (float)_buf.payload_rx_nav_posllh.vAcc * 1e-3f; // from mm to m - _gps_position->alt_ellipsoid = _buf.payload_rx_nav_posllh.height; - - _gps_position->timestamp_position = hrt_absolute_time(); - - _rate_count_lat_lon++; - _got_posllh = true; - - ret = 1; - break; - - case UBX_MSG_NAV_SOL: - UBX_TRACE_RXMSG("Rx NAV-SOL"); - - _gps_position->fix_type = _buf.payload_rx_nav_sol.gpsFix; - _gps_position->s_variance_m_s = (float)_buf.payload_rx_nav_sol.sAcc * 1e-2f; // from cm to m - _gps_position->satellites_used = _buf.payload_rx_nav_sol.numSV; - - _gps_position->timestamp_variance = hrt_absolute_time(); - - ret = 1; - break; - - case UBX_MSG_NAV_DOP: - UBX_TRACE_RXMSG("Rx NAV-DOP"); - - _gps_position->hdop = _buf.payload_rx_nav_dop.hDOP * 0.01f; // from cm to m - _gps_position->vdop = _buf.payload_rx_nav_dop.vDOP * 0.01f; // from cm to m - - _gps_position->timestamp_variance = hrt_absolute_time(); - - ret = 1; - break; - - case UBX_MSG_NAV_TIMEUTC: - UBX_TRACE_RXMSG("Rx NAV-TIMEUTC"); - - if (_buf.payload_rx_nav_timeutc.valid & UBX_RX_NAV_TIMEUTC_VALID_VALIDUTC) { - // convert to unix timestamp - struct tm timeinfo; - timeinfo.tm_year = _buf.payload_rx_nav_timeutc.year - 1900; - timeinfo.tm_mon = _buf.payload_rx_nav_timeutc.month - 1; - timeinfo.tm_mday = _buf.payload_rx_nav_timeutc.day; - timeinfo.tm_hour = _buf.payload_rx_nav_timeutc.hour; - timeinfo.tm_min = _buf.payload_rx_nav_timeutc.min; - timeinfo.tm_sec = _buf.payload_rx_nav_timeutc.sec; - // TODO: this functionality is not available on the Snapdragon yet -#ifndef __PX4_QURT - time_t epoch = mktime(&timeinfo); - - // only set the time if it makes sense - - if (epoch > GPS_EPOCH_SECS) { - // FMUv2+ boards have a hardware RTC, but GPS helps us to configure it - // and control its drift. Since we rely on the HRT for our monotonic - // clock, updating it from time to time is safe. - - timespec ts; - ts.tv_sec = epoch; - ts.tv_nsec = _buf.payload_rx_nav_timeutc.nano; - - if (px4_clock_settime(CLOCK_REALTIME, &ts)) { - warn("failed setting clock"); - } - - _gps_position->time_utc_usec = static_cast(epoch) * 1000000ULL; - _gps_position->time_utc_usec += _buf.payload_rx_nav_timeutc.nano / 1000; - - } else { - _gps_position->time_utc_usec = 0; - } - -#else - _gps_position->time_utc_usec = 0; -#endif - } - - _gps_position->timestamp_time = hrt_absolute_time(); - - ret = 1; - break; - - case UBX_MSG_NAV_SVINFO: - UBX_TRACE_RXMSG("Rx NAV-SVINFO"); - - // _satellite_info already populated by payload_rx_add_svinfo(), just add a timestamp - _satellite_info->timestamp = hrt_absolute_time(); - - ret = 2; - break; - - case UBX_MSG_NAV_VELNED: - UBX_TRACE_RXMSG("Rx NAV-VELNED"); - - _gps_position->vel_m_s = (float)_buf.payload_rx_nav_velned.speed * 1e-2f; - _gps_position->vel_n_m_s = (float)_buf.payload_rx_nav_velned.velN * 1e-2f; /* NED NORTH velocity */ - _gps_position->vel_e_m_s = (float)_buf.payload_rx_nav_velned.velE * 1e-2f; /* NED EAST velocity */ - _gps_position->vel_d_m_s = (float)_buf.payload_rx_nav_velned.velD * 1e-2f; /* NED DOWN velocity */ - _gps_position->cog_rad = (float)_buf.payload_rx_nav_velned.heading * M_DEG_TO_RAD_F * 1e-5f; - _gps_position->c_variance_rad = (float)_buf.payload_rx_nav_velned.cAcc * M_DEG_TO_RAD_F * 1e-5f; - _gps_position->vel_ned_valid = true; - - _gps_position->timestamp_velocity = hrt_absolute_time(); - - _rate_count_vel++; - _got_velned = true; - - ret = 1; - break; - - case UBX_MSG_MON_VER: - UBX_TRACE_RXMSG("Rx MON-VER"); - - ret = 1; - break; - - case UBX_MSG_MON_HW: - UBX_TRACE_RXMSG("Rx MON-HW"); - - switch (_rx_payload_length) { - - case sizeof(ubx_payload_rx_mon_hw_ubx6_t): /* u-blox 6 msg format */ - _gps_position->noise_per_ms = _buf.payload_rx_mon_hw_ubx6.noisePerMS; - _gps_position->jamming_indicator = _buf.payload_rx_mon_hw_ubx6.jamInd; - - ret = 1; - break; - - case sizeof(ubx_payload_rx_mon_hw_ubx7_t): /* u-blox 7+ msg format */ - _gps_position->noise_per_ms = _buf.payload_rx_mon_hw_ubx7.noisePerMS; - _gps_position->jamming_indicator = _buf.payload_rx_mon_hw_ubx7.jamInd; - - ret = 1; - break; - - default: // unexpected payload size: - ret = 0; // don't handle message - break; - } - - break; - - case UBX_MSG_ACK_ACK: - UBX_TRACE_RXMSG("Rx ACK-ACK"); - - if ((_ack_state == UBX_ACK_WAITING) && (_buf.payload_rx_ack_ack.msg == _ack_waiting_msg)) { - _ack_state = UBX_ACK_GOT_ACK; - } - - ret = 1; - break; - - case UBX_MSG_ACK_NAK: - UBX_TRACE_RXMSG("Rx ACK-NAK"); - - if ((_ack_state == UBX_ACK_WAITING) && (_buf.payload_rx_ack_ack.msg == _ack_waiting_msg)) { - _ack_state = UBX_ACK_GOT_NAK; - } - - ret = 1; - break; - - default: - break; - } - - return ret; -} - -void -UBX::decode_init(void) -{ - _decode_state = UBX_DECODE_SYNC1; - _rx_ck_a = 0; - _rx_ck_b = 0; - _rx_payload_length = 0; - _rx_payload_index = 0; -} - -void -UBX::add_byte_to_checksum(const uint8_t b) -{ - _rx_ck_a = _rx_ck_a + b; - _rx_ck_b = _rx_ck_b + _rx_ck_a; -} - -void -UBX::calc_checksum(const uint8_t *buffer, const uint16_t length, ubx_checksum_t *checksum) -{ - for (uint16_t i = 0; i < length; i++) { - checksum->ck_a = checksum->ck_a + buffer[i]; - checksum->ck_b = checksum->ck_b + checksum->ck_a; - } -} - -bool -UBX::configure_message_rate(const uint16_t msg, const uint8_t rate) -{ - ubx_payload_tx_cfg_msg_t cfg_msg; // don't use _buf (allow interleaved operation) - - cfg_msg.msg = msg; - cfg_msg.rate = rate; - - return send_message(UBX_MSG_CFG_MSG, (uint8_t *)&cfg_msg, sizeof(cfg_msg)); -} - -bool -UBX::send_message(const uint16_t msg, const uint8_t *payload, const uint16_t length) -{ - ubx_header_t header = {UBX_SYNC1, UBX_SYNC2}; - ubx_checksum_t checksum = {0, 0}; - - // Populate header - header.msg = msg; - header.length = length; - - // Calculate checksum - calc_checksum(((uint8_t *)&header) + 2, sizeof(header) - 2, &checksum); // skip 2 sync bytes - - if (payload != nullptr) { - calc_checksum(payload, length, &checksum); - } - - // Send message - if (write(_fd, (const void *)&header, sizeof(header)) != sizeof(header)) { - return false; - } - - if (payload && write(_fd, (const void *)payload, length) != length) { - return false; - } - - if (write(_fd, (const void *)&checksum, sizeof(checksum)) != sizeof(checksum)) { - return false; - } - - return true; -} - -uint32_t -UBX::fnv1_32_str(uint8_t *str, uint32_t hval) -{ - uint8_t *s = str; - - /* - * FNV-1 hash each octet in the buffer - */ - while (*s) { - - /* multiply by the 32 bit FNV magic prime mod 2^32 */ -#if defined(NO_FNV_GCC_OPTIMIZATION) - hval *= FNV1_32_PRIME; -#else - hval += (hval << 1) + (hval << 4) + (hval << 7) + (hval << 8) + (hval << 24); -#endif - - /* xor the bottom with the current octet */ - hval ^= (uint32_t) * s++; - } - - /* return our new hash value */ - return hval; -} - diff --git a/src/drivers/gps/ubx.h b/src/drivers/gps/ubx.h deleted file mode 100644 index e5389cdc9a..0000000000 --- a/src/drivers/gps/ubx.h +++ /dev/null @@ -1,575 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012-2016 PX4 Development Team. All rights reserved. - * - * 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 ubx.h - * - * U-Blox protocol definition. Following u-blox 6/7/8 Receiver Description - * including Prototol Specification. - * - * @author Thomas Gubler - * @author Julian Oes - * @author Anton Babushkin - * - * @author Hannes Delago - * (rework, add ubx7+ compatibility) - * - */ - -#ifndef UBX_H_ -#define UBX_H_ - -#include "gps_helper.h" - -#define UBX_SYNC1 0xB5 -#define UBX_SYNC2 0x62 - -/* Message Classes */ -#define UBX_CLASS_NAV 0x01 -#define UBX_CLASS_ACK 0x05 -#define UBX_CLASS_CFG 0x06 -#define UBX_CLASS_MON 0x0A - -/* Message IDs */ -#define UBX_ID_NAV_POSLLH 0x02 -#define UBX_ID_NAV_DOP 0x04 -#define UBX_ID_NAV_SOL 0x06 -#define UBX_ID_NAV_PVT 0x07 -#define UBX_ID_NAV_VELNED 0x12 -#define UBX_ID_NAV_TIMEUTC 0x21 -#define UBX_ID_NAV_SVINFO 0x30 -#define UBX_ID_ACK_NAK 0x00 -#define UBX_ID_ACK_ACK 0x01 -#define UBX_ID_CFG_PRT 0x00 -#define UBX_ID_CFG_MSG 0x01 -#define UBX_ID_CFG_RATE 0x08 -#define UBX_ID_CFG_NAV5 0x24 -#define UBX_ID_CFG_SBAS 0x16 -#define UBX_ID_MON_VER 0x04 -#define UBX_ID_MON_HW 0x09 - -/* Message Classes & IDs */ -#define UBX_MSG_NAV_POSLLH ((UBX_CLASS_NAV) | UBX_ID_NAV_POSLLH << 8) -#define UBX_MSG_NAV_SOL ((UBX_CLASS_NAV) | UBX_ID_NAV_SOL << 8) -#define UBX_MSG_NAV_DOP ((UBX_CLASS_NAV) | UBX_ID_NAV_DOP << 8) -#define UBX_MSG_NAV_PVT ((UBX_CLASS_NAV) | UBX_ID_NAV_PVT << 8) -#define UBX_MSG_NAV_VELNED ((UBX_CLASS_NAV) | UBX_ID_NAV_VELNED << 8) -#define UBX_MSG_NAV_TIMEUTC ((UBX_CLASS_NAV) | UBX_ID_NAV_TIMEUTC << 8) -#define UBX_MSG_NAV_SVINFO ((UBX_CLASS_NAV) | UBX_ID_NAV_SVINFO << 8) -#define UBX_MSG_ACK_NAK ((UBX_CLASS_ACK) | UBX_ID_ACK_NAK << 8) -#define UBX_MSG_ACK_ACK ((UBX_CLASS_ACK) | UBX_ID_ACK_ACK << 8) -#define UBX_MSG_CFG_PRT ((UBX_CLASS_CFG) | UBX_ID_CFG_PRT << 8) -#define UBX_MSG_CFG_MSG ((UBX_CLASS_CFG) | UBX_ID_CFG_MSG << 8) -#define UBX_MSG_CFG_RATE ((UBX_CLASS_CFG) | UBX_ID_CFG_RATE << 8) -#define UBX_MSG_CFG_NAV5 ((UBX_CLASS_CFG) | UBX_ID_CFG_NAV5 << 8) -#define UBX_MSG_CFG_SBAS ((UBX_CLASS_CFG) | UBX_ID_CFG_SBAS << 8) -#define UBX_MSG_MON_HW ((UBX_CLASS_MON) | UBX_ID_MON_HW << 8) -#define UBX_MSG_MON_VER ((UBX_CLASS_MON) | UBX_ID_MON_VER << 8) - -/* RX NAV-PVT message content details */ -/* Bitfield "valid" masks */ -#define UBX_RX_NAV_PVT_VALID_VALIDDATE 0x01 /**< validDate (Valid UTC Date) */ -#define UBX_RX_NAV_PVT_VALID_VALIDTIME 0x02 /**< validTime (Valid UTC Time) */ -#define UBX_RX_NAV_PVT_VALID_FULLYRESOLVED 0x04 /**< fullyResolved (1 = UTC Time of Day has been fully resolved (no seconds uncertainty)) */ - -/* Bitfield "flags" masks */ -#define UBX_RX_NAV_PVT_FLAGS_GNSSFIXOK 0x01 /**< gnssFixOK (A valid fix (i.e within DOP & accuracy masks)) */ -#define UBX_RX_NAV_PVT_FLAGS_DIFFSOLN 0x02 /**< diffSoln (1 if differential corrections were applied) */ -#define UBX_RX_NAV_PVT_FLAGS_PSMSTATE 0x1C /**< psmState (Power Save Mode state (see Power Management)) */ -#define UBX_RX_NAV_PVT_FLAGS_HEADVEHVALID 0x20 /**< headVehValid (Heading of vehicle is valid) */ - -/* RX NAV-TIMEUTC message content details */ -/* Bitfield "valid" masks */ -#define UBX_RX_NAV_TIMEUTC_VALID_VALIDTOW 0x01 /**< validTOW (1 = Valid Time of Week) */ -#define UBX_RX_NAV_TIMEUTC_VALID_VALIDKWN 0x02 /**< validWKN (1 = Valid Week Number) */ -#define UBX_RX_NAV_TIMEUTC_VALID_VALIDUTC 0x04 /**< validUTC (1 = Valid UTC Time) */ -#define UBX_RX_NAV_TIMEUTC_VALID_UTCSTANDARD 0xF0 /**< utcStandard (0..15 = UTC standard identifier) */ - -/* TX CFG-PRT message contents */ -#define UBX_TX_CFG_PRT_PORTID 0x01 /**< UART1 */ -#define UBX_TX_CFG_PRT_PORTID_USB 0x03 /**< USB */ -#define UBX_TX_CFG_PRT_MODE 0x000008D0 /**< 0b0000100011010000: 8N1 */ -#define UBX_TX_CFG_PRT_BAUDRATE 38400 /**< choose 38400 as GPS baudrate */ -#define UBX_TX_CFG_PRT_INPROTOMASK 0x01 /**< UBX in */ -#define UBX_TX_CFG_PRT_OUTPROTOMASK 0x01 /**< UBX out */ - -/* TX CFG-RATE message contents */ -#define UBX_TX_CFG_RATE_MEASINTERVAL 200 /**< 200ms for 5Hz */ -#define UBX_TX_CFG_RATE_NAVRATE 1 /**< cannot be changed */ -#define UBX_TX_CFG_RATE_TIMEREF 0 /**< 0: UTC, 1: GPS time */ - -/* TX CFG-NAV5 message contents */ -#define UBX_TX_CFG_NAV5_MASK 0x0005 /**< Only update dynamic model and fix mode */ -#define UBX_TX_CFG_NAV5_DYNMODEL 7 /**< 0 Portable, 2 Stationary, 3 Pedestrian, 4 Automotive, 5 Sea, 6 Airborne <1g, 7 Airborne <2g, 8 Airborne <4g */ -#define UBX_TX_CFG_NAV5_FIXMODE 2 /**< 1 2D only, 2 3D only, 3 Auto 2D/3D */ - -/* TX CFG-SBAS message contents */ -#define UBX_TX_CFG_SBAS_MODE_ENABLED 1 /**< SBAS enabled */ -#define UBX_TX_CFG_SBAS_MODE_DISABLED 0 /**< SBAS disabled */ -#define UBX_TX_CFG_SBAS_MODE UBX_TX_CFG_SBAS_MODE_DISABLED /**< SBAS enabled or disabled */ - -/* TX CFG-MSG message contents */ -#define UBX_TX_CFG_MSG_RATE1_5HZ 0x01 /**< {0x00, 0x01, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */ -#define UBX_TX_CFG_MSG_RATE1_1HZ 0x05 /**< {0x00, 0x05, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */ -#define UBX_TX_CFG_MSG_RATE1_05HZ 10 - - -/*** u-blox protocol binary message and payload definitions ***/ -#pragma pack(push, 1) - -/* General: Header */ -typedef struct { - uint8_t sync1; - uint8_t sync2; - uint16_t msg; - uint16_t length; -} ubx_header_t; - -/* General: Checksum */ -typedef struct { - uint8_t ck_a; - uint8_t ck_b; -} ubx_checksum_t ; - -/* Rx NAV-POSLLH */ -typedef struct { - uint32_t iTOW; /**< GPS Time of Week [ms] */ - int32_t lon; /**< Longitude [1e-7 deg] */ - int32_t lat; /**< Latitude [1e-7 deg] */ - int32_t height; /**< Height above ellipsoid [mm] */ - int32_t hMSL; /**< Height above mean sea level [mm] */ - uint32_t hAcc; /**< Horizontal accuracy estimate [mm] */ - uint32_t vAcc; /**< Vertical accuracy estimate [mm] */ -} ubx_payload_rx_nav_posllh_t; - -/* Rx NAV-DOP */ -typedef struct { - uint32_t iTOW; /**< GPS Time of Week [ms] */ - uint16_t gDOP; /**< Geometric DOP [0.01] */ - uint16_t pDOP; /**< Position DOP [0.01] */ - uint16_t tDOP; /**< Time DOP [0.01] */ - uint16_t vDOP; /**< Vertical DOP [0.01] */ - uint16_t hDOP; /**< Horizontal DOP [0.01] */ - uint16_t nDOP; /**< Northing DOP [0.01] */ - uint16_t eDOP; /**< Easting DOP [0.01] */ -} ubx_payload_rx_nav_dop_t; - -/* Rx NAV-SOL */ -typedef struct { - uint32_t iTOW; /**< GPS Time of Week [ms] */ - int32_t fTOW; /**< Fractional part of iTOW (range: +/-500000) [ns] */ - int16_t week; /**< GPS week */ - uint8_t gpsFix; /**< GPSfix type: 0 = No fix, 1 = Dead Reckoning only, 2 = 2D fix, 3 = 3d-fix, 4 = GPS + dead reckoning, 5 = time only fix */ - uint8_t flags; - int32_t ecefX; - int32_t ecefY; - int32_t ecefZ; - uint32_t pAcc; - int32_t ecefVX; - int32_t ecefVY; - int32_t ecefVZ; - uint32_t sAcc; - uint16_t pDOP; /**< Position DOP [0.01] */ - uint8_t reserved1; - uint8_t numSV; /**< Number of SVs used in Nav Solution */ - uint32_t reserved2; -} ubx_payload_rx_nav_sol_t; - -/* Rx NAV-PVT (ubx8) */ -typedef struct { - uint32_t iTOW; /**< GPS Time of Week [ms] */ - uint16_t year; /**< Year (UTC)*/ - uint8_t month; /**< Month, range 1..12 (UTC) */ - uint8_t day; /**< Day of month, range 1..31 (UTC) */ - uint8_t hour; /**< Hour of day, range 0..23 (UTC) */ - uint8_t min; /**< Minute of hour, range 0..59 (UTC) */ - uint8_t sec; /**< Seconds of minute, range 0..60 (UTC) */ - uint8_t valid; /**< Validity flags (see UBX_RX_NAV_PVT_VALID_...) */ - uint32_t tAcc; /**< Time accuracy estimate (UTC) [ns] */ - int32_t nano; /**< Fraction of second (UTC) [-1e9...1e9 ns] */ - uint8_t fixType; /**< GNSSfix type: 0 = No fix, 1 = Dead Reckoning only, 2 = 2D fix, 3 = 3d-fix, 4 = GNSS + dead reckoning, 5 = time only fix */ - uint8_t flags; /**< Fix Status Flags (see UBX_RX_NAV_PVT_FLAGS_...) */ - uint8_t reserved1; - uint8_t numSV; /**< Number of SVs used in Nav Solution */ - int32_t lon; /**< Longitude [1e-7 deg] */ - int32_t lat; /**< Latitude [1e-7 deg] */ - int32_t height; /**< Height above ellipsoid [mm] */ - int32_t hMSL; /**< Height above mean sea level [mm] */ - uint32_t hAcc; /**< Horizontal accuracy estimate [mm] */ - uint32_t vAcc; /**< Vertical accuracy estimate [mm] */ - int32_t velN; /**< NED north velocity [mm/s]*/ - int32_t velE; /**< NED east velocity [mm/s]*/ - int32_t velD; /**< NED down velocity [mm/s]*/ - int32_t gSpeed; /**< Ground Speed (2-D) [mm/s] */ - int32_t headMot; /**< Heading of motion (2-D) [1e-5 deg] */ - uint32_t sAcc; /**< Speed accuracy estimate [mm/s] */ - uint32_t headAcc; /**< Heading accuracy estimate (motion and vehicle) [1e-5 deg] */ - uint16_t pDOP; /**< Position DOP [0.01] */ - uint16_t reserved2; - uint32_t reserved3; - int32_t headVeh; /**< (ubx8+ only) Heading of vehicle (2-D) [1e-5 deg] */ - uint32_t reserved4; /**< (ubx8+ only) */ -} ubx_payload_rx_nav_pvt_t; -#define UBX_PAYLOAD_RX_NAV_PVT_SIZE_UBX7 (sizeof(ubx_payload_rx_nav_pvt_t) - 8) -#define UBX_PAYLOAD_RX_NAV_PVT_SIZE_UBX8 (sizeof(ubx_payload_rx_nav_pvt_t)) - -/* Rx NAV-TIMEUTC */ -typedef struct { - uint32_t iTOW; /**< GPS Time of Week [ms] */ - uint32_t tAcc; /**< Time accuracy estimate (UTC) [ns] */ - int32_t nano; /**< Fraction of second, range -1e9 .. 1e9 (UTC) [ns] */ - uint16_t year; /**< Year, range 1999..2099 (UTC) */ - uint8_t month; /**< Month, range 1..12 (UTC) */ - uint8_t day; /**< Day of month, range 1..31 (UTC) */ - uint8_t hour; /**< Hour of day, range 0..23 (UTC) */ - uint8_t min; /**< Minute of hour, range 0..59 (UTC) */ - uint8_t sec; /**< Seconds of minute, range 0..60 (UTC) */ - uint8_t valid; /**< Validity Flags (see UBX_RX_NAV_TIMEUTC_VALID_...) */ -} ubx_payload_rx_nav_timeutc_t; - -/* Rx NAV-SVINFO Part 1 */ -typedef struct { - uint32_t iTOW; /**< GPS Time of Week [ms] */ - uint8_t numCh; /**< Number of channels */ - uint8_t globalFlags; - uint16_t reserved2; -} ubx_payload_rx_nav_svinfo_part1_t; - -/* Rx NAV-SVINFO Part 2 (repeated) */ -typedef struct { - uint8_t chn; /**< Channel number, 255 for SVs not assigned to a channel */ - uint8_t svid; /**< Satellite ID */ - uint8_t flags; - uint8_t quality; - uint8_t cno; /**< Carrier to Noise Ratio (Signal Strength) [dbHz] */ - int8_t elev; /**< Elevation [deg] */ - int16_t azim; /**< Azimuth [deg] */ - int32_t prRes; /**< Pseudo range residual [cm] */ -} ubx_payload_rx_nav_svinfo_part2_t; - -/* Rx NAV-VELNED */ -typedef struct { - uint32_t iTOW; /**< GPS Time of Week [ms] */ - int32_t velN; /**< North velocity component [cm/s]*/ - int32_t velE; /**< East velocity component [cm/s]*/ - int32_t velD; /**< Down velocity component [cm/s]*/ - uint32_t speed; /**< Speed (3-D) [cm/s] */ - uint32_t gSpeed; /**< Ground speed (2-D) [cm/s] */ - int32_t heading; /**< Heading of motion 2-D [1e-5 deg] */ - uint32_t sAcc; /**< Speed accuracy estimate [cm/s] */ - uint32_t cAcc; /**< Course / Heading accuracy estimate [1e-5 deg] */ -} ubx_payload_rx_nav_velned_t; - -/* Rx MON-HW (ubx6) */ -typedef struct { - uint32_t pinSel; - uint32_t pinBank; - uint32_t pinDir; - uint32_t pinVal; - uint16_t noisePerMS; - uint16_t agcCnt; - uint8_t aStatus; - uint8_t aPower; - uint8_t flags; - uint8_t reserved1; - uint32_t usedMask; - uint8_t VP[25]; - uint8_t jamInd; - uint16_t reserved3; - uint32_t pinIrq; - uint32_t pullH; - uint32_t pullL; -} ubx_payload_rx_mon_hw_ubx6_t; - -/* Rx MON-HW (ubx7+) */ -typedef struct { - uint32_t pinSel; - uint32_t pinBank; - uint32_t pinDir; - uint32_t pinVal; - uint16_t noisePerMS; - uint16_t agcCnt; - uint8_t aStatus; - uint8_t aPower; - uint8_t flags; - uint8_t reserved1; - uint32_t usedMask; - uint8_t VP[17]; - uint8_t jamInd; - uint16_t reserved3; - uint32_t pinIrq; - uint32_t pullH; - uint32_t pullL; -} ubx_payload_rx_mon_hw_ubx7_t; - -/* Rx MON-VER Part 1 */ -typedef struct { - uint8_t swVersion[30]; - uint8_t hwVersion[10]; -} ubx_payload_rx_mon_ver_part1_t; - -/* Rx MON-VER Part 2 (repeated) */ -typedef struct { - uint8_t extension[30]; -} ubx_payload_rx_mon_ver_part2_t; - -/* Rx ACK-ACK */ -typedef union { - uint16_t msg; - struct { - uint8_t clsID; - uint8_t msgID; - }; -} ubx_payload_rx_ack_ack_t; - -/* Rx ACK-NAK */ -typedef union { - uint16_t msg; - struct { - uint8_t clsID; - uint8_t msgID; - }; -} ubx_payload_rx_ack_nak_t; - -/* Tx CFG-PRT */ -typedef struct { - uint8_t portID; - uint8_t reserved0; - uint16_t txReady; - uint32_t mode; - uint32_t baudRate; - uint16_t inProtoMask; - uint16_t outProtoMask; - uint16_t flags; - uint16_t reserved5; -} ubx_payload_tx_cfg_prt_t; - -/* Tx CFG-RATE */ -typedef struct { - uint16_t measRate; /**< Measurement Rate, GPS measurements are taken every measRate milliseconds */ - uint16_t navRate; /**< Navigation Rate, in number of measurement cycles. This parameter cannot be changed, and must be set to 1 */ - uint16_t timeRef; /**< Alignment to reference time: 0 = UTC time, 1 = GPS time */ -} ubx_payload_tx_cfg_rate_t; - -/* Tx CFG-NAV5 */ -typedef struct { - uint16_t mask; - uint8_t dynModel; /**< Dynamic Platform model: 0 Portable, 2 Stationary, 3 Pedestrian, 4 Automotive, 5 Sea, 6 Airborne <1g, 7 Airborne <2g, 8 Airborne <4g */ - uint8_t fixMode; /**< Position Fixing Mode: 1 2D only, 2 3D only, 3 Auto 2D/3D */ - int32_t fixedAlt; - uint32_t fixedAltVar; - int8_t minElev; - uint8_t drLimit; - uint16_t pDop; - uint16_t tDop; - uint16_t pAcc; - uint16_t tAcc; - uint8_t staticHoldThresh; - uint8_t dgpsTimeOut; - uint8_t cnoThreshNumSVs; /**< (ubx7+ only, else 0) */ - uint8_t cnoThresh; /**< (ubx7+ only, else 0) */ - uint16_t reserved; - uint16_t staticHoldMaxDist; /**< (ubx8+ only, else 0) */ - uint8_t utcStandard; /**< (ubx8+ only, else 0) */ - uint8_t reserved3; - uint32_t reserved4; -} ubx_payload_tx_cfg_nav5_t; - -/* tx cfg-sbas */ -typedef struct { - uint8_t mode; - uint8_t usage; - uint8_t maxSBAS; - uint8_t scanmode2; - uint32_t scanmode1; -} ubx_payload_tx_cfg_sbas_t; - -/* Tx CFG-MSG */ -typedef struct { - union { - uint16_t msg; - struct { - uint8_t msgClass; - uint8_t msgID; - }; - }; - uint8_t rate; -} ubx_payload_tx_cfg_msg_t; - -/* General message and payload buffer union */ -typedef union { - ubx_payload_rx_nav_pvt_t payload_rx_nav_pvt; - ubx_payload_rx_nav_posllh_t payload_rx_nav_posllh; - ubx_payload_rx_nav_sol_t payload_rx_nav_sol; - ubx_payload_rx_nav_dop_t payload_rx_nav_dop; - ubx_payload_rx_nav_timeutc_t payload_rx_nav_timeutc; - ubx_payload_rx_nav_svinfo_part1_t payload_rx_nav_svinfo_part1; - ubx_payload_rx_nav_svinfo_part2_t payload_rx_nav_svinfo_part2; - ubx_payload_rx_nav_velned_t payload_rx_nav_velned; - ubx_payload_rx_mon_hw_ubx6_t payload_rx_mon_hw_ubx6; - ubx_payload_rx_mon_hw_ubx7_t payload_rx_mon_hw_ubx7; - ubx_payload_rx_mon_ver_part1_t payload_rx_mon_ver_part1; - ubx_payload_rx_mon_ver_part2_t payload_rx_mon_ver_part2; - ubx_payload_rx_ack_ack_t payload_rx_ack_ack; - ubx_payload_rx_ack_nak_t payload_rx_ack_nak; - ubx_payload_tx_cfg_prt_t payload_tx_cfg_prt; - ubx_payload_tx_cfg_rate_t payload_tx_cfg_rate; - ubx_payload_tx_cfg_nav5_t payload_tx_cfg_nav5; - ubx_payload_tx_cfg_sbas_t payload_tx_cfg_sbas; - ubx_payload_tx_cfg_msg_t payload_tx_cfg_msg; -} ubx_buf_t; - -#pragma pack(pop) -/*** END OF u-blox protocol binary message and payload definitions ***/ - -/* Decoder state */ -typedef enum { - UBX_DECODE_SYNC1 = 0, - UBX_DECODE_SYNC2, - UBX_DECODE_CLASS, - UBX_DECODE_ID, - UBX_DECODE_LENGTH1, - UBX_DECODE_LENGTH2, - UBX_DECODE_PAYLOAD, - UBX_DECODE_CHKSUM1, - UBX_DECODE_CHKSUM2 -} ubx_decode_state_t; - -/* Rx message state */ -typedef enum { - UBX_RXMSG_IGNORE = 0, - UBX_RXMSG_HANDLE, - UBX_RXMSG_DISABLE, - UBX_RXMSG_ERROR_LENGTH -} ubx_rxmsg_state_t; - -/* ACK state */ -typedef enum { - UBX_ACK_IDLE = 0, - UBX_ACK_WAITING, - UBX_ACK_GOT_ACK, - UBX_ACK_GOT_NAK -} ubx_ack_state_t; - - -class UBX : public GPS_Helper -{ -public: - UBX(const int &fd, struct vehicle_gps_position_s *gps_position, struct satellite_info_s *satellite_info); - ~UBX(); - int receive(const unsigned timeout); - int configure(unsigned &baudrate); - -private: - - /** - * Parse the binary UBX packet - */ - int parse_char(const uint8_t b); - - /** - * Start payload rx - */ - int payload_rx_init(void); - - /** - * Add payload rx byte - */ - int payload_rx_add(const uint8_t b); - int payload_rx_add_nav_svinfo(const uint8_t b); - int payload_rx_add_mon_ver(const uint8_t b); - - /** - * Finish payload rx - */ - int payload_rx_done(void); - - /** - * Reset the parse state machine for a fresh start - */ - void decode_init(void); - - /** - * While parsing add every byte (except the sync bytes) to the checksum - */ - void add_byte_to_checksum(const uint8_t); - - /** - * Send a message - * @return true on success, false on write error (errno set) - */ - bool send_message(const uint16_t msg, const uint8_t *payload, const uint16_t length); - - /** - * Configure message rate - * @return true on success, false on write error - */ - bool configure_message_rate(const uint16_t msg, const uint8_t rate); - - /** - * Calculate & add checksum for given buffer - */ - void calc_checksum(const uint8_t *buffer, const uint16_t length, ubx_checksum_t *checksum); - - /** - * Wait for message acknowledge - */ - int wait_for_ack(const uint16_t msg, const unsigned timeout, const bool report); - - /** - * Calculate FNV1 hash - */ - uint32_t fnv1_32_str(uint8_t *str, uint32_t hval); - - int _fd; - struct vehicle_gps_position_s *_gps_position; - struct satellite_info_s *_satellite_info; - bool _configured; - ubx_ack_state_t _ack_state; - bool _got_posllh; - bool _got_velned; - ubx_decode_state_t _decode_state; - uint16_t _rx_msg; - ubx_rxmsg_state_t _rx_state; - uint16_t _rx_payload_length; - uint16_t _rx_payload_index; - uint8_t _rx_ck_a; - uint8_t _rx_ck_b; - hrt_abstime _disable_cmd_last; - uint16_t _ack_waiting_msg; - ubx_buf_t _buf; - uint32_t _ubx_version; - bool _use_nav_pvt; -}; - -#endif /* UBX_H_ */