From b37ecc03ab8477d6a4e1b42b21fec78cc5ff61de Mon Sep 17 00:00:00 2001 From: Adam Shapiro Date: Mon, 2 Dec 2024 18:28:15 -0500 Subject: [PATCH] Redefined GNSS attitude messages and deprecated old IDs. - Removed redundant heading convenience fields (compute from the ENU vector or yaw) - Added YPR std dev - Added baseline distance std dev --- python/fusion_engine_client/messages/defs.py | 6 +- .../messages/measurements.py | 66 +++++++++++-------- src/point_one/fusion_engine/messages/defs.h | 7 +- .../fusion_engine/messages/measurements.h | 30 ++++----- 4 files changed, 57 insertions(+), 52 deletions(-) diff --git a/python/fusion_engine_client/messages/defs.py b/python/fusion_engine_client/messages/defs.py index 63e53ad8..bc4ec613 100644 --- a/python/fusion_engine_client/messages/defs.py +++ b/python/fusion_engine_client/messages/defs.py @@ -99,10 +99,12 @@ class MessageType(IntEnum): # Sensor measurement messages. IMU_OUTPUT = 11000 - RAW_GNSS_ATTITUDE_OUTPUT = 11001 + DEPRECATED_RAW_HEADING_OUTPUT = 11001 RAW_IMU_OUTPUT = 11002 - GNSS_ATTITUDE_OUTPUT = 11003 + DEPRECATED_HEADING_OUTPUT = 11003 IMU_INPUT = 11004 + GNSS_ATTITUDE_OUTPUT = 11005 + RAW_GNSS_ATTITUDE_OUTPUT = 11006 # Vehicle measurement messages. DEPRECATED_WHEEL_SPEED_MEASUREMENT = 11101 diff --git a/python/fusion_engine_client/messages/measurements.py b/python/fusion_engine_client/messages/measurements.py index bf831924..1b1ae9c7 100644 --- a/python/fusion_engine_client/messages/measurements.py +++ b/python/fusion_engine_client/messages/measurements.py @@ -1,3 +1,4 @@ +import math import struct from typing import Sequence @@ -1176,7 +1177,7 @@ class GNSSAttitudeOutput(MessagePayload): MESSAGE_TYPE = MessageType.GNSS_ATTITUDE_OUTPUT MESSAGE_VERSION = 0 - _STRUCT = struct.Struct(' (bytes, int): if buffer is None: @@ -1216,7 +1214,9 @@ def pack(self, buffer: bytes = None, offset: int = 0, return_buffer: bool = True self.ypr_deg[0], self.ypr_deg[1], self.ypr_deg[2], - self.heading_true_north_deg) + self.ypr_std_deg[0], + self.ypr_std_deg[1], + self.ypr_std_deg[2]) offset += self._STRUCT.size if return_buffer: @@ -1234,7 +1234,9 @@ def unpack(self, buffer: bytes, offset: int = 0, message_version: int = MessageP self.ypr_deg[0], self.ypr_deg[1], self.ypr_deg[2], - self.heading_true_north_deg) = \ + self.ypr_std_deg[0], + self.ypr_std_deg[1], + self.ypr_std_deg[2]) = \ self._STRUCT.unpack_from(buffer, offset) offset += self._STRUCT.size @@ -1244,15 +1246,16 @@ def unpack(self, buffer: bytes, offset: int = 0, message_version: int = MessageP def __repr__(self): result = super().__repr__()[:-1] - result += f', solution_type={self.solution_type}, heading={self.heading_true_north_deg:.1f} deg]' + ypr_str = '(%.1f, %.1f, %.1f)' % tuple(self.ypr_deg) + result += f', solution_type={self.solution_type}, ypr={ypr_str} deg]' return result def __str__(self): return f"""\ GNSS Attitude Output @ {str(self.details.p1_time)} Solution Type: {self.solution_type} - YPR (ENU) (deg): {self.ypr_deg[0]:.2f}, {self.ypr_deg[1]:.2f}, {self.ypr_deg[2]:.2f} - Heading (deg): {self.heading_true_north_deg:.2f}""" + YPR (deg): {self.ypr_deg[0]:.2f}, {self.ypr_deg[1]:.2f}, {self.ypr_deg[2]:.2f} + YPR std (deg): {self.ypr_std_deg[0]:.2f}, {self.ypr_std_deg[1]:.2f}, {self.ypr_std_deg[2]:.2f}""" @classmethod def calcsize(cls) -> int: @@ -1264,8 +1267,9 @@ def to_numpy(cls, messages: Sequence['GNSSAttitudeOutput']): 'solution_type': np.array([int(m.solution_type) for m in messages], dtype=int), 'flags': np.array([int(m.flags) for m in messages], dtype=np.uint32), 'ypr_deg': np.array([m.ypr_deg for m in messages]).T, - 'heading_true_north_deg': np.array([m.heading_true_north_deg for m in messages], dtype=float), + 'ypr_std_deg': np.array([m.ypr_std_deg for m in messages]).T, } + result['heading_true_north_deg'] = yaw_to_heading(result['ypr_deg']) result.update(MeasurementDetails.to_numpy([m.details for m in messages])) return result @@ -1292,23 +1296,23 @@ def __init__(self): # The position of the secondary GNSS antenna relative to the primary antenna (in meters), resolved with respect # to the local ENU tangent plane: east, north, up. self.relative_position_enu_m = np.full((3,), np.nan) + ## # The position standard deviation (in meters), resolved with respect to the # local ENU tangent plane: east, north, up. self.position_std_enu_m = np.full((3,), np.nan) - ## - # The heading between the primary device antenna and the secondary (in degrees) with - # respect to true north. - # - # @note - # Reported in the range [0, 360). - self.heading_true_north_deg = np.nan - ## # The estimated distance between primary and secondary antennas (in meters). self.baseline_distance_m = np.nan + ## + # The standard deviation of the baseline distance estimate (in meters). + self.baseline_distance_std_m = np.nan + + def get_heading_deg(self): + return math.degrees(math.atan2(self.relative_position_enu_m[1], self.relative_position_enu_m[0])) + def pack(self, buffer: bytes = None, offset: int = 0, return_buffer: bool = True) -> (bytes, int): if buffer is None: buffer = bytearray(self.calcsize()) @@ -1329,8 +1333,8 @@ def pack(self, buffer: bytes = None, offset: int = 0, return_buffer: bool = True self.position_std_enu_m[0], self.position_std_enu_m[1], self.position_std_enu_m[2], - self.heading_true_north_deg, - self.baseline_distance_m) + self.baseline_distance_m, + self.baseline_distance_std_m) offset += self._STRUCT.size if return_buffer: @@ -1351,8 +1355,8 @@ def unpack(self, buffer: bytes, offset: int = 0, message_version: int = MessageP self.position_std_enu_m[0], self.position_std_enu_m[1], self.position_std_enu_m[2], - self.heading_true_north_deg, - self.baseline_distance_m) = self._STRUCT.unpack_from(buffer, offset) + self.baseline_distance_m, + self.baseline_distance_std_m) = self._STRUCT.unpack_from(buffer, offset) offset += self._STRUCT.size self.solution_type = SolutionType(solution_type_int) @@ -1361,7 +1365,9 @@ def unpack(self, buffer: bytes, offset: int = 0, message_version: int = MessageP def __repr__(self): result = super().__repr__()[:-1] - result += f', solution_type={self.solution_type}, heading={self.heading_true_north_deg:.1f} deg, ' \ + enu_str = '(%.2f, %.2f, %.3f)' % tuple(self.relative_position_enu_m) + heading_deg = self.get_heading_deg() + result += f', solution_type={self.solution_type}, enu={enu_str} m, heading={heading_deg:.1f} deg, ' \ f'baseline={self.baseline_distance_m} m]' return result @@ -1371,8 +1377,9 @@ def __str__(self): Solution Type: {self.solution_type} Relative position (ENU) (m): {self.relative_position_enu_m[0]:.2f}, {self.relative_position_enu_m[1]:.2f}, {self.relative_position_enu_m[2]:.2f} Position std (ENU) (m): {self.position_std_enu_m[0]:.2f}, {self.position_std_enu_m[1]:.2f}, {self.position_std_enu_m[2]:.2f} - Heading (deg): {self.heading_true_north_deg:.2f} - Baseline distance (m): {self.baseline_distance_m:.2f}""" + Heading (deg): {self.get_heading_deg():.2f} + Baseline distance (m): {self.baseline_distance_m:.2f} + Baseline std (m): {self.baseline_distance_std_m:.2f}""" @classmethod def calcsize(cls) -> int: @@ -1385,8 +1392,9 @@ def to_numpy(cls, messages: Sequence['RawGNSSAttitudeOutput']): 'flags': np.array([int(m.flags) for m in messages], dtype=np.uint32), 'relative_position_enu_m': np.array([m.relative_position_enu_m for m in messages]).T, 'position_std_enu_m': np.array([m.position_std_enu_m for m in messages]).T, - 'heading_true_north_deg': np.array([float(m.heading_true_north_deg) for m in messages]), 'baseline_distance_m': np.array([float(m.baseline_distance_m) for m in messages]), + 'baseline_distance_std_m': np.array([float(m.baseline_distance_std_m) for m in messages]), + 'heading_true_north_deg': np.array([m.get_heading_deg() for m in messages]).T, } result.update(MeasurementDetails.to_numpy([m.details for m in messages])) return result diff --git a/src/point_one/fusion_engine/messages/defs.h b/src/point_one/fusion_engine/messages/defs.h index bcf22dd1..d7b9d3bc 100644 --- a/src/point_one/fusion_engine/messages/defs.h +++ b/src/point_one/fusion_engine/messages/defs.h @@ -47,11 +47,12 @@ enum class MessageType : uint16_t { // Sensor measurement messages. IMU_OUTPUT = 11000, ///< @ref IMUOutput - RAW_GNSS_ATTITUDE_OUTPUT = 11001, ///< @ref RawGNSSAttitudeOutput + DEPRECATED_RAW_HEADING_OUTPUT = 11001, RAW_IMU_OUTPUT = 11002, ///< @ref RawIMUOutput - // TODO Change the numbers - GNSS_ATTITUDE_OUTPUT = 11003, ///< @ref GNSSAttitudeOutput + DEPRECATED_HEADING_OUTPUT = 11003, IMU_INPUT = 11004, ///< @ref IMUInput + GNSS_ATTITUDE_OUTPUT = 11005, ///< @ref GNSSAttitudeOutput + RAW_GNSS_ATTITUDE_OUTPUT = 11006, ///< @ref RawGNSSAttitudeOutput // Vehicle measurement messages. DEPRECATED_WHEEL_SPEED_MEASUREMENT = diff --git a/src/point_one/fusion_engine/messages/measurements.h b/src/point_one/fusion_engine/messages/measurements.h index d7f74bb0..a1e766e7 100644 --- a/src/point_one/fusion_engine/messages/measurements.h +++ b/src/point_one/fusion_engine/messages/measurements.h @@ -1189,19 +1189,17 @@ struct P1_ALIGNAS(4) GNSSAttitudeOutput : public MessagePayload { * * If any angles are not available, they will be set to `NAN`. For * dual-antenna systems, the device will measure yaw and pitch, but not roll. + * + * Note that yaw is measured from east in a counter-clockwise direction. For + * example, north is +90 degrees. Heading with respect to true north can be + * computed as `heading = 90.0 - ypr_deg[0]`. */ float ypr_deg[3] = {NAN, NAN, NAN}; /** - * The heading angle (in degrees) with respect to true north, pointing from - * the primary antenna to the secondary antenna. - * - * The reported angle includes the user-specified heading offset correction. - * - * @note - * Reported in the range [0, 360). + * The standard deviation of the orientation measurement (in degrees). */ - float heading_true_north_deg = NAN; + float ypr_std_deg[3] = {NAN, NAN, NAN}; }; /** @@ -1253,24 +1251,20 @@ struct P1_ALIGNAS(4) RawGNSSAttitudeOutput : public MessagePayload { float relative_position_enu_m[3] = {NAN, NAN, NAN}; /** - * The position standard deviation (in meters), resolved with respect to the - * local ENU tangent plane: east, north, up. + * The standard deviation of the relative position vector (in meters), + * resolved with respect to the local ENU tangent plane: east, north, up. */ float position_std_enu_m[3] = {NAN, NAN, NAN}; /** - * The measured heading angle (in degrees) with respect to true north, - * pointing from the primary antenna to the secondary antenna. - * - * @note - * Reported in the range [0, 360). + * The estimated distance between primary and secondary antennas (in meters). */ - float heading_true_north_deg = NAN; + float baseline_distance_m = NAN; /** - * The estimated distance between primary and secondary antennas (in meters). + * The standard deviation of the baseline distance estimate (in meters). */ - float baseline_distance_m = NAN; + float baseline_distance_std_m = NAN; }; ////////////////////////////////////////////////////////////////////////////////