diff options
author | Kynos <mail01@delago.net> | 2014-06-08 15:56:42 +0200 |
---|---|---|
committer | Kynos <mail01@delago.net> | 2014-06-08 15:56:42 +0200 |
commit | 018eeeecad748959a8afa4845a536e897a57249d (patch) | |
tree | 265d5565c6f133b69932961744e7e8de58e00b44 /src/drivers | |
parent | 9f754e0e9a2e2ad75a3fe5a15690cf542ce08e8a (diff) | |
download | px4-firmware-018eeeecad748959a8afa4845a536e897a57249d.tar.gz px4-firmware-018eeeecad748959a8afa4845a536e897a57249d.tar.bz2 px4-firmware-018eeeecad748959a8afa4845a536e897a57249d.zip |
Merged ubx driver part of pull request #1031
Diffstat (limited to 'src/drivers')
-rw-r--r-- | src/drivers/gps/ubx.cpp | 15 | ||||
-rw-r--r-- | src/drivers/gps/ubx.h | 2 |
2 files changed, 13 insertions, 4 deletions
diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index dd47572b4..0708e23b4 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -74,6 +74,8 @@ UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position, struct sate _enable_sat_info(enable_sat_info), _configured(false), _waiting_for_ack(false), + _got_posllh(false), + _got_velned(false), _disable_cmd_last(0) { decode_init(); @@ -208,7 +210,7 @@ UBX::configure(unsigned &baudrate) return 1; } - configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_TIMEUTC, 1); + configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_TIMEUTC, 5); if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { warnx("MSG CFG FAIL: NAV TIMEUTC"); @@ -286,9 +288,10 @@ UBX::receive(unsigned timeout) 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]), 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 */ @@ -297,7 +300,9 @@ 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; return handled; } else { @@ -456,7 +461,8 @@ UBX::handle_message() _rate_count_lat_lon++; - ret = 1; + _got_posllh = true; + ret = 1; break; } @@ -552,6 +558,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 90aeffedc..50df3ada2 100644 --- a/src/drivers/gps/ubx.h +++ b/src/drivers/gps/ubx.h @@ -436,6 +436,8 @@ private: bool _enable_sat_info; bool _configured; bool _waiting_for_ack; + bool _got_posllh; + bool _got_velned; uint8_t _message_class_needed; uint8_t _message_id_needed; ubx_decode_state_t _decode_state; |