int
UBX::configure(unsigned &baudrate)
{
_configured = false;
/* try different baudrates */
const unsigned baudrates_to_try[] = {9600, 38400, 19200, 57600, 115200};
int baud_i;
for (baud_i = 0; baud_i < 5; baud_i++) {
baudrate = baudrates_to_try[baud_i];
set_baudrate(_fd, baudrate);
/* 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) {
/* try next baudrate */
continue;
}
/* Send a CFG-PRT message again, this time 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 = 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));
/* no ACK is expected here, but read the buffer anyway in case we actually get an ACK */
wait_for_ack(UBX_CONFIG_TIMEOUT);
if (UBX_CFG_PRT_PAYLOAD_BAUDRATE != baudrate) {
set_baudrate(_fd, UBX_CFG_PRT_PAYLOAD_BAUDRATE);
baudrate = UBX_CFG_PRT_PAYLOAD_BAUDRATE;
}
/* at this point we have correct baudrate on both ends */
break;
}
if (baud_i >= 5) {
return 1;
}
/* 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;
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_config_packet(_fd, (uint8_t *)&cfg_rate_packet, sizeof(cfg_rate_packet));
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
warnx("CFG FAIL: RATE");
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;
send_config_packet(_fd, (uint8_t *)&cfg_nav5_packet, sizeof(cfg_nav5_packet));
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
warnx("CFG FAIL: NAV5");
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);
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
warnx("MSG CFG FAIL: NAV POSLLH");
return 1;
}
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_TIMEUTC, 1);
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
warnx("MSG CFG FAIL: NAV TIMEUTC");
return 1;
}
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SOL, 1);
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
warnx("MSG CFG FAIL: NAV SOL");
return 1;
}
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_VELNED, 1);
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
warnx("MSG CFG FAIL: NAV VELNED");
return 1;
}
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;
}
configure_message_rate(UBX_CLASS_MON, UBX_MESSAGE_MON_HW, 1);
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
warnx("MSG CFG FAIL: MON HW");
return 1;
}
_configured = true;
return 0;
}
/**
* 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
UBX::handle_message()
{
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;
}
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;
_gps_position->fix_type = packet->gpsFix;
_gps_position->s_variance_m_s = (float)packet->sAcc * 1e-2f; // from cm/s to m/s
_gps_position->p_variance_m = (float)packet->pAcc * 1e-2f; // from cm to m
_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;
/* 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);
#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);
#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();
_got_timeutc = true;
ret = 1;
break;
}
case UBX_MESSAGE_NAV_SVINFO: {
//printf("GOT NAV_SVINFO\n");
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]);
/* 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;
}
_gps_position->satellites_visible = satellites_used; // visible ~= used but we are interested in the used ones
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;
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;
_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();
_rate_count_vel++;
_got_velned = true;
ret = 1;
break;
}
default:
break;
}
break;
case UBX_CLASS_ACK: {
/* ignore ACK when already configured */
ret = 1;
break;
}
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;
_gps_position->noise_per_ms = p->noisePerMS;
_gps_position->jamming_indicator = p->jamInd;
ret = 1;
break;
}
default:
break;
}
}
default:
break;
}
if (ret == 0) {
/* message not handled */
warnx("ubx: unknown message received: 0x%02x-0x%02x", (unsigned)_message_class, (unsigned)_message_id);
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;
warnx("ubx: disabling message 0x%02x-0x%02x", (unsigned)_message_class, (unsigned)_message_id);
configure_message_rate(_message_class, _message_id, 0);
}
}
} 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;
}
case UBX_MESSAGE_ACK_NAK: {
// printf("GOT ACK_NAK\n");
warnx("ubx: not acknowledged");
/* configuration obviously not successful */
_waiting_for_ack = false;
ret = -1;
break;
}
default:
break;
}
}
}
decode_init();
return ret;
}
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_DEBUG("%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;
}