/**
* 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 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);
}
}
if (++_rx_payload_index >= _rx_payload_length) {
ret = 1; // payload received completely
}
return ret;
}
int // -1 = NAK, error or timeout, 0 = ACK
UBX::wait_for_ack(const uint16_t msg, const unsigned timeout, const bool report)
{
int ret = -1;
_ack_state = UBX_ACK_WAITING;
_ack_waiting_msg = msg; // memorize sent msg class&ID for ACK check
hrt_abstime time_started = hrt_absolute_time();
while ((_ack_state == UBX_ACK_WAITING) && (hrt_absolute_time() < time_started + timeout * 1000)) {
receive(timeout);
}
if (_ack_state == UBX_ACK_GOT_ACK) {
ret = 0; // ACK received ok
} else if (report) {
if (_ack_state == UBX_ACK_GOT_NAK) {
UBX_WARN("ubx msg 0x%04x NAK", SWAP16((unsigned)msg));
} else {
UBX_WARN("ubx msg 0x%04x ACK timeout", SWAP16((unsigned)msg));
}
}
_ack_state = UBX_ACK_IDLE;
return ret;
}
/**
* Start payload rx
*/
int // -1 = abort, 0 = continue
UBX::payload_rx_init()
{
int ret = 0;
_rx_state = UBX_RXMSG_HANDLE; // handle by default
switch (_rx_msg) {
case UBX_MSG_NAV_PVT:
if ( (_rx_payload_length != UBX_PAYLOAD_RX_NAV_PVT_SIZE_UBX7) /* u-blox 7 msg format */
&& (_rx_payload_length != UBX_PAYLOAD_RX_NAV_PVT_SIZE_UBX8)) /* u-blox 8+ msg format */
_rx_state = UBX_RXMSG_ERROR_LENGTH;
else if (!_configured)
_rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured
else if (!_use_nav_pvt)
_rx_state = UBX_RXMSG_DISABLE; // disable if not using NAV-PVT
break;
case UBX_MSG_NAV_POSLLH:
if (_rx_payload_length != sizeof(ubx_payload_rx_nav_posllh_t))
_rx_state = UBX_RXMSG_ERROR_LENGTH;
else if (!_configured)
_rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured
else if (_use_nav_pvt)
_rx_state = UBX_RXMSG_DISABLE; // disable if using NAV-PVT instead
break;
case UBX_MSG_NAV_SOL:
if (_rx_payload_length != sizeof(ubx_payload_rx_nav_sol_t))
_rx_state = UBX_RXMSG_ERROR_LENGTH;
else if (!_configured)
_rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured
else if (_use_nav_pvt)
_rx_state = UBX_RXMSG_DISABLE; // disable if using NAV-PVT instead
break;
case UBX_MSG_NAV_TIMEUTC:
if (_rx_payload_length != sizeof(ubx_payload_rx_nav_timeutc_t))
_rx_state = UBX_RXMSG_ERROR_LENGTH;
else if (!_configured)
_rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured
else if (_use_nav_pvt)
_rx_state = UBX_RXMSG_DISABLE; // disable if using NAV-PVT instead
break;
case UBX_MSG_NAV_SVINFO:
if (_satellite_info == nullptr)
_rx_state = UBX_RXMSG_DISABLE; // disable if sat info not requested
else if (!_configured)
_rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured
else
memset(_satellite_info, 0, sizeof(*_satellite_info)); // initialize sat info
break;
case UBX_MSG_NAV_VELNED:
if (_rx_payload_length != sizeof(ubx_payload_rx_nav_velned_t))
_rx_state = UBX_RXMSG_ERROR_LENGTH;
else if (!_configured)
_rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured
else if (_use_nav_pvt)
_rx_state = UBX_RXMSG_DISABLE; // disable if using NAV-PVT instead
break;
case UBX_MSG_MON_VER:
break; // unconditionally handle this message
case UBX_MSG_MON_HW:
if ( (_rx_payload_length != sizeof(ubx_payload_rx_mon_hw_ubx6_t)) /* u-blox 6 msg format */
&& (_rx_payload_length != sizeof(ubx_payload_rx_mon_hw_ubx7_t))) /* u-blox 7+ msg format */
_rx_state = UBX_RXMSG_ERROR_LENGTH;
else if (!_configured)
_rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured
break;
case UBX_MSG_ACK_ACK:
if (_rx_payload_length != sizeof(ubx_payload_rx_ack_ack_t))
_rx_state = UBX_RXMSG_ERROR_LENGTH;
else if (_configured)
_rx_state = UBX_RXMSG_IGNORE; // ignore if _configured
break;
case UBX_MSG_ACK_NAK:
if (_rx_payload_length != sizeof(ubx_payload_rx_ack_nak_t))
_rx_state = UBX_RXMSG_ERROR_LENGTH;
else if (_configured)
_rx_state = UBX_RXMSG_IGNORE; // ignore if _configured
break;
default:
_rx_state = UBX_RXMSG_DISABLE; // disable all other messages
break;
}
switch (_rx_state) {
case UBX_RXMSG_HANDLE: // handle message
case UBX_RXMSG_IGNORE: // ignore message but don't report error
ret = 0;
break;
case UBX_RXMSG_DISABLE: // disable unexpected messages
UBX_DEBUG("ubx msg 0x%04x len %u unexpected", SWAP16((unsigned)_rx_msg), (unsigned)_rx_payload_length);
{
hrt_abstime t = hrt_absolute_time();
if (t > _disable_cmd_last + DISABLE_MSG_INTERVAL) {
/* don't attempt for every message to disable, some might not be disabled */
_disable_cmd_last = t;
UBX_DEBUG("ubx disabling msg 0x%04x", SWAP16((unsigned)_rx_msg));
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;
}
int // 0 = decoding, 1 = message handled, 2 = sat info message handled
UBX::parse_char(const uint8_t b)
{
int ret = 0;
switch (_decode_state) {
/* Expecting Sync1 */
case UBX_DECODE_SYNC1:
if (b == UBX_SYNC1) { // Sync1 found --> expecting Sync2
UBX_TRACE_PARSER("\nA");
_decode_state = UBX_DECODE_SYNC2;
}
break;
/* Expecting Sync2 */
case UBX_DECODE_SYNC2:
if (b == UBX_SYNC2) { // Sync2 found --> expecting Class
UBX_TRACE_PARSER("B");
_decode_state = UBX_DECODE_CLASS;
} else { // Sync1 not followed by Sync2: reset parser
decode_init();
}
break;
/* Expecting Class */
case UBX_DECODE_CLASS:
UBX_TRACE_PARSER("C");
add_byte_to_checksum(b); // checksum is calculated for everything except Sync and Checksum bytes
_rx_msg = b;
_decode_state = UBX_DECODE_ID;
break;
/* Expecting ID */
case UBX_DECODE_ID:
UBX_TRACE_PARSER("D");
add_byte_to_checksum(b);
_rx_msg |= b << 8;
_decode_state = UBX_DECODE_LENGTH1;
break;
/* Expecting first length byte */
case UBX_DECODE_LENGTH1:
UBX_TRACE_PARSER("E");
add_byte_to_checksum(b);
_rx_payload_length = b;
_decode_state = UBX_DECODE_LENGTH2;
break;
/* Expecting second length byte */
case UBX_DECODE_LENGTH2:
UBX_TRACE_PARSER("F");
add_byte_to_checksum(b);
_rx_payload_length |= b << 8; // calculate payload size
if (payload_rx_init() != 0) { // start payload reception
// payload will not be handled, discard message
decode_init();
} else {
_decode_state = (_rx_payload_length > 0) ? UBX_DECODE_PAYLOAD : UBX_DECODE_CHKSUM1;
}
break;
/* Expecting payload */
case UBX_DECODE_PAYLOAD:
UBX_TRACE_PARSER(".");
add_byte_to_checksum(b);
switch (_rx_msg) {
case UBX_MSG_NAV_SVINFO:
ret = payload_rx_add_nav_svinfo(b); // add a NAV-SVINFO payload byte
break;
case UBX_MSG_MON_VER:
ret = payload_rx_add_mon_ver(b); // add a MON-VER payload byte
break;
default:
ret = payload_rx_add(b); // add a payload byte
break;
}
if (ret < 0) {
// payload not handled, discard message
decode_init();
} else if (ret > 0) {
// payload complete, expecting checksum
_decode_state = UBX_DECODE_CHKSUM1;
} else {
// expecting more payload, stay in state UBX_DECODE_PAYLOAD
}
ret = 0;
break;
/* Expecting first checksum byte */
case UBX_DECODE_CHKSUM1:
if (_rx_ck_a != b) {
UBX_WARN("ubx checksum err");
decode_init();
} else {
_decode_state = UBX_DECODE_CHKSUM2;
}
break;
/* Expecting second checksum byte */
case UBX_DECODE_CHKSUM2:
if (_rx_ck_b != b) {
UBX_WARN("ubx checksum err");
} else {
ret = payload_rx_done(); // finish payload processing
}
decode_init();
break;
default:
break;
}
return ret;
}
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];
fds[0].fd = _fd;
fds[0].events = POLLIN;
uint8_t buf[128];
/* timeout additional to poll */
uint64_t time_started = hrt_absolute_time();
ssize_t count = 0;
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]), ready_to_return ? UBX_PACKET_TIMEOUT : timeout);
if (ret < 0) {
/* something went wrong when polling */
UBX_WARN("ubx poll() err");
return -1;
} else if (ret == 0) {
/* return success after short delay after receiving a packet or timeout after long delay */
if (ready_to_return) {
_got_posllh = false;
_got_velned = false;
return handled;
} else {
return -1;
}
} else if (ret > 0) {
/* if we have new data from GPS, go handle it */
if (fds[0].revents & POLLIN) {
/*
* We are here because poll says there is some data, so this
* won't block even on a blocking device. But don't read immediately
* by 1-2 bytes, wait for some more data to save expensive read() calls.
* If more bytes are available, we'll go back to poll() again.
*/
usleep(UBX_WAIT_BEFORE_READ * 1000);
count = read(_fd, buf, sizeof(buf));
/* pass received bytes to the packet decoder */
for (int i = 0; i < count; i++) {
handled |= parse_char(buf[i]);
}
}
}
/* abort after timeout if no useful packets received */
if (time_started + timeout * 1000 < hrt_absolute_time()) {
return -1;
}
}
}
int
UBX::configure(unsigned &baudrate)
{
_configured = false;
/* try different baudrates */
const unsigned baudrates[] = {9600, 38400, 19200, 57600, 115200};
unsigned 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();
receive(20);
decode_init();
/* Send a CFG-PRT message to set the UBX protocol for in and out
* and leave the baudrate as it is, we just want an ACK-ACK for this */
memset(&_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;
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_MSG_CFG_PRT, UBX_CONFIG_TIMEOUT, false);
if (UBX_TX_CFG_PRT_BAUDRATE != baudrate) {
set_baudrate(_fd, UBX_TX_CFG_PRT_BAUDRATE);
baudrate = UBX_TX_CFG_PRT_BAUDRATE;
}
/* at this point we have correct baudrate on both ends */
break;
}
if (baud_i >= sizeof(baudrates) / sizeof(baudrates[0])) {
return 1; // connection and/or baudrate detection failed
}
/* Send a CFG-RATE message to define update rate */
memset(&_buf.payload_tx_cfg_rate, 0, sizeof(_buf.payload_tx_cfg_rate));
_buf.payload_tx_cfg_rate.measRate = UBX_TX_CFG_RATE_MEASINTERVAL;
_buf.payload_tx_cfg_rate.navRate = UBX_TX_CFG_RATE_NAVRATE;
_buf.payload_tx_cfg_rate.timeRef = UBX_TX_CFG_RATE_TIMEREF;
send_message(UBX_MSG_CFG_RATE, _buf.raw, sizeof(_buf.payload_tx_cfg_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 */
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_message(UBX_MSG_CFG_NAV5, _buf.raw, sizeof(_buf.payload_tx_cfg_nav5));
if (wait_for_ack(UBX_MSG_CFG_NAV5, UBX_CONFIG_TIMEOUT, true) < 0) {
return 1;
}
#ifdef UBX_CONFIGURE_SBAS
/* send a SBAS message to set the SBAS options */
memset(&_buf.payload_tx_cfg_sbas, 0, sizeof(_buf.payload_tx_cfg_sbas));
_buf.payload_tx_cfg_sbas.mode = UBX_TX_CFG_SBAS_MODE;
send_message(UBX_MSG_CFG_SBAS, _buf.raw, sizeof(_buf.payload_tx_cfg_sbas));
if (wait_for_ack(UBX_MSG_CFG_SBAS, UBX_CONFIG_TIMEOUT, true) < 0) {
return 1;
}
#endif
/* configure message rates */
/* the last argument is divisor for measurement rate (set by CFG RATE), i.e. 1 means 5Hz */
/* try to set rate for NAV-PVT */
/* (implemented for ubx7+ modules only, use NAV-SOL, NAV-POSLLH, NAV-VELNED and NAV-TIMEUTC for ubx6) */
configure_message_rate(UBX_MSG_NAV_PVT, 1);
if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) {
_use_nav_pvt = false;
} else {
_use_nav_pvt = true;
}
UBX_WARN("%susing NAV-PVT", _use_nav_pvt ? "" : "not ");
if (!_use_nav_pvt) {
configure_message_rate(UBX_MSG_NAV_TIMEUTC, 5);
if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) {
return 1;
}
configure_message_rate(UBX_MSG_NAV_POSLLH, 1);
if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) {
return 1;
}
configure_message_rate(UBX_MSG_NAV_SOL, 1);
if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) {
return 1;
}
configure_message_rate(UBX_MSG_NAV_VELNED, 1);
if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) {
return 1;
}
}
configure_message_rate(UBX_MSG_NAV_SVINFO, (_satellite_info != nullptr) ? 5 : 0);
if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) {
return 1;
}
configure_message_rate(UBX_MSG_MON_HW, 1);
if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) {
return 1;
}
/* request module version information by sending an empty MON-VER message */
send_message(UBX_MSG_MON_VER, nullptr, 0);
_configured = true;
return 0;
}
