aboutsummaryrefslogtreecommitdiff
path: root/src/drivers/gps/ubx.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'src/drivers/gps/ubx.cpp')
-rw-r--r--src/drivers/gps/ubx.cpp69
1 files changed, 52 insertions, 17 deletions
diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp
index 8a2afecb7..404607571 100644
--- a/src/drivers/gps/ubx.cpp
+++ b/src/drivers/gps/ubx.cpp
@@ -69,6 +69,9 @@ UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position) :
_gps_position(gps_position),
_configured(false),
_waiting_for_ack(false),
+ _got_posllh(false),
+ _got_velned(false),
+ _got_timeutc(false),
_disable_cmd_last(0)
{
decode_init();
@@ -164,7 +167,7 @@ UBX::configure(unsigned &baudrate)
send_config_packet(_fd, (uint8_t *)&cfg_rate_packet, sizeof(cfg_rate_packet));
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
- warnx("ubx: configuration failed: RATE");
+ warnx("CFG FAIL: RATE");
return 1;
}
@@ -185,7 +188,7 @@ UBX::configure(unsigned &baudrate)
send_config_packet(_fd, (uint8_t *)&cfg_nav5_packet, sizeof(cfg_nav5_packet));
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
- warnx("ubx: configuration failed: NAV5");
+ warnx("CFG FAIL: NAV5");
return 1;
}
@@ -194,35 +197,42 @@ UBX::configure(unsigned &baudrate)
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_POSLLH, 1);
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
- warnx("ubx: msg rate configuration failed: NAV POSLLH");
+ 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("ubx: msg rate configuration failed: NAV TIMEUTC");
+ 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("ubx: msg rate configuration failed: NAV SOL");
+ 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("ubx: msg rate configuration failed: NAV VELNED");
+ 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("ubx: msg rate configuration failed: NAV SVINFO");
+ 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;
}
@@ -268,18 +278,22 @@ UBX::receive(unsigned timeout)
bool handled = false;
while (true) {
+ bool ready_to_return = _configured ? (_got_posllh && _got_velned && _got_timeutc) : 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 */
- warnx("ubx: poll error");
+ warnx("poll error");
return -1;
} 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;
+ _got_timeutc = false;
return 1;
} else {
@@ -310,7 +324,7 @@ UBX::receive(unsigned timeout)
/* abort after timeout if no useful packets received */
if (time_started + timeout * 1000 < hrt_absolute_time()) {
- warnx("ubx: timeout - no useful messages");
+ warnx("timeout - no useful messages");
return -1;
}
}
@@ -383,7 +397,7 @@ UBX::parse_char(uint8_t b)
return 1; // message received successfully
} else {
- warnx("ubx: checksum wrong");
+ warnx("checksum wrong");
decode_init();
return -1;
}
@@ -392,7 +406,7 @@ UBX::parse_char(uint8_t b)
_rx_count++;
} else {
- warnx("ubx: buffer full");
+ warnx("buffer full");
decode_init();
return -1;
}
@@ -425,12 +439,13 @@ UBX::handle_message()
_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->eph = (float)packet->hAcc * 1e-3f; // from mm to m
+ _gps_position->epv = (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;
}
@@ -440,8 +455,8 @@ UBX::handle_message()
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 = packet->sAcc;
- _gps_position->p_variance_m = packet->pAcc;
+ _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;
@@ -475,6 +490,7 @@ UBX::handle_message()
_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;
}
@@ -550,6 +566,7 @@ UBX::handle_message()
_rate_count_vel++;
+ _got_velned = true;
ret = 1;
break;
}
@@ -566,6 +583,24 @@ UBX::handle_message()
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;
}
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-13 16:39:42 +0000