From 1cca3ca8a5469e66dbb5bebbe518b55e4af426d8 Mon Sep 17 00:00:00 2001 From: Kynos Date: Thu, 3 Jul 2014 13:33:29 +0200 Subject: [PATCH] Use NAV-PVT with ubx7 and ubx8 modules This replaces NAV-SOL, NAV-POSLLH, NAV-VELNED and NAV-TIMEUTC. --- src/drivers/gps/gps.cpp | 1 - src/drivers/gps/ubx.cpp | 125 +++++++++++++++--- src/drivers/gps/ubx.h | 10 +- src/modules/mavlink/mavlink_receiver.cpp | 1 - .../uORB/topics/vehicle_gps_position.h | 1 - 5 files changed, 113 insertions(+), 25 deletions(-) diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 241e3bdf3b..401b65dd4f 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -299,7 +299,6 @@ GPS::task_main() _report_gps_pos.alt = (int32_t)1200e3f; _report_gps_pos.timestamp_variance = hrt_absolute_time(); _report_gps_pos.s_variance_m_s = 10.0f; - _report_gps_pos.p_variance_m = 10.0f; _report_gps_pos.c_variance_rad = 0.1f; _report_gps_pos.fix_type = 3; _report_gps_pos.eph = 0.9f; diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index 44434a1df9..d0854f5e9a 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -95,7 +95,8 @@ UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position, struct sate _got_velned(false), _disable_cmd_last(0), _ack_waiting_msg(0), - _ubx_version(0) + _ubx_version(0), + _use_nav_pvt(false) { decode_init(); } @@ -190,38 +191,45 @@ UBX::configure(unsigned &baudrate) /* configure message rates */ /* the last argument is divisor for measurement rate (set by CFG RATE), i.e. 1 means 5Hz */ - configure_message_rate(UBX_MSG_NAV_POSLLH, 1); + /* 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) { - return 1; + _use_nav_pvt = false; + } else { + _use_nav_pvt = true; } + UBX_WARN("%susing NAV-PVT", _use_nav_pvt ? "" : "not "); - configure_message_rate(UBX_MSG_NAV_TIMEUTC, 5); + 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; + } - 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); + configure_message_rate(UBX_MSG_NAV_SOL, 1); + if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) { + return 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_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; } @@ -454,11 +462,23 @@ UBX::payload_rx_init() _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: @@ -466,6 +486,8 @@ UBX::payload_rx_init() _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: @@ -473,6 +495,8 @@ UBX::payload_rx_init() _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: @@ -489,6 +513,8 @@ UBX::payload_rx_init() _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: @@ -675,6 +701,68 @@ UBX::payload_rx_done(void) // handle message switch (_rx_msg) { + case UBX_MSG_NAV_PVT: + UBX_TRACE_RXMSG("Rx NAV-PVT\n"); + + _gps_position->fix_type = _buf.payload_rx_nav_pvt.fixType; + _gps_position->satellites_used = _buf.payload_rx_nav_pvt.numSV; + + _gps_position->lat = _buf.payload_rx_nav_pvt.lat; + _gps_position->lon = _buf.payload_rx_nav_pvt.lon; + _gps_position->alt = _buf.payload_rx_nav_pvt.hMSL; + + _gps_position->eph = (float)_buf.payload_rx_nav_pvt.hAcc * 1e-3f; + _gps_position->epv = (float)_buf.payload_rx_nav_pvt.vAcc * 1e-3f; + _gps_position->s_variance_m_s = (float)_buf.payload_rx_nav_pvt.sAcc * 1e-3f; + + _gps_position->vel_m_s = (float)_buf.payload_rx_nav_pvt.gSpeed * 1e-3f; + + _gps_position->vel_n_m_s = (float)_buf.payload_rx_nav_pvt.velN * 1e-3f; + _gps_position->vel_e_m_s = (float)_buf.payload_rx_nav_pvt.velE * 1e-3f; + _gps_position->vel_d_m_s = (float)_buf.payload_rx_nav_pvt.velD * 1e-3f; + _gps_position->vel_ned_valid = true; + + _gps_position->cog_rad = (float)_buf.payload_rx_nav_pvt.headMot * M_DEG_TO_RAD_F * 1e-5f; + _gps_position->c_variance_rad = (float)_buf.payload_rx_nav_pvt.headAcc * M_DEG_TO_RAD_F * 1e-5f; + + { + /* convert to unix timestamp */ + struct tm timeinfo; + timeinfo.tm_year = _buf.payload_rx_nav_pvt.year - 1900; + timeinfo.tm_mon = _buf.payload_rx_nav_pvt.month - 1; + timeinfo.tm_mday = _buf.payload_rx_nav_pvt.day; + timeinfo.tm_hour = _buf.payload_rx_nav_pvt.hour; + timeinfo.tm_min = _buf.payload_rx_nav_pvt.min; + timeinfo.tm_sec = _buf.payload_rx_nav_pvt.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 = _buf.payload_rx_nav_pvt.nano; + 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)(_buf.payload_rx_nav_pvt.nano * 1e-3f); + } + + _gps_position->timestamp_time = hrt_absolute_time(); + _gps_position->timestamp_velocity = hrt_absolute_time(); + _gps_position->timestamp_variance = hrt_absolute_time(); + _gps_position->timestamp_position = hrt_absolute_time(); + + _rate_count_vel++; + _rate_count_lat_lon++; + + _got_posllh = true; + _got_velned = true; + + ret = 1; + break; + case UBX_MSG_NAV_POSLLH: UBX_TRACE_RXMSG("Rx NAV-POSLLH\n"); @@ -697,7 +785,6 @@ UBX::payload_rx_done(void) _gps_position->fix_type = _buf.payload_rx_nav_sol.gpsFix; _gps_position->s_variance_m_s = (float)_buf.payload_rx_nav_sol.sAcc * 1e-2f; // from cm to m - _gps_position->p_variance_m = (float)_buf.payload_rx_nav_sol.pAcc * 1e-2f; // from cm to m _gps_position->satellites_used = _buf.payload_rx_nav_sol.numSV; _gps_position->timestamp_variance = hrt_absolute_time(); diff --git a/src/drivers/gps/ubx.h b/src/drivers/gps/ubx.h index 65f2dd2ba9..219a5762aa 100644 --- a/src/drivers/gps/ubx.h +++ b/src/drivers/gps/ubx.h @@ -183,7 +183,7 @@ typedef struct { uint32_t reserved2; } ubx_payload_rx_nav_sol_t; -/* Rx NAV-PVT */ +/* Rx NAV-PVT (ubx8) */ typedef struct { uint32_t iTOW; /**< GPS Time of Week [ms] */ uint16_t year; /**< Year (UTC)*/ @@ -215,9 +215,11 @@ typedef struct { uint16_t pDOP; /**< Position DOP [0.01] */ uint16_t reserved2; uint32_t reserved3; - int32_t headVeh; /**< Heading of vehicle (2-D) [1e-5 deg] */ - uint32_t reserved4; + int32_t headVeh; /**< (ubx8+ only) Heading of vehicle (2-D) [1e-5 deg] */ + uint32_t reserved4; /**< (ubx8+ only) */ } ubx_payload_rx_nav_pvt_t; +#define UBX_PAYLOAD_RX_NAV_PVT_SIZE_UBX7 (sizeof(ubx_payload_rx_nav_pvt_t) - 8) +#define UBX_PAYLOAD_RX_NAV_PVT_SIZE_UBX8 (sizeof(ubx_payload_rx_nav_pvt_t)) /* Rx NAV-TIMEUTC */ typedef struct { @@ -395,6 +397,7 @@ typedef struct { /* General message and payload buffer union */ typedef union { + ubx_payload_rx_nav_pvt_t payload_rx_nav_pvt; ubx_payload_rx_nav_posllh_t payload_rx_nav_posllh; ubx_payload_rx_nav_sol_t payload_rx_nav_sol; ubx_payload_rx_nav_timeutc_t payload_rx_nav_timeutc; @@ -533,6 +536,7 @@ private: uint16_t _ack_waiting_msg; ubx_buf_t _buf; uint32_t _ubx_version; + bool _use_nav_pvt; }; #endif /* UBX_H_ */ diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 53a1638a31..6d361052c6 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -741,7 +741,6 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg) hil_gps.timestamp_variance = timestamp; hil_gps.s_variance_m_s = 5.0f; - hil_gps.p_variance_m = hil_gps.eph * hil_gps.eph; hil_gps.timestamp_velocity = timestamp; hil_gps.vel_m_s = (float)gps.vel * 1e-2f; // from cm/s to m/s diff --git a/src/modules/uORB/topics/vehicle_gps_position.h b/src/modules/uORB/topics/vehicle_gps_position.h index dbd59f4f37..80d65cd697 100644 --- a/src/modules/uORB/topics/vehicle_gps_position.h +++ b/src/modules/uORB/topics/vehicle_gps_position.h @@ -61,7 +61,6 @@ struct vehicle_gps_position_s { uint64_t timestamp_variance; float s_variance_m_s; /**< speed accuracy estimate m/s */ - float p_variance_m; /**< position accuracy estimate m */ float c_variance_rad; /**< course accuracy estimate rad */ uint8_t fix_type; /**< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. */