From a97b3dc933c7bcdf59f49c641fb7f7feb3ff312e 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 - Compute heading angle as needed in Analyzer --- .../fusion_engine_client/analysis/analyzer.py | 14 ++-- python/fusion_engine_client/messages/defs.py | 6 +- .../messages/measurements.py | 64 ++++++++++--------- src/point_one/fusion_engine/messages/defs.h | 21 ++++-- .../fusion_engine/messages/measurements.h | 30 ++++----- 5 files changed, 74 insertions(+), 61 deletions(-) diff --git a/python/fusion_engine_client/analysis/analyzer.py b/python/fusion_engine_client/analysis/analyzer.py index 3f4eb570..c9e68878 100755 --- a/python/fusion_engine_client/analysis/analyzer.py +++ b/python/fusion_engine_client/analysis/analyzer.py @@ -2012,10 +2012,11 @@ def plot_gnss_attitude_measurements(self): # Corrected heading plot if len(heading_data.p1_time) > 0: heading_time = heading_data.p1_time - float(self.t0) + heading_deg = 90.0 - heading_data.ypr_deg[0, :] fig.add_trace( go.Scatter( x=heading_time, - y=heading_data.heading_true_north_deg, + y=heading_deg, customdata=heading_data.p1_time, mode='markers', marker={'size': 2, "color": "green"}, @@ -2030,6 +2031,9 @@ def plot_gnss_attitude_measurements(self): # Uncorrected heading plot if len(raw_heading_data.p1_time) > 0: raw_heading_time = raw_heading_data.p1_time - float(self.t0) + raw_heading_deg = np.degrees(np.arctan2(raw_heading_data.relative_position_enu_m[1, :], + raw_heading_data.relative_position_enu_m[0, :])) + # Compute heading uncertainty envelop. denom = raw_heading_data.relative_position_enu_m[0]**2 + raw_heading_data.relative_position_enu_m[1]**2 dh_e = raw_heading_data.relative_position_enu_m[0] / denom @@ -2047,7 +2051,7 @@ def plot_gnss_attitude_measurements(self): fig.add_trace( go.Scatter( x=raw_heading_time, - y=raw_heading_data.heading_true_north_deg, + y=raw_heading_deg, customdata=raw_heading_data.p1_time, mode='markers', marker={'size': 2, "color": "red"}, @@ -2058,12 +2062,12 @@ def plot_gnss_attitude_measurements(self): ), row=1, col=1 ) - idx = ~np.isnan(raw_heading_data.heading_true_north_deg) + idx = ~np.isnan(raw_heading_deg) fig.add_trace( go.Scatter( x=raw_heading_time[idx], - y=raw_heading_data.heading_true_north_deg[idx] + envelope[idx], + y=raw_heading_deg[idx] + envelope[idx], mode='lines', marker={'size': 2, "color": "red"}, line=dict(width=0), @@ -2077,7 +2081,7 @@ def plot_gnss_attitude_measurements(self): fig.add_trace( go.Scatter( x=raw_heading_time[idx], - y=raw_heading_data.heading_true_north_deg[idx] - envelope[idx], + y=raw_heading_deg[idx] - envelope[idx], mode='lines', marker={'size': 2, "color": "red"}, line=dict(width=0), 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..e45ce835 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,7 +1267,7 @@ 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.update(MeasurementDetails.to_numpy([m.details for m in messages])) return result @@ -1292,23 +1295,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 +1332,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 +1354,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 +1364,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 +1376,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 +1391,8 @@ 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]), } 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..255820a1 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 = @@ -152,18 +153,24 @@ P1_CONSTEXPR_FUNC const char* to_string(MessageType type) { case MessageType::IMU_OUTPUT: return "IMU Output"; - case MessageType::RAW_GNSS_ATTITUDE_OUTPUT: - return "Raw GNSS Attitude Output"; + case MessageType::DEPRECATED_RAW_HEADING_OUTPUT: + return "Raw GNSS Heading Output"; case MessageType::RAW_IMU_OUTPUT: return "Raw IMU Output"; - case MessageType::GNSS_ATTITUDE_OUTPUT: - return "GNSS Attitude Output"; + case MessageType::DEPRECATED_HEADING_OUTPUT: + return "GNSS Heading Output"; case MessageType::IMU_INPUT: return "IMU Input"; + case MessageType::GNSS_ATTITUDE_OUTPUT: + return "GNSS Attitude Output"; + + case MessageType::RAW_GNSS_ATTITUDE_OUTPUT: + return "Raw GNSS Attitude Output"; + case MessageType::DEPRECATED_WHEEL_SPEED_MEASUREMENT: return "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; }; ////////////////////////////////////////////////////////////////////////////////