From 2587b13ae7f6860696ff30272edd51060d98ae8d Mon Sep 17 00:00:00 2001 From: Michael du Breuil Date: Tue, 3 Sep 2019 12:32:09 -0700 Subject: [PATCH] AP_GPS: UBLOX: Parse RELPOSNED messages This does not yet: - validate the receiver configuration - manage timing out stale GPS heading info - relPosNormalized usage isn't clear, which may defeat the STRICT_LENGTH_CHECK --- libraries/AP_GPS/AP_GPS.h | 13 ++++++---- libraries/AP_GPS/AP_GPS_UBLOX.cpp | 42 +++++++++++++++++++++++++++++++ libraries/AP_GPS/AP_GPS_UBLOX.h | 38 ++++++++++++++++++++++++++++ 3 files changed, 88 insertions(+), 5 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h index 68ef226d64..bac34602c7 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/libraries/AP_GPS/AP_GPS.h @@ -145,11 +145,13 @@ public: float speed_accuracy; ///< 3D velocity RMS accuracy estimate in m/s float horizontal_accuracy; ///< horizontal RMS accuracy estimate in m float vertical_accuracy; ///< vertical RMS accuracy estimate in m + float gps_yaw_accuracy; ///< heading accuracy of the GPS in degrees bool have_vertical_velocity; ///< does GPS give vertical velocity? Set to true only once available. bool have_speed_accuracy; ///< does GPS give speed accuracy? Set to true only once available. bool have_horizontal_accuracy; ///< does GPS give horizontal position accuracy? Set to true only once available. bool have_vertical_accuracy; ///< does GPS give vertical position accuracy? Set to true only once available. bool have_gps_yaw; ///< does GPS give yaw? Set to true only once available. + bool have_gps_yaw_accuracy; ///< does the GPS give a heading accuracy estimate? Set to true only once available uint32_t last_gps_time_ms; ///< the system time we got the last GPS timestamp, milliseconds uint32_t uart_timestamp_ms; ///< optional timestamp from set_uart_timestamp() @@ -270,11 +272,12 @@ public: return false; } yaw_deg = state[instance].gps_yaw; - // None of the GPS backends can provide this yet, so we hard - // code a fixed value of 10 degrees, which seems like a - // reasonable guess. Once a backend can provide a proper - // estimate we can implement it - accuracy_deg = 10; + if (state[instance].have_gps_yaw_accuracy) { + accuracy_deg = state[instance].gps_yaw_accuracy; + } else { + // fall back to 10 degrees as a generic default + accuracy_deg = 10; + } return true; } bool gps_yaw_deg(float &yaw_deg, float &accuracy_deg) const { diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp index 74cce72ad0..48f6db7a4d 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.cpp +++ b/libraries/AP_GPS/AP_GPS_UBLOX.cpp @@ -1032,8 +1032,50 @@ AP_GPS_UBLOX::_parse_gps(void) state.hdop = 130; #endif break; + case MSG_RELPOSNED: + { + const Vector3f &offset0 = gps._antenna_offset[0].get(); + const Vector3f &offset1 = gps._antenna_offset[1].get(); + const uint32_t valid_mask = static_cast(RELPOSNED::relPosHeadingValid) | + static_cast(RELPOSNED::relPosValid) | + static_cast(RELPOSNED::gnssFixOK) | + static_cast(RELPOSNED::isMoving) | + (2 << static_cast(RELPOSNED::carrSoln)); // require fixed ambiguity + const uint32_t invalid_mask = static_cast(RELPOSNED::refPosMiss) | + static_cast(RELPOSNED::refObsMiss); + + if (((_buffer.relposned.flags & valid_mask) == valid_mask) && + ((_buffer.relposned.flags & invalid_mask) == 0) +#if defined(STRICT_LENGTH_CHECK) + // force the estimated position to be within + && (fabsf(offset0.distance(offset1) - (_buffer.relposned.relPosLength * 1e-2)) < STRICT_LENGTH_CHECK) +#endif // defined(STRICT_LENGTH_CHECK) + ) { + float rotation_offset_rad; + if (offset0.is_zero()) { + rotation_offset_rad = Vector2f(offset1.x, offset1.y).angle(); + } else if (offset1.is_zero()) { + rotation_offset_rad = Vector2f(offset0.x, offset0.y).angle(); + } else { + const Vector3f diff = offset0 - offset1; + rotation_offset_rad = Vector2f(diff.x, diff.y).angle(); + } + if (state.instance != 0) { + rotation_offset_rad += M_PI; + } + state.gps_yaw = wrap_360(_buffer.relposned.relPosHeading * 1e-5 + degrees(rotation_offset_rad)); + state.have_gps_yaw = true; + state.gps_yaw_accuracy = _buffer.relposned.accHeading * 1e-5; + state.have_gps_yaw_accuracy = true; + } else { + state.have_gps_yaw = false; + state.have_gps_yaw_accuracy = false; + } + } + break; case MSG_PVT: Debug("MSG_PVT"); + havePvtMsg = true; // position _check_new_itow(_buffer.pvt.itow); diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.h b/libraries/AP_GPS/AP_GPS_UBLOX.h index d57ca83c4b..4cb4b99a12 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.h +++ b/libraries/AP_GPS/AP_GPS_UBLOX.h @@ -281,6 +281,30 @@ private: uint32_t headVeh; uint8_t reserved2[4]; }; + struct PACKED ubx_nav_relposned { + uint8_t version; + uint8_t reserved1; + uint16_t refStationId; + uint32_t iTOW; + int32_t relPosN; + int32_t relPosE; + int32_t relPosD; + int32_t relPosLength; + int32_t relPosHeading; + uint8_t reserved2[4]; + int8_t relPosHPN; + int8_t relPosHPE; + int8_t relPosHPD; + int8_t relPosHPLength; + uint32_t accN; + uint32_t accE; + uint32_t accD; + uint32_t accLength; + uint32_t accHeading; + uint8_t reserved3[4]; + uint32_t flags; + }; + struct PACKED ubx_nav_velned { uint32_t itow; // GPS msToW int32_t ned_north; @@ -444,6 +468,7 @@ private: #endif ubx_cfg_sbas sbas; ubx_nav_svinfo_header svinfo_header; + ubx_nav_relposned relposned; #if UBLOX_RXM_RAW_LOGGING ubx_rxm_raw rxm_raw; ubx_rxm_rawx rxm_rawx; @@ -451,6 +476,18 @@ private: ubx_ack_ack ack; } _buffer; + enum class RELPOSNED { + gnssFixOK = 1U << 0, + diffSoln = 1U << 1, + relPosValid = 1U << 2, + carrSoln = 1U << 3, + isMoving = 1U << 5, + refPosMiss = 1U << 6, + refObsMiss = 1U << 7, + relPosHeadingValid = 1U << 8, + relPosNormalized = 1U << 9 + }; + enum ubs_protocol_bytes { PREAMBLE1 = 0xb5, PREAMBLE2 = 0x62, @@ -467,6 +504,7 @@ private: MSG_SOL = 0x6, MSG_PVT = 0x7, MSG_TIMEGPS = 0x20, + MSG_RELPOSNED = 0x3c, MSG_VELNED = 0x12, MSG_CFG_CFG = 0x09, MSG_CFG_RATE = 0x08,