From 6df2fe9aebb008932055b85f134908127d8b3931 Mon Sep 17 00:00:00 2001 From: Kynos Date: Thu, 29 May 2014 21:23:57 +0200 Subject: U-blox driver rework, step 1 Handle u-blox 7+8 GNSS, too. Disabled NAV-SVINFO for now (will be rewritten as an optional feature in a later step). Reduced parser buffer size from 300 to 80. --- src/drivers/gps/ubx.cpp | 37 ++++++++++++++++++++++++--------- src/drivers/gps/ubx.h | 55 ++++++++++++++++++++++++++++++++++++++++++------- 2 files changed, 74 insertions(+), 18 deletions(-) diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index 19cf5beec..ee53e2ec4 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -219,12 +219,14 @@ UBX::configure(unsigned &baudrate) return 1; } +#ifdef UBX_ENABLE_NAV_SVINFO configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO, 5); if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { warnx("MSG CFG FAIL: NAV SVINFO"); return 1; } +#endif configure_message_rate(UBX_CLASS_MON, UBX_MESSAGE_MON_HW, 1); @@ -386,7 +388,6 @@ UBX::parse_char(uint8_t b) if (_rx_count >= _payload_size + 1) { //+1 because of 2 checksum bytes /* compare checksum */ if (_rx_ck_a == _rx_buffer[_rx_count - 1] && _rx_ck_b == _rx_buffer[_rx_count]) { - decode_init(); return 1; // message received successfully } else { @@ -399,7 +400,7 @@ UBX::parse_char(uint8_t b) _rx_count++; } else { - warnx("buffer full"); + warnx("ubx: buffer full 0x%02x-0x%02x L%u", (unsigned)_message_class, (unsigned)_message_id, _payload_size); decode_init(); return -1; } @@ -450,6 +451,7 @@ UBX::handle_message() _gps_position->s_variance_m_s = packet->sAcc; _gps_position->p_variance_m = packet->pAcc; _gps_position->timestamp_variance = hrt_absolute_time(); + _gps_position->satellites_visible = packet->numSV; ret = 1; break; @@ -486,6 +488,7 @@ UBX::handle_message() break; } +#ifdef UBX_ENABLE_NAV_SVINFO case UBX_MESSAGE_NAV_SVINFO: { //printf("GOT NAV_SVINFO\n"); const int length_part1 = 8; @@ -527,7 +530,9 @@ UBX::handle_message() _gps_position->satellite_azimuth[i] = 0; } - _gps_position->satellites_visible = satellites_used; // visible ~= used but we are interested in the used ones + /* Note: _gps_position->satellites_visible is taken from NAV-SOL now. */ + /* _gps_position->satellites_visible = satellites_used; */ // visible ~= used but we are interested in the used ones + /* TODO: satellites_used may be written into a new field after sat monitoring data got separated from _gps_position */ if (packet_part1->numCh > 0) { _gps_position->satellite_info_available = true; @@ -541,6 +546,7 @@ UBX::handle_message() ret = 1; break; } +#endif /* #ifdef UBX_ENABLE_NAV_SVINFO */ case UBX_MESSAGE_NAV_VELNED: { // printf("GOT NAV_VELNED\n"); @@ -573,23 +579,34 @@ UBX::handle_message() break; } - case UBX_CLASS_MON: { + case UBX_CLASS_MON: switch (_message_id) { - case UBX_MESSAGE_MON_HW: { - struct gps_bin_mon_hw_packet *p = (struct gps_bin_mon_hw_packet*) _rx_buffer; + case UBX_MESSAGE_MON_HW: + switch (_payload_size) { - _gps_position->noise_per_ms = p->noisePerMS; - _gps_position->jamming_indicator = p->jamInd; + case UBX_MON_HW_UBX6_RX_PAYLOAD_SIZE: /* u-blox 6 msg format */ + _gps_position->noise_per_ms = ((struct gps_bin_mon_hw_ubx6_packet*) _rx_buffer)->noisePerMS; + _gps_position->jamming_indicator = ((struct gps_bin_mon_hw_ubx6_packet*) _rx_buffer)->jamInd; + ret = 1; + break; + case UBX_MON_HW_UBX7_RX_PAYLOAD_SIZE: /* u-blox 7+ msg format */ + _gps_position->noise_per_ms = ((struct gps_bin_mon_hw_ubx7_packet*) _rx_buffer)->noisePerMS; + _gps_position->jamming_indicator = ((struct gps_bin_mon_hw_ubx7_packet*) _rx_buffer)->jamInd; ret = 1; break; + + default: // unexpected payload size: + ret = 1; // ignore but don't disable msg + break; } + break; default: break; } - } + break; default: break; @@ -597,7 +614,7 @@ UBX::handle_message() if (ret == 0) { /* message not handled */ - warnx("ubx: unknown message received: 0x%02x-0x%02x", (unsigned)_message_class, (unsigned)_message_id); + warnx("ubx: message not handled: 0x%02x-0x%02x L%u", (unsigned)_message_class, (unsigned)_message_id, _payload_size); hrt_abstime t = hrt_absolute_time(); diff --git a/src/drivers/gps/ubx.h b/src/drivers/gps/ubx.h index 5cf47b60b..56f73fa63 100644 --- a/src/drivers/gps/ubx.h +++ b/src/drivers/gps/ubx.h @@ -48,6 +48,10 @@ #include "gps_helper.h" +/* TODO: processing of NAV_SVINFO disabled, has to be re-written as an optional feature */ +/* TODO: make this a command line option, allocate buffer dynamically */ +#undef UBX_ENABLE_NAV_SVINFO + #define UBX_SYNC1 0xB5 #define UBX_SYNC2 0x62 @@ -60,10 +64,8 @@ /* MessageIDs (the ones that are used) */ #define UBX_MESSAGE_NAV_POSLLH 0x02 -//#define UBX_MESSAGE_NAV_DOP 0x04 #define UBX_MESSAGE_NAV_SOL 0x06 #define UBX_MESSAGE_NAV_VELNED 0x12 -//#define UBX_MESSAGE_RXM_SVSI 0x20 #define UBX_MESSAGE_NAV_TIMEUTC 0x21 #define UBX_MESSAGE_NAV_SVINFO 0x30 #define UBX_MESSAGE_ACK_NAK 0x00 @@ -72,9 +74,22 @@ #define UBX_MESSAGE_CFG_MSG 0x01 #define UBX_MESSAGE_CFG_RATE 0x08 #define UBX_MESSAGE_CFG_NAV5 0x24 - #define UBX_MESSAGE_MON_HW 0x09 +/* Rx msg payload sizes */ +#define UBX_NAV_POSLLH_RX_PAYLOAD_SIZE 28 /**< NAV_POSLLH Rx msg payload size */ +#define UBX_NAV_SOL_RX_PAYLOAD_SIZE 52 /**< NAV_SOL Rx msg payload size */ +#define UBX_NAV_VELNED_RX_PAYLOAD_SIZE 36 /**< NAV_VELNED Rx msg payload size */ +#define UBX_NAV_TIMEUTC_RX_PAYLOAD_SIZE 20 /**< NAV_TIMEUTC Rx msg payload size */ +#define UBX_MON_HW_UBX6_RX_PAYLOAD_SIZE 68 /**< MON_HW Rx msg payload size for u-blox 6 and below */ +#define UBX_MON_HW_UBX7_RX_PAYLOAD_SIZE 60 /**< MON_HW Rx msg payload size for u-blox 7 and above */ +#define UBX_MAX_RX_PAYLOAD_SIZE 70 /**< arbitrary maximum for calculating parser buffer size w/o NAV_SVINFO active */ + +/* NAV_SVINFO has variable length w/o a published max size, so limit processing to UBX_MAX_NUM_SAT */ +#define UBX_MAX_NUM_SAT 50 /**< Practical observed max number of satellites in SVNFO msg */ +#define UBX_NAV_SVINFO_RX_PAYLOAD_SIZE (8 + 12 * UBX_MAX_NUM_SAT) /**< NAV_SVINFO Rx msg payload size */ + +/* CFG class Tx msg defs */ #define UBX_CFG_PRT_LENGTH 20 #define UBX_CFG_PRT_PAYLOAD_PORTID 0x01 /**< UART1 */ #define UBX_CFG_PRT_PAYLOAD_MODE 0x000008D0 /**< 0b0000100011010000: 8N1 */ @@ -87,7 +102,6 @@ #define UBX_CFG_RATE_PAYLOAD_NAVRATE 1 /**< cannot be changed */ #define UBX_CFG_RATE_PAYLOAD_TIMEREF 0 /**< 0: UTC, 1: GPS time */ - #define UBX_CFG_NAV5_LENGTH 36 #define UBX_CFG_NAV5_PAYLOAD_MASK 0x0005 /**< XXX only update dynamic model and fix mode */ #define UBX_CFG_NAV5_PAYLOAD_DYNMODEL 7 /**< 0: portable, 2: stationary, 3: pedestrian, 4: automotive, 5: sea, 6: airborne <1g, 7: airborne <2g, 8: airborne <4g */ @@ -98,7 +112,6 @@ #define UBX_CFG_MSG_PAYLOAD_RATE1_1HZ 0x05 /**< {0x00, 0x05, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */ #define UBX_CFG_MSG_PAYLOAD_RATE1_05HZ 10 -#define UBX_MAX_PAYLOAD_LENGTH 500 // ************ /** the structures of the binary packets */ @@ -140,7 +153,7 @@ typedef struct { uint32_t sAcc; uint16_t pDOP; uint8_t reserved1; - uint8_t numSV; + uint8_t numSV; /**< Number of SVs used in Nav Solution */ uint32_t reserved2; uint8_t ck_a; uint8_t ck_b; @@ -213,7 +226,7 @@ typedef struct { uint8_t ck_b; } gps_bin_nav_velned_packet_t; -struct gps_bin_mon_hw_packet { +struct gps_bin_mon_hw_ubx6_packet { uint32_t pinSel; uint32_t pinBank; uint32_t pinDir; @@ -233,6 +246,25 @@ struct gps_bin_mon_hw_packet { uint32_t pullL; }; +struct gps_bin_mon_hw_ubx7_packet { + 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; +}; //typedef struct { // int32_t time_milliseconds; /**< Measurement integer millisecond GPS time of week */ @@ -343,7 +375,14 @@ typedef enum { //typedef type_gps_bin_ubx_state gps_bin_ubx_state_t; #pragma pack(pop) -#define RECV_BUFFER_SIZE 300 //The NAV-SOL messages really need such a big buffer +/* calculate parser rx buffer size dependent on NAV_SVINFO enabled or not */ +/* TODO: make this a command line option, allocate buffer dynamically */ +#define RECV_BUFFER_OVERHEAD 10 // add this to the maximum Rx msg payload size to account for msg overhead */ +#ifdef UBX_ENABLE_NAV_SVINFO + #define RECV_BUFFER_SIZE (UBX_NAV_SVINFO_RX_PAYLOAD_SIZE + RECV_BUFFER_OVERHEAD) +#else + #define RECV_BUFFER_SIZE (UBX_MAX_RX_PAYLOAD_SIZE + RECV_BUFFER_OVERHEAD) +#endif class UBX : public GPS_Helper { -- cgit v1.2.3 From 7349fa90376714b00fe920538950b31a46169d60 Mon Sep 17 00:00:00 2001 From: Kynos Date: Fri, 30 May 2014 11:03:16 +0200 Subject: Beautifications for tab size 8 Beautifications for tab size 8 --- src/drivers/gps/ubx.cpp | 6 ++-- src/drivers/gps/ubx.h | 74 ++++++++++++++++++++++++------------------------- 2 files changed, 40 insertions(+), 40 deletions(-) diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index ee53e2ec4..d7e9454ae 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -59,10 +59,10 @@ #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 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 UBX_WAIT_BEFORE_READ 20 // ms, wait before reading to save read() calls -#define DISABLE_MSG_INTERVAL 1000000 // us, try to disable message with this interval +#define DISABLE_MSG_INTERVAL 1000000 // us, try to disable message with this interval UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position) : _fd(fd), diff --git a/src/drivers/gps/ubx.h b/src/drivers/gps/ubx.h index 56f73fa63..b844c315e 100644 --- a/src/drivers/gps/ubx.h +++ b/src/drivers/gps/ubx.h @@ -34,7 +34,7 @@ /** * @file ubx.h * - * U-Blox protocol definition. Following u-blox 6/7 Receiver Description + * U-Blox protocol definition. Following u-blox 6/7/8 Receiver Description * including Prototol Specification. * * @author Thomas Gubler @@ -86,7 +86,7 @@ #define UBX_MAX_RX_PAYLOAD_SIZE 70 /**< arbitrary maximum for calculating parser buffer size w/o NAV_SVINFO active */ /* NAV_SVINFO has variable length w/o a published max size, so limit processing to UBX_MAX_NUM_SAT */ -#define UBX_MAX_NUM_SAT 50 /**< Practical observed max number of satellites in SVNFO msg */ +#define UBX_MAX_NUM_SAT 50 /**< Practical observed max number of satellites in SVNFO msg */ #define UBX_NAV_SVINFO_RX_PAYLOAD_SIZE (8 + 12 * UBX_MAX_NUM_SAT) /**< NAV_SVINFO Rx msg payload size */ /* CFG class Tx msg defs */ @@ -127,21 +127,21 @@ struct ubx_header { typedef struct { uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */ - int32_t lon; /**< Longitude * 1e-7, deg */ - int32_t lat; /**< Latitude * 1e-7, deg */ - int32_t height; /**< Height above Ellipsoid, mm */ - int32_t height_msl; /**< Height above mean sea level, mm */ - uint32_t hAcc; /**< Horizontal Accuracy Estimate, mm */ - uint32_t vAcc; /**< Vertical Accuracy Estimate, mm */ + int32_t lon; /**< Longitude * 1e-7, deg */ + int32_t lat; /**< Latitude * 1e-7, deg */ + int32_t height; /**< Height above Ellipsoid, mm */ + int32_t height_msl; /**< Height above mean sea level, mm */ + uint32_t hAcc; /**< Horizontal Accuracy Estimate, mm */ + uint32_t vAcc; /**< Vertical Accuracy Estimate, mm */ uint8_t ck_a; uint8_t ck_b; } gps_bin_nav_posllh_packet_t; typedef struct { - uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */ + uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */ int32_t time_nanoseconds; /**< Fractional Nanoseconds remainder of rounded ms above, range -500000 .. 500000 */ - int16_t week; /**< GPS week (GPS time) */ - uint8_t gpsFix; /**< GPS Fix: 0 = No fix, 1 = Dead Reckoning only, 2 = 2D fix, 3 = 3d-fix, 4 = GPS + dead reckoning, 5 = time only fix */ + int16_t week; /**< GPS week (GPS time) */ + uint8_t gpsFix; /**< GPS Fix: 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; @@ -153,7 +153,7 @@ typedef struct { uint32_t sAcc; uint16_t pDOP; uint8_t reserved1; - uint8_t numSV; /**< Number of SVs used in Nav Solution */ + uint8_t numSV; /**< Number of SVs used in Nav Solution */ uint32_t reserved2; uint8_t ck_a; uint8_t ck_b; @@ -163,47 +163,47 @@ typedef struct { uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */ uint32_t time_accuracy; /**< Time Accuracy Estimate, ns */ int32_t time_nanoseconds; /**< Nanoseconds of second, range -1e9 .. 1e9 (UTC) */ - 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..59 (UTC) */ + 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..59 (UTC) */ uint8_t valid_flag; /**< Validity Flags (see ubx documentation) */ uint8_t ck_a; uint8_t ck_b; } gps_bin_nav_timeutc_packet_t; //typedef struct { -// uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */ -// uint16_t gDOP; /**< Geometric DOP (scaling 0.01) */ -// uint16_t pDOP; /**< Position DOP (scaling 0.01) */ -// uint16_t tDOP; /**< Time DOP (scaling 0.01) */ -// uint16_t vDOP; /**< Vertical DOP (scaling 0.01) */ -// uint16_t hDOP; /**< Horizontal DOP (scaling 0.01) */ -// uint16_t nDOP; /**< Northing DOP (scaling 0.01) */ -// uint16_t eDOP; /**< Easting DOP (scaling 0.01) */ +// uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */ +// uint16_t gDOP; /**< Geometric DOP (scaling 0.01) */ +// uint16_t pDOP; /**< Position DOP (scaling 0.01) */ +// uint16_t tDOP; /**< Time DOP (scaling 0.01) */ +// uint16_t vDOP; /**< Vertical DOP (scaling 0.01) */ +// uint16_t hDOP; /**< Horizontal DOP (scaling 0.01) */ +// uint16_t nDOP; /**< Northing DOP (scaling 0.01) */ +// uint16_t eDOP; /**< Easting DOP (scaling 0.01) */ // uint8_t ck_a; // uint8_t ck_b; //} gps_bin_nav_dop_packet_t; typedef struct { - uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */ - uint8_t numCh; /**< Number of channels */ + uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */ + uint8_t numCh; /**< Number of channels */ uint8_t globalFlags; uint16_t reserved2; } gps_bin_nav_svinfo_part1_packet_t; typedef struct { - uint8_t chn; /**< Channel number, 255 for SVs not assigned to a channel */ - uint8_t svid; /**< Satellite ID */ + 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 in integer degrees */ - int16_t azim; /**< Azimuth in integer degrees */ - int32_t prRes; /**< Pseudo range residual in centimetres */ + uint8_t cno; /**< Carrier to Noise Ratio (Signal Strength), dbHz */ + int8_t elev; /**< Elevation in integer degrees */ + int16_t azim; /**< Azimuth in integer degrees */ + int32_t prRes; /**< Pseudo range residual in centimetres */ } gps_bin_nav_svinfo_part2_packet_t; @@ -268,8 +268,8 @@ struct gps_bin_mon_hw_ubx7_packet { //typedef struct { // int32_t time_milliseconds; /**< Measurement integer millisecond GPS time of week */ -// int16_t week; /**< Measurement GPS week number */ -// uint8_t numVis; /**< Number of visible satellites */ +// int16_t week; /**< Measurement GPS week number */ +// uint8_t numVis; /**< Number of visible satellites */ // // //... rest of package is not used in this implementation // @@ -395,7 +395,7 @@ public: private: /** - * Parse the binary MTK packet + * Parse the binary UBX packet */ int parse_char(uint8_t b); -- cgit v1.2.3 From 9bad828bc0033c7017978de2321d3a8698c0afc6 Mon Sep 17 00:00:00 2001 From: Kynos Date: Fri, 30 May 2014 14:30:25 +0200 Subject: U-blox driver rework, step 2 Moved satellite info from vehicle_gps_position_s into a new uORB topic satellite_info. Renamed satellites_visible to satellites_used to reflect true content. sdlog2 will log info for GPS satellites only for now. --- src/drivers/blinkm/blinkm.cpp | 8 +-- src/drivers/gps/gps.cpp | 2 +- src/drivers/gps/mtk.cpp | 2 +- src/drivers/gps/ubx.cpp | 3 +- src/drivers/hott/messages.cpp | 2 +- src/modules/mavlink/mavlink_messages.cpp | 2 +- src/modules/mavlink/mavlink_receiver.cpp | 3 +- src/modules/sdlog2/sdlog2.c | 48 ++++++++------ src/modules/uORB/Subscription.cpp | 2 + src/modules/uORB/objects_common.cpp | 3 + src/modules/uORB/topics/satellite_info.h | 86 ++++++++++++++++++++++++++ src/modules/uORB/topics/vehicle_gps_position.h | 9 +-- 12 files changed, 130 insertions(+), 40 deletions(-) create mode 100644 src/modules/uORB/topics/satellite_info.h diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp index 5c502f682..39bf1fcfc 100644 --- a/src/drivers/blinkm/blinkm.cpp +++ b/src/drivers/blinkm/blinkm.cpp @@ -559,13 +559,7 @@ BlinkM::led() } /* get number of used satellites in navigation */ - num_of_used_sats = 0; - - for(unsigned satloop=0; satloopget_position_update_rate()); diff --git a/src/drivers/gps/mtk.cpp b/src/drivers/gps/mtk.cpp index 680f00d97..7b957e55d 100644 --- a/src/drivers/gps/mtk.cpp +++ b/src/drivers/gps/mtk.cpp @@ -263,7 +263,7 @@ MTK::handle_message(gps_mtk_packet_t &packet) _gps_position->epv_m = _gps_position->eph_m; // unknown in mtk custom mode, so we cheat with eph _gps_position->vel_m_s = ((float)packet.ground_speed) * 1e-2f; // 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_visible = packet.satellites; + _gps_position->satellites_used = packet.satellites; /* convert time and date information to unix timestamp */ struct tm timeinfo; //TODO: test this conversion diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index d7e9454ae..462875b6c 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -55,6 +55,7 @@ #include #include #include +#include #include #include "ubx.h" @@ -451,7 +452,7 @@ UBX::handle_message() _gps_position->s_variance_m_s = packet->sAcc; _gps_position->p_variance_m = packet->pAcc; _gps_position->timestamp_variance = hrt_absolute_time(); - _gps_position->satellites_visible = packet->numSV; + _gps_position->satellites_used = packet->numSV; ret = 1; break; diff --git a/src/drivers/hott/messages.cpp b/src/drivers/hott/messages.cpp index 1e779e8dc..086132573 100644 --- a/src/drivers/hott/messages.cpp +++ b/src/drivers/hott/messages.cpp @@ -226,7 +226,7 @@ build_gps_response(uint8_t *buffer, size_t *size) msg.sensor_id = GPS_SENSOR_ID; msg.sensor_text_id = GPS_SENSOR_TEXT_ID; - msg.gps_num_sat = gps.satellites_visible; + msg.gps_num_sat = gps.satellites_used; /* The GPS fix type: 0 = none, 2 = 2D, 3 = 3D */ msg.gps_fix_char = (uint8_t)(gps.fix_type + 48); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 933478f56..146e2d4b8 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -548,7 +548,7 @@ protected: cm_uint16_from_m_float(gps->epv_m), gps->vel_m_s * 100.0f, _wrap_2pi(gps->cog_rad) * M_RAD_TO_DEG_F * 1e2f, - gps->satellites_visible); + gps->satellites_used); } } }; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 53769e0cf..46da1a0be 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -682,9 +682,8 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg) hil_gps.vel_ned_valid = true; hil_gps.cog_rad = _wrap_pi(gps.cog * M_DEG_TO_RAD_F * 1e-2f); - hil_gps.timestamp_satellites = timestamp; hil_gps.fix_type = gps.fix_type; - hil_gps.satellites_visible = gps.satellites_visible; + hil_gps.satellites_used = gps.satellites_visible; //TODO: rename mavlink_hil_gps_t sats visible to used? if (_gps_pub < 0) { _gps_pub = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps); diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 577cadfbb..4ff6c30aa 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -74,6 +74,7 @@ #include #include #include +#include #include #include #include @@ -139,6 +140,8 @@ PARAM_DEFINE_INT32(SDLOG_EXT, -1); fds[fdsc_count].events = POLLIN; \ fdsc_count++; +#define MIN(X,Y) ((X) < (Y) ? (X) : (Y)) + static bool main_thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ static int deamon_task; /**< Handle of deamon task / thread */ @@ -941,6 +944,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct estimator_status_report estimator_status; struct system_power_s system_power; struct servorail_status_s servorail_status; + struct satellite_info_s sat_info; } buf; memset(&buf, 0, sizeof(buf)); @@ -1000,6 +1004,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int global_pos_sub; int triplet_sub; int gps_pos_sub; + int sat_info_sub; int vicon_pos_sub; int flow_sub; int rc_sub; @@ -1017,6 +1022,7 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); subs.status_sub = orb_subscribe(ORB_ID(vehicle_status)); subs.gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); + subs.sat_info_sub = orb_subscribe(ORB_ID(satellite_info)); subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); subs.att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); subs.att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); @@ -1053,6 +1059,9 @@ int sdlog2_thread_main(int argc, char *argv[]) hrt_abstime barometer_timestamp = 0; hrt_abstime differential_pressure_timestamp = 0; + /* initialize calculated mean SNR */ + float snr_mean = 0.0f; + /* enable logging on start if needed */ if (log_on_start) { /* check GPS topic to get GPS time */ @@ -1115,14 +1124,6 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- GPS POSITION - UNIT #1 --- */ if (gps_pos_updated) { - float snr_mean = 0.0f; - - for (unsigned i = 0; i < buf_gps_pos.satellites_visible; i++) { - snr_mean += buf_gps_pos.satellite_snr[i]; - } - - snr_mean /= buf_gps_pos.satellites_visible; - log_msg.msg_type = LOG_GPS_MSG; log_msg.body.log_GPS.gps_time = buf_gps_pos.time_gps_usec; log_msg.body.log_GPS.fix_type = buf_gps_pos.fix_type; @@ -1135,44 +1136,55 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_GPS.vel_e = buf_gps_pos.vel_e_m_s; log_msg.body.log_GPS.vel_d = buf_gps_pos.vel_d_m_s; log_msg.body.log_GPS.cog = buf_gps_pos.cog_rad; - log_msg.body.log_GPS.sats = buf_gps_pos.satellites_visible; + log_msg.body.log_GPS.sats = buf_gps_pos.satellites_used; log_msg.body.log_GPS.snr_mean = snr_mean; log_msg.body.log_GPS.noise_per_ms = buf_gps_pos.noise_per_ms; log_msg.body.log_GPS.jamming_indicator = buf_gps_pos.jamming_indicator; LOGBUFFER_WRITE_AND_COUNT(GPS); + } + + /* --- SATELLITE INFO - UNIT #1 --- */ + if (_extended_logging) { + + if (copy_if_updated(ORB_ID(satellite_info), subs.sat_info_sub, &buf.sat_info)) { - if (_extended_logging) { /* log the SNR of each satellite for a detailed view of signal quality */ - unsigned gps_msg_max_snr = sizeof(buf_gps_pos.satellite_snr) / sizeof(buf_gps_pos.satellite_snr[0]); + unsigned sat_info_count = MIN(buf.sat_info.count, sizeof(buf.sat_info.snr) / sizeof(buf.sat_info.snr[0])); unsigned log_max_snr = sizeof(log_msg.body.log_GS0A.satellite_snr) / sizeof(log_msg.body.log_GS0A.satellite_snr[0]); log_msg.msg_type = LOG_GS0A_MSG; memset(&log_msg.body.log_GS0A, 0, sizeof(log_msg.body.log_GS0A)); - /* fill set A */ - for (unsigned i = 0; i < gps_msg_max_snr; i++) { + snr_mean = 0.0f; - int satindex = buf_gps_pos.satellite_prn[i] - 1; + /* fill set A and calculate mean SNR */ + for (unsigned i = 0; i < sat_info_count; i++) { + + snr_mean += buf.sat_info.snr[i]; + + int satindex = buf.sat_info.svid[i] - 1; /* handles index exceeding and wraps to to arithmetic errors */ if ((satindex >= 0) && (satindex < (int)log_max_snr)) { /* map satellites by their ID so that logs from two receivers can be compared */ - log_msg.body.log_GS0A.satellite_snr[satindex] = buf_gps_pos.satellite_snr[i]; + log_msg.body.log_GS0A.satellite_snr[satindex] = buf.sat_info.snr[i]; } } LOGBUFFER_WRITE_AND_COUNT(GS0A); + snr_mean /= sat_info_count; log_msg.msg_type = LOG_GS0B_MSG; memset(&log_msg.body.log_GS0B, 0, sizeof(log_msg.body.log_GS0B)); + /* fill set B */ - for (unsigned i = 0; i < gps_msg_max_snr; i++) { + for (unsigned i = 0; i < sat_info_count; i++) { /* get second bank of satellites, thus deduct bank size from index */ - int satindex = buf_gps_pos.satellite_prn[i] - 1 - log_max_snr; + int satindex = buf.sat_info.svid[i] - 1 - log_max_snr; /* handles index exceeding and wraps to to arithmetic errors */ if ((satindex >= 0) && (satindex < (int)log_max_snr)) { /* map satellites by their ID so that logs from two receivers can be compared */ - log_msg.body.log_GS0B.satellite_snr[satindex] = buf_gps_pos.satellite_snr[i]; + log_msg.body.log_GS0B.satellite_snr[satindex] = buf.sat_info.snr[i]; } } LOGBUFFER_WRITE_AND_COUNT(GS0B); diff --git a/src/modules/uORB/Subscription.cpp b/src/modules/uORB/Subscription.cpp index c1d1a938f..44b6debc7 100644 --- a/src/modules/uORB/Subscription.cpp +++ b/src/modules/uORB/Subscription.cpp @@ -40,6 +40,7 @@ #include "topics/parameter_update.h" #include "topics/actuator_controls.h" #include "topics/vehicle_gps_position.h" +#include "topics/satellite_info.h" #include "topics/sensor_combined.h" #include "topics/vehicle_attitude.h" #include "topics/vehicle_global_position.h" @@ -88,6 +89,7 @@ T Subscription::getData() { template class __EXPORT Subscription; template class __EXPORT Subscription; template class __EXPORT Subscription; +template class __EXPORT Subscription; template class __EXPORT Subscription; template class __EXPORT Subscription; template class __EXPORT Subscription; diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 90675bb2e..fc12b9ed5 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -75,6 +75,9 @@ ORB_DEFINE(sensor_combined, struct sensor_combined_s); #include "topics/vehicle_gps_position.h" ORB_DEFINE(vehicle_gps_position, struct vehicle_gps_position_s); +#include "topics/satellite_info.h" +ORB_DEFINE(satellite_info, struct satellite_info_s); + #include "topics/home_position.h" ORB_DEFINE(home_position, struct home_position_s); diff --git a/src/modules/uORB/topics/satellite_info.h b/src/modules/uORB/topics/satellite_info.h new file mode 100644 index 000000000..5f799eda4 --- /dev/null +++ b/src/modules/uORB/topics/satellite_info.h @@ -0,0 +1,86 @@ +/**************************************************************************** + * + * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * Author: @author Thomas Gubler + * @author Julian Oes + * @author Lorenz Meier + * + * 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 satellite_info.h + * Definition of the GNSS satellite info uORB topic. + */ + +#ifndef TOPIC_SAT_INFO_H_ +#define TOPIC_SAT_INFO_H_ + +#include +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ + +/** + * GNSS Satellite Info. + */ +struct satellite_info_s { + uint64_t timestamp; /**< Timestamp of satellite information */ + uint8_t count; /**< Number of satellites in satellite_...[] arrays */ + uint8_t svid[20]; /**< Space vehicle ID [1..255], see scheme below */ + uint8_t used[20]; /**< 0: Satellite not used, 1: used for navigation */ + uint8_t elevation[20]; /**< Elevation (0: right on top of receiver, 90: on the horizon) of satellite */ + uint8_t azimuth[20]; /**< Direction of satellite, 0: 0 deg, 255: 360 deg. */ + uint8_t snr[20]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99, zero when not tracking this satellite. */ +}; + +/** + * NAV_SVINFO space vehicle ID (svid) scheme according to u-blox protocol specs + * u-bloxM8-V15_ReceiverDescriptionProtocolSpec_Public_(UBX-13003221).pdf + * + * GPS 1-32 + * SBAS 120-158 + * Galileo 211-246 + * BeiDou 159-163, 33-64 + * QZSS 193-197 + * GLONASS 65-96, 255 + * + */ + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(satellite_info); + +#endif diff --git a/src/modules/uORB/topics/vehicle_gps_position.h b/src/modules/uORB/topics/vehicle_gps_position.h index 5924a324d..a50f02139 100644 --- a/src/modules/uORB/topics/vehicle_gps_position.h +++ b/src/modules/uORB/topics/vehicle_gps_position.h @@ -82,14 +82,7 @@ struct vehicle_gps_position_s { uint64_t timestamp_time; /**< Timestamp for time information */ uint64_t time_gps_usec; /**< Timestamp (microseconds in GPS format), this is the timestamp which comes from the gps module */ - uint64_t timestamp_satellites; /**< Timestamp for sattelite information */ - uint8_t satellites_visible; /**< Number of satellites visible. If unknown, set to 255 */ - uint8_t satellite_prn[20]; /**< Global satellite ID */ - uint8_t satellite_used[20]; /**< 0: Satellite not used, 1: used for localization */ - uint8_t satellite_elevation[20]; /**< Elevation (0: right on top of receiver, 90: on the horizon) of satellite */ - uint8_t satellite_azimuth[20]; /**< Direction of satellite, 0: 0 deg, 255: 360 deg. */ - uint8_t satellite_snr[20]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99, zero when not tracking this satellite. */ - bool satellite_info_available; /**< 0 for no info, 1 for info available */ + uint8_t satellites_used; /**< Number of satellites used */ }; /** -- cgit v1.2.3 From 9f754e0e9a2e2ad75a3fe5a15690cf542ce08e8a Mon Sep 17 00:00:00 2001 From: Kynos Date: Sun, 8 Jun 2014 14:05:35 +0200 Subject: U-blox driver rework, step 3 More sat info isolation Flush input after changing baudrate to allow driver restart w/o GNSS module restart --- src/drivers/gps/gps.cpp | 98 ++++++++++++++----------- src/drivers/gps/ubx.cpp | 119 ++++++++++++++----------------- src/drivers/gps/ubx.h | 10 +-- src/modules/uORB/topics/satellite_info.h | 17 +++-- 4 files changed, 126 insertions(+), 118 deletions(-) diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index d2ffc30ba..d8e2bd797 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -63,6 +63,7 @@ #include #include #include +#include #include @@ -106,8 +107,10 @@ private: bool _mode_changed; ///< flag that the GPS mode has changed gps_driver_mode_t _mode; ///< current mode GPS_Helper *_Helper; ///< instance of GPS parser - struct vehicle_gps_position_s _report; ///< uORB topic for gps position - orb_advert_t _report_pub; ///< uORB pub for gps position + struct vehicle_gps_position_s _report_gps_pos; ///< uORB topic for gps position + orb_advert_t _report_gps_pos_pub; ///< uORB pub for gps position + struct satellite_info_s _report_sat_info; ///< uORB topic for satellite info + orb_advert_t _report_sat_info_pub; ///< uORB pub for satellite info float _rate; ///< position update rate bool _fake_gps; ///< fake gps output @@ -161,7 +164,8 @@ GPS::GPS(const char *uart_path, bool fake_gps) : _mode_changed(false), _mode(GPS_DRIVER_MODE_UBX), _Helper(nullptr), - _report_pub(-1), + _report_gps_pos_pub(-1), + _report_sat_info_pub(-1), _rate(0.0f), _fake_gps(fake_gps) { @@ -172,7 +176,8 @@ GPS::GPS(const char *uart_path, bool fake_gps) : /* we need this potentially before it could be set in task_main */ g_dev = this; - memset(&_report, 0, sizeof(_report)); + memset(&_report_gps_pos, 0, sizeof(_report_gps_pos)); + memset(&_report_sat_info, 0, sizeof(_report_sat_info)); _debug_enabled = true; } @@ -271,33 +276,33 @@ GPS::task_main() if (_fake_gps) { - _report.timestamp_position = hrt_absolute_time(); - _report.lat = (int32_t)47.378301e7f; - _report.lon = (int32_t)8.538777e7f; - _report.alt = (int32_t)1200e3f; - _report.timestamp_variance = hrt_absolute_time(); - _report.s_variance_m_s = 10.0f; - _report.p_variance_m = 10.0f; - _report.c_variance_rad = 0.1f; - _report.fix_type = 3; - _report.eph_m = 0.9f; - _report.epv_m = 1.8f; - _report.timestamp_velocity = hrt_absolute_time(); - _report.vel_n_m_s = 0.0f; - _report.vel_e_m_s = 0.0f; - _report.vel_d_m_s = 0.0f; - _report.vel_m_s = sqrtf(_report.vel_n_m_s * _report.vel_n_m_s + _report.vel_e_m_s * _report.vel_e_m_s + _report.vel_d_m_s * _report.vel_d_m_s); - _report.cog_rad = 0.0f; - _report.vel_ned_valid = true; + _report_gps_pos.timestamp_position = hrt_absolute_time(); + _report_gps_pos.lat = (int32_t)47.378301e7f; + _report_gps_pos.lon = (int32_t)8.538777e7f; + _report_gps_pos.alt = (int32_t)1200e3f; + _report_gps_pos.timestamp_variance = hrt_absolute_time(); + _report_gps_pos.s_variance_m_s = 10.0f; + _report_gps_pos.p_variance_m = 10.0f; + _report_gps_pos.c_variance_rad = 0.1f; + _report_gps_pos.fix_type = 3; + _report_gps_pos.eph_m = 0.9f; + _report_gps_pos.epv_m = 1.8f; + _report_gps_pos.timestamp_velocity = hrt_absolute_time(); + _report_gps_pos.vel_n_m_s = 0.0f; + _report_gps_pos.vel_e_m_s = 0.0f; + _report_gps_pos.vel_d_m_s = 0.0f; + _report_gps_pos.vel_m_s = sqrtf(_report_gps_pos.vel_n_m_s * _report_gps_pos.vel_n_m_s + _report_gps_pos.vel_e_m_s * _report_gps_pos.vel_e_m_s + _report_gps_pos.vel_d_m_s * _report_gps_pos.vel_d_m_s); + _report_gps_pos.cog_rad = 0.0f; + _report_gps_pos.vel_ned_valid = true; //no time and satellite information simulated if (!(_pub_blocked)) { - if (_report_pub > 0) { - orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); + if (_report_gps_pos_pub > 0) { + orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos); } else { - _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); + _report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos); } } @@ -313,11 +318,11 @@ GPS::task_main() switch (_mode) { case GPS_DRIVER_MODE_UBX: - _Helper = new UBX(_serial_fd, &_report); + _Helper = new UBX(_serial_fd, &_report_gps_pos, &_report_sat_info, UBX_ENABLE_NAV_SVINFO); break; case GPS_DRIVER_MODE_MTK: - _Helper = new MTK(_serial_fd, &_report); + _Helper = new MTK(_serial_fd, &_report_gps_pos); break; default: @@ -332,20 +337,33 @@ GPS::task_main() // GPS is obviously detected successfully, reset statistics _Helper->reset_update_rates(); - while (_Helper->receive(TIMEOUT_5HZ) > 0 && !_task_should_exit) { + int helper_ret; + while ((helper_ret = _Helper->receive(TIMEOUT_5HZ)) > 0 && !_task_should_exit) { // lock(); /* opportunistic publishing - else invalid data would end up on the bus */ if (!(_pub_blocked)) { - if (_report_pub > 0) { - orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); + if (helper_ret & 1) { + if (_report_gps_pos_pub > 0) { + orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos); - } else { - _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); + } else { + _report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos); + } + } + if (helper_ret & 2) { + if (_report_sat_info_pub > 0) { + orb_publish(ORB_ID(satellite_info), _report_sat_info_pub, &_report_sat_info); + + } else { + _report_sat_info_pub = orb_advertise(ORB_ID(satellite_info), &_report_sat_info); + } } } - last_rate_count++; + if (helper_ret & 1) { // consider only pos info updates for rate calculation */ + last_rate_count++; + } /* measure update rate every 5 seconds */ if (hrt_absolute_time() - last_rate_measurement > RATE_MEASUREMENT_PERIOD) { @@ -357,7 +375,7 @@ GPS::task_main() } if (!_healthy) { - char *mode_str = "unknown"; + const char *mode_str = "unknown"; switch (_mode) { case GPS_DRIVER_MODE_UBX: @@ -447,11 +465,11 @@ GPS::print_info() warnx("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_healthy) ? "OK" : "NOT OK"); - if (_report.timestamp_position != 0) { - warnx("position lock: %dD, satellites: %d, last update: %8.4fms ago", (int)_report.fix_type, - _report.satellites_used, (double)(hrt_absolute_time() - _report.timestamp_position) / 1000.0f); - warnx("lat: %d, lon: %d, alt: %d", _report.lat, _report.lon, _report.alt); - warnx("eph: %.2fm, epv: %.2fm", (double)_report.eph_m, (double)_report.epv_m); + if (_report_gps_pos.timestamp_position != 0) { + warnx("position lock: %dD, satellites: %d, last update: %8.4fms ago", (int)_report_gps_pos.fix_type, + _report_gps_pos.satellites_used, (double)(hrt_absolute_time() - _report_gps_pos.timestamp_position) / 1000.0); + warnx("lat: %d, lon: %d, alt: %d", _report_gps_pos.lat, _report_gps_pos.lon, _report_gps_pos.alt); + warnx("eph: %.2fm, epv: %.2fm", (double)_report_gps_pos.eph_m, (double)_report_gps_pos.epv_m); warnx("rate position: \t%6.2f Hz", (double)_Helper->get_position_update_rate()); warnx("rate velocity: \t%6.2f Hz", (double)_Helper->get_velocity_update_rate()); warnx("rate publication:\t%6.2f Hz", (double)_rate); @@ -578,7 +596,7 @@ gps_main(int argc, char *argv[]) { /* set to default */ - char *device_name = GPS_DEFAULT_UART_PORT; + const char *device_name = GPS_DEFAULT_UART_PORT; bool fake_gps = false; /* diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index 462875b6c..dd47572b4 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -65,9 +65,13 @@ #define UBX_WAIT_BEFORE_READ 20 // ms, wait before reading to save read() calls #define DISABLE_MSG_INTERVAL 1000000 // us, try to disable message with this interval -UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position) : +#define MIN(X,Y) ((X) < (Y) ? (X) : (Y)) + +UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position, struct satellite_info_s *satellite_info, const bool &enable_sat_info) : _fd(fd), _gps_position(gps_position), + _satellite_info(satellite_info), + _enable_sat_info(enable_sat_info), _configured(false), _waiting_for_ack(false), _disable_cmd_last(0) @@ -84,14 +88,19 @@ UBX::configure(unsigned &baudrate) { _configured = false; /* try different baudrates */ - const unsigned baudrates_to_try[] = {9600, 38400, 19200, 57600, 115200}; + const unsigned baudrates[] = {9600, 38400, 19200, 57600, 115200}; - int baud_i; + unsigned baud_i; - for (baud_i = 0; baud_i < 5; baud_i++) { - baudrate = baudrates_to_try[baud_i]; + 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(); + wait_for_ack(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 from this */ @@ -107,7 +116,7 @@ UBX::configure(unsigned &baudrate) cfg_prt_packet.msgID = UBX_MESSAGE_CFG_PRT; cfg_prt_packet.length = UBX_CFG_PRT_LENGTH; cfg_prt_packet.portID = UBX_CFG_PRT_PAYLOAD_PORTID; - cfg_prt_packet.mode = UBX_CFG_PRT_PAYLOAD_MODE; + cfg_prt_packet.mode = UBX_CFG_PRT_PAYLOAD_MODE; cfg_prt_packet.baudRate = baudrate; cfg_prt_packet.inProtoMask = UBX_CFG_PRT_PAYLOAD_INPROTOMASK; cfg_prt_packet.outProtoMask = UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK; @@ -125,7 +134,7 @@ UBX::configure(unsigned &baudrate) cfg_prt_packet.msgID = UBX_MESSAGE_CFG_PRT; cfg_prt_packet.length = UBX_CFG_PRT_LENGTH; cfg_prt_packet.portID = UBX_CFG_PRT_PAYLOAD_PORTID; - cfg_prt_packet.mode = UBX_CFG_PRT_PAYLOAD_MODE; + cfg_prt_packet.mode = UBX_CFG_PRT_PAYLOAD_MODE; cfg_prt_packet.baudRate = UBX_CFG_PRT_PAYLOAD_BAUDRATE; cfg_prt_packet.inProtoMask = UBX_CFG_PRT_PAYLOAD_INPROTOMASK; cfg_prt_packet.outProtoMask = UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK; @@ -144,7 +153,7 @@ UBX::configure(unsigned &baudrate) break; } - if (baud_i >= 5) { + if (baud_i >= sizeof(baudrates) / sizeof(baudrates[0])) { return 1; } @@ -220,14 +229,14 @@ UBX::configure(unsigned &baudrate) return 1; } -#ifdef UBX_ENABLE_NAV_SVINFO - configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO, 5); + if (_enable_sat_info) { + configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO, 5); - if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { - warnx("MSG CFG FAIL: NAV SVINFO"); - return 1; + if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { + warnx("MSG CFG FAIL: NAV SVINFO"); + return 1; + } } -#endif configure_message_rate(UBX_CLASS_MON, UBX_MESSAGE_MON_HW, 1); @@ -244,14 +253,13 @@ int UBX::wait_for_ack(unsigned timeout) { _waiting_for_ack = true; - uint64_t time_started = hrt_absolute_time(); + uint64_t time_end = hrt_absolute_time() + timeout * 1000; - while (hrt_absolute_time() < time_started + timeout * 1000) { + while ((timeout = (time_end - hrt_absolute_time()) / 1000) > 0) { if (receive(timeout) > 0) { if (!_waiting_for_ack) { return 1; } - } else { return -1; // timeout or error receiving, or NAK } @@ -275,7 +283,7 @@ UBX::receive(unsigned timeout) ssize_t count = 0; - bool handled = false; + int handled = 0; while (true) { @@ -290,7 +298,7 @@ UBX::receive(unsigned timeout) } else if (ret == 0) { /* return success after short delay after receiving a packet or timeout after long delay */ if (handled) { - return 1; + return handled; } else { return -1; @@ -311,8 +319,7 @@ UBX::receive(unsigned timeout) /* pass received bytes to the packet decoder */ for (int i = 0; i < count; i++) { if (parse_char(buf[i]) > 0) { - if (handle_message() > 0) - handled = true; + handled |= handle_message(); } } } @@ -334,6 +341,7 @@ UBX::parse_char(uint8_t b) case UBX_DECODE_UNINIT: if (b == UBX_SYNC1) { _decode_state = UBX_DECODE_GOT_SYNC1; + //### printf("\nA"); } break; @@ -342,11 +350,13 @@ UBX::parse_char(uint8_t b) case UBX_DECODE_GOT_SYNC1: if (b == UBX_SYNC2) { _decode_state = UBX_DECODE_GOT_SYNC2; + //### printf("B"); } else { /* Second start symbol was wrong, reset state machine */ decode_init(); /* don't return error, it can be just false sync1 */ + //### printf("-"); } break; @@ -357,24 +367,28 @@ UBX::parse_char(uint8_t b) add_byte_to_checksum(b); _message_class = b; _decode_state = UBX_DECODE_GOT_CLASS; + //### printf("C"); break; case UBX_DECODE_GOT_CLASS: add_byte_to_checksum(b); _message_id = b; _decode_state = UBX_DECODE_GOT_MESSAGEID; + //### printf("D"); break; case UBX_DECODE_GOT_MESSAGEID: add_byte_to_checksum(b); _payload_size = b; //this is the first length byte _decode_state = UBX_DECODE_GOT_LENGTH1; + //### printf("E"); break; case UBX_DECODE_GOT_LENGTH1: add_byte_to_checksum(b); _payload_size += b << 8; // here comes the second byte of length _decode_state = UBX_DECODE_GOT_LENGTH2; + //### printf("F"); break; case UBX_DECODE_GOT_LENGTH2: @@ -389,6 +403,7 @@ UBX::parse_char(uint8_t b) if (_rx_count >= _payload_size + 1) { //+1 because of 2 checksum bytes /* compare checksum */ if (_rx_ck_a == _rx_buffer[_rx_count - 1] && _rx_ck_b == _rx_buffer[_rx_count]) { + //### printf("O\n"); return 1; // message received successfully } else { @@ -399,6 +414,7 @@ UBX::parse_char(uint8_t b) } else if (_rx_count < RECV_BUFFER_SIZE) { _rx_count++; + //### printf("."); } else { warnx("ubx: buffer full 0x%02x-0x%02x L%u", (unsigned)_message_class, (unsigned)_message_id, _payload_size); @@ -489,65 +505,37 @@ UBX::handle_message() break; } -#ifdef UBX_ENABLE_NAV_SVINFO case UBX_MESSAGE_NAV_SVINFO: { //printf("GOT NAV_SVINFO\n"); + if (!_enable_sat_info) { // if satellite info processing not enabled: + break; // ignore and disable NAV-SVINFO message + } const int length_part1 = 8; gps_bin_nav_svinfo_part1_packet_t *packet_part1 = (gps_bin_nav_svinfo_part1_packet_t *) _rx_buffer; const int length_part2 = 12; gps_bin_nav_svinfo_part2_packet_t *packet_part2; - uint8_t satellites_used = 0; - int i; - //printf("Number of Channels: %d\n", packet_part1->numCh); - for (i = 0; i < packet_part1->numCh; i++) { - /* set pointer to sattelite_i information */ - packet_part2 = (gps_bin_nav_svinfo_part2_packet_t *) & (_rx_buffer[length_part1 + i * length_part2]); + for (_satellite_info->count = 0; + _satellite_info->count < MIN(packet_part1->numCh, sizeof(_satellite_info->snr) / sizeof(_satellite_info->snr[0])); + _satellite_info->count++) { + /* set pointer to satellite_i information */ + packet_part2 = (gps_bin_nav_svinfo_part2_packet_t *) & (_rx_buffer[length_part1 + _satellite_info->count* length_part2]); /* write satellite information to global storage */ - uint8_t sv_used = packet_part2->flags & 0x01; - - if (sv_used) { - /* count SVs used for NAV */ - satellites_used++; - } - - /* record info for all channels, whether or not the SV is used for NAV */ - _gps_position->satellite_used[i] = sv_used; - _gps_position->satellite_snr[i] = packet_part2->cno; - _gps_position->satellite_elevation[i] = (uint8_t)(packet_part2->elev); - _gps_position->satellite_azimuth[i] = (uint8_t)((float)packet_part2->azim * 255.0f / 360.0f); - _gps_position->satellite_prn[i] = packet_part2->svid; - //printf("SAT %d: %d %d %d %d\n", i, (int)sv_used, (int)packet_part2->cno, (int)(uint8_t)(packet_part2->elev), (int)packet_part2->svid); - } - - for (i = packet_part1->numCh; i < 20; i++) { - /* unused channels have to be set to zero for e.g. MAVLink */ - _gps_position->satellite_prn[i] = 0; - _gps_position->satellite_used[i] = 0; - _gps_position->satellite_snr[i] = 0; - _gps_position->satellite_elevation[i] = 0; - _gps_position->satellite_azimuth[i] = 0; + _satellite_info->used[_satellite_info->count] = packet_part2->flags & 0x01; + _satellite_info->snr[_satellite_info->count] = packet_part2->cno; + _satellite_info->elevation[_satellite_info->count] = (uint8_t)(packet_part2->elev); + _satellite_info->azimuth[_satellite_info->count] = (uint8_t)((float)packet_part2->azim * 255.0f / 360.0f); + _satellite_info->svid[_satellite_info->count] = packet_part2->svid; + //printf("SAT %d: %d %d %d %d\n", _satellite_info->count, (int)sv_used, (int)packet_part2->cno, (int)(uint8_t)(packet_part2->elev), (int)packet_part2->svid); } - /* Note: _gps_position->satellites_visible is taken from NAV-SOL now. */ - /* _gps_position->satellites_visible = satellites_used; */ // visible ~= used but we are interested in the used ones - /* TODO: satellites_used may be written into a new field after sat monitoring data got separated from _gps_position */ + _satellite_info->timestamp = hrt_absolute_time(); - if (packet_part1->numCh > 0) { - _gps_position->satellite_info_available = true; - - } else { - _gps_position->satellite_info_available = false; - } - - _gps_position->timestamp_satellites = hrt_absolute_time(); - - ret = 1; + ret = 2; break; } -#endif /* #ifdef UBX_ENABLE_NAV_SVINFO */ case UBX_MESSAGE_NAV_VELNED: { // printf("GOT NAV_VELNED\n"); @@ -650,7 +638,6 @@ UBX::handle_message() warnx("ubx: not acknowledged"); /* configuration obviously not successful */ _waiting_for_ack = false; - ret = -1; break; } diff --git a/src/drivers/gps/ubx.h b/src/drivers/gps/ubx.h index b844c315e..90aeffedc 100644 --- a/src/drivers/gps/ubx.h +++ b/src/drivers/gps/ubx.h @@ -48,9 +48,7 @@ #include "gps_helper.h" -/* TODO: processing of NAV_SVINFO disabled, has to be re-written as an optional feature */ -/* TODO: make this a command line option, allocate buffer dynamically */ -#undef UBX_ENABLE_NAV_SVINFO +#define UBX_ENABLE_NAV_SVINFO 1 /* TODO: make this a command line option, allocate buffer(s) dynamically */ #define UBX_SYNC1 0xB5 #define UBX_SYNC2 0x62 @@ -378,7 +376,7 @@ typedef enum { /* calculate parser rx buffer size dependent on NAV_SVINFO enabled or not */ /* TODO: make this a command line option, allocate buffer dynamically */ #define RECV_BUFFER_OVERHEAD 10 // add this to the maximum Rx msg payload size to account for msg overhead */ -#ifdef UBX_ENABLE_NAV_SVINFO +#if (UBX_ENABLE_NAV_SVINFO != 0) #define RECV_BUFFER_SIZE (UBX_NAV_SVINFO_RX_PAYLOAD_SIZE + RECV_BUFFER_OVERHEAD) #else #define RECV_BUFFER_SIZE (UBX_MAX_RX_PAYLOAD_SIZE + RECV_BUFFER_OVERHEAD) @@ -387,7 +385,7 @@ typedef enum { class UBX : public GPS_Helper { public: - UBX(const int &fd, struct vehicle_gps_position_s *gps_position); + UBX(const int &fd, struct vehicle_gps_position_s *gps_position, struct satellite_info_s *satellite_info, const bool &enable_sat_info); ~UBX(); int receive(unsigned timeout); int configure(unsigned &baudrate); @@ -434,6 +432,8 @@ private: int _fd; struct vehicle_gps_position_s *_gps_position; + struct satellite_info_s *_satellite_info; + bool _enable_sat_info; bool _configured; bool _waiting_for_ack; uint8_t _message_class_needed; diff --git a/src/modules/uORB/topics/satellite_info.h b/src/modules/uORB/topics/satellite_info.h index 5f799eda4..37c2faa96 100644 --- a/src/modules/uORB/topics/satellite_info.h +++ b/src/modules/uORB/topics/satellite_info.h @@ -53,14 +53,17 @@ /** * GNSS Satellite Info. */ + +#define SAT_INFO_MAX_SATELLITES 20 + struct satellite_info_s { - uint64_t timestamp; /**< Timestamp of satellite information */ - uint8_t count; /**< Number of satellites in satellite_...[] arrays */ - uint8_t svid[20]; /**< Space vehicle ID [1..255], see scheme below */ - uint8_t used[20]; /**< 0: Satellite not used, 1: used for navigation */ - uint8_t elevation[20]; /**< Elevation (0: right on top of receiver, 90: on the horizon) of satellite */ - uint8_t azimuth[20]; /**< Direction of satellite, 0: 0 deg, 255: 360 deg. */ - uint8_t snr[20]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99, zero when not tracking this satellite. */ + uint64_t timestamp; /**< Timestamp of satellite info */ + uint8_t count; /**< Number of satellites in satellite info */ + uint8_t svid[SAT_INFO_MAX_SATELLITES]; /**< Space vehicle ID [1..255], see scheme below */ + uint8_t used[SAT_INFO_MAX_SATELLITES]; /**< 0: Satellite not used, 1: used for navigation */ + uint8_t elevation[SAT_INFO_MAX_SATELLITES]; /**< Elevation (0: right on top of receiver, 90: on the horizon) of satellite */ + uint8_t azimuth[SAT_INFO_MAX_SATELLITES]; /**< Direction of satellite, 0: 0 deg, 255: 360 deg. */ + uint8_t snr[SAT_INFO_MAX_SATELLITES]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99, zero when not tracking this satellite. */ }; /** -- cgit v1.2.3 From 018eeeecad748959a8afa4845a536e897a57249d Mon Sep 17 00:00:00 2001 From: Kynos Date: Sun, 8 Jun 2014 15:56:42 +0200 Subject: Merged ubx driver part of pull request #1031 --- src/drivers/gps/ubx.cpp | 15 +++++++++++---- src/drivers/gps/ubx.h | 2 ++ 2 files changed, 13 insertions(+), 4 deletions(-) diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index dd47572b4..0708e23b4 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -74,6 +74,8 @@ UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position, struct sate _enable_sat_info(enable_sat_info), _configured(false), _waiting_for_ack(false), + _got_posllh(false), + _got_velned(false), _disable_cmd_last(0) { decode_init(); @@ -208,7 +210,7 @@ UBX::configure(unsigned &baudrate) return 1; } - configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_TIMEUTC, 1); + configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_TIMEUTC, 5); if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { warnx("MSG CFG FAIL: NAV TIMEUTC"); @@ -286,9 +288,10 @@ UBX::receive(unsigned timeout) int handled = 0; while (true) { + bool ready_to_return = _configured ? (_got_posllh && _got_velned) : handled; /* poll for new data, wait for only UBX_PACKET_TIMEOUT (2ms) if something already received */ - int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), handled ? UBX_PACKET_TIMEOUT : timeout); + int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), ready_to_return ? UBX_PACKET_TIMEOUT : timeout); if (ret < 0) { /* something went wrong when polling */ @@ -297,7 +300,9 @@ UBX::receive(unsigned timeout) } else if (ret == 0) { /* return success after short delay after receiving a packet or timeout after long delay */ - if (handled) { + if (ready_to_return) { + _got_posllh = false; + _got_velned = false; return handled; } else { @@ -456,7 +461,8 @@ UBX::handle_message() _rate_count_lat_lon++; - ret = 1; + _got_posllh = true; + ret = 1; break; } @@ -552,6 +558,7 @@ UBX::handle_message() _rate_count_vel++; + _got_velned = true; ret = 1; break; } diff --git a/src/drivers/gps/ubx.h b/src/drivers/gps/ubx.h index 90aeffedc..50df3ada2 100644 --- a/src/drivers/gps/ubx.h +++ b/src/drivers/gps/ubx.h @@ -436,6 +436,8 @@ private: bool _enable_sat_info; bool _configured; bool _waiting_for_ack; + bool _got_posllh; + bool _got_velned; uint8_t _message_class_needed; uint8_t _message_id_needed; ubx_decode_state_t _decode_state; -- cgit v1.2.3 From 3538773b99d4231bf8a64e9c257cfe0299a93021 Mon Sep 17 00:00:00 2001 From: Kynos Date: Sun, 8 Jun 2014 16:05:04 +0200 Subject: Initialize rate values Initialize rate values (else invalid rates wil be displayed during the first measurement interval). --- src/drivers/gps/gps_helper.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/gps/gps_helper.h b/src/drivers/gps/gps_helper.h index d14a95afe..3623397b2 100644 --- a/src/drivers/gps/gps_helper.h +++ b/src/drivers/gps/gps_helper.h @@ -62,8 +62,8 @@ protected: uint8_t _rate_count_lat_lon; uint8_t _rate_count_vel; - float _rate_lat_lon; - float _rate_vel; + float _rate_lat_lon = 0.0f; + float _rate_vel = 0.0f; uint64_t _interval_rate_start; }; -- cgit v1.2.3 From 07458b284d4cea48bc3b6e0cb5dcaba1c2c1d4d6 Mon Sep 17 00:00:00 2001 From: Kynos Date: Sun, 8 Jun 2014 17:54:50 +0200 Subject: Gps driver: make enable_sat_info a command line option --- src/drivers/gps/gps.cpp | 28 +++++++++++++++++++--------- 1 file changed, 19 insertions(+), 9 deletions(-) diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index d8e2bd797..f4d2f4b44 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -83,7 +83,7 @@ static const int ERROR = -1; class GPS : public device::CDev { public: - GPS(const char *uart_path, bool fake_gps); + GPS(const char *uart_path, bool fake_gps, bool enable_sat_info); virtual ~GPS(); virtual int init(); @@ -113,6 +113,7 @@ private: orb_advert_t _report_sat_info_pub; ///< uORB pub for satellite info float _rate; ///< position update rate bool _fake_gps; ///< fake gps output + bool _enable_sat_info; ///< enable sat info /** @@ -157,7 +158,7 @@ GPS *g_dev; } -GPS::GPS(const char *uart_path, bool fake_gps) : +GPS::GPS(const char *uart_path, bool fake_gps, bool enable_sat_info) : CDev("gps", GPS_DEVICE_PATH), _task_should_exit(false), _healthy(false), @@ -167,7 +168,8 @@ GPS::GPS(const char *uart_path, bool fake_gps) : _report_gps_pos_pub(-1), _report_sat_info_pub(-1), _rate(0.0f), - _fake_gps(fake_gps) + _fake_gps(fake_gps), + _enable_sat_info(enable_sat_info) { /* store port name */ strncpy(_port, uart_path, sizeof(_port)); @@ -318,7 +320,7 @@ GPS::task_main() switch (_mode) { case GPS_DRIVER_MODE_UBX: - _Helper = new UBX(_serial_fd, &_report_gps_pos, &_report_sat_info, UBX_ENABLE_NAV_SVINFO); + _Helper = new UBX(_serial_fd, &_report_gps_pos, &_report_sat_info, _enable_sat_info); break; case GPS_DRIVER_MODE_MTK: @@ -464,6 +466,7 @@ GPS::print_info() } warnx("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_healthy) ? "OK" : "NOT OK"); + warnx("sat info: %s", (_enable_sat_info) ? "enabled" : "disabled"); if (_report_gps_pos.timestamp_position != 0) { warnx("position lock: %dD, satellites: %d, last update: %8.4fms ago", (int)_report_gps_pos.fix_type, @@ -487,7 +490,7 @@ namespace gps GPS *g_dev; -void start(const char *uart_path, bool fake_gps); +void start(const char *uart_path, bool fake_gps, bool enable_sat_info); void stop(); void test(); void reset(); @@ -497,7 +500,7 @@ void info(); * Start the driver. */ void -start(const char *uart_path, bool fake_gps) +start(const char *uart_path, bool fake_gps, bool enable_sat_info) { int fd; @@ -505,7 +508,7 @@ start(const char *uart_path, bool fake_gps) errx(1, "already started"); /* create the driver */ - g_dev = new GPS(uart_path, fake_gps); + g_dev = new GPS(uart_path, fake_gps, enable_sat_info); if (g_dev == nullptr) goto fail; @@ -598,6 +601,7 @@ gps_main(int argc, char *argv[]) /* set to default */ const char *device_name = GPS_DEFAULT_UART_PORT; bool fake_gps = false; + bool enable_sat_info = false; /* * Start/load the driver. @@ -619,7 +623,13 @@ gps_main(int argc, char *argv[]) fake_gps = true; } - gps::start(device_name, fake_gps); + /* Detect sat info option */ + for (int i = 2; i < argc; i++) { + if (!strcmp(argv[i], "-s")) + enable_sat_info = true; + } + + gps::start(device_name, fake_gps, enable_sat_info); } if (!strcmp(argv[1], "stop")) @@ -644,5 +654,5 @@ gps_main(int argc, char *argv[]) gps::info(); out: - errx(1, "unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status' [-d /dev/ttyS0-n][-f]"); + errx(1, "unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status' [-d /dev/ttyS0-n][-f][-s]"); } -- cgit v1.2.3 From b6b3ad6e1e31503d230ad6e7bc8f2d7b5b596ad7 Mon Sep 17 00:00:00 2001 From: Kynos Date: Fri, 13 Jun 2014 14:05:47 +0200 Subject: U-blox driver rework,, step 4 Config phase and parser rewrite --- src/drivers/gps/gps.cpp | 41 ++- src/drivers/gps/ubx.cpp | 903 ++++++++++++++++++++++++++---------------------- src/drivers/gps/ubx.h | 662 ++++++++++++++++++----------------- 3 files changed, 849 insertions(+), 757 deletions(-) diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index f4d2f4b44..6ae325297 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -80,6 +80,14 @@ #endif static const int ERROR = -1; +/* class for dynamic allocation of satellite info data */ +class GPS_Sat_Info +{ +public: + struct satellite_info_s _data; +}; + + class GPS : public device::CDev { public: @@ -103,17 +111,17 @@ private: char _port[20]; ///< device / serial port path volatile int _task; //< worker task bool _healthy; ///< flag to signal if the GPS is ok - bool _baudrate_changed; ///< flag to signal that the baudrate with the GPS has changed + bool _baudrate_changed; ///< flag to signal that the baudrate with the GPS has changed bool _mode_changed; ///< flag that the GPS mode has changed gps_driver_mode_t _mode; ///< current mode GPS_Helper *_Helper; ///< instance of GPS parser - struct vehicle_gps_position_s _report_gps_pos; ///< uORB topic for gps position + GPS_Sat_Info *_Sat_Info; ///< instance of GPS sat info data object + struct vehicle_gps_position_s _report_gps_pos; ///< uORB topic for gps position orb_advert_t _report_gps_pos_pub; ///< uORB pub for gps position - struct satellite_info_s _report_sat_info; ///< uORB topic for satellite info + struct satellite_info_s *_p_report_sat_info; ///< pointer to uORB topic for satellite info orb_advert_t _report_sat_info_pub; ///< uORB pub for satellite info float _rate; ///< position update rate bool _fake_gps; ///< fake gps output - bool _enable_sat_info; ///< enable sat info /** @@ -165,11 +173,12 @@ GPS::GPS(const char *uart_path, bool fake_gps, bool enable_sat_info) : _mode_changed(false), _mode(GPS_DRIVER_MODE_UBX), _Helper(nullptr), + _Sat_Info(nullptr), _report_gps_pos_pub(-1), + _p_report_sat_info(nullptr), _report_sat_info_pub(-1), _rate(0.0f), - _fake_gps(fake_gps), - _enable_sat_info(enable_sat_info) + _fake_gps(fake_gps) { /* store port name */ strncpy(_port, uart_path, sizeof(_port)); @@ -179,7 +188,13 @@ GPS::GPS(const char *uart_path, bool fake_gps, bool enable_sat_info) : /* we need this potentially before it could be set in task_main */ g_dev = this; memset(&_report_gps_pos, 0, sizeof(_report_gps_pos)); - memset(&_report_sat_info, 0, sizeof(_report_sat_info)); + + /* create satellite info data object if requested */ + if (enable_sat_info) { + _Sat_Info = new(GPS_Sat_Info); + _p_report_sat_info = &_Sat_Info->_data; + memset(_p_report_sat_info, 0, sizeof(*_p_report_sat_info)); + } _debug_enabled = true; } @@ -214,7 +229,7 @@ GPS::init() /* start the GPS driver worker task */ _task = task_spawn_cmd("gps", SCHED_DEFAULT, - SCHED_PRIORITY_SLOW_DRIVER, 2000, (main_t)&GPS::task_main_trampoline, nullptr); + SCHED_PRIORITY_SLOW_DRIVER, 1500, (main_t)&GPS::task_main_trampoline, nullptr); if (_task < 0) { warnx("task start failed: %d", errno); @@ -320,7 +335,7 @@ GPS::task_main() switch (_mode) { case GPS_DRIVER_MODE_UBX: - _Helper = new UBX(_serial_fd, &_report_gps_pos, &_report_sat_info, _enable_sat_info); + _Helper = new UBX(_serial_fd, &_report_gps_pos, _p_report_sat_info); break; case GPS_DRIVER_MODE_MTK: @@ -353,12 +368,12 @@ GPS::task_main() _report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos); } } - if (helper_ret & 2) { + if (_p_report_sat_info && (helper_ret & 2)) { if (_report_sat_info_pub > 0) { - orb_publish(ORB_ID(satellite_info), _report_sat_info_pub, &_report_sat_info); + orb_publish(ORB_ID(satellite_info), _report_sat_info_pub, _p_report_sat_info); } else { - _report_sat_info_pub = orb_advertise(ORB_ID(satellite_info), &_report_sat_info); + _report_sat_info_pub = orb_advertise(ORB_ID(satellite_info), _p_report_sat_info); } } } @@ -466,7 +481,7 @@ GPS::print_info() } warnx("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_healthy) ? "OK" : "NOT OK"); - warnx("sat info: %s", (_enable_sat_info) ? "enabled" : "disabled"); + warnx("sat info: %s", (_p_report_sat_info != nullptr) ? "enabled" : "disabled"); if (_report_gps_pos.timestamp_position != 0) { warnx("position lock: %dD, satellites: %d, last update: %8.4fms ago", (int)_report_gps_pos.fix_type, diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index 0708e23b4..1783ae0f7 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2012, 2013, 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 @@ -34,14 +34,18 @@ /** * @file ubx.cpp * - * U-Blox protocol implementation. Following u-blox 6/7 Receiver Description + * 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 http://www.u-blox.com/images/downloads/Product_Docs/u-blox6_ReceiverDescriptionProtocolSpec_%28GPS.G6-SW-10018%29.pdf + * @see http://www.u-blox.com/images/downloads/Product_Docs/u-bloxM8-V15_ReceiverDescriptionProtocolSpec_Public_%28UBX-13003221%29.pdf */ #include @@ -65,18 +69,28 @@ #define UBX_WAIT_BEFORE_READ 20 // ms, wait before reading to save read() calls #define DISABLE_MSG_INTERVAL 1000000 // us, try to disable message with this interval -#define MIN(X,Y) ((X) < (Y) ? (X) : (Y)) +#define MIN(X,Y) ((X) < (Y) ? (X) : (Y)) +#define SWAP16(X) ((((X) >> 8) & 0x00ff) | (((X) << 8) & 0xff00)) + +/**** Trace macros, disable for production builds */ +#define UBX_TRACE_PARSER(s, ...) {/*printf(s, ## __VA_ARGS__);*/} /* decoding progress in parse_char() */ +#define UBX_TRACE_RXMSG(s, ...) {/*printf(s, ## __VA_ARGS__);*/} /* Rx msgs in payload_rx_done() */ +#define UBX_TRACE_SVINFO(s, ...) {/*printf(s, ## __VA_ARGS__);*/} /* NAV-SVINFO processing (debug use only, will cause rx buffer overflows) */ + +/**** Warning macros, disable to save memory */ +#define UBX_WARN(s, ...) {warnx(s, ## __VA_ARGS__);} -UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position, struct satellite_info_s *satellite_info, const bool &enable_sat_info) : + +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), - _enable_sat_info(enable_sat_info), _configured(false), - _waiting_for_ack(false), + _ack_state(UBX_ACK_IDLE), _got_posllh(false), _got_velned(false), - _disable_cmd_last(0) + _disable_cmd_last(0), + _ack_waiting_msg(0) { decode_init(); } @@ -100,55 +114,41 @@ UBX::configure(unsigned &baudrate) /* flush input and wait for at least 20 ms silence */ decode_init(); - wait_for_ack(20); + 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 from this - */ - type_gps_bin_cfg_prt_packet_t cfg_prt_packet; - /* Set everything else of the packet to 0, otherwise the module wont accept it */ - memset(&cfg_prt_packet, 0, sizeof(cfg_prt_packet)); - - _message_class_needed = UBX_CLASS_CFG; - _message_id_needed = UBX_MESSAGE_CFG_PRT; - - /* Define the package contents, don't change the baudrate */ - cfg_prt_packet.clsID = UBX_CLASS_CFG; - cfg_prt_packet.msgID = UBX_MESSAGE_CFG_PRT; - cfg_prt_packet.length = UBX_CFG_PRT_LENGTH; - cfg_prt_packet.portID = UBX_CFG_PRT_PAYLOAD_PORTID; - cfg_prt_packet.mode = UBX_CFG_PRT_PAYLOAD_MODE; - cfg_prt_packet.baudRate = baudrate; - cfg_prt_packet.inProtoMask = UBX_CFG_PRT_PAYLOAD_INPROTOMASK; - cfg_prt_packet.outProtoMask = UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK; - - send_config_packet(_fd, (uint8_t *)&cfg_prt_packet, sizeof(cfg_prt_packet)); - - if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { + * and leave the baudrate as it is, we just want an ACK-ACK for this */ + memset(&_buf.payload_tx_cfg_prt, 0, sizeof(_buf.payload_tx_cfg_prt)); + _buf.payload_tx_cfg_prt.portID = UBX_TX_CFG_PRT_PORTID; + _buf.payload_tx_cfg_prt.mode = UBX_TX_CFG_PRT_MODE; + _buf.payload_tx_cfg_prt.baudRate = baudrate; + _buf.payload_tx_cfg_prt.inProtoMask = UBX_TX_CFG_PRT_INPROTOMASK; + _buf.payload_tx_cfg_prt.outProtoMask = UBX_TX_CFG_PRT_OUTPROTOMASK; + + send_message(UBX_MSG_CFG_PRT, _buf.raw, sizeof(_buf.payload_tx_cfg_prt)); + + 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(&_buf.payload_tx_cfg_prt, 0, sizeof(_buf.payload_tx_cfg_prt)); + _buf.payload_tx_cfg_prt.portID = UBX_TX_CFG_PRT_PORTID; + _buf.payload_tx_cfg_prt.mode = UBX_TX_CFG_PRT_MODE; + _buf.payload_tx_cfg_prt.baudRate = UBX_TX_CFG_PRT_BAUDRATE; + _buf.payload_tx_cfg_prt.inProtoMask = UBX_TX_CFG_PRT_INPROTOMASK; + _buf.payload_tx_cfg_prt.outProtoMask = UBX_TX_CFG_PRT_OUTPROTOMASK; - cfg_prt_packet.clsID = UBX_CLASS_CFG; - cfg_prt_packet.msgID = UBX_MESSAGE_CFG_PRT; - cfg_prt_packet.length = UBX_CFG_PRT_LENGTH; - cfg_prt_packet.portID = UBX_CFG_PRT_PAYLOAD_PORTID; - cfg_prt_packet.mode = UBX_CFG_PRT_PAYLOAD_MODE; - cfg_prt_packet.baudRate = UBX_CFG_PRT_PAYLOAD_BAUDRATE; - cfg_prt_packet.inProtoMask = UBX_CFG_PRT_PAYLOAD_INPROTOMASK; - cfg_prt_packet.outProtoMask = UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK; - - send_config_packet(_fd, (uint8_t *)&cfg_prt_packet, sizeof(cfg_prt_packet)); + send_message(UBX_MSG_CFG_PRT, _buf.raw, sizeof(_buf.payload_tx_cfg_prt)); /* no ACK is expected here, but read the buffer anyway in case we actually get an ACK */ - wait_for_ack(UBX_CONFIG_TIMEOUT); + wait_for_ack(UBX_MSG_CFG_PRT, UBX_CONFIG_TIMEOUT, false); - if (UBX_CFG_PRT_PAYLOAD_BAUDRATE != baudrate) { - set_baudrate(_fd, UBX_CFG_PRT_PAYLOAD_BAUDRATE); - baudrate = UBX_CFG_PRT_PAYLOAD_BAUDRATE; + 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 */ @@ -156,94 +156,68 @@ UBX::configure(unsigned &baudrate) } if (baud_i >= sizeof(baudrates) / sizeof(baudrates[0])) { - return 1; + return 1; // connection and/or baudrate detection failed } - /* send a CFG-RATE message to define update rate */ - type_gps_bin_cfg_rate_packet_t cfg_rate_packet; - memset(&cfg_rate_packet, 0, sizeof(cfg_rate_packet)); - - _message_class_needed = UBX_CLASS_CFG; - _message_id_needed = UBX_MESSAGE_CFG_RATE; + /* 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; - cfg_rate_packet.clsID = UBX_CLASS_CFG; - cfg_rate_packet.msgID = UBX_MESSAGE_CFG_RATE; - cfg_rate_packet.length = UBX_CFG_RATE_LENGTH; - cfg_rate_packet.measRate = UBX_CFG_RATE_PAYLOAD_MEASINTERVAL; - cfg_rate_packet.navRate = UBX_CFG_RATE_PAYLOAD_NAVRATE; - cfg_rate_packet.timeRef = UBX_CFG_RATE_PAYLOAD_TIMEREF; + send_message(UBX_MSG_CFG_RATE, _buf.raw, sizeof(_buf.payload_tx_cfg_rate)); - send_config_packet(_fd, (uint8_t *)&cfg_rate_packet, sizeof(cfg_rate_packet)); - - if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { - warnx("CFG FAIL: RATE"); + 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 */ - type_gps_bin_cfg_nav5_packet_t cfg_nav5_packet; - memset(&cfg_nav5_packet, 0, sizeof(cfg_nav5_packet)); - - _message_class_needed = UBX_CLASS_CFG; - _message_id_needed = UBX_MESSAGE_CFG_NAV5; - - cfg_nav5_packet.clsID = UBX_CLASS_CFG; - cfg_nav5_packet.msgID = UBX_MESSAGE_CFG_NAV5; - cfg_nav5_packet.length = UBX_CFG_NAV5_LENGTH; - cfg_nav5_packet.mask = UBX_CFG_NAV5_PAYLOAD_MASK; - cfg_nav5_packet.dynModel = UBX_CFG_NAV5_PAYLOAD_DYNMODEL; - cfg_nav5_packet.fixMode = UBX_CFG_NAV5_PAYLOAD_FIXMODE; + 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; - send_config_packet(_fd, (uint8_t *)&cfg_nav5_packet, sizeof(cfg_nav5_packet)); + send_message(UBX_MSG_CFG_NAV5, _buf.raw, sizeof(_buf.payload_tx_cfg_nav5)); - if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { - warnx("CFG FAIL: NAV5"); + if (wait_for_ack(UBX_MSG_CFG_NAV5, UBX_CONFIG_TIMEOUT, true) < 0) { return 1; } /* configure message rates */ /* the last argument is divisor for measurement rate (set by CFG RATE), i.e. 1 means 5Hz */ - configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_POSLLH, 1); + configure_message_rate(UBX_MSG_NAV_POSLLH, 1); - if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { - warnx("MSG CFG FAIL: NAV POSLLH"); + if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) { return 1; } - configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_TIMEUTC, 5); + configure_message_rate(UBX_MSG_NAV_TIMEUTC, 5); - if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { - warnx("MSG CFG FAIL: NAV TIMEUTC"); + if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) { return 1; } - configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SOL, 1); + configure_message_rate(UBX_MSG_NAV_SOL, 1); - if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { - warnx("MSG CFG FAIL: NAV SOL"); + if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) { return 1; } - configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_VELNED, 1); + configure_message_rate(UBX_MSG_NAV_VELNED, 1); - if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { - warnx("MSG CFG FAIL: NAV VELNED"); + if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) { return 1; } - if (_enable_sat_info) { - configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO, 5); + configure_message_rate(UBX_MSG_NAV_SVINFO, (_satellite_info != nullptr) ? 5 : 0); - if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { - warnx("MSG CFG FAIL: NAV SVINFO"); - return 1; - } + if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) { + return 1; } - configure_message_rate(UBX_CLASS_MON, UBX_MESSAGE_MON_HW, 1); + configure_message_rate(UBX_MSG_MON_HW, 1); - if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { - warnx("MSG CFG FAIL: MON HW"); + if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) { return 1; } @@ -251,27 +225,36 @@ UBX::configure(unsigned &baudrate) return 0; } -int -UBX::wait_for_ack(unsigned timeout) +int // -1 = NAK, error or timeout, 0 = ACK +UBX::wait_for_ack(const uint16_t msg, const unsigned timeout, const bool report) { - _waiting_for_ack = true; - uint64_t time_end = hrt_absolute_time() + timeout * 1000; + int ret = -1; - while ((timeout = (time_end - hrt_absolute_time()) / 1000) > 0) { - if (receive(timeout) > 0) { - if (!_waiting_for_ack) { - return 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_WARN("ubx msg 0x%04x NAK", SWAP16((unsigned)msg)); } else { - return -1; // timeout or error receiving, or NAK + UBX_WARN("ubx msg 0x%04x ACK timeout", SWAP16((unsigned)msg)); } } - return -1; // timeout + _ack_state = UBX_ACK_IDLE; + return ret; } -int -UBX::receive(unsigned timeout) +int // -1 = error, 0 = no message handled, 1 = message handled, 2 = sat info message handled +UBX::receive(const unsigned timeout) { /* poll descriptor */ pollfd fds[1]; @@ -295,7 +278,7 @@ UBX::receive(unsigned timeout) if (ret < 0) { /* something went wrong when polling */ - warnx("poll error"); + UBX_WARN("ubx poll() err"); return -1; } else if (ret == 0) { @@ -323,433 +306,517 @@ UBX::receive(unsigned timeout) /* pass received bytes to the packet decoder */ for (int i = 0; i < count; i++) { - if (parse_char(buf[i]) > 0) { - handled |= handle_message(); - } + handled |= parse_char(buf[i]); } } } /* abort after timeout if no useful packets received */ if (time_started + timeout * 1000 < hrt_absolute_time()) { - warnx("timeout - no useful messages"); return -1; } } } -int -UBX::parse_char(uint8_t b) +int // 0 = decoding, 1 = message handled, 2 = sat info message handled +UBX::parse_char(const uint8_t b) { + int ret = 0; + switch (_decode_state) { - /* First, look for sync1 */ - case UBX_DECODE_UNINIT: - if (b == UBX_SYNC1) { - _decode_state = UBX_DECODE_GOT_SYNC1; - //### printf("\nA"); - } + /* Expecting Sync1 */ + case UBX_DECODE_SYNC1: + if (b == UBX_SYNC1) { // Sync1 found --> expecting Sync2 + UBX_TRACE_PARSER("\nA"); + _decode_state = UBX_DECODE_SYNC2; + } break; - /* Second, look for sync2 */ - case UBX_DECODE_GOT_SYNC1: - if (b == UBX_SYNC2) { - _decode_state = UBX_DECODE_GOT_SYNC2; - //### printf("B"); + /* Expecting Sync2 */ + case UBX_DECODE_SYNC2: + if (b == UBX_SYNC2) { // Sync2 found --> expecting Class + UBX_TRACE_PARSER("B"); + _decode_state = UBX_DECODE_CLASS; - } else { - /* Second start symbol was wrong, reset state machine */ + } else { // Sync1 not followed by Sync2: reset parser decode_init(); - /* don't return error, it can be just false sync1 */ - //### printf("-"); } + 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; - /* Now look for class */ - case UBX_DECODE_GOT_SYNC2: - /* everything except sync1 and sync2 needs to be added to the checksum */ + /* Expecting ID */ + case UBX_DECODE_ID: + UBX_TRACE_PARSER("D"); add_byte_to_checksum(b); - _message_class = b; - _decode_state = UBX_DECODE_GOT_CLASS; - //### printf("C"); + _rx_msg |= b << 8; + _decode_state = UBX_DECODE_LENGTH1; break; - case UBX_DECODE_GOT_CLASS: + /* Expecting first length byte */ + case UBX_DECODE_LENGTH1: + UBX_TRACE_PARSER("E"); add_byte_to_checksum(b); - _message_id = b; - _decode_state = UBX_DECODE_GOT_MESSAGEID; - //### printf("D"); + _rx_payload_length = b; + _decode_state = UBX_DECODE_LENGTH2; break; - case UBX_DECODE_GOT_MESSAGEID: + /* Expecting second length byte */ + case UBX_DECODE_LENGTH2: + UBX_TRACE_PARSER("F"); add_byte_to_checksum(b); - _payload_size = b; //this is the first length byte - _decode_state = UBX_DECODE_GOT_LENGTH1; - //### printf("E"); + _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; - case UBX_DECODE_GOT_LENGTH1: + /* Expecting payload */ + case UBX_DECODE_PAYLOAD: + UBX_TRACE_PARSER("."); add_byte_to_checksum(b); - _payload_size += b << 8; // here comes the second byte of length - _decode_state = UBX_DECODE_GOT_LENGTH2; - //### printf("F"); + if (_rx_msg != UBX_MSG_NAV_SVINFO) { + ret = payload_rx_add(b); // add a payload byte + } else { + ret = payload_rx_add_svinfo(b); // add a svinfo payload byte + } + 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; - case UBX_DECODE_GOT_LENGTH2: + default: + break; + } - /* Add to checksum if not yet at checksum byte */ - if (_rx_count < _payload_size) - add_byte_to_checksum(b); + return ret; +} - _rx_buffer[_rx_count] = b; +/** + * Start payload rx + */ +int // -1 = abort, 0 = continue +UBX::payload_rx_init() +{ + int ret = 0; - /* once the payload has arrived, we can process the information */ - if (_rx_count >= _payload_size + 1) { //+1 because of 2 checksum bytes - /* compare checksum */ - if (_rx_ck_a == _rx_buffer[_rx_count - 1] && _rx_ck_b == _rx_buffer[_rx_count]) { - //### printf("O\n"); - return 1; // message received successfully + _rx_state = UBX_RXMSG_HANDLE; // handle by default - } else { - warnx("checksum wrong"); - decode_init(); - return -1; - } + switch (_rx_msg) { + 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 + break; - } else if (_rx_count < RECV_BUFFER_SIZE) { - _rx_count++; - //### printf("."); + 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 + break; - } else { - warnx("ubx: buffer full 0x%02x-0x%02x L%u", (unsigned)_message_class, (unsigned)_message_id, _payload_size); - decode_init(); - return -1; - } + 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 + 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 + break; + + 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; } - return 0; // message decoding in progress + 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_WARN("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_WARN("ubx disabling msg 0x%04x", SWAP16((unsigned)_rx_msg)); + configure_message_rate(_rx_msg, 0); + } + } + + 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; + + _buf.raw[_rx_payload_index] = b; + if (++_rx_payload_index >= _rx_payload_length) { + ret = 1; // payload received completely + } -int -UBX::handle_message() + return ret; +} + +/** + * Add svinfo payload rx byte + */ +int // -1 = error, 0 = ok, 1 = payload completed +UBX::payload_rx_add_svinfo(const uint8_t b) { int ret = 0; - if (_configured) { - /* handle only info messages when configured */ - switch (_message_class) { - case UBX_CLASS_NAV: - switch (_message_id) { - case UBX_MESSAGE_NAV_POSLLH: { - // printf("GOT NAV_POSLLH\n"); - gps_bin_nav_posllh_packet_t *packet = (gps_bin_nav_posllh_packet_t *) _rx_buffer; - - _gps_position->lat = packet->lat; - _gps_position->lon = packet->lon; - _gps_position->alt = packet->height_msl; - _gps_position->eph_m = (float)packet->hAcc * 1e-3f; // from mm to m - _gps_position->epv_m = (float)packet->vAcc * 1e-3f; // from mm to m - _gps_position->timestamp_position = hrt_absolute_time(); - - _rate_count_lat_lon++; - - _got_posllh = true; - ret = 1; - break; - } + if (_rx_payload_index < sizeof(ubx_payload_rx_nav_svinfo_part1_t)) { + // Fill Part 1 buffer + _buf.raw[_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, SAT_INFO_MAX_SATELLITES); + UBX_TRACE_SVINFO("SVINFO len %u numCh %u\n", (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); + _buf.raw[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\n", + (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] + ); + } + } + } - case UBX_MESSAGE_NAV_SOL: { - // printf("GOT NAV_SOL\n"); - gps_bin_nav_sol_packet_t *packet = (gps_bin_nav_sol_packet_t *) _rx_buffer; + if (++_rx_payload_index >= _rx_payload_length) { + ret = 1; // payload received completely + } - _gps_position->fix_type = packet->gpsFix; - _gps_position->s_variance_m_s = packet->sAcc; - _gps_position->p_variance_m = packet->pAcc; - _gps_position->timestamp_variance = hrt_absolute_time(); - _gps_position->satellites_used = packet->numSV; + return ret; +} - ret = 1; - break; - } +/** + * 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_POSLLH: + UBX_TRACE_RXMSG("Rx NAV-POSLLH\n"); + + _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_m = (float)_buf.payload_rx_nav_posllh.hAcc * 1e-3f; // from mm to m + _gps_position->epv_m = (float)_buf.payload_rx_nav_posllh.vAcc * 1e-3f; // from mm to m + + _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\n"); + + _gps_position->fix_type = _buf.payload_rx_nav_sol.gpsFix; + _gps_position->s_variance_m_s = _buf.payload_rx_nav_sol.sAcc; + _gps_position->p_variance_m = _buf.payload_rx_nav_sol.pAcc; + _gps_position->satellites_used = _buf.payload_rx_nav_sol.numSV; + + _gps_position->timestamp_variance = hrt_absolute_time(); + + ret = 1; + break; - case UBX_MESSAGE_NAV_TIMEUTC: { - // printf("GOT NAV_TIMEUTC\n"); - gps_bin_nav_timeutc_packet_t *packet = (gps_bin_nav_timeutc_packet_t *) _rx_buffer; + case UBX_MSG_NAV_TIMEUTC: + UBX_TRACE_RXMSG("Rx NAV-TIMEUTC\n"); - /* convert to unix timestamp */ - struct tm timeinfo; - timeinfo.tm_year = packet->year - 1900; - timeinfo.tm_mon = packet->month - 1; - timeinfo.tm_mday = packet->day; - timeinfo.tm_hour = packet->hour; - timeinfo.tm_min = packet->min; - timeinfo.tm_sec = packet->sec; - time_t epoch = mktime(&timeinfo); + { + /* 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; + time_t epoch = mktime(&timeinfo); #ifndef CONFIG_RTC - //Since we lack a hardware RTC, set the system time clock based on GPS UTC - //TODO generalize this by moving into gps.cpp? - timespec ts; - ts.tv_sec = epoch; - ts.tv_nsec = packet->time_nanoseconds; - clock_settime(CLOCK_REALTIME, &ts); + //Since we lack a hardware RTC, set the system time clock based on GPS UTC + //TODO generalize this by moving into gps.cpp? + timespec ts; + ts.tv_sec = epoch; + ts.tv_nsec = _buf.payload_rx_nav_timeutc.nano; + clock_settime(CLOCK_REALTIME, &ts); #endif - _gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this - _gps_position->time_gps_usec += (uint64_t)(packet->time_nanoseconds * 1e-3f); - _gps_position->timestamp_time = hrt_absolute_time(); + _gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this + _gps_position->time_gps_usec += (uint64_t)(_buf.payload_rx_nav_timeutc.nano * 1e-3f); + } - ret = 1; - break; - } + _gps_position->timestamp_time = hrt_absolute_time(); - case UBX_MESSAGE_NAV_SVINFO: { - //printf("GOT NAV_SVINFO\n"); - if (!_enable_sat_info) { // if satellite info processing not enabled: - break; // ignore and disable NAV-SVINFO message - } - const int length_part1 = 8; - gps_bin_nav_svinfo_part1_packet_t *packet_part1 = (gps_bin_nav_svinfo_part1_packet_t *) _rx_buffer; - const int length_part2 = 12; - gps_bin_nav_svinfo_part2_packet_t *packet_part2; - - //printf("Number of Channels: %d\n", packet_part1->numCh); - for (_satellite_info->count = 0; - _satellite_info->count < MIN(packet_part1->numCh, sizeof(_satellite_info->snr) / sizeof(_satellite_info->snr[0])); - _satellite_info->count++) { - /* set pointer to satellite_i information */ - packet_part2 = (gps_bin_nav_svinfo_part2_packet_t *) & (_rx_buffer[length_part1 + _satellite_info->count* length_part2]); - - /* write satellite information to global storage */ - _satellite_info->used[_satellite_info->count] = packet_part2->flags & 0x01; - _satellite_info->snr[_satellite_info->count] = packet_part2->cno; - _satellite_info->elevation[_satellite_info->count] = (uint8_t)(packet_part2->elev); - _satellite_info->azimuth[_satellite_info->count] = (uint8_t)((float)packet_part2->azim * 255.0f / 360.0f); - _satellite_info->svid[_satellite_info->count] = packet_part2->svid; - //printf("SAT %d: %d %d %d %d\n", _satellite_info->count, (int)sv_used, (int)packet_part2->cno, (int)(uint8_t)(packet_part2->elev), (int)packet_part2->svid); - } - - _satellite_info->timestamp = hrt_absolute_time(); - - ret = 2; - break; - } + ret = 1; + break; - case UBX_MESSAGE_NAV_VELNED: { - // printf("GOT NAV_VELNED\n"); - gps_bin_nav_velned_packet_t *packet = (gps_bin_nav_velned_packet_t *) _rx_buffer; + case UBX_MSG_NAV_SVINFO: + UBX_TRACE_RXMSG("Rx NAV-SVINFO\n"); - _gps_position->vel_m_s = (float)packet->speed * 1e-2f; - _gps_position->vel_n_m_s = (float)packet->velN * 1e-2f; /* NED NORTH velocity */ - _gps_position->vel_e_m_s = (float)packet->velE * 1e-2f; /* NED EAST velocity */ - _gps_position->vel_d_m_s = (float)packet->velD * 1e-2f; /* NED DOWN velocity */ - _gps_position->cog_rad = (float)packet->heading * M_DEG_TO_RAD_F * 1e-5f; - _gps_position->c_variance_rad = (float)packet->cAcc * M_DEG_TO_RAD_F * 1e-5f; - _gps_position->vel_ned_valid = true; - _gps_position->timestamp_velocity = hrt_absolute_time(); + // _satellite_info already populated by payload_rx_add_svinfo(), just add a timestamp + _satellite_info->timestamp = hrt_absolute_time(); - _rate_count_vel++; + ret = 2; + break; - _got_velned = true; - ret = 1; - break; - } + case UBX_MSG_NAV_VELNED: + UBX_TRACE_RXMSG("Rx NAV-VELNED\n"); - default: - break; - } + _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; - break; + _gps_position->timestamp_velocity = hrt_absolute_time(); - case UBX_CLASS_ACK: { - /* ignore ACK when already configured */ - ret = 1; - break; - } + _rate_count_vel++; + _got_velned = true; - case UBX_CLASS_MON: - switch (_message_id) { + ret = 1; + break; - case UBX_MESSAGE_MON_HW: - switch (_payload_size) { + case UBX_MSG_MON_HW: + UBX_TRACE_RXMSG("Rx MON-HW\n"); - case UBX_MON_HW_UBX6_RX_PAYLOAD_SIZE: /* u-blox 6 msg format */ - _gps_position->noise_per_ms = ((struct gps_bin_mon_hw_ubx6_packet*) _rx_buffer)->noisePerMS; - _gps_position->jamming_indicator = ((struct gps_bin_mon_hw_ubx6_packet*) _rx_buffer)->jamInd; - ret = 1; - break; + switch (_rx_payload_length) { - case UBX_MON_HW_UBX7_RX_PAYLOAD_SIZE: /* u-blox 7+ msg format */ - _gps_position->noise_per_ms = ((struct gps_bin_mon_hw_ubx7_packet*) _rx_buffer)->noisePerMS; - _gps_position->jamming_indicator = ((struct gps_bin_mon_hw_ubx7_packet*) _rx_buffer)->jamInd; - ret = 1; - break; + 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; - default: // unexpected payload size: - ret = 1; // ignore but don't disable msg - break; - } - break; + ret = 1; + break; - default: - 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: + default: // unexpected payload size: + ret = 0; // don't handle message break; } + break; - if (ret == 0) { - /* message not handled */ - warnx("ubx: message not handled: 0x%02x-0x%02x L%u", (unsigned)_message_class, (unsigned)_message_id, _payload_size); - - hrt_abstime t = hrt_absolute_time(); + case UBX_MSG_ACK_ACK: + UBX_TRACE_RXMSG("Rx ACK-ACK\n"); - 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; - warnx("ubx: disabling message 0x%02x-0x%02x", (unsigned)_message_class, (unsigned)_message_id); - configure_message_rate(_message_class, _message_id, 0); - } + if ((_ack_state == UBX_ACK_WAITING) && (_buf.payload_rx_ack_ack.msg == _ack_waiting_msg)) { + _ack_state = UBX_ACK_GOT_ACK; } - } else { - /* handle only ACK while configuring */ - if (_message_class == UBX_CLASS_ACK) { - switch (_message_id) { - case UBX_MESSAGE_ACK_ACK: { - // printf("GOT ACK_ACK\n"); - gps_bin_ack_ack_packet_t *packet = (gps_bin_ack_ack_packet_t *) _rx_buffer; - - if (_waiting_for_ack) { - if (packet->clsID == _message_class_needed && packet->msgID == _message_id_needed) { - _waiting_for_ack = false; - ret = 1; - } - } - - break; - } + ret = 1; + break; - case UBX_MESSAGE_ACK_NAK: { - // printf("GOT ACK_NAK\n"); - warnx("ubx: not acknowledged"); - /* configuration obviously not successful */ - _waiting_for_ack = false; - break; - } + case UBX_MSG_ACK_NAK: + UBX_TRACE_RXMSG("Rx ACK-NAK\n"); - default: - break; - } + 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; } - decode_init(); return ret; } void UBX::decode_init(void) { + _decode_state = UBX_DECODE_SYNC1; _rx_ck_a = 0; _rx_ck_b = 0; - _rx_count = 0; - _decode_state = UBX_DECODE_UNINIT; - _payload_size = 0; - /* don't reset _message_class, _message_id, _rx_buffer leave it for message handler */ + _rx_payload_length = 0; + _rx_payload_index = 0; } void -UBX::add_byte_to_checksum(uint8_t b) +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::add_checksum_to_message(uint8_t *message, const unsigned length) +UBX::calc_checksum(const uint8_t *buffer, const uint16_t length, ubx_checksum_t *checksum) { - uint8_t ck_a = 0; - uint8_t ck_b = 0; - unsigned i; - - for (i = 0; i < length - 2; i++) { - ck_a = ck_a + message[i]; - ck_b = ck_b + ck_a; + 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; } - - /* the checksum is written to the last to bytes of a message */ - message[length - 2] = ck_a; - message[length - 1] = ck_b; } void -UBX::add_checksum(uint8_t *message, const unsigned length, uint8_t &ck_a, uint8_t &ck_b) +UBX::configure_message_rate(const uint16_t msg, const uint8_t rate) { - for (unsigned i = 0; i < length; i++) { - ck_a = ck_a + message[i]; - ck_b = ck_b + ck_a; - } -} + ubx_payload_tx_cfg_msg_t cfg_msg; // don't use _buf (allow interleaved operation) -void -UBX::configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate) -{ - struct ubx_cfg_msg_rate msg; - msg.msg_class = msg_class; - msg.msg_id = msg_id; - msg.rate = rate; - send_message(UBX_CLASS_CFG, UBX_MESSAGE_CFG_MSG, &msg, sizeof(msg)); + cfg_msg.msg = msg; + cfg_msg.rate = rate; + + send_message(UBX_MSG_CFG_MSG, (uint8_t *)&cfg_msg, sizeof(cfg_msg)); } void -UBX::send_config_packet(const int &fd, uint8_t *packet, const unsigned length) +UBX::send_message(const uint16_t msg, const uint8_t *payload, const uint16_t length) { - ssize_t ret = 0; + ubx_header_t header = {UBX_SYNC1, UBX_SYNC2}; + ubx_checksum_t checksum = {0, 0}; - /* calculate the checksum now */ - add_checksum_to_message(packet, length); + // Populate header + header.msg = msg; + header.length = length; - const uint8_t sync_bytes[] = {UBX_SYNC1, UBX_SYNC2}; + // Calculate checksum + calc_checksum(((uint8_t*)&header) + 2, sizeof(header) - 2, &checksum); // skip 2 sync bytes + calc_checksum(payload, length, &checksum); - /* start with the two sync bytes */ - ret += write(fd, sync_bytes, sizeof(sync_bytes)); - ret += write(fd, packet, length); - - if (ret != (int)length + (int)sizeof(sync_bytes)) // XXX is there a neater way to get rid of the unsigned signed warning? - warnx("ubx: configuration write fail"); -} - -void -UBX::send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint8_t size) -{ - struct ubx_header header; - uint8_t ck_a = 0, ck_b = 0; - header.sync1 = UBX_SYNC1; - header.sync2 = UBX_SYNC2; - header.msg_class = msg_class; - header.msg_id = msg_id; - header.length = size; - - add_checksum((uint8_t *)&header.msg_class, sizeof(header) - 2, ck_a, ck_b); - add_checksum((uint8_t *)msg, size, ck_a, ck_b); - - /* configure ACK check */ - _message_class_needed = msg_class; - _message_id_needed = msg_id; - - write(_fd, (const char *)&header, sizeof(header)); - write(_fd, (const char *)msg, size); - write(_fd, (const char *)&ck_a, 1); - write(_fd, (const char *)&ck_b, 1); + // Send message + write(_fd, (const void *)&header, sizeof(header)); + write(_fd, (const void *)payload, length); + write(_fd, (const void *)&checksum, sizeof(checksum)); } diff --git a/src/drivers/gps/ubx.h b/src/drivers/gps/ubx.h index 50df3ada2..f084fc655 100644 --- a/src/drivers/gps/ubx.h +++ b/src/drivers/gps/ubx.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2012, 2013, 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 @@ -41,6 +41,9 @@ * @author Julian Oes * @author Anton Babushkin * + * @author Hannes Delago + * (rework, add ubx7+ compatibility) + * */ #ifndef UBX_H_ @@ -48,346 +51,341 @@ #include "gps_helper.h" -#define UBX_ENABLE_NAV_SVINFO 1 /* TODO: make this a command line option, allocate buffer(s) dynamically */ - #define UBX_SYNC1 0xB5 #define UBX_SYNC2 0x62 -/* ClassIDs (the ones that are used) */ -#define UBX_CLASS_NAV 0x01 -//#define UBX_CLASS_RXM 0x02 -#define UBX_CLASS_ACK 0x05 -#define UBX_CLASS_CFG 0x06 -#define UBX_CLASS_MON 0x0A - -/* MessageIDs (the ones that are used) */ -#define UBX_MESSAGE_NAV_POSLLH 0x02 -#define UBX_MESSAGE_NAV_SOL 0x06 -#define UBX_MESSAGE_NAV_VELNED 0x12 -#define UBX_MESSAGE_NAV_TIMEUTC 0x21 -#define UBX_MESSAGE_NAV_SVINFO 0x30 -#define UBX_MESSAGE_ACK_NAK 0x00 -#define UBX_MESSAGE_ACK_ACK 0x01 -#define UBX_MESSAGE_CFG_PRT 0x00 -#define UBX_MESSAGE_CFG_MSG 0x01 -#define UBX_MESSAGE_CFG_RATE 0x08 -#define UBX_MESSAGE_CFG_NAV5 0x24 -#define UBX_MESSAGE_MON_HW 0x09 - -/* Rx msg payload sizes */ -#define UBX_NAV_POSLLH_RX_PAYLOAD_SIZE 28 /**< NAV_POSLLH Rx msg payload size */ -#define UBX_NAV_SOL_RX_PAYLOAD_SIZE 52 /**< NAV_SOL Rx msg payload size */ -#define UBX_NAV_VELNED_RX_PAYLOAD_SIZE 36 /**< NAV_VELNED Rx msg payload size */ -#define UBX_NAV_TIMEUTC_RX_PAYLOAD_SIZE 20 /**< NAV_TIMEUTC Rx msg payload size */ -#define UBX_MON_HW_UBX6_RX_PAYLOAD_SIZE 68 /**< MON_HW Rx msg payload size for u-blox 6 and below */ -#define UBX_MON_HW_UBX7_RX_PAYLOAD_SIZE 60 /**< MON_HW Rx msg payload size for u-blox 7 and above */ -#define UBX_MAX_RX_PAYLOAD_SIZE 70 /**< arbitrary maximum for calculating parser buffer size w/o NAV_SVINFO active */ +/* 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_SOL 0x06 +#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_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_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_MON_HW ((UBX_CLASS_MON) | UBX_ID_MON_HW << 8) /* NAV_SVINFO has variable length w/o a published max size, so limit processing to UBX_MAX_NUM_SAT */ -#define UBX_MAX_NUM_SAT 50 /**< Practical observed max number of satellites in SVNFO msg */ -#define UBX_NAV_SVINFO_RX_PAYLOAD_SIZE (8 + 12 * UBX_MAX_NUM_SAT) /**< NAV_SVINFO Rx msg payload size */ - -/* CFG class Tx msg defs */ -#define UBX_CFG_PRT_LENGTH 20 -#define UBX_CFG_PRT_PAYLOAD_PORTID 0x01 /**< UART1 */ -#define UBX_CFG_PRT_PAYLOAD_MODE 0x000008D0 /**< 0b0000100011010000: 8N1 */ -#define UBX_CFG_PRT_PAYLOAD_BAUDRATE 38400 /**< choose 38400 as GPS baudrate */ -#define UBX_CFG_PRT_PAYLOAD_INPROTOMASK 0x01 /**< UBX in */ -#define UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK 0x01 /**< UBX out */ - -#define UBX_CFG_RATE_LENGTH 6 -#define UBX_CFG_RATE_PAYLOAD_MEASINTERVAL 200 /**< 200ms for 5Hz */ -#define UBX_CFG_RATE_PAYLOAD_NAVRATE 1 /**< cannot be changed */ -#define UBX_CFG_RATE_PAYLOAD_TIMEREF 0 /**< 0: UTC, 1: GPS time */ - -#define UBX_CFG_NAV5_LENGTH 36 -#define UBX_CFG_NAV5_PAYLOAD_MASK 0x0005 /**< XXX only update dynamic model and fix mode */ -#define UBX_CFG_NAV5_PAYLOAD_DYNMODEL 7 /**< 0: portable, 2: stationary, 3: pedestrian, 4: automotive, 5: sea, 6: airborne <1g, 7: airborne <2g, 8: airborne <4g */ -#define UBX_CFG_NAV5_PAYLOAD_FIXMODE 2 /**< 1: 2D only, 2: 3D only, 3: Auto 2D/3D */ - -#define UBX_CFG_MSG_LENGTH 8 -#define UBX_CFG_MSG_PAYLOAD_RATE1_5HZ 0x01 /**< {0x00, 0x01, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */ -#define UBX_CFG_MSG_PAYLOAD_RATE1_1HZ 0x05 /**< {0x00, 0x05, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */ -#define UBX_CFG_MSG_PAYLOAD_RATE1_05HZ 10 - - -// ************ -/** the structures of the binary packets */ -#pragma pack(push, 1) +#define UBX_MAX_NUM_SAT_SVINFO 50 /**< Practical observed max number of satellites in SVNFO msg */ +#define UBX_PAYLOAD_NAV_SVINFO (8 + 12 * UBX_MAX_NUM_SAT_SVINFO) /**< NAV_SVINFO Rx msg payload size */ -struct ubx_header { - uint8_t sync1; - uint8_t sync2; - uint8_t msg_class; - uint8_t msg_id; - uint16_t length; -}; +/* TX CFG-PRT message contents */ +#define UBX_TX_CFG_PRT_PORTID 0x01 /**< UART1 */ +#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 */ -typedef struct { - uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */ - int32_t lon; /**< Longitude * 1e-7, deg */ - int32_t lat; /**< Latitude * 1e-7, deg */ - int32_t height; /**< Height above Ellipsoid, mm */ - int32_t height_msl; /**< Height above mean sea level, mm */ - uint32_t hAcc; /**< Horizontal Accuracy Estimate, mm */ - uint32_t vAcc; /**< Vertical Accuracy Estimate, mm */ - uint8_t ck_a; - uint8_t ck_b; -} gps_bin_nav_posllh_packet_t; +/* 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 */ -typedef struct { - uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */ - int32_t time_nanoseconds; /**< Fractional Nanoseconds remainder of rounded ms above, range -500000 .. 500000 */ - int16_t week; /**< GPS week (GPS time) */ - uint8_t gpsFix; /**< GPS Fix: 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; - uint8_t reserved1; - uint8_t numSV; /**< Number of SVs used in Nav Solution */ - uint32_t reserved2; - uint8_t ck_a; - uint8_t ck_b; -} gps_bin_nav_sol_packet_t; +/* 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 */ -typedef struct { - uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */ - uint32_t time_accuracy; /**< Time Accuracy Estimate, ns */ - int32_t time_nanoseconds; /**< Nanoseconds of second, range -1e9 .. 1e9 (UTC) */ - 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..59 (UTC) */ - uint8_t valid_flag; /**< Validity Flags (see ubx documentation) */ - uint8_t ck_a; - uint8_t ck_b; -} gps_bin_nav_timeutc_packet_t; - -//typedef struct { -// uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */ -// uint16_t gDOP; /**< Geometric DOP (scaling 0.01) */ -// uint16_t pDOP; /**< Position DOP (scaling 0.01) */ -// uint16_t tDOP; /**< Time DOP (scaling 0.01) */ -// uint16_t vDOP; /**< Vertical DOP (scaling 0.01) */ -// uint16_t hDOP; /**< Horizontal DOP (scaling 0.01) */ -// uint16_t nDOP; /**< Northing DOP (scaling 0.01) */ -// uint16_t eDOP; /**< Easting DOP (scaling 0.01) */ -// uint8_t ck_a; -// uint8_t ck_b; -//} gps_bin_nav_dop_packet_t; +/* 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 -typedef struct { - uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */ - uint8_t numCh; /**< Number of channels */ - uint8_t globalFlags; - uint16_t reserved2; -} gps_bin_nav_svinfo_part1_packet_t; +/*** u-blox protocol binary message and payload definitions ***/ +#pragma pack(push, 1) +/* General: Header */ 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 in integer degrees */ - int16_t azim; /**< Azimuth in integer degrees */ - int32_t prRes; /**< Pseudo range residual in centimetres */ - -} gps_bin_nav_svinfo_part2_packet_t; + 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; -} gps_bin_nav_svinfo_part3_packet_t; + uint8_t ck_a; + uint8_t ck_b; +} ubx_checksum_t ; +/* Rx NAV-POSLLH */ typedef struct { - uint32_t time_milliseconds; // GPS Millisecond Time of Week - int32_t velN; //NED north velocity, cm/s - int32_t velE; //NED east velocity, cm/s - int32_t velD; //NED down velocity, 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, deg, scaling: 1e-5 - uint32_t sAcc; //Speed Accuracy Estimate, cm/s - uint32_t cAcc; //Course / Heading Accuracy Estimate, scaling: 1e-5 - uint8_t ck_a; - uint8_t ck_b; -} gps_bin_nav_velned_packet_t; - -struct gps_bin_mon_hw_ubx6_packet { - 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; -}; - -struct gps_bin_mon_hw_ubx7_packet { - 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; -}; - -//typedef struct { -// int32_t time_milliseconds; /**< Measurement integer millisecond GPS time of week */ -// int16_t week; /**< Measurement GPS week number */ -// uint8_t numVis; /**< Number of visible satellites */ -// -// //... rest of package is not used in this implementation -// -//} gps_bin_rxm_svsi_packet_t; - + 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-SOL */ typedef struct { - uint8_t clsID; - uint8_t msgID; - uint8_t ck_a; - uint8_t ck_b; -} gps_bin_ack_ack_packet_t; - + 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; + uint8_t reserved1; + uint8_t numSV; /**< Number of SVs used in Nav Solution */ + uint32_t reserved2; +} ubx_payload_rx_nav_sol_t; + +/* Rx NAV-TIMEUTC */ typedef struct { - uint8_t clsID; - uint8_t msgID; - uint8_t ck_a; - uint8_t ck_b; -} gps_bin_ack_nak_packet_t; - + 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 documentation) */ +} ubx_payload_rx_nav_timeutc_t; + +/* Rx NAV-SVINFO Part 1 */ typedef struct { - uint8_t clsID; - uint8_t msgID; - uint16_t length; - uint8_t portID; - uint8_t res0; - uint16_t res1; - uint32_t mode; - uint32_t baudRate; - uint16_t inProtoMask; - uint16_t outProtoMask; - uint16_t flags; - uint16_t pad; - uint8_t ck_a; - uint8_t ck_b; -} type_gps_bin_cfg_prt_packet_t; + 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 clsID; - uint8_t msgID; - uint16_t length; - uint16_t measRate; - uint16_t navRate; - uint16_t timeRef; - uint8_t ck_a; - uint8_t ck_b; -} type_gps_bin_cfg_rate_packet_t; - + 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 { - uint8_t clsID; - uint8_t msgID; - uint16_t length; - uint16_t mask; - uint8_t dynModel; - uint8_t fixMode; - 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; - uint32_t reserved2; - uint32_t reserved3; - uint32_t reserved4; - uint8_t ck_a; - uint8_t ck_b; -} type_gps_bin_cfg_nav5_packet_t; - + 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 { - uint8_t clsID; - uint8_t msgID; - uint16_t length; - uint8_t msgClass_payload; - uint8_t msgID_payload; - uint8_t rate; - uint8_t ck_a; - uint8_t ck_b; -} type_gps_bin_cfg_msg_packet_t; + 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 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; -struct ubx_cfg_msg_rate { - uint8_t msg_class; - uint8_t msg_id; +/* 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-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_posllh_t payload_rx_nav_posllh; + ubx_payload_rx_nav_sol_t payload_rx_nav_sol; + 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_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_msg_t payload_tx_cfg_msg; + uint8_t raw[]; +} ubx_buf_t; +#pragma pack(pop) +/*** END OF u-blox protocol binary message and payload definitions ***/ -// END the structures of the binary packets -// ************ - +/* Decoder state */ typedef enum { - UBX_DECODE_UNINIT = 0, - UBX_DECODE_GOT_SYNC1, - UBX_DECODE_GOT_SYNC2, - UBX_DECODE_GOT_CLASS, - UBX_DECODE_GOT_MESSAGEID, - UBX_DECODE_GOT_LENGTH1, - UBX_DECODE_GOT_LENGTH2 + 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; -//typedef type_gps_bin_ubx_state gps_bin_ubx_state_t; -#pragma pack(pop) +/* 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; -/* calculate parser rx buffer size dependent on NAV_SVINFO enabled or not */ -/* TODO: make this a command line option, allocate buffer dynamically */ -#define RECV_BUFFER_OVERHEAD 10 // add this to the maximum Rx msg payload size to account for msg overhead */ -#if (UBX_ENABLE_NAV_SVINFO != 0) - #define RECV_BUFFER_SIZE (UBX_NAV_SVINFO_RX_PAYLOAD_SIZE + RECV_BUFFER_OVERHEAD) -#else - #define RECV_BUFFER_SIZE (UBX_MAX_RX_PAYLOAD_SIZE + RECV_BUFFER_OVERHEAD) -#endif class UBX : public GPS_Helper { public: - UBX(const int &fd, struct vehicle_gps_position_s *gps_position, struct satellite_info_s *satellite_info, const bool &enable_sat_info); + UBX(const int &fd, struct vehicle_gps_position_s *gps_position, struct satellite_info_s *satellite_info); ~UBX(); - int receive(unsigned timeout); + int receive(const unsigned timeout); int configure(unsigned &baudrate); private: @@ -395,12 +393,23 @@ private: /** * Parse the binary UBX packet */ - int parse_char(uint8_t b); + 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_svinfo(const uint8_t b); /** - * Handle the package once it has arrived + * Finish payload rx */ - int handle_message(void); + int payload_rx_done(void); /** * Reset the parse state machine for a fresh start @@ -410,45 +419,46 @@ private: /** * While parsing add every byte (except the sync bytes) to the checksum */ - void add_byte_to_checksum(uint8_t); + void add_byte_to_checksum(const uint8_t); /** - * Add the two checksum bytes to an outgoing message + * Send a message */ - void add_checksum_to_message(uint8_t *message, const unsigned length); + void send_message(const uint16_t msg, const uint8_t *payload, const uint16_t length); /** - * Helper to send a config packet + * Configure message rate */ - void send_config_packet(const int &fd, uint8_t *packet, const unsigned length); - - void configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate); + void configure_message_rate(const uint16_t msg, const uint8_t rate); - void send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint8_t size); - - void add_checksum(uint8_t *message, const unsigned length, uint8_t &ck_a, uint8_t &ck_b); + /** + * Calculate & add checksum for given buffer + */ + void calc_checksum(const uint8_t *buffer, const uint16_t length, ubx_checksum_t *checksum); - int wait_for_ack(unsigned timeout); + /** + * Wait for message acknowledge + */ + int wait_for_ack(const uint16_t msg, const unsigned timeout, const bool report); int _fd; struct vehicle_gps_position_s *_gps_position; struct satellite_info_s *_satellite_info; bool _enable_sat_info; bool _configured; - bool _waiting_for_ack; + ubx_ack_state_t _ack_state; bool _got_posllh; bool _got_velned; - uint8_t _message_class_needed; - uint8_t _message_id_needed; ubx_decode_state_t _decode_state; - uint8_t _rx_buffer[RECV_BUFFER_SIZE]; - unsigned _rx_count; + 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; - uint8_t _message_class; - uint8_t _message_id; - unsigned _payload_size; hrt_abstime _disable_cmd_last; + uint16_t _ack_waiting_msg; + ubx_buf_t _buf; }; #endif /* UBX_H_ */ -- cgit v1.2.3 From 7724e3d889eecd6733c6e85341a9f0b97faa37cf Mon Sep 17 00:00:00 2001 From: Kynos Date: Fri, 13 Jun 2014 23:18:58 +0200 Subject: Remove unreferenced SVINFO length defines --- src/drivers/gps/ubx.h | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/drivers/gps/ubx.h b/src/drivers/gps/ubx.h index f084fc655..8fd8d4da6 100644 --- a/src/drivers/gps/ubx.h +++ b/src/drivers/gps/ubx.h @@ -88,10 +88,6 @@ #define UBX_MSG_CFG_NAV5 ((UBX_CLASS_CFG) | UBX_ID_CFG_NAV5 << 8) #define UBX_MSG_MON_HW ((UBX_CLASS_MON) | UBX_ID_MON_HW << 8) -/* NAV_SVINFO has variable length w/o a published max size, so limit processing to UBX_MAX_NUM_SAT */ -#define UBX_MAX_NUM_SAT_SVINFO 50 /**< Practical observed max number of satellites in SVNFO msg */ -#define UBX_PAYLOAD_NAV_SVINFO (8 + 12 * UBX_MAX_NUM_SAT_SVINFO) /**< NAV_SVINFO Rx msg payload size */ - /* TX CFG-PRT message contents */ #define UBX_TX_CFG_PRT_PORTID 0x01 /**< UART1 */ #define UBX_TX_CFG_PRT_MODE 0x000008D0 /**< 0b0000100011010000: 8N1 */ -- cgit v1.2.3 From 1b8e72be1b9614bff4e1bdf81ad7f3265754e062 Mon Sep 17 00:00:00 2001 From: Kynos Date: Sun, 15 Jun 2014 17:26:14 +0200 Subject: Merge sAcc & pAcc scaling fix from branch inav_gps_delay --- src/drivers/gps/ubx.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index 1783ae0f7..56e7a8bd7 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -643,8 +643,8 @@ UBX::payload_rx_done(void) UBX_TRACE_RXMSG("Rx NAV-SOL\n"); _gps_position->fix_type = _buf.payload_rx_nav_sol.gpsFix; - _gps_position->s_variance_m_s = _buf.payload_rx_nav_sol.sAcc; - _gps_position->p_variance_m = _buf.payload_rx_nav_sol.pAcc; + _gps_position->s_variance_m_s = (float)_buf.payload_rx_nav_sol.sAcc * 1e-2f; // from cm to m + _gps_position->p_variance_m = (float)_buf.payload_rx_nav_sol.pAcc * 1e-2f; // from cm to m _gps_position->satellites_used = _buf.payload_rx_nav_sol.numSV; _gps_position->timestamp_variance = hrt_absolute_time(); -- cgit v1.2.3 From 243db01a37d5d3adecc2e010ac4d9819cf3d6c37 Mon Sep 17 00:00:00 2001 From: Kynos Date: Mon, 16 Jun 2014 01:43:44 +0200 Subject: Request and display MON-VER message at startup --- src/drivers/gps/ubx.cpp | 73 +++++++++++++++++++++++++++++++++++++++++++------ src/drivers/gps/ubx.h | 21 ++++++++++++-- 2 files changed, 84 insertions(+), 10 deletions(-) diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index 56e7a8bd7..be19ce488 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -93,6 +93,8 @@ UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position, struct sate _ack_waiting_msg(0) { decode_init(); + memset(_ubx_sw_version, 0, sizeof(_ubx_sw_version)); + memset(_ubx_hw_version, 0, sizeof(_ubx_hw_version)); } UBX::~UBX() @@ -221,6 +223,9 @@ UBX::configure(unsigned &baudrate) return 1; } + /* request module version information by sending an empty MON-VER message */ + send_message(UBX_MSG_MON_VER, nullptr, 0); + _configured = true; return 0; } @@ -385,10 +390,16 @@ UBX::parse_char(const uint8_t b) case UBX_DECODE_PAYLOAD: UBX_TRACE_PARSER("."); add_byte_to_checksum(b); - if (_rx_msg != UBX_MSG_NAV_SVINFO) { - ret = payload_rx_add(b); // add a payload byte - } else { - ret = payload_rx_add_svinfo(b); // add a svinfo payload byte + 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 @@ -477,6 +488,9 @@ UBX::payload_rx_init() _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured 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 */ @@ -559,10 +573,10 @@ UBX::payload_rx_add(const uint8_t b) } /** - * Add svinfo payload rx byte + * Add NAV-SVINFO payload rx byte */ int // -1 = error, 0 = ok, 1 = payload completed -UBX::payload_rx_add_svinfo(const uint8_t b) +UBX::payload_rx_add_nav_svinfo(const uint8_t b) { int ret = 0; @@ -606,6 +620,41 @@ UBX::payload_rx_add_svinfo(const uint8_t b) 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; + + if (_rx_payload_index < sizeof(ubx_payload_rx_mon_ver_part1_t)) { + // Fill Part 1 buffer + _buf.raw[_rx_payload_index] = b; + } else { + if (_rx_payload_index == sizeof(ubx_payload_rx_mon_ver_part1_t)) { + // Part 1 complete: decode Part 1 buffer + strncpy(_ubx_sw_version, (char*)_buf.payload_rx_mon_ver_part1.swVersion, sizeof(_ubx_sw_version)); + strncpy(_ubx_hw_version, (char*)_buf.payload_rx_mon_ver_part1.hwVersion, sizeof(_ubx_hw_version)); + UBX_WARN("VER sw %30s", _ubx_sw_version); + UBX_WARN("VER hw %10s", _ubx_hw_version); + } + // 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); + _buf.raw[buf_index] = b; + if (buf_index == sizeof(ubx_payload_rx_mon_ver_part2_t) - 1) { + // Part 2 complete: decode Part 2 buffer + UBX_WARN("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 */ @@ -712,6 +761,12 @@ UBX::payload_rx_done(void) ret = 1; break; + case UBX_MSG_MON_VER: + UBX_TRACE_RXMSG("Rx MON-VER\n"); + + ret = 1; + break; + case UBX_MSG_MON_HW: UBX_TRACE_RXMSG("Rx MON-HW\n"); @@ -813,10 +868,12 @@ UBX::send_message(const uint16_t msg, const uint8_t *payload, const uint16_t len // Calculate checksum calc_checksum(((uint8_t*)&header) + 2, sizeof(header) - 2, &checksum); // skip 2 sync bytes - calc_checksum(payload, length, &checksum); + if (payload != nullptr) + calc_checksum(payload, length, &checksum); // Send message write(_fd, (const void *)&header, sizeof(header)); - write(_fd, (const void *)payload, length); + if (payload != nullptr) + write(_fd, (const void *)payload, length); write(_fd, (const void *)&checksum, sizeof(checksum)); } diff --git a/src/drivers/gps/ubx.h b/src/drivers/gps/ubx.h index 8fd8d4da6..c8c89dfad 100644 --- a/src/drivers/gps/ubx.h +++ b/src/drivers/gps/ubx.h @@ -72,6 +72,7 @@ #define UBX_ID_CFG_MSG 0x01 #define UBX_ID_CFG_RATE 0x08 #define UBX_ID_CFG_NAV5 0x24 +#define UBX_ID_MON_VER 0x04 #define UBX_ID_MON_HW 0x09 /* Message Classes & IDs */ @@ -87,6 +88,7 @@ #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_MON_HW ((UBX_CLASS_MON) | UBX_ID_MON_HW << 8) +#define UBX_MSG_MON_VER ((UBX_CLASS_MON) | UBX_ID_MON_VER << 8) /* TX CFG-PRT message contents */ #define UBX_TX_CFG_PRT_PORTID 0x01 /**< UART1 */ @@ -249,6 +251,17 @@ typedef struct { 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; @@ -333,6 +346,8 @@ typedef union { 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; @@ -366,7 +381,6 @@ typedef enum { UBX_RXMSG_ERROR_LENGTH } ubx_rxmsg_state_t; - /* ACK state */ typedef enum { UBX_ACK_IDLE = 0, @@ -400,7 +414,8 @@ private: * Add payload rx byte */ int payload_rx_add(const uint8_t b); - int payload_rx_add_svinfo(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 @@ -455,6 +470,8 @@ private: hrt_abstime _disable_cmd_last; uint16_t _ack_waiting_msg; ubx_buf_t _buf; + char _ubx_sw_version[30]; + char _ubx_hw_version[10]; }; #endif /* UBX_H_ */ -- cgit v1.2.3 From 0ffb123701c2e6124d7c735cdb55c250564d12c6 Mon Sep 17 00:00:00 2001 From: Kynos Date: Mon, 16 Jun 2014 15:35:38 +0200 Subject: Store hash instead of full strings for SW & HW version --- src/drivers/gps/ubx.cpp | 48 +++++++++++++++++++++++++++++++++++++++--------- src/drivers/gps/ubx.h | 8 ++++++-- 2 files changed, 45 insertions(+), 11 deletions(-) diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index be19ce488..adc7130dd 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -72,6 +72,10 @@ #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, ...) {/*printf(s, ## __VA_ARGS__);*/} /* decoding progress in parse_char() */ #define UBX_TRACE_RXMSG(s, ...) {/*printf(s, ## __VA_ARGS__);*/} /* Rx msgs in payload_rx_done() */ @@ -90,11 +94,10 @@ UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position, struct sate _got_posllh(false), _got_velned(false), _disable_cmd_last(0), - _ack_waiting_msg(0) + _ack_waiting_msg(0), + _ubx_version(0) { decode_init(); - memset(_ubx_sw_version, 0, sizeof(_ubx_sw_version)); - memset(_ubx_hw_version, 0, sizeof(_ubx_hw_version)); } UBX::~UBX() @@ -633,18 +636,19 @@ UBX::payload_rx_add_mon_ver(const uint8_t b) _buf.raw[_rx_payload_index] = b; } else { if (_rx_payload_index == sizeof(ubx_payload_rx_mon_ver_part1_t)) { - // Part 1 complete: decode Part 1 buffer - strncpy(_ubx_sw_version, (char*)_buf.payload_rx_mon_ver_part1.swVersion, sizeof(_ubx_sw_version)); - strncpy(_ubx_hw_version, (char*)_buf.payload_rx_mon_ver_part1.hwVersion, sizeof(_ubx_hw_version)); - UBX_WARN("VER sw %30s", _ubx_sw_version); - UBX_WARN("VER hw %10s", _ubx_hw_version); + // 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_WARN("VER hash 0x%08x", _ubx_version); + UBX_WARN("VER hw \"%10s\"", _buf.payload_rx_mon_ver_part1.hwVersion); + UBX_WARN("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); _buf.raw[buf_index] = b; if (buf_index == sizeof(ubx_payload_rx_mon_ver_part2_t) - 1) { // Part 2 complete: decode Part 2 buffer - UBX_WARN("VER ext %30s", _buf.payload_rx_mon_ver_part2.extension); + UBX_WARN("VER ext \" %30s\"", _buf.payload_rx_mon_ver_part2.extension); } } @@ -877,3 +881,29 @@ UBX::send_message(const uint16_t msg, const uint8_t *payload, const uint16_t len write(_fd, (const void *)payload, length); write(_fd, (const void *)&checksum, sizeof(checksum)); } + +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 index c8c89dfad..f01b697a3 100644 --- a/src/drivers/gps/ubx.h +++ b/src/drivers/gps/ubx.h @@ -452,6 +452,11 @@ private: */ 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; @@ -470,8 +475,7 @@ private: hrt_abstime _disable_cmd_last; uint16_t _ack_waiting_msg; ubx_buf_t _buf; - char _ubx_sw_version[30]; - char _ubx_hw_version[10]; + uint32_t _ubx_version; }; #endif /* UBX_H_ */ -- cgit v1.2.3 From f1a5be1f57d9a15f29a428e962c3ed7f75777d22 Mon Sep 17 00:00:00 2001 From: Kynos Date: Sat, 21 Jun 2014 20:42:33 +0200 Subject: Define NAV-PVT message --- src/drivers/gps/ubx.h | 50 ++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 50 insertions(+) diff --git a/src/drivers/gps/ubx.h b/src/drivers/gps/ubx.h index f01b697a3..1a9e7cb0c 100644 --- a/src/drivers/gps/ubx.h +++ b/src/drivers/gps/ubx.h @@ -63,6 +63,7 @@ /* Message IDs */ #define UBX_ID_NAV_POSLLH 0x02 #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 @@ -78,6 +79,7 @@ /* 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_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) @@ -90,6 +92,18 @@ #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) */ + /* TX CFG-PRT message contents */ #define UBX_TX_CFG_PRT_PORTID 0x01 /**< UART1 */ #define UBX_TX_CFG_PRT_MODE 0x000008D0 /**< 0b0000100011010000: 8N1 */ @@ -162,6 +176,42 @@ typedef struct { uint32_t reserved2; } ubx_payload_rx_nav_sol_t; +/* Rx NAV-PVT */ +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; /**< Heading of vehicle (2-D) [1e-5 deg] */ + uint32_t reserved4; +} ubx_payload_rx_nav_pvt_t; + /* Rx NAV-TIMEUTC */ typedef struct { uint32_t iTOW; /**< GPS Time of Week [ms] */ -- cgit v1.2.3 From 43b9d96cf4306bc472d16956f5437dbef7c630c0 Mon Sep 17 00:00:00 2001 From: Kynos Date: Mon, 30 Jun 2014 01:55:40 +0200 Subject: Merged upstream master, add missing change --- src/modules/mavlink/mavlink_messages.cpp | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 472e2810d..06c2e737e 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -669,16 +669,16 @@ protected: if (gps_sub->update(&gps_time, &gps)) { mavlink_msg_gps_raw_int_send(_channel, - gps->timestamp_position, - gps->fix_type, - gps->lat, - gps->lon, - gps->alt, - cm_uint16_from_m_float(gps->eph), - cm_uint16_from_m_float(gps->epv), - gps->vel_m_s * 100.0f, - _wrap_2pi(gps->cog_rad) * M_RAD_TO_DEG_F * 1e2f, - gps->satellites_used); + gps.timestamp_position, + gps.fix_type, + gps.lat, + gps.lon, + gps.alt, + cm_uint16_from_m_float(gps.eph), + cm_uint16_from_m_float(gps.epv), + gps.vel_m_s * 100.0f, + _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F * 1e2f, + gps.satellites_used); } } }; -- cgit v1.2.3 From 4cd66a3242aa2dfb03a3e58ffcc8d27e1350b7ac Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 2 Jul 2014 00:25:19 +0200 Subject: Fix sdlog2 GPS time copy for log_on_start situation --- src/modules/sdlog2/sdlog2.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 9b071ff99..f99aec34e 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1070,7 +1070,7 @@ int sdlog2_thread_main(int argc, char *argv[]) if (log_on_start) { /* check GPS topic to get GPS time */ if (log_name_timestamp) { - if (copy_if_updated(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf_gps_pos)) { + if (!orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf_gps_pos)) { gps_time = buf_gps_pos.time_gps_usec; } } -- cgit v1.2.3