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