diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2014-06-13 22:33:31 +0200 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2014-06-13 22:33:31 +0200 |
commit | 8a25c48071ed794b33e23f7ff7737cded58bea11 (patch) | |
tree | 25e2d5393124a78e14d9d6a0b93ea34eac5c6d34 /src/drivers/gps | |
parent | 16ca6c1605e0864d37cce4cfdbe790df5746bb38 (diff) | |
parent | 251760679a4bed86fdb10746062fde2cc23e460f (diff) | |
download | px4-firmware-8a25c48071ed794b33e23f7ff7737cded58bea11.tar.gz px4-firmware-8a25c48071ed794b33e23f7ff7737cded58bea11.tar.bz2 px4-firmware-8a25c48071ed794b33e23f7ff7737cded58bea11.zip |
Merge branch 'master' into mpc_in_flight_lock
Diffstat (limited to 'src/drivers/gps')
-rw-r--r-- | src/drivers/gps/ubx.cpp | 18 | ||||
-rw-r--r-- | src/drivers/gps/ubx.h | 3 |
2 files changed, 17 insertions, 4 deletions
diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index 19cf5beec..c143eeb0c 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(); @@ -275,9 +278,10 @@ 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 */ @@ -286,7 +290,10 @@ UBX::receive(unsigned timeout) } 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 { @@ -438,6 +445,7 @@ UBX::handle_message() _rate_count_lat_lon++; + _got_posllh = true; ret = 1; break; } @@ -447,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; @@ -482,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; } @@ -557,6 +566,7 @@ UBX::handle_message() _rate_count_vel++; + _got_velned = true; ret = 1; break; } diff --git a/src/drivers/gps/ubx.h b/src/drivers/gps/ubx.h index 5cf47b60b..43d688893 100644 --- a/src/drivers/gps/ubx.h +++ b/src/drivers/gps/ubx.h @@ -397,6 +397,9 @@ private: struct vehicle_gps_position_s *_gps_position; bool _configured; bool _waiting_for_ack; + bool _got_posllh; + bool _got_velned; + bool _got_timeutc; uint8_t _message_class_needed; uint8_t _message_id_needed; ubx_decode_state_t _decode_state; |