diff options
Diffstat (limited to 'src/drivers/gps')
-rw-r--r-- | src/drivers/gps/ubx.cpp | 73 | ||||
-rw-r--r-- | 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; @@ -607,6 +621,41 @@ UBX::payload_rx_add_svinfo(const uint8_t b) } /** + * 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 */ int // 0 = no message handled, 1 = message handled, 2 = sat info message handled @@ -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_ */ |